diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..e102057 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "thirdparty/sse2neon"] + path = thirdparty/sse2neon + url = https://github.com/jratcliff63367/sse2neon.git +[submodule "test/googletest"] + path = test/googletest + url = https://github.com/google/googletest.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5cea4e4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,189 @@ +SET(PROJECT_NAME DMVIO) + +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + + +IF(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo) +ENDIF() +set(BUILD_TYPE RelWithDebInfo) + +set(EXECUTABLE_OUTPUT_PATH bin) +set(LIBRARY_OUTPUT_PATH lib) +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) + +# required libraries +find_package(SuiteParse REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost COMPONENTS system thread filesystem chrono serialization date_time timer) +find_package(GTSAM REQUIRED) +find_package(yaml-cpp REQUIRED) + +IF(${Boost_VERSION} GREATER_EQUAL 106500) + message("Building with stacktrace support.") + set(STACKTRACE_LIBRARIES dl) + set(STACKTRACE_DEFINES "-DSTACKTRACE -DBOOST_STACKTRACE_USE_ADDR2LINE") +ENDIF() + +# optional libraries +find_package(LibZip QUIET) +find_package(Pangolin 0.2 QUIET) +find_package(OpenCV QUIET) + + +IF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + MESSAGE(Mac OS X specific code) + include_directories(/usr/local/include) + link_directories(/usr/local/lib) +ENDIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + + +# flags +add_definitions("-DENABLE_SSE") +set(CMAKE_CXX_FLAGS + "${SSE_FLAGS} -std=c++14" +) +set(CMAKE_CXX_FLAGS_DEBUG + "-O2 -g -fno-omit-frame-pointer -DEIGEN_INITIALIZE_MATRICES_WITH_NAN -DDEBUG ${STACKTRACE_DEFINES}" +) +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO + "-O3 -g -fno-omit-frame-pointer ${STACKTRACE_DEFINES}" +) +set(CMAKE_CXX_FLAGS_RELEASE + "-O3 -DNDEBUG" +) + +if (MSVC) + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc") +endif (MSVC) + +set(DSO_SOURCE_DIR ${PROJECT_SOURCE_DIR}/src/dso) + +# DSO Source files +set(dso_SOURCE_FILES + ${DSO_SOURCE_DIR}/FullSystem/FullSystem.cpp + ${DSO_SOURCE_DIR}/FullSystem/FullSystemOptimize.cpp + ${DSO_SOURCE_DIR}/FullSystem/FullSystemOptPoint.cpp + ${DSO_SOURCE_DIR}/FullSystem/FullSystemDebugStuff.cpp + ${DSO_SOURCE_DIR}/FullSystem/FullSystemMarginalize.cpp + ${DSO_SOURCE_DIR}/FullSystem/Residuals.cpp + ${DSO_SOURCE_DIR}/FullSystem/CoarseTracker.cpp + ${DSO_SOURCE_DIR}/FullSystem/CoarseInitializer.cpp + ${DSO_SOURCE_DIR}/FullSystem/ImmaturePoint.cpp + ${DSO_SOURCE_DIR}/FullSystem/HessianBlocks.cpp + ${DSO_SOURCE_DIR}/FullSystem/PixelSelector2.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctional.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedTopHessian.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedSCHessian.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctionalStructs.cpp + ${DSO_SOURCE_DIR}/util/settings.cpp + ${DSO_SOURCE_DIR}/util/Undistort.cpp + ${DSO_SOURCE_DIR}/util/globalCalib.cpp + ) + +set(dmvio_SOURCE_FILES + src/IMU/IMUIntegration.cpp + src/GTSAMIntegration/Sim3GTSAM.cpp + src/IMUInitialization/GravityInitializer.cpp + src/IMU/IMUTypes.cpp + src/IMU/IMUSettings.cpp + src/util/TimeMeasurement.cpp + src/util/SettingsUtil.cpp + src/GTSAMIntegration/BAGTSAMIntegration.cpp + src/IMU/CoarseIMULogic.cpp + src/IMU/BAIMULogic.cpp + src/GTSAMIntegration/PoseTransformation.cpp + src/GTSAMIntegration/Marginalization.cpp + src/GTSAMIntegration/PoseTransformationIMU.cpp + src/GTSAMIntegration/PoseTransformationFactor.cpp + src/IMUInitialization/CoarseIMUInitOptimizer.cpp + src/IMUInitialization/IMUInitializer.cpp + src/IMU/IMUUtils.cpp + src/IMUInitialization/IMUInitSettings.cpp + src/GTSAMIntegration/GTSAMUtils.cpp + src/GTSAMIntegration/DelayedMarginalization.cpp + src/IMUInitialization/PoseGraphBundleAdjustment.cpp + src/GTSAMIntegration/FEJValues.cpp + src/IMUInitialization/IMUInitializerStates.cpp + src/IMUInitialization/IMUInitializerLogic.cpp + src/IMUInitialization/IMUInitializerTransitions.cpp + src/GTSAMIntegration/AugmentedScatter.cpp + ) + + +include_directories( + ${PROJECT_SOURCE_DIR}/src + ${PROJECT_SOURCE_DIR}/src/dso + ${PROJECT_SOURCE_DIR}/thirdparty/Sophus + ${PROJECT_SOURCE_DIR}/thirdparty/sse2neon + ${EIGEN3_INCLUDE_DIR} + ${GTSAM_INCLUDE_DIR} +) + + +# decide if we have pangolin +if (Pangolin_FOUND) + message("--- found PANGOLIN, compiling with pangolin library.") + include_directories( ${Pangolin_INCLUDE_DIRS} ) + set(dso_pangolin_SOURCE_FILES + ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/KeyFrameDisplay.cpp + ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/PangolinDSOViewer.cpp) + set(HAS_PANGOLIN 1) +else () + message("--- could not find PANGOLIN, not compiling with pangolin library.") + message(" this means there will be no 3D display / GUI available for dmvio_dataset.") + set(dso_pangolin_SOURCE_FILES ) + set(HAS_PANGOLIN 0) +endif () + +# decide if we have openCV +if (OpenCV_FOUND) + message("--- found OpenCV, compiling with opencv library.") + include_directories( ${OpenCV_INCLUDE_DIRS} ) + set(dso_opencv_SOURCE_FILES + ${DSO_SOURCE_DIR}/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp + ${DSO_SOURCE_DIR}/IOWrapper/OpenCV/ImageRW_OpenCV.cpp) + set(HAS_OPENCV 1) +else () + message("--- could not find OpenCV, not compiling with opencv library.") + message(" this means there will be no image display, and image read / load functionality.") + set(dso_opencv_SOURCE_FILES + ${DSO_SOURCE_DIR}/IOWrapper/ImageDisplay_dummy.cpp + ${DSO_SOURCE_DIR}/IOWrapper/ImageRW_dummy.cpp) + set(HAS_OPENCV 0) +endif () + +# decide if we have ziplib. +if (LIBZIP_LIBRARY) + message("--- found ziplib (${LIBZIP_VERSION}), compiling with zip capability.") + add_definitions(-DHAS_ZIPLIB=1) + include_directories( ${LIBZIP_INCLUDE_DIR_ZIP} ${LIBZIP_INCLUDE_DIR_ZIPCONF} ) +else() + message("--- not found ziplib (${LIBZIP_LIBRARY}), compiling without zip capability.") + set(LIBZIP_LIBRARY "") +endif() + +# compile main library. +include_directories( ${CSPARSE_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR} ${YAML_CPP_INCLUDE_DIR}) +add_library(dmvio ${dso_SOURCE_FILES} ${dmvio_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SOURCE_FILES}) + + +if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX + set(BOOST_THREAD_LIBRARY boost_thread-mt) +else() + set(BOOST_THREAD_LIBRARY boost_thread) +endif() + + +# build main executable (only if we have both OpenCV and Pangolin) +if (OpenCV_FOUND AND Pangolin_FOUND) + message("--- compiling dmvio_dataset.") + add_executable(dmvio_dataset ${PROJECT_SOURCE_DIR}/src/main_dmvio_dataset.cpp) + set(DMVIO_LINKED_LIBRARIES boost_system cxsparse ${BOOST_THREAD_LIBRARY} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS} gtsam ${YAML_CPP_LIBRARIES} ${STACKTRACE_LIBRARIES}) + target_link_libraries(dmvio_dataset dmvio ${DMVIO_LINKED_LIBRARIES}) +else() + message("--- not building dmvio_dataset, since either don't have openCV or Pangolin.") +endif() + +add_subdirectory(test) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..ef7e7ef --- /dev/null +++ b/LICENSE @@ -0,0 +1,674 @@ +GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + {one line to give the program's name and a brief idea of what it does.} + Copyright (C) {year} {name of author} + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program 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 General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + {project} Copyright (C) {year} {fullname} + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/README.md b/README.md index 12cd2eb..4c04ec0 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,173 @@ -

DM-VIO: Delayed Marginalization
Visual-Inertial Odometry

Paper | Video | Project Page

-[comment]: <> ([**Paper**](todo) | [**Video**](todo) | [**Project Page**](http://vision.in.tum.de/dm-vio)) +When using this project in academic work, please consider citing: + + @article{stumberg22dmvio, + author = {L. von Stumberg and D. Cremers}, + title = {{DM-VIO}: Delayed Marginalization Visual-Inertial Odometry}, + journal = {{IEEE} Robotics and Automation Letters ({RA-L})}, + year = {2022}, + volume = {7}, + number = {2}, + pages = {1408-1415}, + doi = {10.1109/LRA.2021.3140129} + } + +### 1. Related Papers +* **[DM-VIO: Delayed Marginalization Visual-Inertial Odometry](https://vision.in.tum.de/dm-vio)**, L. von Stumberg and D. Cremers, In IEEE Robotics and Automation Letters (RA-L), volume 7, 2022 +* **[Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization](https://vision.in.tum.de/vi-dso)**, L. von Stumberg, V. Usenko and D. Cremers, In International Conference on Robotics and Automation (ICRA), 2018 +* **[Direct Sparse Odometry](https://vision.in.tum.de/dso)**, *J. Engel, V. Koltun, D. Cremers*, In TPAMI, vol. 40, 2018 + +### 2. Installation + + git clone https://github.com/lukasvst/dm-vio.git + +The following instructions have been tested with Ubuntu 20.04. +The system is also known to work well on Ubuntu 16.04, 18.04 and MacOS Big Sur (only Intel Macs have been tested so far). + +#### 2.1 Required Dependencies + +##### Suitesparse, Eigen3, Boost, yaml-cpp (required). +Required, install with + + sudo apt-get install cmake libsuitesparse-dev libeigen3-dev libboost-all-dev libyaml-cpp-dev + +On MacOS we recommend Homebrew to install the dependencies. It might be necessary +to install boost@1.60 instead of the newest boost, in order for the used GTSAM version to work. + + +##### GTSAM (required). +Build from source with + + sudo apt install libtbb-dev + git clone https://github.com/borglab/gtsam.git + cd gtsam + git checkout a738529af9754c7a085903f90ae8559bbaa82e75 + mkdir build && cd build + cmake -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_USE_SYSTEM_EIGEN=ON .. + make -j + sudo make install + +##### OpenCV. +Used to read, write and display images. +Install with + + sudo apt-get install libopencv-dev + + +##### Pangolin. +Like for DSO, this is used for the GUI. You should install v0.6. +Install from [https://github.com/stevenlovegrove/Pangolin](https://github.com/stevenlovegrove/Pangolin) + + + sudo apt install libgl1-mesa-dev libglew-dev pkg-config libegl1-mesa-dev libwayland-dev libxkbcommon-dev wayland-protocols + git clone https://github.com/stevenlovegrove/Pangolin.git + cd Pangolin + git checkout v0.6 + mkdir build + cd build + cmake .. + cmake --build . + sudo make install + + + +#### 2.2 Recommended Dependencies + +##### GTest (optional). +For running tests, install with `git submodule update --init`. + +##### ziplib (optional). +Used to read datasets with images as .zip. +See [src/dso/README.md](src/dso/README.md) for instructions. + +##### sse2neon (required for ARM builds). +After cloning, run `git submodule update --init` to include this. + +#### 2.3 Build + + cd dm-vio + mkdir build + cd build + cmake .. + make -j + +This compiles `dmvio_dataset` to run DM-VIO on datasets (needs both OpenCV and Pangolin installed). +It also compiles the library `libdmvio.a`, which other projects can link to. + +#### Trouble-Shooting +The project is based on DSO and only has two additional dependencies with GTSAM and yaml-cpp. +In case of problems with compilation we recommend trying to compile https://github.com/JakobEngel/dso +first and seeing if it works. + +### 3 Running +Download a TUM-VI sequence (download in the format `Euroc / DSO 512x512`) at https://vision.in.tum.de/data/datasets/visual-inertial-dataset + + bin/dmvio_dataset + files=XXXX/datasetXXXX/dso/cam0/images + vignette=XXXX/datasetXXXX/dso/cam0/vignette.png + imuFile=XXXX/datasetXXXX/dso/imu.txt + gtFile=XXXX/datasetXXXX/dso/gt_imu.csv + calib=PATH_TO_DMVIO/configs/tumvi_calib/camera02.txt + gamma=PATH_TO_DMVIO/configs/tumvi_calib/pcalib.txt + imuCalib=PATH_TO_DMVIO/configs/tumvi_calib/camchain.yaml + mode=0 + use16Bit=1 + preset=0 # use 1 for realtime + nogui=0 # use 1 to enable GUI + resultsPrefix=/PATH_TO_RESULTS/ + settingsFile=PATH_TO_DMVIO/configs/tumvi.yaml + start=2 + +Instead of typing this long command you can use the [python tools](https://github.com/lukasvst/dm-vio-python-tools). + +#### Running on EuRoC, 4Seasons, reproducing paper results, etc. +We **strongly recommend** using the python-dm-vio tools published at: https://github.com/lukasvst/dm-vio-python-tools + +They can be used to +* prepare the EuRoC and 4Seasons sequences for usage with DM-VIO. +* run on all (or some) sequences of EuRoC, TUM-VI and 4Seasons and gather the results. +* create a Python evaluation script for inspecting the results and generating the plots shown in the paper. + +#### Commandline arguments +There are two types of commandline arguments: +1. Main arguments defined `in main_dmvio_dataset.cpp` (see `parseArgument`). Most of these are derived from +DSO, so you can read [src/dso/README.md](src/dso/README.md) for documentation on them. +2. Lots of additional settings are defined using the `SettingsUtil`. They can be set either using comandline +or by placing them in the yaml file defined with the commandline argument `settingsFile`. +All of them are printed to commandline when the program starts (and also into the file `usedSettingsdso.txt`). +Most of these are documented in the header file they are defined in +(see `src/IMU/IMUSettings.h`, `src/IMUInitialization/IMUInitSettings.h`). + +#### Running on your own datasets +To run on your own dataset you need +* to pass the folder containing files with `files=...` +* an accurate camera calibration! For tips on calibration and the format of camera.txt see +[src/dso/README.md](src/dso/README.md). +* to set the `mode=1` unless you have a photometric calibration (vignette.png and pcalib.txt). +* a file times.txt which contains **exactly** one timestamp for each image in the image folder. + +When enabling IMU data you also need + +* IMU calibration (transformation between camera and IMU) as a `camchain.yaml`. Note that only the field `cam0/T_cam_imu` +and optionally the noise values are read from this file. +* a file containing IMU data. For each image it **must** contain an IMU 'measurement' with exactly the same timestamp. +If the sensor does not output this, a fake measurement with this timestamp has to be interpolated in advance. + The [DM-VIO python tools](https://github.com/lukasvst/dm-vio-python-tools) contain a script to do this. +* You should also set the IMU noise values (see `configs/tumvi.yaml`, `configs/euroc.yaml`, and `configs/4seasons.yaml`). +You can read them from an Allan-Variance plot (either computed yourself or taken from datasheet of IMU). +Note that often times these values are too small in practice and should be inflated by a large factor for optimal results. +You can first set `useimu=0` to try the visual-only system (basically DSO). If this does not work well for +comparably slow motions, there is likely a problem with camera calibration which should be addressed first. -### Official repository, code will be published soon. ### +### 4 License +DM-VIO is based on Direct Sparse Odometry (DSO), which was developed by Jakob Engel +at the Technical University of Munich and Intel. +Like DSO, DM-VIO is licensed under the GNU General Public License +Version 3 (GPLv3). diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake new file mode 100644 index 0000000..9c546a0 --- /dev/null +++ b/cmake/FindEigen3.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) + diff --git a/cmake/FindLibZip.cmake b/cmake/FindLibZip.cmake new file mode 100644 index 0000000..76ab224 --- /dev/null +++ b/cmake/FindLibZip.cmake @@ -0,0 +1,37 @@ +# Finds libzip. +# +# This module defines: +# LIBZIP_INCLUDE_DIR_ZIP +# LIBZIP_INCLUDE_DIR_ZIPCONF +# LIBZIP_LIBRARY +# + +find_package(PkgConfig) +pkg_check_modules(PC_LIBZIP QUIET libzip) + +find_path(LIBZIP_INCLUDE_DIR_ZIP + NAMES zip.h + HINTS ${PC_LIBZIP_INCLUDE_DIRS}) + +find_path(LIBZIP_INCLUDE_DIR_ZIPCONF + NAMES zipconf.h + HINTS ${PC_LIBZIP_INCLUDE_DIRS}) + +find_library(LIBZIP_LIBRARY + NAMES zip) + +include(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( + LIBZIP DEFAULT_MSG + LIBZIP_LIBRARY LIBZIP_INCLUDE_DIR_ZIP LIBZIP_INCLUDE_DIR_ZIPCONF) + +set(LIBZIP_VERSION 0) + +if (LIBZIP_INCLUDE_DIR_ZIPCONF) + FILE(READ "${LIBZIP_INCLUDE_DIR_ZIPCONF}/zipconf.h" _LIBZIP_VERSION_CONTENTS) + if (_LIBZIP_VERSION_CONTENTS) + STRING(REGEX REPLACE ".*#define LIBZIP_VERSION \"([0-9.]+)\".*" "\\1" LIBZIP_VERSION "${_LIBZIP_VERSION_CONTENTS}") + endif () +endif () + +set(LIBZIP_VERSION ${LIBZIP_VERSION} CACHE STRING "Version number of libzip") diff --git a/cmake/FindSuiteParse.cmake b/cmake/FindSuiteParse.cmake new file mode 100644 index 0000000..b39eaec --- /dev/null +++ b/cmake/FindSuiteParse.cmake @@ -0,0 +1,128 @@ +FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h + PATHS + ${SUITE_SPARSE_ROOT}/include + /usr/include/suitesparse + /usr/include/ufsparse + /opt/local/include/ufsparse + /usr/local/include/ufsparse + /sw/include/ufsparse + ) + +FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +FIND_LIBRARY(CAMD_LIBRARY NAMES camd + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + +# Different platforms seemingly require linking against different sets of libraries +IF(CYGWIN) + FIND_PACKAGE(PkgConfig) + FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + PKG_CHECK_MODULES(LAPACK lapack) + + SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) + +# MacPorts build of the SparseSuite requires linking against extra libraries + +ELSEIF(APPLE) + + FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + FIND_LIBRARY(METIS_LIBRARY NAMES metis + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") +ELSE(APPLE) + SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) +ENDIF(CYGWIN) + +IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + SET(CHOLMOD_FOUND TRUE) +ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + SET(CHOLMOD_FOUND FALSE) +ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + +# Look for csparse; note the difference in the directory specifications! +FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h + PATHS + /usr/include/suitesparse + /usr/include + /opt/local/include + /usr/local/include + /sw/include + /usr/include/ufsparse + /opt/local/include/ufsparse + /usr/local/include/ufsparse + /sw/include/ufsparse + ) + +FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) + SET(CSPARSE_FOUND TRUE) +ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) + SET(CSPARSE_FOUND FALSE) +ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) + diff --git a/configs/4seasons.yaml b/configs/4seasons.yaml new file mode 100644 index 0000000..65c7f13 --- /dev/null +++ b/configs/4seasons.yaml @@ -0,0 +1,8 @@ +accelerometer_noise_density: 0.10200528 +gyroscope_noise_density: 4.120916e-02 +accelerometer_random_walk: 9.8082e-04 +gyroscope_random_walk: 3.8785e-04 +integration_sigma: 0.316227 +setting_weightZeroPriorDSOInitY: 5e09 +setting_weightZeroPriorDSOInitX: 5e09 +setting_forceNoKFTranslationThresh: 0.01 \ No newline at end of file diff --git a/configs/4seasons_calib/camchain.yaml b/configs/4seasons_calib/camchain.yaml new file mode 100644 index 0000000..592a90e --- /dev/null +++ b/configs/4seasons_calib/camchain.yaml @@ -0,0 +1,31 @@ +cam0: + cam_overlaps: [1] + T_cam_imu: + - [-0.9998852242642406, -0.013522961078544133, 0.006831385051241187, 0.17541216744862287] + - [-0.006890161859766396, 0.004304637029338462, -0.9999669973402087, 0.0036894333751345677] + - [0.01349310815180704, -0.9998992947410829, -0.004397318352110671, -0.05810612695941222] + - [0.0, 0.0, 0.0, 1.0] + intrinsics: [527.9990706330082, 527.963495807245, 399.18451401412665, 172.8193108347693] + resolution: [800, 400] + distortion_coeffs: [-0.03559759964255725, -0.005093721310999416, 0.019716282737702494, -0.01583280039499382] + camera_model: pinhole + timeshift_cam_imu: -4.926753519571215e-06 + distortion_model: equidistant +cam1: + cam_overlaps: [0] + T_cam_imu: + - [-0.9999948729203221, -0.00041104034603049364, -0.0031757170690647394, -0.12432919293024627] + - [0.003173467277625304, 0.005362979695895791, -0.9999805835886111, 0.0023471917960152505] + - [0.0004280636712633416, -0.9999855346426845, -0.005361647776383633, -0.05662052461099226] + - [0.0, 0.0, 0.0, 1.0] + intrinsics: [529.2496538273606, 529.4013679656194, 412.4733148308946, 172.1405434152354] + resolution: [800, 400] + distortion_coeffs: [-0.03696481607810142, 0.0102400942942071, -0.01632902510720569, 0.009597717025035516] + camera_model: pinhole + timeshift_cam_imu: -4.5217727146455115e-06 + distortion_model: equidistant +T_cam1_cam0: + - [0.999867211010726, 0.009591886382536711, -0.013174067756669, -0.3004961618953472] + - [-0.009603884302752923, 0.9999535222750985, -0.0008477593549240888, 0.0002870696089010898] + - [0.0131653238445598, 0.0009741690043478774, 0.9999128588246174, 0.00337890826837825] + - [0.0, 0.0, 0.0, 1.0] diff --git a/configs/4seasons_calib/cropped_calib304.txt b/configs/4seasons_calib/cropped_calib304.txt new file mode 100644 index 0000000..6752c00 --- /dev/null +++ b/configs/4seasons_calib/cropped_calib304.txt @@ -0,0 +1,4 @@ +Pinhole 501.4757919305817 501.4757919305817 421.7953735163109 167.65799492501083 0.0 0.0 0.0 0.0 +800 304 +none +800 304 diff --git a/configs/4seasons_calib/undistorted_calib304.txt b/configs/4seasons_calib/undistorted_calib304.txt new file mode 100644 index 0000000..916f4c4 --- /dev/null +++ b/configs/4seasons_calib/undistorted_calib304.txt @@ -0,0 +1,4 @@ +Pinhole 0.626844739913227125 1.25368947982645425 0.527869216895388625 0.420394987312527075 0.0 0.0 0.0 0.0 +800 400 +0.626844739913227125 1.64959142082428190789 0.527869216895388625 0.5531512990954303618 0 +800 304 diff --git a/configs/ablations/4seasonsNoInitialReadvancing.yaml b/configs/ablations/4seasonsNoInitialReadvancing.yaml new file mode 100644 index 0000000..cc23149 --- /dev/null +++ b/configs/ablations/4seasonsNoInitialReadvancing.yaml @@ -0,0 +1,11 @@ +accelerometer_noise_density: 0.10200528 +gyroscope_noise_density: 4.120916e-02 +accelerometer_random_walk: 9.8082e-04 +gyroscope_random_walk: 3.8785e-04 +integration_sigma: 0.316227 +setting_weightZeroPriorDSOInitY: 5e09 +setting_weightZeroPriorDSOInitX: 5e09 +setting_forceNoKFTranslationThresh: 0.01 +init_transitionModel: 4 +init_pgba_reinitScaleUncertaintyThresh: 1e6 +init_scalePriorAfterInit: 1.0 \ No newline at end of file diff --git a/configs/ablations/4seasonsNoPGBA.yaml b/configs/ablations/4seasonsNoPGBA.yaml new file mode 100644 index 0000000..77395c1 --- /dev/null +++ b/configs/ablations/4seasonsNoPGBA.yaml @@ -0,0 +1,10 @@ +accelerometer_noise_density: 0.10200528 +gyroscope_noise_density: 4.120916e-02 +accelerometer_random_walk: 9.8082e-04 +gyroscope_random_walk: 3.8785e-04 +integration_sigma: 0.316227 +setting_weightZeroPriorDSOInitY: 5e09 +setting_weightZeroPriorDSOInitX: 5e09 +setting_forceNoKFTranslationThresh: 0.01 +init_transitionModel: 5 +init_scalePriorAfterInit: 1.0 \ No newline at end of file diff --git a/configs/ablations/4seasonsNoReinitAndMargReplacement.yaml b/configs/ablations/4seasonsNoReinitAndMargReplacement.yaml new file mode 100644 index 0000000..495f360 --- /dev/null +++ b/configs/ablations/4seasonsNoReinitAndMargReplacement.yaml @@ -0,0 +1,10 @@ +accelerometer_noise_density: 0.10200528 +gyroscope_noise_density: 4.120916e-02 +accelerometer_random_walk: 9.8082e-04 +gyroscope_random_walk: 3.8785e-04 +integration_sigma: 0.316227 +setting_weightZeroPriorDSOInitY: 5e09 +setting_weightZeroPriorDSOInitX: 5e09 +setting_forceNoKFTranslationThresh: 0.01 +init_transitionModel: 1 +init_pgba_reinitScaleUncertaintyThresh: 1e6 \ No newline at end of file diff --git a/configs/euroc.yaml b/configs/euroc.yaml new file mode 100644 index 0000000..008f390 --- /dev/null +++ b/configs/euroc.yaml @@ -0,0 +1,5 @@ +accelerometer_noise_density: 0.2 +gyroscope_noise_density: 0.016968 +accelerometer_random_walk: 0.003 +gyroscope_random_walk: 0.000019393 +integration_sigma: 0.316227 \ No newline at end of file diff --git a/configs/tumvi.yaml b/configs/tumvi.yaml new file mode 100644 index 0000000..c1e7b73 --- /dev/null +++ b/configs/tumvi.yaml @@ -0,0 +1,5 @@ +accelerometer_noise_density: 0.224 +gyroscope_noise_density: 0.01280 +accelerometer_random_walk: 0.0430 +gyroscope_random_walk: 0.00110 +integration_sigma: 0.316227 \ No newline at end of file diff --git a/configs/tumvi_calib/camchain.yaml b/configs/tumvi_calib/camchain.yaml new file mode 100644 index 0000000..a336a7c --- /dev/null +++ b/configs/tumvi_calib/camchain.yaml @@ -0,0 +1,33 @@ +cam0: + T_cam_imu: + - [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392] + - [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084] + - [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, + 0.00020293673591811182] + distortion_model: equidistant + intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] + resolution: [512, 512] + rostopic: /cam0/image_raw +cam1: + T_cam_imu: + - [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734] + - [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924] + - [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9999994457734953, -0.0008233639921576076, -0.0006561436136444361, -0.10106110275180535] + - [0.0007916877528168117, 0.9988994619156757, -0.04689603624058988, -0.0019764575873431013] + - [0.0006940340102242531, 0.04689549078870055, 0.9988995601463054, -0.0011756424802046581] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, + 0.0003299517423931039] + distortion_model: equidistant + intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] + resolution: [512, 512] + rostopic: /cam1/image_raw diff --git a/configs/tumvi_calib/camera02.txt b/configs/tumvi_calib/camera02.txt new file mode 100644 index 0000000..2ca9de3 --- /dev/null +++ b/configs/tumvi_calib/camera02.txt @@ -0,0 +1,4 @@ +EquiDistant 190.978477 190.973307 254.931706 256.897443 0.00348238940225 0.000715034845216 -0.00205323614187 0.000202936735918 +512 512 +0.2 0.2 0.499 0.499 0 +512 512 \ No newline at end of file diff --git a/configs/tumvi_calib/pcalib.txt b/configs/tumvi_calib/pcalib.txt new file mode 100644 index 0000000..5a74728 --- /dev/null +++ b/configs/tumvi_calib/pcalib.txt @@ -0,0 +1 @@ +0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 11.0 12.0 13.0 14.0 15.0 16.0 17.0 18.0 19.0 20.0 21.0 22.0 23.0 24.0 25.0 26.0 27.0 28.0 29.0 30.0 31.0 32.0 33.0 34.0 35.0 36.0 37.0 38.0 39.0 40.0 41.0 42.0 43.0 44.0 45.0 46.0 47.0 48.0 49.0 50.0 51.0 52.0 53.0 54.0 55.0 56.0 57.0 58.0 59.0 60.0 61.0 62.0 63.0 64.0 65.0 66.0 67.0 68.0 69.0 70.0 71.0 72.0 73.0 74.0 75.0 76.0 77.0 78.0 79.0 80.0 81.0 82.0 83.0 84.0 85.0 86.0 87.0 88.0 89.0 90.0 91.0 92.0 93.0 94.0 95.0 96.0 97.0 98.0 99.0 100.0 101.0 102.0 103.0 104.0 105.0 106.0 107.0 108.0 109.0 110.0 111.0 112.0 113.0 114.0 115.0 116.0 117.0 118.0 119.0 120.0 121.0 122.0 123.0 124.0 125.0 126.0 127.0 128.0 129.0 130.0 131.0 132.0 133.0 134.0 135.0 136.0 137.0 138.0 139.0 140.0 141.0 142.0 143.0 144.0 145.0 146.0 147.0 148.0 149.0 150.0 151.0 152.0 153.0 154.0 155.0 156.0 157.0 158.0 159.0 160.0 161.0 162.0 163.0 164.0 165.0 166.0 167.0 168.0 169.0 170.0 171.0 172.0 173.0 174.0 175.0 176.0 177.0 178.0 179.0 180.0 181.0 182.0 183.0 184.0 185.0 186.0 187.0 188.0 189.0 190.0 191.0 192.0 193.0 194.0 195.0 196.0 197.0 198.0 199.0 200.0 201.0 202.0 203.0 204.0 205.0 206.0 207.0 208.0 209.0 210.0 211.0 212.0 213.0 214.0 215.0 216.0 217.0 218.0 219.0 220.0 221.0 222.0 223.0 224.0 225.0 226.0 227.0 228.0 229.0 230.0 231.0 232.0 233.0 234.0 235.0 236.0 237.0 238.0 239.0 240.0 241.0 242.0 243.0 244.0 245.0 246.0 247.0 248.0 249.0 250.0 251.0 252.0 253.0 254.0 255.0 256.0 257.0 258.0 259.0 260.0 261.0 262.0 263.0 264.0 265.0 266.0 267.0 268.0 269.0 270.0 271.0 272.0 273.0 274.0 275.0 276.0 277.0 278.0 279.0 280.0 281.0 282.0 283.0 284.0 285.0 286.0 287.0 288.0 289.0 290.0 291.0 292.0 293.0 294.0 295.0 296.0 297.0 298.0 299.0 300.0 301.0 302.0 303.0 304.0 305.0 306.0 307.0 308.0 309.0 310.0 311.0 312.0 313.0 314.0 315.0 316.0 317.0 318.0 319.0 320.0 321.0 322.0 323.0 324.0 325.0 326.0 327.0 328.0 329.0 330.0 331.0 332.0 333.0 334.0 335.0 336.0 337.0 338.0 339.0 340.0 341.0 342.0 343.0 344.0 345.0 346.0 347.0 348.0 349.0 350.0 351.0 352.0 353.0 354.0 355.0 356.0 357.0 358.0 359.0 360.0 361.0 362.0 363.0 364.0 365.0 366.0 367.0 368.0 369.0 370.0 371.0 372.0 373.0 374.0 375.0 376.0 377.0 378.0 379.0 380.0 381.0 382.0 383.0 384.0 385.0 386.0 387.0 388.0 389.0 390.0 391.0 392.0 393.0 394.0 395.0 396.0 397.0 398.0 399.0 400.0 401.0 402.0 403.0 404.0 405.0 406.0 407.0 408.0 409.0 410.0 411.0 412.0 413.0 414.0 415.0 416.0 417.0 418.0 419.0 420.0 421.0 422.0 423.0 424.0 425.0 426.0 427.0 428.0 429.0 430.0 431.0 432.0 433.0 434.0 435.0 436.0 437.0 438.0 439.0 440.0 441.0 442.0 443.0 444.0 445.0 446.0 447.0 448.0 449.0 450.0 451.0 452.0 453.0 454.0 455.0 456.0 457.0 458.0 459.0 460.0 461.0 462.0 463.0 464.0 465.0 466.0 467.0 468.0 469.0 470.0 471.0 472.0 473.0 474.0 475.0 476.0 477.0 478.0 479.0 480.0 481.0 482.0 483.0 484.0 485.0 486.0 487.0 488.0 489.0 490.0 491.0 492.0 493.0 494.0 495.0 496.0 497.0 498.0 499.0 500.0 501.0 502.0 503.0 504.0 505.0 506.0 507.0 508.0 509.0 510.0 511.0 512.0 513.0 514.0 515.0 516.0 517.0 518.0 519.0 520.0 521.0 522.0 523.0 524.0 525.0 526.0 527.0 528.0 529.0 530.0 531.0 532.0 533.0 534.0 535.0 536.0 537.0 538.0 539.0 540.0 541.0 542.0 543.0 544.0 545.0 546.0 547.0 548.0 549.0 550.0 551.0 552.0 553.0 554.0 555.0 556.0 557.0 558.0 559.0 560.0 561.0 562.0 563.0 564.0 565.0 566.0 567.0 568.0 569.0 570.0 571.0 572.0 573.0 574.0 575.0 576.0 577.0 578.0 579.0 580.0 581.0 582.0 583.0 584.0 585.0 586.0 587.0 588.0 589.0 590.0 591.0 592.0 593.0 594.0 595.0 596.0 597.0 598.0 599.0 600.0 601.0 602.0 603.0 604.0 605.0 606.0 607.0 608.0 609.0 610.0 611.0 612.0 613.0 614.0 615.0 616.0 617.0 618.0 619.0 620.0 621.0 622.0 623.0 624.0 625.0 626.0 627.0 628.0 629.0 630.0 631.0 632.0 633.0 634.0 635.0 636.0 637.0 638.0 639.0 640.0 641.0 642.0 643.0 644.0 645.0 646.0 647.0 648.0 649.0 650.0 651.0 652.0 653.0 654.0 655.0 656.0 657.0 658.0 659.0 660.0 661.0 662.0 663.0 664.0 665.0 666.0 667.0 668.0 669.0 670.0 671.0 672.0 673.0 674.0 675.0 676.0 677.0 678.0 679.0 680.0 681.0 682.0 683.0 684.0 685.0 686.0 687.0 688.0 689.0 690.0 691.0 692.0 693.0 694.0 695.0 696.0 697.0 698.0 699.0 700.0 701.0 702.0 703.0 704.0 705.0 706.0 707.0 708.0 709.0 710.0 711.0 712.0 713.0 714.0 715.0 716.0 717.0 718.0 719.0 720.0 721.0 722.0 723.0 724.0 725.0 726.0 727.0 728.0 729.0 730.0 731.0 732.0 733.0 734.0 735.0 736.0 737.0 738.0 739.0 740.0 741.0 742.0 743.0 744.0 745.0 746.0 747.0 748.0 749.0 750.0 751.0 752.0 753.0 754.0 755.0 756.0 757.0 758.0 759.0 760.0 761.0 762.0 763.0 764.0 765.0 766.0 767.0 768.0 769.0 770.0 771.0 772.0 773.0 774.0 775.0 776.0 777.0 778.0 779.0 780.0 781.0 782.0 783.0 784.0 785.0 786.0 787.0 788.0 789.0 790.0 791.0 792.0 793.0 794.0 795.0 796.0 797.0 798.0 799.0 800.0 801.0 802.0 803.0 804.0 805.0 806.0 807.0 808.0 809.0 810.0 811.0 812.0 813.0 814.0 815.0 816.0 817.0 818.0 819.0 820.0 821.0 822.0 823.0 824.0 825.0 826.0 827.0 828.0 829.0 830.0 831.0 832.0 833.0 834.0 835.0 836.0 837.0 838.0 839.0 840.0 841.0 842.0 843.0 844.0 845.0 846.0 847.0 848.0 849.0 850.0 851.0 852.0 853.0 854.0 855.0 856.0 857.0 858.0 859.0 860.0 861.0 862.0 863.0 864.0 865.0 866.0 867.0 868.0 869.0 870.0 871.0 872.0 873.0 874.0 875.0 876.0 877.0 878.0 879.0 880.0 881.0 882.0 883.0 884.0 885.0 886.0 887.0 888.0 889.0 890.0 891.0 892.0 893.0 894.0 895.0 896.0 897.0 898.0 899.0 900.0 901.0 902.0 903.0 904.0 905.0 906.0 907.0 908.0 909.0 910.0 911.0 912.0 913.0 914.0 915.0 916.0 917.0 918.0 919.0 920.0 921.0 922.0 923.0 924.0 925.0 926.0 927.0 928.0 929.0 930.0 931.0 932.0 933.0 934.0 935.0 936.0 937.0 938.0 939.0 940.0 941.0 942.0 943.0 944.0 945.0 946.0 947.0 948.0 949.0 950.0 951.0 952.0 953.0 954.0 955.0 956.0 957.0 958.0 959.0 960.0 961.0 962.0 963.0 964.0 965.0 966.0 967.0 968.0 969.0 970.0 971.0 972.0 973.0 974.0 975.0 976.0 977.0 978.0 979.0 980.0 981.0 982.0 983.0 984.0 985.0 986.0 987.0 988.0 989.0 990.0 991.0 992.0 993.0 994.0 995.0 996.0 997.0 998.0 999.0 1000.0 1001.0 1002.0 1003.0 1004.0 1005.0 1006.0 1007.0 1008.0 1009.0 1010.0 1011.0 1012.0 1013.0 1014.0 1015.0 1016.0 1017.0 1018.0 1019.0 1020.0 1021.0 1022.0 1023.0 1024.0 1025.0 1026.0 1027.0 1028.0 1029.0 1030.0 1031.0 1032.0 1033.0 1034.0 1035.0 1036.0 1037.0 1038.0 1039.0 1040.0 1041.0 1042.0 1043.0 1044.0 1045.0 1046.0 1047.0 1048.0 1049.0 1050.0 1051.0 1052.0 1053.0 1054.0 1055.0 1056.0 1057.0 1058.0 1059.0 1060.0 1061.0 1062.0 1063.0 1064.0 1065.0 1066.0 1067.0 1068.0 1069.0 1070.0 1071.0 1072.0 1073.0 1074.0 1075.0 1076.0 1077.0 1078.0 1079.0 1080.0 1081.0 1082.0 1083.0 1084.0 1085.0 1086.0 1087.0 1088.0 1089.0 1090.0 1091.0 1092.0 1093.0 1094.0 1095.0 1096.0 1097.0 1098.0 1099.0 1100.0 1101.0 1102.0 1103.0 1104.0 1105.0 1106.0 1107.0 1108.0 1109.0 1110.0 1111.0 1112.0 1113.0 1114.0 1115.0 1116.0 1117.0 1118.0 1119.0 1120.0 1121.0 1122.0 1123.0 1124.0 1125.0 1126.0 1127.0 1128.0 1129.0 1130.0 1131.0 1132.0 1133.0 1134.0 1135.0 1136.0 1137.0 1138.0 1139.0 1140.0 1141.0 1142.0 1143.0 1144.0 1145.0 1146.0 1147.0 1148.0 1149.0 1150.0 1151.0 1152.0 1153.0 1154.0 1155.0 1156.0 1157.0 1158.0 1159.0 1160.0 1161.0 1162.0 1163.0 1164.0 1165.0 1166.0 1167.0 1168.0 1169.0 1170.0 1171.0 1172.0 1173.0 1174.0 1175.0 1176.0 1177.0 1178.0 1179.0 1180.0 1181.0 1182.0 1183.0 1184.0 1185.0 1186.0 1187.0 1188.0 1189.0 1190.0 1191.0 1192.0 1193.0 1194.0 1195.0 1196.0 1197.0 1198.0 1199.0 1200.0 1201.0 1202.0 1203.0 1204.0 1205.0 1206.0 1207.0 1208.0 1209.0 1210.0 1211.0 1212.0 1213.0 1214.0 1215.0 1216.0 1217.0 1218.0 1219.0 1220.0 1221.0 1222.0 1223.0 1224.0 1225.0 1226.0 1227.0 1228.0 1229.0 1230.0 1231.0 1232.0 1233.0 1234.0 1235.0 1236.0 1237.0 1238.0 1239.0 1240.0 1241.0 1242.0 1243.0 1244.0 1245.0 1246.0 1247.0 1248.0 1249.0 1250.0 1251.0 1252.0 1253.0 1254.0 1255.0 1256.0 1257.0 1258.0 1259.0 1260.0 1261.0 1262.0 1263.0 1264.0 1265.0 1266.0 1267.0 1268.0 1269.0 1270.0 1271.0 1272.0 1273.0 1274.0 1275.0 1276.0 1277.0 1278.0 1279.0 1280.0 1281.0 1282.0 1283.0 1284.0 1285.0 1286.0 1287.0 1288.0 1289.0 1290.0 1291.0 1292.0 1293.0 1294.0 1295.0 1296.0 1297.0 1298.0 1299.0 1300.0 1301.0 1302.0 1303.0 1304.0 1305.0 1306.0 1307.0 1308.0 1309.0 1310.0 1311.0 1312.0 1313.0 1314.0 1315.0 1316.0 1317.0 1318.0 1319.0 1320.0 1321.0 1322.0 1323.0 1324.0 1325.0 1326.0 1327.0 1328.0 1329.0 1330.0 1331.0 1332.0 1333.0 1334.0 1335.0 1336.0 1337.0 1338.0 1339.0 1340.0 1341.0 1342.0 1343.0 1344.0 1345.0 1346.0 1347.0 1348.0 1349.0 1350.0 1351.0 1352.0 1353.0 1354.0 1355.0 1356.0 1357.0 1358.0 1359.0 1360.0 1361.0 1362.0 1363.0 1364.0 1365.0 1366.0 1367.0 1368.0 1369.0 1370.0 1371.0 1372.0 1373.0 1374.0 1375.0 1376.0 1377.0 1378.0 1379.0 1380.0 1381.0 1382.0 1383.0 1384.0 1385.0 1386.0 1387.0 1388.0 1389.0 1390.0 1391.0 1392.0 1393.0 1394.0 1395.0 1396.0 1397.0 1398.0 1399.0 1400.0 1401.0 1402.0 1403.0 1404.0 1405.0 1406.0 1407.0 1408.0 1409.0 1410.0 1411.0 1412.0 1413.0 1414.0 1415.0 1416.0 1417.0 1418.0 1419.0 1420.0 1421.0 1422.0 1423.0 1424.0 1425.0 1426.0 1427.0 1428.0 1429.0 1430.0 1431.0 1432.0 1433.0 1434.0 1435.0 1436.0 1437.0 1438.0 1439.0 1440.0 1441.0 1442.0 1443.0 1444.0 1445.0 1446.0 1447.0 1448.0 1449.0 1450.0 1451.0 1452.0 1453.0 1454.0 1455.0 1456.0 1457.0 1458.0 1459.0 1460.0 1461.0 1462.0 1463.0 1464.0 1465.0 1466.0 1467.0 1468.0 1469.0 1470.0 1471.0 1472.0 1473.0 1474.0 1475.0 1476.0 1477.0 1478.0 1479.0 1480.0 1481.0 1482.0 1483.0 1484.0 1485.0 1486.0 1487.0 1488.0 1489.0 1490.0 1491.0 1492.0 1493.0 1494.0 1495.0 1496.0 1497.0 1498.0 1499.0 1500.0 1501.0 1502.0 1503.0 1504.0 1505.0 1506.0 1507.0 1508.0 1509.0 1510.0 1511.0 1512.0 1513.0 1514.0 1515.0 1516.0 1517.0 1518.0 1519.0 1520.0 1521.0 1522.0 1523.0 1524.0 1525.0 1526.0 1527.0 1528.0 1529.0 1530.0 1531.0 1532.0 1533.0 1534.0 1535.0 1536.0 1537.0 1538.0 1539.0 1540.0 1541.0 1542.0 1543.0 1544.0 1545.0 1546.0 1547.0 1548.0 1549.0 1550.0 1551.0 1552.0 1553.0 1554.0 1555.0 1556.0 1557.0 1558.0 1559.0 1560.0 1561.0 1562.0 1563.0 1564.0 1565.0 1566.0 1567.0 1568.0 1569.0 1570.0 1571.0 1572.0 1573.0 1574.0 1575.0 1576.0 1577.0 1578.0 1579.0 1580.0 1581.0 1582.0 1583.0 1584.0 1585.0 1586.0 1587.0 1588.0 1589.0 1590.0 1591.0 1592.0 1593.0 1594.0 1595.0 1596.0 1597.0 1598.0 1599.0 1600.0 1601.0 1602.0 1603.0 1604.0 1605.0 1606.0 1607.0 1608.0 1609.0 1610.0 1611.0 1612.0 1613.0 1614.0 1615.0 1616.0 1617.0 1618.0 1619.0 1620.0 1621.0 1622.0 1623.0 1624.0 1625.0 1626.0 1627.0 1628.0 1629.0 1630.0 1631.0 1632.0 1633.0 1634.0 1635.0 1636.0 1637.0 1638.0 1639.0 1640.0 1641.0 1642.0 1643.0 1644.0 1645.0 1646.0 1647.0 1648.0 1649.0 1650.0 1651.0 1652.0 1653.0 1654.0 1655.0 1656.0 1657.0 1658.0 1659.0 1660.0 1661.0 1662.0 1663.0 1664.0 1665.0 1666.0 1667.0 1668.0 1669.0 1670.0 1671.0 1672.0 1673.0 1674.0 1675.0 1676.0 1677.0 1678.0 1679.0 1680.0 1681.0 1682.0 1683.0 1684.0 1685.0 1686.0 1687.0 1688.0 1689.0 1690.0 1691.0 1692.0 1693.0 1694.0 1695.0 1696.0 1697.0 1698.0 1699.0 1700.0 1701.0 1702.0 1703.0 1704.0 1705.0 1706.0 1707.0 1708.0 1709.0 1710.0 1711.0 1712.0 1713.0 1714.0 1715.0 1716.0 1717.0 1718.0 1719.0 1720.0 1721.0 1722.0 1723.0 1724.0 1725.0 1726.0 1727.0 1728.0 1729.0 1730.0 1731.0 1732.0 1733.0 1734.0 1735.0 1736.0 1737.0 1738.0 1739.0 1740.0 1741.0 1742.0 1743.0 1744.0 1745.0 1746.0 1747.0 1748.0 1749.0 1750.0 1751.0 1752.0 1753.0 1754.0 1755.0 1756.0 1757.0 1758.0 1759.0 1760.0 1761.0 1762.0 1763.0 1764.0 1765.0 1766.0 1767.0 1768.0 1769.0 1770.0 1771.0 1772.0 1773.0 1774.0 1775.0 1776.0 1777.0 1778.0 1779.0 1780.0 1781.0 1782.0 1783.0 1784.0 1785.0 1786.0 1787.0 1788.0 1789.0 1790.0 1791.0 1792.0 1793.0 1794.0 1795.0 1796.0 1797.0 1798.0 1799.0 1800.0 1801.0 1802.0 1803.0 1804.0 1805.0 1806.0 1807.0 1808.0 1809.0 1810.0 1811.0 1812.0 1813.0 1814.0 1815.0 1816.0 1817.0 1818.0 1819.0 1820.0 1821.0 1822.0 1823.0 1824.0 1825.0 1826.0 1827.0 1828.0 1829.0 1830.0 1831.0 1832.0 1833.0 1834.0 1835.0 1836.0 1837.0 1838.0 1839.0 1840.0 1841.0 1842.0 1843.0 1844.0 1845.0 1846.0 1847.0 1848.0 1849.0 1850.0 1851.0 1852.0 1853.0 1854.0 1855.0 1856.0 1857.0 1858.0 1859.0 1860.0 1861.0 1862.0 1863.0 1864.0 1865.0 1866.0 1867.0 1868.0 1869.0 1870.0 1871.0 1872.0 1873.0 1874.0 1875.0 1876.0 1877.0 1878.0 1879.0 1880.0 1881.0 1882.0 1883.0 1884.0 1885.0 1886.0 1887.0 1888.0 1889.0 1890.0 1891.0 1892.0 1893.0 1894.0 1895.0 1896.0 1897.0 1898.0 1899.0 1900.0 1901.0 1902.0 1903.0 1904.0 1905.0 1906.0 1907.0 1908.0 1909.0 1910.0 1911.0 1912.0 1913.0 1914.0 1915.0 1916.0 1917.0 1918.0 1919.0 1920.0 1921.0 1922.0 1923.0 1924.0 1925.0 1926.0 1927.0 1928.0 1929.0 1930.0 1931.0 1932.0 1933.0 1934.0 1935.0 1936.0 1937.0 1938.0 1939.0 1940.0 1941.0 1942.0 1943.0 1944.0 1945.0 1946.0 1947.0 1948.0 1949.0 1950.0 1951.0 1952.0 1953.0 1954.0 1955.0 1956.0 1957.0 1958.0 1959.0 1960.0 1961.0 1962.0 1963.0 1964.0 1965.0 1966.0 1967.0 1968.0 1969.0 1970.0 1971.0 1972.0 1973.0 1974.0 1975.0 1976.0 1977.0 1978.0 1979.0 1980.0 1981.0 1982.0 1983.0 1984.0 1985.0 1986.0 1987.0 1988.0 1989.0 1990.0 1991.0 1992.0 1993.0 1994.0 1995.0 1996.0 1997.0 1998.0 1999.0 2000.0 2001.0 2002.0 2003.0 2004.0 2005.0 2006.0 2007.0 2008.0 2009.0 2010.0 2011.0 2012.0 2013.0 2014.0 2015.0 2016.0 2017.0 2018.0 2019.0 2020.0 2021.0 2022.0 2023.0 2024.0 2025.0 2026.0 2027.0 2028.0 2029.0 2030.0 2031.0 2032.0 2033.0 2034.0 2035.0 2036.0 2037.0 2038.0 2039.0 2040.0 2041.0 2042.0 2043.0 2044.0 2045.0 2046.0 2047.0 2048.0 2049.0 2050.0 2051.0 2052.0 2053.0 2054.0 2055.0 2056.0 2057.0 2058.0 2059.0 2060.0 2061.0 2062.0 2063.0 2064.0 2065.0 2066.0 2067.0 2068.0 2069.0 2070.0 2071.0 2072.0 2073.0 2074.0 2075.0 2076.0 2077.0 2078.0 2079.0 2080.0 2081.0 2082.0 2083.0 2084.0 2085.0 2086.0 2087.0 2088.0 2089.0 2090.0 2091.0 2092.0 2093.0 2094.0 2095.0 2096.0 2097.0 2098.0 2099.0 2100.0 2101.0 2102.0 2103.0 2104.0 2105.0 2106.0 2107.0 2108.0 2109.0 2110.0 2111.0 2112.0 2113.0 2114.0 2115.0 2116.0 2117.0 2118.0 2119.0 2120.0 2121.0 2122.0 2123.0 2124.0 2125.0 2126.0 2127.0 2128.0 2129.0 2130.0 2131.0 2132.0 2133.0 2134.0 2135.0 2136.0 2137.0 2138.0 2139.0 2140.0 2141.0 2142.0 2143.0 2144.0 2145.0 2146.0 2147.0 2148.0 2149.0 2150.0 2151.0 2152.0 2153.0 2154.0 2155.0 2156.0 2157.0 2158.0 2159.0 2160.0 2161.0 2162.0 2163.0 2164.0 2165.0 2166.0 2167.0 2168.0 2169.0 2170.0 2171.0 2172.0 2173.0 2174.0 2175.0 2176.0 2177.0 2178.0 2179.0 2180.0 2181.0 2182.0 2183.0 2184.0 2185.0 2186.0 2187.0 2188.0 2189.0 2190.0 2191.0 2192.0 2193.0 2194.0 2195.0 2196.0 2197.0 2198.0 2199.0 2200.0 2201.0 2202.0 2203.0 2204.0 2205.0 2206.0 2207.0 2208.0 2209.0 2210.0 2211.0 2212.0 2213.0 2214.0 2215.0 2216.0 2217.0 2218.0 2219.0 2220.0 2221.0 2222.0 2223.0 2224.0 2225.0 2226.0 2227.0 2228.0 2229.0 2230.0 2231.0 2232.0 2233.0 2234.0 2235.0 2236.0 2237.0 2238.0 2239.0 2240.0 2241.0 2242.0 2243.0 2244.0 2245.0 2246.0 2247.0 2248.0 2249.0 2250.0 2251.0 2252.0 2253.0 2254.0 2255.0 2256.0 2257.0 2258.0 2259.0 2260.0 2261.0 2262.0 2263.0 2264.0 2265.0 2266.0 2267.0 2268.0 2269.0 2270.0 2271.0 2272.0 2273.0 2274.0 2275.0 2276.0 2277.0 2278.0 2279.0 2280.0 2281.0 2282.0 2283.0 2284.0 2285.0 2286.0 2287.0 2288.0 2289.0 2290.0 2291.0 2292.0 2293.0 2294.0 2295.0 2296.0 2297.0 2298.0 2299.0 2300.0 2301.0 2302.0 2303.0 2304.0 2305.0 2306.0 2307.0 2308.0 2309.0 2310.0 2311.0 2312.0 2313.0 2314.0 2315.0 2316.0 2317.0 2318.0 2319.0 2320.0 2321.0 2322.0 2323.0 2324.0 2325.0 2326.0 2327.0 2328.0 2329.0 2330.0 2331.0 2332.0 2333.0 2334.0 2335.0 2336.0 2337.0 2338.0 2339.0 2340.0 2341.0 2342.0 2343.0 2344.0 2345.0 2346.0 2347.0 2348.0 2349.0 2350.0 2351.0 2352.0 2353.0 2354.0 2355.0 2356.0 2357.0 2358.0 2359.0 2360.0 2361.0 2362.0 2363.0 2364.0 2365.0 2366.0 2367.0 2368.0 2369.0 2370.0 2371.0 2372.0 2373.0 2374.0 2375.0 2376.0 2377.0 2378.0 2379.0 2380.0 2381.0 2382.0 2383.0 2384.0 2385.0 2386.0 2387.0 2388.0 2389.0 2390.0 2391.0 2392.0 2393.0 2394.0 2395.0 2396.0 2397.0 2398.0 2399.0 2400.0 2401.0 2402.0 2403.0 2404.0 2405.0 2406.0 2407.0 2408.0 2409.0 2410.0 2411.0 2412.0 2413.0 2414.0 2415.0 2416.0 2417.0 2418.0 2419.0 2420.0 2421.0 2422.0 2423.0 2424.0 2425.0 2426.0 2427.0 2428.0 2429.0 2430.0 2431.0 2432.0 2433.0 2434.0 2435.0 2436.0 2437.0 2438.0 2439.0 2440.0 2441.0 2442.0 2443.0 2444.0 2445.0 2446.0 2447.0 2448.0 2449.0 2450.0 2451.0 2452.0 2453.0 2454.0 2455.0 2456.0 2457.0 2458.0 2459.0 2460.0 2461.0 2462.0 2463.0 2464.0 2465.0 2466.0 2467.0 2468.0 2469.0 2470.0 2471.0 2472.0 2473.0 2474.0 2475.0 2476.0 2477.0 2478.0 2479.0 2480.0 2481.0 2482.0 2483.0 2484.0 2485.0 2486.0 2487.0 2488.0 2489.0 2490.0 2491.0 2492.0 2493.0 2494.0 2495.0 2496.0 2497.0 2498.0 2499.0 2500.0 2501.0 2502.0 2503.0 2504.0 2505.0 2506.0 2507.0 2508.0 2509.0 2510.0 2511.0 2512.0 2513.0 2514.0 2515.0 2516.0 2517.0 2518.0 2519.0 2520.0 2521.0 2522.0 2523.0 2524.0 2525.0 2526.0 2527.0 2528.0 2529.0 2530.0 2531.0 2532.0 2533.0 2534.0 2535.0 2536.0 2537.0 2538.0 2539.0 2540.0 2541.0 2542.0 2543.0 2544.0 2545.0 2546.0 2547.0 2548.0 2549.0 2550.0 2551.0 2552.0 2553.0 2554.0 2555.0 2556.0 2557.0 2558.0 2559.0 2560.0 2561.0 2562.0 2563.0 2564.0 2565.0 2566.0 2567.0 2568.0 2569.0 2570.0 2571.0 2572.0 2573.0 2574.0 2575.0 2576.0 2577.0 2578.0 2579.0 2580.0 2581.0 2582.0 2583.0 2584.0 2585.0 2586.0 2587.0 2588.0 2589.0 2590.0 2591.0 2592.0 2593.0 2594.0 2595.0 2596.0 2597.0 2598.0 2599.0 2600.0 2601.0 2602.0 2603.0 2604.0 2605.0 2606.0 2607.0 2608.0 2609.0 2610.0 2611.0 2612.0 2613.0 2614.0 2615.0 2616.0 2617.0 2618.0 2619.0 2620.0 2621.0 2622.0 2623.0 2624.0 2625.0 2626.0 2627.0 2628.0 2629.0 2630.0 2631.0 2632.0 2633.0 2634.0 2635.0 2636.0 2637.0 2638.0 2639.0 2640.0 2641.0 2642.0 2643.0 2644.0 2645.0 2646.0 2647.0 2648.0 2649.0 2650.0 2651.0 2652.0 2653.0 2654.0 2655.0 2656.0 2657.0 2658.0 2659.0 2660.0 2661.0 2662.0 2663.0 2664.0 2665.0 2666.0 2667.0 2668.0 2669.0 2670.0 2671.0 2672.0 2673.0 2674.0 2675.0 2676.0 2677.0 2678.0 2679.0 2680.0 2681.0 2682.0 2683.0 2684.0 2685.0 2686.0 2687.0 2688.0 2689.0 2690.0 2691.0 2692.0 2693.0 2694.0 2695.0 2696.0 2697.0 2698.0 2699.0 2700.0 2701.0 2702.0 2703.0 2704.0 2705.0 2706.0 2707.0 2708.0 2709.0 2710.0 2711.0 2712.0 2713.0 2714.0 2715.0 2716.0 2717.0 2718.0 2719.0 2720.0 2721.0 2722.0 2723.0 2724.0 2725.0 2726.0 2727.0 2728.0 2729.0 2730.0 2731.0 2732.0 2733.0 2734.0 2735.0 2736.0 2737.0 2738.0 2739.0 2740.0 2741.0 2742.0 2743.0 2744.0 2745.0 2746.0 2747.0 2748.0 2749.0 2750.0 2751.0 2752.0 2753.0 2754.0 2755.0 2756.0 2757.0 2758.0 2759.0 2760.0 2761.0 2762.0 2763.0 2764.0 2765.0 2766.0 2767.0 2768.0 2769.0 2770.0 2771.0 2772.0 2773.0 2774.0 2775.0 2776.0 2777.0 2778.0 2779.0 2780.0 2781.0 2782.0 2783.0 2784.0 2785.0 2786.0 2787.0 2788.0 2789.0 2790.0 2791.0 2792.0 2793.0 2794.0 2795.0 2796.0 2797.0 2798.0 2799.0 2800.0 2801.0 2802.0 2803.0 2804.0 2805.0 2806.0 2807.0 2808.0 2809.0 2810.0 2811.0 2812.0 2813.0 2814.0 2815.0 2816.0 2817.0 2818.0 2819.0 2820.0 2821.0 2822.0 2823.0 2824.0 2825.0 2826.0 2827.0 2828.0 2829.0 2830.0 2831.0 2832.0 2833.0 2834.0 2835.0 2836.0 2837.0 2838.0 2839.0 2840.0 2841.0 2842.0 2843.0 2844.0 2845.0 2846.0 2847.0 2848.0 2849.0 2850.0 2851.0 2852.0 2853.0 2854.0 2855.0 2856.0 2857.0 2858.0 2859.0 2860.0 2861.0 2862.0 2863.0 2864.0 2865.0 2866.0 2867.0 2868.0 2869.0 2870.0 2871.0 2872.0 2873.0 2874.0 2875.0 2876.0 2877.0 2878.0 2879.0 2880.0 2881.0 2882.0 2883.0 2884.0 2885.0 2886.0 2887.0 2888.0 2889.0 2890.0 2891.0 2892.0 2893.0 2894.0 2895.0 2896.0 2897.0 2898.0 2899.0 2900.0 2901.0 2902.0 2903.0 2904.0 2905.0 2906.0 2907.0 2908.0 2909.0 2910.0 2911.0 2912.0 2913.0 2914.0 2915.0 2916.0 2917.0 2918.0 2919.0 2920.0 2921.0 2922.0 2923.0 2924.0 2925.0 2926.0 2927.0 2928.0 2929.0 2930.0 2931.0 2932.0 2933.0 2934.0 2935.0 2936.0 2937.0 2938.0 2939.0 2940.0 2941.0 2942.0 2943.0 2944.0 2945.0 2946.0 2947.0 2948.0 2949.0 2950.0 2951.0 2952.0 2953.0 2954.0 2955.0 2956.0 2957.0 2958.0 2959.0 2960.0 2961.0 2962.0 2963.0 2964.0 2965.0 2966.0 2967.0 2968.0 2969.0 2970.0 2971.0 2972.0 2973.0 2974.0 2975.0 2976.0 2977.0 2978.0 2979.0 2980.0 2981.0 2982.0 2983.0 2984.0 2985.0 2986.0 2987.0 2988.0 2989.0 2990.0 2991.0 2992.0 2993.0 2994.0 2995.0 2996.0 2997.0 2998.0 2999.0 3000.0 3001.0 3002.0 3003.0 3004.0 3005.0 3006.0 3007.0 3008.0 3009.0 3010.0 3011.0 3012.0 3013.0 3014.0 3015.0 3016.0 3017.0 3018.0 3019.0 3020.0 3021.0 3022.0 3023.0 3024.0 3025.0 3026.0 3027.0 3028.0 3029.0 3030.0 3031.0 3032.0 3033.0 3034.0 3035.0 3036.0 3037.0 3038.0 3039.0 3040.0 3041.0 3042.0 3043.0 3044.0 3045.0 3046.0 3047.0 3048.0 3049.0 3050.0 3051.0 3052.0 3053.0 3054.0 3055.0 3056.0 3057.0 3058.0 3059.0 3060.0 3061.0 3062.0 3063.0 3064.0 3065.0 3066.0 3067.0 3068.0 3069.0 3070.0 3071.0 3072.0 3073.0 3074.0 3075.0 3076.0 3077.0 3078.0 3079.0 3080.0 3081.0 3082.0 3083.0 3084.0 3085.0 3086.0 3087.0 3088.0 3089.0 3090.0 3091.0 3092.0 3093.0 3094.0 3095.0 3096.0 3097.0 3098.0 3099.0 3100.0 3101.0 3102.0 3103.0 3104.0 3105.0 3106.0 3107.0 3108.0 3109.0 3110.0 3111.0 3112.0 3113.0 3114.0 3115.0 3116.0 3117.0 3118.0 3119.0 3120.0 3121.0 3122.0 3123.0 3124.0 3125.0 3126.0 3127.0 3128.0 3129.0 3130.0 3131.0 3132.0 3133.0 3134.0 3135.0 3136.0 3137.0 3138.0 3139.0 3140.0 3141.0 3142.0 3143.0 3144.0 3145.0 3146.0 3147.0 3148.0 3149.0 3150.0 3151.0 3152.0 3153.0 3154.0 3155.0 3156.0 3157.0 3158.0 3159.0 3160.0 3161.0 3162.0 3163.0 3164.0 3165.0 3166.0 3167.0 3168.0 3169.0 3170.0 3171.0 3172.0 3173.0 3174.0 3175.0 3176.0 3177.0 3178.0 3179.0 3180.0 3181.0 3182.0 3183.0 3184.0 3185.0 3186.0 3187.0 3188.0 3189.0 3190.0 3191.0 3192.0 3193.0 3194.0 3195.0 3196.0 3197.0 3198.0 3199.0 3200.0 3201.0 3202.0 3203.0 3204.0 3205.0 3206.0 3207.0 3208.0 3209.0 3210.0 3211.0 3212.0 3213.0 3214.0 3215.0 3216.0 3217.0 3218.0 3219.0 3220.0 3221.0 3222.0 3223.0 3224.0 3225.0 3226.0 3227.0 3228.0 3229.0 3230.0 3231.0 3232.0 3233.0 3234.0 3235.0 3236.0 3237.0 3238.0 3239.0 3240.0 3241.0 3242.0 3243.0 3244.0 3245.0 3246.0 3247.0 3248.0 3249.0 3250.0 3251.0 3252.0 3253.0 3254.0 3255.0 3256.0 3257.0 3258.0 3259.0 3260.0 3261.0 3262.0 3263.0 3264.0 3265.0 3266.0 3267.0 3268.0 3269.0 3270.0 3271.0 3272.0 3273.0 3274.0 3275.0 3276.0 3277.0 3278.0 3279.0 3280.0 3281.0 3282.0 3283.0 3284.0 3285.0 3286.0 3287.0 3288.0 3289.0 3290.0 3291.0 3292.0 3293.0 3294.0 3295.0 3296.0 3297.0 3298.0 3299.0 3300.0 3301.0 3302.0 3303.0 3304.0 3305.0 3306.0 3307.0 3308.0 3309.0 3310.0 3311.0 3312.0 3313.0 3314.0 3315.0 3316.0 3317.0 3318.0 3319.0 3320.0 3321.0 3322.0 3323.0 3324.0 3325.0 3326.0 3327.0 3328.0 3329.0 3330.0 3331.0 3332.0 3333.0 3334.0 3335.0 3336.0 3337.0 3338.0 3339.0 3340.0 3341.0 3342.0 3343.0 3344.0 3345.0 3346.0 3347.0 3348.0 3349.0 3350.0 3351.0 3352.0 3353.0 3354.0 3355.0 3356.0 3357.0 3358.0 3359.0 3360.0 3361.0 3362.0 3363.0 3364.0 3365.0 3366.0 3367.0 3368.0 3369.0 3370.0 3371.0 3372.0 3373.0 3374.0 3375.0 3376.0 3377.0 3378.0 3379.0 3380.0 3381.0 3382.0 3383.0 3384.0 3385.0 3386.0 3387.0 3388.0 3389.0 3390.0 3391.0 3392.0 3393.0 3394.0 3395.0 3396.0 3397.0 3398.0 3399.0 3400.0 3401.0 3402.0 3403.0 3404.0 3405.0 3406.0 3407.0 3408.0 3409.0 3410.0 3411.0 3412.0 3413.0 3414.0 3415.0 3416.0 3417.0 3418.0 3419.0 3420.0 3421.0 3422.0 3423.0 3424.0 3425.0 3426.0 3427.0 3428.0 3429.0 3430.0 3431.0 3432.0 3433.0 3434.0 3435.0 3436.0 3437.0 3438.0 3439.0 3440.0 3441.0 3442.0 3443.0 3444.0 3445.0 3446.0 3447.0 3448.0 3449.0 3450.0 3451.0 3452.0 3453.0 3454.0 3455.0 3456.0 3457.0 3458.0 3459.0 3460.0 3461.0 3462.0 3463.0 3464.0 3465.0 3466.0 3467.0 3468.0 3469.0 3470.0 3471.0 3472.0 3473.0 3474.0 3475.0 3476.0 3477.0 3478.0 3479.0 3480.0 3481.0 3482.0 3483.0 3484.0 3485.0 3486.0 3487.0 3488.0 3489.0 3490.0 3491.0 3492.0 3493.0 3494.0 3495.0 3496.0 3497.0 3498.0 3499.0 3500.0 3501.0 3502.0 3503.0 3504.0 3505.0 3506.0 3507.0 3508.0 3509.0 3510.0 3511.0 3512.0 3513.0 3514.0 3515.0 3516.0 3517.0 3518.0 3519.0 3520.0 3521.0 3522.0 3523.0 3524.0 3525.0 3526.0 3527.0 3528.0 3529.0 3530.0 3531.0 3532.0 3533.0 3534.0 3535.0 3536.0 3537.0 3538.0 3539.0 3540.0 3541.0 3542.0 3543.0 3544.0 3545.0 3546.0 3547.0 3548.0 3549.0 3550.0 3551.0 3552.0 3553.0 3554.0 3555.0 3556.0 3557.0 3558.0 3559.0 3560.0 3561.0 3562.0 3563.0 3564.0 3565.0 3566.0 3567.0 3568.0 3569.0 3570.0 3571.0 3572.0 3573.0 3574.0 3575.0 3576.0 3577.0 3578.0 3579.0 3580.0 3581.0 3582.0 3583.0 3584.0 3585.0 3586.0 3587.0 3588.0 3589.0 3590.0 3591.0 3592.0 3593.0 3594.0 3595.0 3596.0 3597.0 3598.0 3599.0 3600.0 3601.0 3602.0 3603.0 3604.0 3605.0 3606.0 3607.0 3608.0 3609.0 3610.0 3611.0 3612.0 3613.0 3614.0 3615.0 3616.0 3617.0 3618.0 3619.0 3620.0 3621.0 3622.0 3623.0 3624.0 3625.0 3626.0 3627.0 3628.0 3629.0 3630.0 3631.0 3632.0 3633.0 3634.0 3635.0 3636.0 3637.0 3638.0 3639.0 3640.0 3641.0 3642.0 3643.0 3644.0 3645.0 3646.0 3647.0 3648.0 3649.0 3650.0 3651.0 3652.0 3653.0 3654.0 3655.0 3656.0 3657.0 3658.0 3659.0 3660.0 3661.0 3662.0 3663.0 3664.0 3665.0 3666.0 3667.0 3668.0 3669.0 3670.0 3671.0 3672.0 3673.0 3674.0 3675.0 3676.0 3677.0 3678.0 3679.0 3680.0 3681.0 3682.0 3683.0 3684.0 3685.0 3686.0 3687.0 3688.0 3689.0 3690.0 3691.0 3692.0 3693.0 3694.0 3695.0 3696.0 3697.0 3698.0 3699.0 3700.0 3701.0 3702.0 3703.0 3704.0 3705.0 3706.0 3707.0 3708.0 3709.0 3710.0 3711.0 3712.0 3713.0 3714.0 3715.0 3716.0 3717.0 3718.0 3719.0 3720.0 3721.0 3722.0 3723.0 3724.0 3725.0 3726.0 3727.0 3728.0 3729.0 3730.0 3731.0 3732.0 3733.0 3734.0 3735.0 3736.0 3737.0 3738.0 3739.0 3740.0 3741.0 3742.0 3743.0 3744.0 3745.0 3746.0 3747.0 3748.0 3749.0 3750.0 3751.0 3752.0 3753.0 3754.0 3755.0 3756.0 3757.0 3758.0 3759.0 3760.0 3761.0 3762.0 3763.0 3764.0 3765.0 3766.0 3767.0 3768.0 3769.0 3770.0 3771.0 3772.0 3773.0 3774.0 3775.0 3776.0 3777.0 3778.0 3779.0 3780.0 3781.0 3782.0 3783.0 3784.0 3785.0 3786.0 3787.0 3788.0 3789.0 3790.0 3791.0 3792.0 3793.0 3794.0 3795.0 3796.0 3797.0 3798.0 3799.0 3800.0 3801.0 3802.0 3803.0 3804.0 3805.0 3806.0 3807.0 3808.0 3809.0 3810.0 3811.0 3812.0 3813.0 3814.0 3815.0 3816.0 3817.0 3818.0 3819.0 3820.0 3821.0 3822.0 3823.0 3824.0 3825.0 3826.0 3827.0 3828.0 3829.0 3830.0 3831.0 3832.0 3833.0 3834.0 3835.0 3836.0 3837.0 3838.0 3839.0 3840.0 3841.0 3842.0 3843.0 3844.0 3845.0 3846.0 3847.0 3848.0 3849.0 3850.0 3851.0 3852.0 3853.0 3854.0 3855.0 3856.0 3857.0 3858.0 3859.0 3860.0 3861.0 3862.0 3863.0 3864.0 3865.0 3866.0 3867.0 3868.0 3869.0 3870.0 3871.0 3872.0 3873.0 3874.0 3875.0 3876.0 3877.0 3878.0 3879.0 3880.0 3881.0 3882.0 3883.0 3884.0 3885.0 3886.0 3887.0 3888.0 3889.0 3890.0 3891.0 3892.0 3893.0 3894.0 3895.0 3896.0 3897.0 3898.0 3899.0 3900.0 3901.0 3902.0 3903.0 3904.0 3905.0 3906.0 3907.0 3908.0 3909.0 3910.0 3911.0 3912.0 3913.0 3914.0 3915.0 3916.0 3917.0 3918.0 3919.0 3920.0 3921.0 3922.0 3923.0 3924.0 3925.0 3926.0 3927.0 3928.0 3929.0 3930.0 3931.0 3932.0 3933.0 3934.0 3935.0 3936.0 3937.0 3938.0 3939.0 3940.0 3941.0 3942.0 3943.0 3944.0 3945.0 3946.0 3947.0 3948.0 3949.0 3950.0 3951.0 3952.0 3953.0 3954.0 3955.0 3956.0 3957.0 3958.0 3959.0 3960.0 3961.0 3962.0 3963.0 3964.0 3965.0 3966.0 3967.0 3968.0 3969.0 3970.0 3971.0 3972.0 3973.0 3974.0 3975.0 3976.0 3977.0 3978.0 3979.0 3980.0 3981.0 3982.0 3983.0 3984.0 3985.0 3986.0 3987.0 3988.0 3989.0 3990.0 3991.0 3992.0 3993.0 3994.0 3995.0 3996.0 3997.0 3998.0 3999.0 4000.0 4001.0 4002.0 4003.0 4004.0 4005.0 4006.0 4007.0 4008.0 4009.0 4010.0 4011.0 4012.0 4013.0 4014.0 4015.0 4016.0 4017.0 4018.0 4019.0 4020.0 4021.0 4022.0 4023.0 4024.0 4025.0 4026.0 4027.0 4028.0 4029.0 4030.0 4031.0 4032.0 4033.0 4034.0 4035.0 4036.0 4037.0 4038.0 4039.0 4040.0 4041.0 4042.0 4043.0 4044.0 4045.0 4046.0 4047.0 4048.0 4049.0 4050.0 4051.0 4052.0 4053.0 4054.0 4055.0 4056.0 4057.0 4058.0 4059.0 4060.0 4061.0 4062.0 4063.0 4064.0 4065.0 4066.0 4067.0 4068.0 4069.0 4070.0 4071.0 4072.0 4073.0 4074.0 4075.0 4076.0 4077.0 4078.0 4079.0 4080.0 4081.0 4082.0 4083.0 4084.0 4085.0 4086.0 4087.0 4088.0 4089.0 4090.0 4091.0 4092.0 4093.0 4094.0 4095.0 4096.0 4097.0 4098.0 4099.0 4100.0 4101.0 4102.0 4103.0 4104.0 4105.0 4106.0 4107.0 4108.0 4109.0 4110.0 4111.0 4112.0 4113.0 4114.0 4115.0 4116.0 4117.0 4118.0 4119.0 4120.0 4121.0 4122.0 4123.0 4124.0 4125.0 4126.0 4127.0 4128.0 4129.0 4130.0 4131.0 4132.0 4133.0 4134.0 4135.0 4136.0 4137.0 4138.0 4139.0 4140.0 4141.0 4142.0 4143.0 4144.0 4145.0 4146.0 4147.0 4148.0 4149.0 4150.0 4151.0 4152.0 4153.0 4154.0 4155.0 4156.0 4157.0 4158.0 4159.0 4160.0 4161.0 4162.0 4163.0 4164.0 4165.0 4166.0 4167.0 4168.0 4169.0 4170.0 4171.0 4172.0 4173.0 4174.0 4175.0 4176.0 4177.0 4178.0 4179.0 4180.0 4181.0 4182.0 4183.0 4184.0 4185.0 4186.0 4187.0 4188.0 4189.0 4190.0 4191.0 4192.0 4193.0 4194.0 4195.0 4196.0 4197.0 4198.0 4199.0 4200.0 4201.0 4202.0 4203.0 4204.0 4205.0 4206.0 4207.0 4208.0 4209.0 4210.0 4211.0 4212.0 4213.0 4214.0 4215.0 4216.0 4217.0 4218.0 4219.0 4220.0 4221.0 4222.0 4223.0 4224.0 4225.0 4226.0 4227.0 4228.0 4229.0 4230.0 4231.0 4232.0 4233.0 4234.0 4235.0 4236.0 4237.0 4238.0 4239.0 4240.0 4241.0 4242.0 4243.0 4244.0 4245.0 4246.0 4247.0 4248.0 4249.0 4250.0 4251.0 4252.0 4253.0 4254.0 4255.0 4256.0 4257.0 4258.0 4259.0 4260.0 4261.0 4262.0 4263.0 4264.0 4265.0 4266.0 4267.0 4268.0 4269.0 4270.0 4271.0 4272.0 4273.0 4274.0 4275.0 4276.0 4277.0 4278.0 4279.0 4280.0 4281.0 4282.0 4283.0 4284.0 4285.0 4286.0 4287.0 4288.0 4289.0 4290.0 4291.0 4292.0 4293.0 4294.0 4295.0 4296.0 4297.0 4298.0 4299.0 4300.0 4301.0 4302.0 4303.0 4304.0 4305.0 4306.0 4307.0 4308.0 4309.0 4310.0 4311.0 4312.0 4313.0 4314.0 4315.0 4316.0 4317.0 4318.0 4319.0 4320.0 4321.0 4322.0 4323.0 4324.0 4325.0 4326.0 4327.0 4328.0 4329.0 4330.0 4331.0 4332.0 4333.0 4334.0 4335.0 4336.0 4337.0 4338.0 4339.0 4340.0 4341.0 4342.0 4343.0 4344.0 4345.0 4346.0 4347.0 4348.0 4349.0 4350.0 4351.0 4352.0 4353.0 4354.0 4355.0 4356.0 4357.0 4358.0 4359.0 4360.0 4361.0 4362.0 4363.0 4364.0 4365.0 4366.0 4367.0 4368.0 4369.0 4370.0 4371.0 4372.0 4373.0 4374.0 4375.0 4376.0 4377.0 4378.0 4379.0 4380.0 4381.0 4382.0 4383.0 4384.0 4385.0 4386.0 4387.0 4388.0 4389.0 4390.0 4391.0 4392.0 4393.0 4394.0 4395.0 4396.0 4397.0 4398.0 4399.0 4400.0 4401.0 4402.0 4403.0 4404.0 4405.0 4406.0 4407.0 4408.0 4409.0 4410.0 4411.0 4412.0 4413.0 4414.0 4415.0 4416.0 4417.0 4418.0 4419.0 4420.0 4421.0 4422.0 4423.0 4424.0 4425.0 4426.0 4427.0 4428.0 4429.0 4430.0 4431.0 4432.0 4433.0 4434.0 4435.0 4436.0 4437.0 4438.0 4439.0 4440.0 4441.0 4442.0 4443.0 4444.0 4445.0 4446.0 4447.0 4448.0 4449.0 4450.0 4451.0 4452.0 4453.0 4454.0 4455.0 4456.0 4457.0 4458.0 4459.0 4460.0 4461.0 4462.0 4463.0 4464.0 4465.0 4466.0 4467.0 4468.0 4469.0 4470.0 4471.0 4472.0 4473.0 4474.0 4475.0 4476.0 4477.0 4478.0 4479.0 4480.0 4481.0 4482.0 4483.0 4484.0 4485.0 4486.0 4487.0 4488.0 4489.0 4490.0 4491.0 4492.0 4493.0 4494.0 4495.0 4496.0 4497.0 4498.0 4499.0 4500.0 4501.0 4502.0 4503.0 4504.0 4505.0 4506.0 4507.0 4508.0 4509.0 4510.0 4511.0 4512.0 4513.0 4514.0 4515.0 4516.0 4517.0 4518.0 4519.0 4520.0 4521.0 4522.0 4523.0 4524.0 4525.0 4526.0 4527.0 4528.0 4529.0 4530.0 4531.0 4532.0 4533.0 4534.0 4535.0 4536.0 4537.0 4538.0 4539.0 4540.0 4541.0 4542.0 4543.0 4544.0 4545.0 4546.0 4547.0 4548.0 4549.0 4550.0 4551.0 4552.0 4553.0 4554.0 4555.0 4556.0 4557.0 4558.0 4559.0 4560.0 4561.0 4562.0 4563.0 4564.0 4565.0 4566.0 4567.0 4568.0 4569.0 4570.0 4571.0 4572.0 4573.0 4574.0 4575.0 4576.0 4577.0 4578.0 4579.0 4580.0 4581.0 4582.0 4583.0 4584.0 4585.0 4586.0 4587.0 4588.0 4589.0 4590.0 4591.0 4592.0 4593.0 4594.0 4595.0 4596.0 4597.0 4598.0 4599.0 4600.0 4601.0 4602.0 4603.0 4604.0 4605.0 4606.0 4607.0 4608.0 4609.0 4610.0 4611.0 4612.0 4613.0 4614.0 4615.0 4616.0 4617.0 4618.0 4619.0 4620.0 4621.0 4622.0 4623.0 4624.0 4625.0 4626.0 4627.0 4628.0 4629.0 4630.0 4631.0 4632.0 4633.0 4634.0 4635.0 4636.0 4637.0 4638.0 4639.0 4640.0 4641.0 4642.0 4643.0 4644.0 4645.0 4646.0 4647.0 4648.0 4649.0 4650.0 4651.0 4652.0 4653.0 4654.0 4655.0 4656.0 4657.0 4658.0 4659.0 4660.0 4661.0 4662.0 4663.0 4664.0 4665.0 4666.0 4667.0 4668.0 4669.0 4670.0 4671.0 4672.0 4673.0 4674.0 4675.0 4676.0 4677.0 4678.0 4679.0 4680.0 4681.0 4682.0 4683.0 4684.0 4685.0 4686.0 4687.0 4688.0 4689.0 4690.0 4691.0 4692.0 4693.0 4694.0 4695.0 4696.0 4697.0 4698.0 4699.0 4700.0 4701.0 4702.0 4703.0 4704.0 4705.0 4706.0 4707.0 4708.0 4709.0 4710.0 4711.0 4712.0 4713.0 4714.0 4715.0 4716.0 4717.0 4718.0 4719.0 4720.0 4721.0 4722.0 4723.0 4724.0 4725.0 4726.0 4727.0 4728.0 4729.0 4730.0 4731.0 4732.0 4733.0 4734.0 4735.0 4736.0 4737.0 4738.0 4739.0 4740.0 4741.0 4742.0 4743.0 4744.0 4745.0 4746.0 4747.0 4748.0 4749.0 4750.0 4751.0 4752.0 4753.0 4754.0 4755.0 4756.0 4757.0 4758.0 4759.0 4760.0 4761.0 4762.0 4763.0 4764.0 4765.0 4766.0 4767.0 4768.0 4769.0 4770.0 4771.0 4772.0 4773.0 4774.0 4775.0 4776.0 4777.0 4778.0 4779.0 4780.0 4781.0 4782.0 4783.0 4784.0 4785.0 4786.0 4787.0 4788.0 4789.0 4790.0 4791.0 4792.0 4793.0 4794.0 4795.0 4796.0 4797.0 4798.0 4799.0 4800.0 4801.0 4802.0 4803.0 4804.0 4805.0 4806.0 4807.0 4808.0 4809.0 4810.0 4811.0 4812.0 4813.0 4814.0 4815.0 4816.0 4817.0 4818.0 4819.0 4820.0 4821.0 4822.0 4823.0 4824.0 4825.0 4826.0 4827.0 4828.0 4829.0 4830.0 4831.0 4832.0 4833.0 4834.0 4835.0 4836.0 4837.0 4838.0 4839.0 4840.0 4841.0 4842.0 4843.0 4844.0 4845.0 4846.0 4847.0 4848.0 4849.0 4850.0 4851.0 4852.0 4853.0 4854.0 4855.0 4856.0 4857.0 4858.0 4859.0 4860.0 4861.0 4862.0 4863.0 4864.0 4865.0 4866.0 4867.0 4868.0 4869.0 4870.0 4871.0 4872.0 4873.0 4874.0 4875.0 4876.0 4877.0 4878.0 4879.0 4880.0 4881.0 4882.0 4883.0 4884.0 4885.0 4886.0 4887.0 4888.0 4889.0 4890.0 4891.0 4892.0 4893.0 4894.0 4895.0 4896.0 4897.0 4898.0 4899.0 4900.0 4901.0 4902.0 4903.0 4904.0 4905.0 4906.0 4907.0 4908.0 4909.0 4910.0 4911.0 4912.0 4913.0 4914.0 4915.0 4916.0 4917.0 4918.0 4919.0 4920.0 4921.0 4922.0 4923.0 4924.0 4925.0 4926.0 4927.0 4928.0 4929.0 4930.0 4931.0 4932.0 4933.0 4934.0 4935.0 4936.0 4937.0 4938.0 4939.0 4940.0 4941.0 4942.0 4943.0 4944.0 4945.0 4946.0 4947.0 4948.0 4949.0 4950.0 4951.0 4952.0 4953.0 4954.0 4955.0 4956.0 4957.0 4958.0 4959.0 4960.0 4961.0 4962.0 4963.0 4964.0 4965.0 4966.0 4967.0 4968.0 4969.0 4970.0 4971.0 4972.0 4973.0 4974.0 4975.0 4976.0 4977.0 4978.0 4979.0 4980.0 4981.0 4982.0 4983.0 4984.0 4985.0 4986.0 4987.0 4988.0 4989.0 4990.0 4991.0 4992.0 4993.0 4994.0 4995.0 4996.0 4997.0 4998.0 4999.0 5000.0 5001.0 5002.0 5003.0 5004.0 5005.0 5006.0 5007.0 5008.0 5009.0 5010.0 5011.0 5012.0 5013.0 5014.0 5015.0 5016.0 5017.0 5018.0 5019.0 5020.0 5021.0 5022.0 5023.0 5024.0 5025.0 5026.0 5027.0 5028.0 5029.0 5030.0 5031.0 5032.0 5033.0 5034.0 5035.0 5036.0 5037.0 5038.0 5039.0 5040.0 5041.0 5042.0 5043.0 5044.0 5045.0 5046.0 5047.0 5048.0 5049.0 5050.0 5051.0 5052.0 5053.0 5054.0 5055.0 5056.0 5057.0 5058.0 5059.0 5060.0 5061.0 5062.0 5063.0 5064.0 5065.0 5066.0 5067.0 5068.0 5069.0 5070.0 5071.0 5072.0 5073.0 5074.0 5075.0 5076.0 5077.0 5078.0 5079.0 5080.0 5081.0 5082.0 5083.0 5084.0 5085.0 5086.0 5087.0 5088.0 5089.0 5090.0 5091.0 5092.0 5093.0 5094.0 5095.0 5096.0 5097.0 5098.0 5099.0 5100.0 5101.0 5102.0 5103.0 5104.0 5105.0 5106.0 5107.0 5108.0 5109.0 5110.0 5111.0 5112.0 5113.0 5114.0 5115.0 5116.0 5117.0 5118.0 5119.0 5120.0 5121.0 5122.0 5123.0 5124.0 5125.0 5126.0 5127.0 5128.0 5129.0 5130.0 5131.0 5132.0 5133.0 5134.0 5135.0 5136.0 5137.0 5138.0 5139.0 5140.0 5141.0 5142.0 5143.0 5144.0 5145.0 5146.0 5147.0 5148.0 5149.0 5150.0 5151.0 5152.0 5153.0 5154.0 5155.0 5156.0 5157.0 5158.0 5159.0 5160.0 5161.0 5162.0 5163.0 5164.0 5165.0 5166.0 5167.0 5168.0 5169.0 5170.0 5171.0 5172.0 5173.0 5174.0 5175.0 5176.0 5177.0 5178.0 5179.0 5180.0 5181.0 5182.0 5183.0 5184.0 5185.0 5186.0 5187.0 5188.0 5189.0 5190.0 5191.0 5192.0 5193.0 5194.0 5195.0 5196.0 5197.0 5198.0 5199.0 5200.0 5201.0 5202.0 5203.0 5204.0 5205.0 5206.0 5207.0 5208.0 5209.0 5210.0 5211.0 5212.0 5213.0 5214.0 5215.0 5216.0 5217.0 5218.0 5219.0 5220.0 5221.0 5222.0 5223.0 5224.0 5225.0 5226.0 5227.0 5228.0 5229.0 5230.0 5231.0 5232.0 5233.0 5234.0 5235.0 5236.0 5237.0 5238.0 5239.0 5240.0 5241.0 5242.0 5243.0 5244.0 5245.0 5246.0 5247.0 5248.0 5249.0 5250.0 5251.0 5252.0 5253.0 5254.0 5255.0 5256.0 5257.0 5258.0 5259.0 5260.0 5261.0 5262.0 5263.0 5264.0 5265.0 5266.0 5267.0 5268.0 5269.0 5270.0 5271.0 5272.0 5273.0 5274.0 5275.0 5276.0 5277.0 5278.0 5279.0 5280.0 5281.0 5282.0 5283.0 5284.0 5285.0 5286.0 5287.0 5288.0 5289.0 5290.0 5291.0 5292.0 5293.0 5294.0 5295.0 5296.0 5297.0 5298.0 5299.0 5300.0 5301.0 5302.0 5303.0 5304.0 5305.0 5306.0 5307.0 5308.0 5309.0 5310.0 5311.0 5312.0 5313.0 5314.0 5315.0 5316.0 5317.0 5318.0 5319.0 5320.0 5321.0 5322.0 5323.0 5324.0 5325.0 5326.0 5327.0 5328.0 5329.0 5330.0 5331.0 5332.0 5333.0 5334.0 5335.0 5336.0 5337.0 5338.0 5339.0 5340.0 5341.0 5342.0 5343.0 5344.0 5345.0 5346.0 5347.0 5348.0 5349.0 5350.0 5351.0 5352.0 5353.0 5354.0 5355.0 5356.0 5357.0 5358.0 5359.0 5360.0 5361.0 5362.0 5363.0 5364.0 5365.0 5366.0 5367.0 5368.0 5369.0 5370.0 5371.0 5372.0 5373.0 5374.0 5375.0 5376.0 5377.0 5378.0 5379.0 5380.0 5381.0 5382.0 5383.0 5384.0 5385.0 5386.0 5387.0 5388.0 5389.0 5390.0 5391.0 5392.0 5393.0 5394.0 5395.0 5396.0 5397.0 5398.0 5399.0 5400.0 5401.0 5402.0 5403.0 5404.0 5405.0 5406.0 5407.0 5408.0 5409.0 5410.0 5411.0 5412.0 5413.0 5414.0 5415.0 5416.0 5417.0 5418.0 5419.0 5420.0 5421.0 5422.0 5423.0 5424.0 5425.0 5426.0 5427.0 5428.0 5429.0 5430.0 5431.0 5432.0 5433.0 5434.0 5435.0 5436.0 5437.0 5438.0 5439.0 5440.0 5441.0 5442.0 5443.0 5444.0 5445.0 5446.0 5447.0 5448.0 5449.0 5450.0 5451.0 5452.0 5453.0 5454.0 5455.0 5456.0 5457.0 5458.0 5459.0 5460.0 5461.0 5462.0 5463.0 5464.0 5465.0 5466.0 5467.0 5468.0 5469.0 5470.0 5471.0 5472.0 5473.0 5474.0 5475.0 5476.0 5477.0 5478.0 5479.0 5480.0 5481.0 5482.0 5483.0 5484.0 5485.0 5486.0 5487.0 5488.0 5489.0 5490.0 5491.0 5492.0 5493.0 5494.0 5495.0 5496.0 5497.0 5498.0 5499.0 5500.0 5501.0 5502.0 5503.0 5504.0 5505.0 5506.0 5507.0 5508.0 5509.0 5510.0 5511.0 5512.0 5513.0 5514.0 5515.0 5516.0 5517.0 5518.0 5519.0 5520.0 5521.0 5522.0 5523.0 5524.0 5525.0 5526.0 5527.0 5528.0 5529.0 5530.0 5531.0 5532.0 5533.0 5534.0 5535.0 5536.0 5537.0 5538.0 5539.0 5540.0 5541.0 5542.0 5543.0 5544.0 5545.0 5546.0 5547.0 5548.0 5549.0 5550.0 5551.0 5552.0 5553.0 5554.0 5555.0 5556.0 5557.0 5558.0 5559.0 5560.0 5561.0 5562.0 5563.0 5564.0 5565.0 5566.0 5567.0 5568.0 5569.0 5570.0 5571.0 5572.0 5573.0 5574.0 5575.0 5576.0 5577.0 5578.0 5579.0 5580.0 5581.0 5582.0 5583.0 5584.0 5585.0 5586.0 5587.0 5588.0 5589.0 5590.0 5591.0 5592.0 5593.0 5594.0 5595.0 5596.0 5597.0 5598.0 5599.0 5600.0 5601.0 5602.0 5603.0 5604.0 5605.0 5606.0 5607.0 5608.0 5609.0 5610.0 5611.0 5612.0 5613.0 5614.0 5615.0 5616.0 5617.0 5618.0 5619.0 5620.0 5621.0 5622.0 5623.0 5624.0 5625.0 5626.0 5627.0 5628.0 5629.0 5630.0 5631.0 5632.0 5633.0 5634.0 5635.0 5636.0 5637.0 5638.0 5639.0 5640.0 5641.0 5642.0 5643.0 5644.0 5645.0 5646.0 5647.0 5648.0 5649.0 5650.0 5651.0 5652.0 5653.0 5654.0 5655.0 5656.0 5657.0 5658.0 5659.0 5660.0 5661.0 5662.0 5663.0 5664.0 5665.0 5666.0 5667.0 5668.0 5669.0 5670.0 5671.0 5672.0 5673.0 5674.0 5675.0 5676.0 5677.0 5678.0 5679.0 5680.0 5681.0 5682.0 5683.0 5684.0 5685.0 5686.0 5687.0 5688.0 5689.0 5690.0 5691.0 5692.0 5693.0 5694.0 5695.0 5696.0 5697.0 5698.0 5699.0 5700.0 5701.0 5702.0 5703.0 5704.0 5705.0 5706.0 5707.0 5708.0 5709.0 5710.0 5711.0 5712.0 5713.0 5714.0 5715.0 5716.0 5717.0 5718.0 5719.0 5720.0 5721.0 5722.0 5723.0 5724.0 5725.0 5726.0 5727.0 5728.0 5729.0 5730.0 5731.0 5732.0 5733.0 5734.0 5735.0 5736.0 5737.0 5738.0 5739.0 5740.0 5741.0 5742.0 5743.0 5744.0 5745.0 5746.0 5747.0 5748.0 5749.0 5750.0 5751.0 5752.0 5753.0 5754.0 5755.0 5756.0 5757.0 5758.0 5759.0 5760.0 5761.0 5762.0 5763.0 5764.0 5765.0 5766.0 5767.0 5768.0 5769.0 5770.0 5771.0 5772.0 5773.0 5774.0 5775.0 5776.0 5777.0 5778.0 5779.0 5780.0 5781.0 5782.0 5783.0 5784.0 5785.0 5786.0 5787.0 5788.0 5789.0 5790.0 5791.0 5792.0 5793.0 5794.0 5795.0 5796.0 5797.0 5798.0 5799.0 5800.0 5801.0 5802.0 5803.0 5804.0 5805.0 5806.0 5807.0 5808.0 5809.0 5810.0 5811.0 5812.0 5813.0 5814.0 5815.0 5816.0 5817.0 5818.0 5819.0 5820.0 5821.0 5822.0 5823.0 5824.0 5825.0 5826.0 5827.0 5828.0 5829.0 5830.0 5831.0 5832.0 5833.0 5834.0 5835.0 5836.0 5837.0 5838.0 5839.0 5840.0 5841.0 5842.0 5843.0 5844.0 5845.0 5846.0 5847.0 5848.0 5849.0 5850.0 5851.0 5852.0 5853.0 5854.0 5855.0 5856.0 5857.0 5858.0 5859.0 5860.0 5861.0 5862.0 5863.0 5864.0 5865.0 5866.0 5867.0 5868.0 5869.0 5870.0 5871.0 5872.0 5873.0 5874.0 5875.0 5876.0 5877.0 5878.0 5879.0 5880.0 5881.0 5882.0 5883.0 5884.0 5885.0 5886.0 5887.0 5888.0 5889.0 5890.0 5891.0 5892.0 5893.0 5894.0 5895.0 5896.0 5897.0 5898.0 5899.0 5900.0 5901.0 5902.0 5903.0 5904.0 5905.0 5906.0 5907.0 5908.0 5909.0 5910.0 5911.0 5912.0 5913.0 5914.0 5915.0 5916.0 5917.0 5918.0 5919.0 5920.0 5921.0 5922.0 5923.0 5924.0 5925.0 5926.0 5927.0 5928.0 5929.0 5930.0 5931.0 5932.0 5933.0 5934.0 5935.0 5936.0 5937.0 5938.0 5939.0 5940.0 5941.0 5942.0 5943.0 5944.0 5945.0 5946.0 5947.0 5948.0 5949.0 5950.0 5951.0 5952.0 5953.0 5954.0 5955.0 5956.0 5957.0 5958.0 5959.0 5960.0 5961.0 5962.0 5963.0 5964.0 5965.0 5966.0 5967.0 5968.0 5969.0 5970.0 5971.0 5972.0 5973.0 5974.0 5975.0 5976.0 5977.0 5978.0 5979.0 5980.0 5981.0 5982.0 5983.0 5984.0 5985.0 5986.0 5987.0 5988.0 5989.0 5990.0 5991.0 5992.0 5993.0 5994.0 5995.0 5996.0 5997.0 5998.0 5999.0 6000.0 6001.0 6002.0 6003.0 6004.0 6005.0 6006.0 6007.0 6008.0 6009.0 6010.0 6011.0 6012.0 6013.0 6014.0 6015.0 6016.0 6017.0 6018.0 6019.0 6020.0 6021.0 6022.0 6023.0 6024.0 6025.0 6026.0 6027.0 6028.0 6029.0 6030.0 6031.0 6032.0 6033.0 6034.0 6035.0 6036.0 6037.0 6038.0 6039.0 6040.0 6041.0 6042.0 6043.0 6044.0 6045.0 6046.0 6047.0 6048.0 6049.0 6050.0 6051.0 6052.0 6053.0 6054.0 6055.0 6056.0 6057.0 6058.0 6059.0 6060.0 6061.0 6062.0 6063.0 6064.0 6065.0 6066.0 6067.0 6068.0 6069.0 6070.0 6071.0 6072.0 6073.0 6074.0 6075.0 6076.0 6077.0 6078.0 6079.0 6080.0 6081.0 6082.0 6083.0 6084.0 6085.0 6086.0 6087.0 6088.0 6089.0 6090.0 6091.0 6092.0 6093.0 6094.0 6095.0 6096.0 6097.0 6098.0 6099.0 6100.0 6101.0 6102.0 6103.0 6104.0 6105.0 6106.0 6107.0 6108.0 6109.0 6110.0 6111.0 6112.0 6113.0 6114.0 6115.0 6116.0 6117.0 6118.0 6119.0 6120.0 6121.0 6122.0 6123.0 6124.0 6125.0 6126.0 6127.0 6128.0 6129.0 6130.0 6131.0 6132.0 6133.0 6134.0 6135.0 6136.0 6137.0 6138.0 6139.0 6140.0 6141.0 6142.0 6143.0 6144.0 6145.0 6146.0 6147.0 6148.0 6149.0 6150.0 6151.0 6152.0 6153.0 6154.0 6155.0 6156.0 6157.0 6158.0 6159.0 6160.0 6161.0 6162.0 6163.0 6164.0 6165.0 6166.0 6167.0 6168.0 6169.0 6170.0 6171.0 6172.0 6173.0 6174.0 6175.0 6176.0 6177.0 6178.0 6179.0 6180.0 6181.0 6182.0 6183.0 6184.0 6185.0 6186.0 6187.0 6188.0 6189.0 6190.0 6191.0 6192.0 6193.0 6194.0 6195.0 6196.0 6197.0 6198.0 6199.0 6200.0 6201.0 6202.0 6203.0 6204.0 6205.0 6206.0 6207.0 6208.0 6209.0 6210.0 6211.0 6212.0 6213.0 6214.0 6215.0 6216.0 6217.0 6218.0 6219.0 6220.0 6221.0 6222.0 6223.0 6224.0 6225.0 6226.0 6227.0 6228.0 6229.0 6230.0 6231.0 6232.0 6233.0 6234.0 6235.0 6236.0 6237.0 6238.0 6239.0 6240.0 6241.0 6242.0 6243.0 6244.0 6245.0 6246.0 6247.0 6248.0 6249.0 6250.0 6251.0 6252.0 6253.0 6254.0 6255.0 6256.0 6257.0 6258.0 6259.0 6260.0 6261.0 6262.0 6263.0 6264.0 6265.0 6266.0 6267.0 6268.0 6269.0 6270.0 6271.0 6272.0 6273.0 6274.0 6275.0 6276.0 6277.0 6278.0 6279.0 6280.0 6281.0 6282.0 6283.0 6284.0 6285.0 6286.0 6287.0 6288.0 6289.0 6290.0 6291.0 6292.0 6293.0 6294.0 6295.0 6296.0 6297.0 6298.0 6299.0 6300.0 6301.0 6302.0 6303.0 6304.0 6305.0 6306.0 6307.0 6308.0 6309.0 6310.0 6311.0 6312.0 6313.0 6314.0 6315.0 6316.0 6317.0 6318.0 6319.0 6320.0 6321.0 6322.0 6323.0 6324.0 6325.0 6326.0 6327.0 6328.0 6329.0 6330.0 6331.0 6332.0 6333.0 6334.0 6335.0 6336.0 6337.0 6338.0 6339.0 6340.0 6341.0 6342.0 6343.0 6344.0 6345.0 6346.0 6347.0 6348.0 6349.0 6350.0 6351.0 6352.0 6353.0 6354.0 6355.0 6356.0 6357.0 6358.0 6359.0 6360.0 6361.0 6362.0 6363.0 6364.0 6365.0 6366.0 6367.0 6368.0 6369.0 6370.0 6371.0 6372.0 6373.0 6374.0 6375.0 6376.0 6377.0 6378.0 6379.0 6380.0 6381.0 6382.0 6383.0 6384.0 6385.0 6386.0 6387.0 6388.0 6389.0 6390.0 6391.0 6392.0 6393.0 6394.0 6395.0 6396.0 6397.0 6398.0 6399.0 6400.0 6401.0 6402.0 6403.0 6404.0 6405.0 6406.0 6407.0 6408.0 6409.0 6410.0 6411.0 6412.0 6413.0 6414.0 6415.0 6416.0 6417.0 6418.0 6419.0 6420.0 6421.0 6422.0 6423.0 6424.0 6425.0 6426.0 6427.0 6428.0 6429.0 6430.0 6431.0 6432.0 6433.0 6434.0 6435.0 6436.0 6437.0 6438.0 6439.0 6440.0 6441.0 6442.0 6443.0 6444.0 6445.0 6446.0 6447.0 6448.0 6449.0 6450.0 6451.0 6452.0 6453.0 6454.0 6455.0 6456.0 6457.0 6458.0 6459.0 6460.0 6461.0 6462.0 6463.0 6464.0 6465.0 6466.0 6467.0 6468.0 6469.0 6470.0 6471.0 6472.0 6473.0 6474.0 6475.0 6476.0 6477.0 6478.0 6479.0 6480.0 6481.0 6482.0 6483.0 6484.0 6485.0 6486.0 6487.0 6488.0 6489.0 6490.0 6491.0 6492.0 6493.0 6494.0 6495.0 6496.0 6497.0 6498.0 6499.0 6500.0 6501.0 6502.0 6503.0 6504.0 6505.0 6506.0 6507.0 6508.0 6509.0 6510.0 6511.0 6512.0 6513.0 6514.0 6515.0 6516.0 6517.0 6518.0 6519.0 6520.0 6521.0 6522.0 6523.0 6524.0 6525.0 6526.0 6527.0 6528.0 6529.0 6530.0 6531.0 6532.0 6533.0 6534.0 6535.0 6536.0 6537.0 6538.0 6539.0 6540.0 6541.0 6542.0 6543.0 6544.0 6545.0 6546.0 6547.0 6548.0 6549.0 6550.0 6551.0 6552.0 6553.0 6554.0 6555.0 6556.0 6557.0 6558.0 6559.0 6560.0 6561.0 6562.0 6563.0 6564.0 6565.0 6566.0 6567.0 6568.0 6569.0 6570.0 6571.0 6572.0 6573.0 6574.0 6575.0 6576.0 6577.0 6578.0 6579.0 6580.0 6581.0 6582.0 6583.0 6584.0 6585.0 6586.0 6587.0 6588.0 6589.0 6590.0 6591.0 6592.0 6593.0 6594.0 6595.0 6596.0 6597.0 6598.0 6599.0 6600.0 6601.0 6602.0 6603.0 6604.0 6605.0 6606.0 6607.0 6608.0 6609.0 6610.0 6611.0 6612.0 6613.0 6614.0 6615.0 6616.0 6617.0 6618.0 6619.0 6620.0 6621.0 6622.0 6623.0 6624.0 6625.0 6626.0 6627.0 6628.0 6629.0 6630.0 6631.0 6632.0 6633.0 6634.0 6635.0 6636.0 6637.0 6638.0 6639.0 6640.0 6641.0 6642.0 6643.0 6644.0 6645.0 6646.0 6647.0 6648.0 6649.0 6650.0 6651.0 6652.0 6653.0 6654.0 6655.0 6656.0 6657.0 6658.0 6659.0 6660.0 6661.0 6662.0 6663.0 6664.0 6665.0 6666.0 6667.0 6668.0 6669.0 6670.0 6671.0 6672.0 6673.0 6674.0 6675.0 6676.0 6677.0 6678.0 6679.0 6680.0 6681.0 6682.0 6683.0 6684.0 6685.0 6686.0 6687.0 6688.0 6689.0 6690.0 6691.0 6692.0 6693.0 6694.0 6695.0 6696.0 6697.0 6698.0 6699.0 6700.0 6701.0 6702.0 6703.0 6704.0 6705.0 6706.0 6707.0 6708.0 6709.0 6710.0 6711.0 6712.0 6713.0 6714.0 6715.0 6716.0 6717.0 6718.0 6719.0 6720.0 6721.0 6722.0 6723.0 6724.0 6725.0 6726.0 6727.0 6728.0 6729.0 6730.0 6731.0 6732.0 6733.0 6734.0 6735.0 6736.0 6737.0 6738.0 6739.0 6740.0 6741.0 6742.0 6743.0 6744.0 6745.0 6746.0 6747.0 6748.0 6749.0 6750.0 6751.0 6752.0 6753.0 6754.0 6755.0 6756.0 6757.0 6758.0 6759.0 6760.0 6761.0 6762.0 6763.0 6764.0 6765.0 6766.0 6767.0 6768.0 6769.0 6770.0 6771.0 6772.0 6773.0 6774.0 6775.0 6776.0 6777.0 6778.0 6779.0 6780.0 6781.0 6782.0 6783.0 6784.0 6785.0 6786.0 6787.0 6788.0 6789.0 6790.0 6791.0 6792.0 6793.0 6794.0 6795.0 6796.0 6797.0 6798.0 6799.0 6800.0 6801.0 6802.0 6803.0 6804.0 6805.0 6806.0 6807.0 6808.0 6809.0 6810.0 6811.0 6812.0 6813.0 6814.0 6815.0 6816.0 6817.0 6818.0 6819.0 6820.0 6821.0 6822.0 6823.0 6824.0 6825.0 6826.0 6827.0 6828.0 6829.0 6830.0 6831.0 6832.0 6833.0 6834.0 6835.0 6836.0 6837.0 6838.0 6839.0 6840.0 6841.0 6842.0 6843.0 6844.0 6845.0 6846.0 6847.0 6848.0 6849.0 6850.0 6851.0 6852.0 6853.0 6854.0 6855.0 6856.0 6857.0 6858.0 6859.0 6860.0 6861.0 6862.0 6863.0 6864.0 6865.0 6866.0 6867.0 6868.0 6869.0 6870.0 6871.0 6872.0 6873.0 6874.0 6875.0 6876.0 6877.0 6878.0 6879.0 6880.0 6881.0 6882.0 6883.0 6884.0 6885.0 6886.0 6887.0 6888.0 6889.0 6890.0 6891.0 6892.0 6893.0 6894.0 6895.0 6896.0 6897.0 6898.0 6899.0 6900.0 6901.0 6902.0 6903.0 6904.0 6905.0 6906.0 6907.0 6908.0 6909.0 6910.0 6911.0 6912.0 6913.0 6914.0 6915.0 6916.0 6917.0 6918.0 6919.0 6920.0 6921.0 6922.0 6923.0 6924.0 6925.0 6926.0 6927.0 6928.0 6929.0 6930.0 6931.0 6932.0 6933.0 6934.0 6935.0 6936.0 6937.0 6938.0 6939.0 6940.0 6941.0 6942.0 6943.0 6944.0 6945.0 6946.0 6947.0 6948.0 6949.0 6950.0 6951.0 6952.0 6953.0 6954.0 6955.0 6956.0 6957.0 6958.0 6959.0 6960.0 6961.0 6962.0 6963.0 6964.0 6965.0 6966.0 6967.0 6968.0 6969.0 6970.0 6971.0 6972.0 6973.0 6974.0 6975.0 6976.0 6977.0 6978.0 6979.0 6980.0 6981.0 6982.0 6983.0 6984.0 6985.0 6986.0 6987.0 6988.0 6989.0 6990.0 6991.0 6992.0 6993.0 6994.0 6995.0 6996.0 6997.0 6998.0 6999.0 7000.0 7001.0 7002.0 7003.0 7004.0 7005.0 7006.0 7007.0 7008.0 7009.0 7010.0 7011.0 7012.0 7013.0 7014.0 7015.0 7016.0 7017.0 7018.0 7019.0 7020.0 7021.0 7022.0 7023.0 7024.0 7025.0 7026.0 7027.0 7028.0 7029.0 7030.0 7031.0 7032.0 7033.0 7034.0 7035.0 7036.0 7037.0 7038.0 7039.0 7040.0 7041.0 7042.0 7043.0 7044.0 7045.0 7046.0 7047.0 7048.0 7049.0 7050.0 7051.0 7052.0 7053.0 7054.0 7055.0 7056.0 7057.0 7058.0 7059.0 7060.0 7061.0 7062.0 7063.0 7064.0 7065.0 7066.0 7067.0 7068.0 7069.0 7070.0 7071.0 7072.0 7073.0 7074.0 7075.0 7076.0 7077.0 7078.0 7079.0 7080.0 7081.0 7082.0 7083.0 7084.0 7085.0 7086.0 7087.0 7088.0 7089.0 7090.0 7091.0 7092.0 7093.0 7094.0 7095.0 7096.0 7097.0 7098.0 7099.0 7100.0 7101.0 7102.0 7103.0 7104.0 7105.0 7106.0 7107.0 7108.0 7109.0 7110.0 7111.0 7112.0 7113.0 7114.0 7115.0 7116.0 7117.0 7118.0 7119.0 7120.0 7121.0 7122.0 7123.0 7124.0 7125.0 7126.0 7127.0 7128.0 7129.0 7130.0 7131.0 7132.0 7133.0 7134.0 7135.0 7136.0 7137.0 7138.0 7139.0 7140.0 7141.0 7142.0 7143.0 7144.0 7145.0 7146.0 7147.0 7148.0 7149.0 7150.0 7151.0 7152.0 7153.0 7154.0 7155.0 7156.0 7157.0 7158.0 7159.0 7160.0 7161.0 7162.0 7163.0 7164.0 7165.0 7166.0 7167.0 7168.0 7169.0 7170.0 7171.0 7172.0 7173.0 7174.0 7175.0 7176.0 7177.0 7178.0 7179.0 7180.0 7181.0 7182.0 7183.0 7184.0 7185.0 7186.0 7187.0 7188.0 7189.0 7190.0 7191.0 7192.0 7193.0 7194.0 7195.0 7196.0 7197.0 7198.0 7199.0 7200.0 7201.0 7202.0 7203.0 7204.0 7205.0 7206.0 7207.0 7208.0 7209.0 7210.0 7211.0 7212.0 7213.0 7214.0 7215.0 7216.0 7217.0 7218.0 7219.0 7220.0 7221.0 7222.0 7223.0 7224.0 7225.0 7226.0 7227.0 7228.0 7229.0 7230.0 7231.0 7232.0 7233.0 7234.0 7235.0 7236.0 7237.0 7238.0 7239.0 7240.0 7241.0 7242.0 7243.0 7244.0 7245.0 7246.0 7247.0 7248.0 7249.0 7250.0 7251.0 7252.0 7253.0 7254.0 7255.0 7256.0 7257.0 7258.0 7259.0 7260.0 7261.0 7262.0 7263.0 7264.0 7265.0 7266.0 7267.0 7268.0 7269.0 7270.0 7271.0 7272.0 7273.0 7274.0 7275.0 7276.0 7277.0 7278.0 7279.0 7280.0 7281.0 7282.0 7283.0 7284.0 7285.0 7286.0 7287.0 7288.0 7289.0 7290.0 7291.0 7292.0 7293.0 7294.0 7295.0 7296.0 7297.0 7298.0 7299.0 7300.0 7301.0 7302.0 7303.0 7304.0 7305.0 7306.0 7307.0 7308.0 7309.0 7310.0 7311.0 7312.0 7313.0 7314.0 7315.0 7316.0 7317.0 7318.0 7319.0 7320.0 7321.0 7322.0 7323.0 7324.0 7325.0 7326.0 7327.0 7328.0 7329.0 7330.0 7331.0 7332.0 7333.0 7334.0 7335.0 7336.0 7337.0 7338.0 7339.0 7340.0 7341.0 7342.0 7343.0 7344.0 7345.0 7346.0 7347.0 7348.0 7349.0 7350.0 7351.0 7352.0 7353.0 7354.0 7355.0 7356.0 7357.0 7358.0 7359.0 7360.0 7361.0 7362.0 7363.0 7364.0 7365.0 7366.0 7367.0 7368.0 7369.0 7370.0 7371.0 7372.0 7373.0 7374.0 7375.0 7376.0 7377.0 7378.0 7379.0 7380.0 7381.0 7382.0 7383.0 7384.0 7385.0 7386.0 7387.0 7388.0 7389.0 7390.0 7391.0 7392.0 7393.0 7394.0 7395.0 7396.0 7397.0 7398.0 7399.0 7400.0 7401.0 7402.0 7403.0 7404.0 7405.0 7406.0 7407.0 7408.0 7409.0 7410.0 7411.0 7412.0 7413.0 7414.0 7415.0 7416.0 7417.0 7418.0 7419.0 7420.0 7421.0 7422.0 7423.0 7424.0 7425.0 7426.0 7427.0 7428.0 7429.0 7430.0 7431.0 7432.0 7433.0 7434.0 7435.0 7436.0 7437.0 7438.0 7439.0 7440.0 7441.0 7442.0 7443.0 7444.0 7445.0 7446.0 7447.0 7448.0 7449.0 7450.0 7451.0 7452.0 7453.0 7454.0 7455.0 7456.0 7457.0 7458.0 7459.0 7460.0 7461.0 7462.0 7463.0 7464.0 7465.0 7466.0 7467.0 7468.0 7469.0 7470.0 7471.0 7472.0 7473.0 7474.0 7475.0 7476.0 7477.0 7478.0 7479.0 7480.0 7481.0 7482.0 7483.0 7484.0 7485.0 7486.0 7487.0 7488.0 7489.0 7490.0 7491.0 7492.0 7493.0 7494.0 7495.0 7496.0 7497.0 7498.0 7499.0 7500.0 7501.0 7502.0 7503.0 7504.0 7505.0 7506.0 7507.0 7508.0 7509.0 7510.0 7511.0 7512.0 7513.0 7514.0 7515.0 7516.0 7517.0 7518.0 7519.0 7520.0 7521.0 7522.0 7523.0 7524.0 7525.0 7526.0 7527.0 7528.0 7529.0 7530.0 7531.0 7532.0 7533.0 7534.0 7535.0 7536.0 7537.0 7538.0 7539.0 7540.0 7541.0 7542.0 7543.0 7544.0 7545.0 7546.0 7547.0 7548.0 7549.0 7550.0 7551.0 7552.0 7553.0 7554.0 7555.0 7556.0 7557.0 7558.0 7559.0 7560.0 7561.0 7562.0 7563.0 7564.0 7565.0 7566.0 7567.0 7568.0 7569.0 7570.0 7571.0 7572.0 7573.0 7574.0 7575.0 7576.0 7577.0 7578.0 7579.0 7580.0 7581.0 7582.0 7583.0 7584.0 7585.0 7586.0 7587.0 7588.0 7589.0 7590.0 7591.0 7592.0 7593.0 7594.0 7595.0 7596.0 7597.0 7598.0 7599.0 7600.0 7601.0 7602.0 7603.0 7604.0 7605.0 7606.0 7607.0 7608.0 7609.0 7610.0 7611.0 7612.0 7613.0 7614.0 7615.0 7616.0 7617.0 7618.0 7619.0 7620.0 7621.0 7622.0 7623.0 7624.0 7625.0 7626.0 7627.0 7628.0 7629.0 7630.0 7631.0 7632.0 7633.0 7634.0 7635.0 7636.0 7637.0 7638.0 7639.0 7640.0 7641.0 7642.0 7643.0 7644.0 7645.0 7646.0 7647.0 7648.0 7649.0 7650.0 7651.0 7652.0 7653.0 7654.0 7655.0 7656.0 7657.0 7658.0 7659.0 7660.0 7661.0 7662.0 7663.0 7664.0 7665.0 7666.0 7667.0 7668.0 7669.0 7670.0 7671.0 7672.0 7673.0 7674.0 7675.0 7676.0 7677.0 7678.0 7679.0 7680.0 7681.0 7682.0 7683.0 7684.0 7685.0 7686.0 7687.0 7688.0 7689.0 7690.0 7691.0 7692.0 7693.0 7694.0 7695.0 7696.0 7697.0 7698.0 7699.0 7700.0 7701.0 7702.0 7703.0 7704.0 7705.0 7706.0 7707.0 7708.0 7709.0 7710.0 7711.0 7712.0 7713.0 7714.0 7715.0 7716.0 7717.0 7718.0 7719.0 7720.0 7721.0 7722.0 7723.0 7724.0 7725.0 7726.0 7727.0 7728.0 7729.0 7730.0 7731.0 7732.0 7733.0 7734.0 7735.0 7736.0 7737.0 7738.0 7739.0 7740.0 7741.0 7742.0 7743.0 7744.0 7745.0 7746.0 7747.0 7748.0 7749.0 7750.0 7751.0 7752.0 7753.0 7754.0 7755.0 7756.0 7757.0 7758.0 7759.0 7760.0 7761.0 7762.0 7763.0 7764.0 7765.0 7766.0 7767.0 7768.0 7769.0 7770.0 7771.0 7772.0 7773.0 7774.0 7775.0 7776.0 7777.0 7778.0 7779.0 7780.0 7781.0 7782.0 7783.0 7784.0 7785.0 7786.0 7787.0 7788.0 7789.0 7790.0 7791.0 7792.0 7793.0 7794.0 7795.0 7796.0 7797.0 7798.0 7799.0 7800.0 7801.0 7802.0 7803.0 7804.0 7805.0 7806.0 7807.0 7808.0 7809.0 7810.0 7811.0 7812.0 7813.0 7814.0 7815.0 7816.0 7817.0 7818.0 7819.0 7820.0 7821.0 7822.0 7823.0 7824.0 7825.0 7826.0 7827.0 7828.0 7829.0 7830.0 7831.0 7832.0 7833.0 7834.0 7835.0 7836.0 7837.0 7838.0 7839.0 7840.0 7841.0 7842.0 7843.0 7844.0 7845.0 7846.0 7847.0 7848.0 7849.0 7850.0 7851.0 7852.0 7853.0 7854.0 7855.0 7856.0 7857.0 7858.0 7859.0 7860.0 7861.0 7862.0 7863.0 7864.0 7865.0 7866.0 7867.0 7868.0 7869.0 7870.0 7871.0 7872.0 7873.0 7874.0 7875.0 7876.0 7877.0 7878.0 7879.0 7880.0 7881.0 7882.0 7883.0 7884.0 7885.0 7886.0 7887.0 7888.0 7889.0 7890.0 7891.0 7892.0 7893.0 7894.0 7895.0 7896.0 7897.0 7898.0 7899.0 7900.0 7901.0 7902.0 7903.0 7904.0 7905.0 7906.0 7907.0 7908.0 7909.0 7910.0 7911.0 7912.0 7913.0 7914.0 7915.0 7916.0 7917.0 7918.0 7919.0 7920.0 7921.0 7922.0 7923.0 7924.0 7925.0 7926.0 7927.0 7928.0 7929.0 7930.0 7931.0 7932.0 7933.0 7934.0 7935.0 7936.0 7937.0 7938.0 7939.0 7940.0 7941.0 7942.0 7943.0 7944.0 7945.0 7946.0 7947.0 7948.0 7949.0 7950.0 7951.0 7952.0 7953.0 7954.0 7955.0 7956.0 7957.0 7958.0 7959.0 7960.0 7961.0 7962.0 7963.0 7964.0 7965.0 7966.0 7967.0 7968.0 7969.0 7970.0 7971.0 7972.0 7973.0 7974.0 7975.0 7976.0 7977.0 7978.0 7979.0 7980.0 7981.0 7982.0 7983.0 7984.0 7985.0 7986.0 7987.0 7988.0 7989.0 7990.0 7991.0 7992.0 7993.0 7994.0 7995.0 7996.0 7997.0 7998.0 7999.0 8000.0 8001.0 8002.0 8003.0 8004.0 8005.0 8006.0 8007.0 8008.0 8009.0 8010.0 8011.0 8012.0 8013.0 8014.0 8015.0 8016.0 8017.0 8018.0 8019.0 8020.0 8021.0 8022.0 8023.0 8024.0 8025.0 8026.0 8027.0 8028.0 8029.0 8030.0 8031.0 8032.0 8033.0 8034.0 8035.0 8036.0 8037.0 8038.0 8039.0 8040.0 8041.0 8042.0 8043.0 8044.0 8045.0 8046.0 8047.0 8048.0 8049.0 8050.0 8051.0 8052.0 8053.0 8054.0 8055.0 8056.0 8057.0 8058.0 8059.0 8060.0 8061.0 8062.0 8063.0 8064.0 8065.0 8066.0 8067.0 8068.0 8069.0 8070.0 8071.0 8072.0 8073.0 8074.0 8075.0 8076.0 8077.0 8078.0 8079.0 8080.0 8081.0 8082.0 8083.0 8084.0 8085.0 8086.0 8087.0 8088.0 8089.0 8090.0 8091.0 8092.0 8093.0 8094.0 8095.0 8096.0 8097.0 8098.0 8099.0 8100.0 8101.0 8102.0 8103.0 8104.0 8105.0 8106.0 8107.0 8108.0 8109.0 8110.0 8111.0 8112.0 8113.0 8114.0 8115.0 8116.0 8117.0 8118.0 8119.0 8120.0 8121.0 8122.0 8123.0 8124.0 8125.0 8126.0 8127.0 8128.0 8129.0 8130.0 8131.0 8132.0 8133.0 8134.0 8135.0 8136.0 8137.0 8138.0 8139.0 8140.0 8141.0 8142.0 8143.0 8144.0 8145.0 8146.0 8147.0 8148.0 8149.0 8150.0 8151.0 8152.0 8153.0 8154.0 8155.0 8156.0 8157.0 8158.0 8159.0 8160.0 8161.0 8162.0 8163.0 8164.0 8165.0 8166.0 8167.0 8168.0 8169.0 8170.0 8171.0 8172.0 8173.0 8174.0 8175.0 8176.0 8177.0 8178.0 8179.0 8180.0 8181.0 8182.0 8183.0 8184.0 8185.0 8186.0 8187.0 8188.0 8189.0 8190.0 8191.0 8192.0 8193.0 8194.0 8195.0 8196.0 8197.0 8198.0 8199.0 8200.0 8201.0 8202.0 8203.0 8204.0 8205.0 8206.0 8207.0 8208.0 8209.0 8210.0 8211.0 8212.0 8213.0 8214.0 8215.0 8216.0 8217.0 8218.0 8219.0 8220.0 8221.0 8222.0 8223.0 8224.0 8225.0 8226.0 8227.0 8228.0 8229.0 8230.0 8231.0 8232.0 8233.0 8234.0 8235.0 8236.0 8237.0 8238.0 8239.0 8240.0 8241.0 8242.0 8243.0 8244.0 8245.0 8246.0 8247.0 8248.0 8249.0 8250.0 8251.0 8252.0 8253.0 8254.0 8255.0 8256.0 8257.0 8258.0 8259.0 8260.0 8261.0 8262.0 8263.0 8264.0 8265.0 8266.0 8267.0 8268.0 8269.0 8270.0 8271.0 8272.0 8273.0 8274.0 8275.0 8276.0 8277.0 8278.0 8279.0 8280.0 8281.0 8282.0 8283.0 8284.0 8285.0 8286.0 8287.0 8288.0 8289.0 8290.0 8291.0 8292.0 8293.0 8294.0 8295.0 8296.0 8297.0 8298.0 8299.0 8300.0 8301.0 8302.0 8303.0 8304.0 8305.0 8306.0 8307.0 8308.0 8309.0 8310.0 8311.0 8312.0 8313.0 8314.0 8315.0 8316.0 8317.0 8318.0 8319.0 8320.0 8321.0 8322.0 8323.0 8324.0 8325.0 8326.0 8327.0 8328.0 8329.0 8330.0 8331.0 8332.0 8333.0 8334.0 8335.0 8336.0 8337.0 8338.0 8339.0 8340.0 8341.0 8342.0 8343.0 8344.0 8345.0 8346.0 8347.0 8348.0 8349.0 8350.0 8351.0 8352.0 8353.0 8354.0 8355.0 8356.0 8357.0 8358.0 8359.0 8360.0 8361.0 8362.0 8363.0 8364.0 8365.0 8366.0 8367.0 8368.0 8369.0 8370.0 8371.0 8372.0 8373.0 8374.0 8375.0 8376.0 8377.0 8378.0 8379.0 8380.0 8381.0 8382.0 8383.0 8384.0 8385.0 8386.0 8387.0 8388.0 8389.0 8390.0 8391.0 8392.0 8393.0 8394.0 8395.0 8396.0 8397.0 8398.0 8399.0 8400.0 8401.0 8402.0 8403.0 8404.0 8405.0 8406.0 8407.0 8408.0 8409.0 8410.0 8411.0 8412.0 8413.0 8414.0 8415.0 8416.0 8417.0 8418.0 8419.0 8420.0 8421.0 8422.0 8423.0 8424.0 8425.0 8426.0 8427.0 8428.0 8429.0 8430.0 8431.0 8432.0 8433.0 8434.0 8435.0 8436.0 8437.0 8438.0 8439.0 8440.0 8441.0 8442.0 8443.0 8444.0 8445.0 8446.0 8447.0 8448.0 8449.0 8450.0 8451.0 8452.0 8453.0 8454.0 8455.0 8456.0 8457.0 8458.0 8459.0 8460.0 8461.0 8462.0 8463.0 8464.0 8465.0 8466.0 8467.0 8468.0 8469.0 8470.0 8471.0 8472.0 8473.0 8474.0 8475.0 8476.0 8477.0 8478.0 8479.0 8480.0 8481.0 8482.0 8483.0 8484.0 8485.0 8486.0 8487.0 8488.0 8489.0 8490.0 8491.0 8492.0 8493.0 8494.0 8495.0 8496.0 8497.0 8498.0 8499.0 8500.0 8501.0 8502.0 8503.0 8504.0 8505.0 8506.0 8507.0 8508.0 8509.0 8510.0 8511.0 8512.0 8513.0 8514.0 8515.0 8516.0 8517.0 8518.0 8519.0 8520.0 8521.0 8522.0 8523.0 8524.0 8525.0 8526.0 8527.0 8528.0 8529.0 8530.0 8531.0 8532.0 8533.0 8534.0 8535.0 8536.0 8537.0 8538.0 8539.0 8540.0 8541.0 8542.0 8543.0 8544.0 8545.0 8546.0 8547.0 8548.0 8549.0 8550.0 8551.0 8552.0 8553.0 8554.0 8555.0 8556.0 8557.0 8558.0 8559.0 8560.0 8561.0 8562.0 8563.0 8564.0 8565.0 8566.0 8567.0 8568.0 8569.0 8570.0 8571.0 8572.0 8573.0 8574.0 8575.0 8576.0 8577.0 8578.0 8579.0 8580.0 8581.0 8582.0 8583.0 8584.0 8585.0 8586.0 8587.0 8588.0 8589.0 8590.0 8591.0 8592.0 8593.0 8594.0 8595.0 8596.0 8597.0 8598.0 8599.0 8600.0 8601.0 8602.0 8603.0 8604.0 8605.0 8606.0 8607.0 8608.0 8609.0 8610.0 8611.0 8612.0 8613.0 8614.0 8615.0 8616.0 8617.0 8618.0 8619.0 8620.0 8621.0 8622.0 8623.0 8624.0 8625.0 8626.0 8627.0 8628.0 8629.0 8630.0 8631.0 8632.0 8633.0 8634.0 8635.0 8636.0 8637.0 8638.0 8639.0 8640.0 8641.0 8642.0 8643.0 8644.0 8645.0 8646.0 8647.0 8648.0 8649.0 8650.0 8651.0 8652.0 8653.0 8654.0 8655.0 8656.0 8657.0 8658.0 8659.0 8660.0 8661.0 8662.0 8663.0 8664.0 8665.0 8666.0 8667.0 8668.0 8669.0 8670.0 8671.0 8672.0 8673.0 8674.0 8675.0 8676.0 8677.0 8678.0 8679.0 8680.0 8681.0 8682.0 8683.0 8684.0 8685.0 8686.0 8687.0 8688.0 8689.0 8690.0 8691.0 8692.0 8693.0 8694.0 8695.0 8696.0 8697.0 8698.0 8699.0 8700.0 8701.0 8702.0 8703.0 8704.0 8705.0 8706.0 8707.0 8708.0 8709.0 8710.0 8711.0 8712.0 8713.0 8714.0 8715.0 8716.0 8717.0 8718.0 8719.0 8720.0 8721.0 8722.0 8723.0 8724.0 8725.0 8726.0 8727.0 8728.0 8729.0 8730.0 8731.0 8732.0 8733.0 8734.0 8735.0 8736.0 8737.0 8738.0 8739.0 8740.0 8741.0 8742.0 8743.0 8744.0 8745.0 8746.0 8747.0 8748.0 8749.0 8750.0 8751.0 8752.0 8753.0 8754.0 8755.0 8756.0 8757.0 8758.0 8759.0 8760.0 8761.0 8762.0 8763.0 8764.0 8765.0 8766.0 8767.0 8768.0 8769.0 8770.0 8771.0 8772.0 8773.0 8774.0 8775.0 8776.0 8777.0 8778.0 8779.0 8780.0 8781.0 8782.0 8783.0 8784.0 8785.0 8786.0 8787.0 8788.0 8789.0 8790.0 8791.0 8792.0 8793.0 8794.0 8795.0 8796.0 8797.0 8798.0 8799.0 8800.0 8801.0 8802.0 8803.0 8804.0 8805.0 8806.0 8807.0 8808.0 8809.0 8810.0 8811.0 8812.0 8813.0 8814.0 8815.0 8816.0 8817.0 8818.0 8819.0 8820.0 8821.0 8822.0 8823.0 8824.0 8825.0 8826.0 8827.0 8828.0 8829.0 8830.0 8831.0 8832.0 8833.0 8834.0 8835.0 8836.0 8837.0 8838.0 8839.0 8840.0 8841.0 8842.0 8843.0 8844.0 8845.0 8846.0 8847.0 8848.0 8849.0 8850.0 8851.0 8852.0 8853.0 8854.0 8855.0 8856.0 8857.0 8858.0 8859.0 8860.0 8861.0 8862.0 8863.0 8864.0 8865.0 8866.0 8867.0 8868.0 8869.0 8870.0 8871.0 8872.0 8873.0 8874.0 8875.0 8876.0 8877.0 8878.0 8879.0 8880.0 8881.0 8882.0 8883.0 8884.0 8885.0 8886.0 8887.0 8888.0 8889.0 8890.0 8891.0 8892.0 8893.0 8894.0 8895.0 8896.0 8897.0 8898.0 8899.0 8900.0 8901.0 8902.0 8903.0 8904.0 8905.0 8906.0 8907.0 8908.0 8909.0 8910.0 8911.0 8912.0 8913.0 8914.0 8915.0 8916.0 8917.0 8918.0 8919.0 8920.0 8921.0 8922.0 8923.0 8924.0 8925.0 8926.0 8927.0 8928.0 8929.0 8930.0 8931.0 8932.0 8933.0 8934.0 8935.0 8936.0 8937.0 8938.0 8939.0 8940.0 8941.0 8942.0 8943.0 8944.0 8945.0 8946.0 8947.0 8948.0 8949.0 8950.0 8951.0 8952.0 8953.0 8954.0 8955.0 8956.0 8957.0 8958.0 8959.0 8960.0 8961.0 8962.0 8963.0 8964.0 8965.0 8966.0 8967.0 8968.0 8969.0 8970.0 8971.0 8972.0 8973.0 8974.0 8975.0 8976.0 8977.0 8978.0 8979.0 8980.0 8981.0 8982.0 8983.0 8984.0 8985.0 8986.0 8987.0 8988.0 8989.0 8990.0 8991.0 8992.0 8993.0 8994.0 8995.0 8996.0 8997.0 8998.0 8999.0 9000.0 9001.0 9002.0 9003.0 9004.0 9005.0 9006.0 9007.0 9008.0 9009.0 9010.0 9011.0 9012.0 9013.0 9014.0 9015.0 9016.0 9017.0 9018.0 9019.0 9020.0 9021.0 9022.0 9023.0 9024.0 9025.0 9026.0 9027.0 9028.0 9029.0 9030.0 9031.0 9032.0 9033.0 9034.0 9035.0 9036.0 9037.0 9038.0 9039.0 9040.0 9041.0 9042.0 9043.0 9044.0 9045.0 9046.0 9047.0 9048.0 9049.0 9050.0 9051.0 9052.0 9053.0 9054.0 9055.0 9056.0 9057.0 9058.0 9059.0 9060.0 9061.0 9062.0 9063.0 9064.0 9065.0 9066.0 9067.0 9068.0 9069.0 9070.0 9071.0 9072.0 9073.0 9074.0 9075.0 9076.0 9077.0 9078.0 9079.0 9080.0 9081.0 9082.0 9083.0 9084.0 9085.0 9086.0 9087.0 9088.0 9089.0 9090.0 9091.0 9092.0 9093.0 9094.0 9095.0 9096.0 9097.0 9098.0 9099.0 9100.0 9101.0 9102.0 9103.0 9104.0 9105.0 9106.0 9107.0 9108.0 9109.0 9110.0 9111.0 9112.0 9113.0 9114.0 9115.0 9116.0 9117.0 9118.0 9119.0 9120.0 9121.0 9122.0 9123.0 9124.0 9125.0 9126.0 9127.0 9128.0 9129.0 9130.0 9131.0 9132.0 9133.0 9134.0 9135.0 9136.0 9137.0 9138.0 9139.0 9140.0 9141.0 9142.0 9143.0 9144.0 9145.0 9146.0 9147.0 9148.0 9149.0 9150.0 9151.0 9152.0 9153.0 9154.0 9155.0 9156.0 9157.0 9158.0 9159.0 9160.0 9161.0 9162.0 9163.0 9164.0 9165.0 9166.0 9167.0 9168.0 9169.0 9170.0 9171.0 9172.0 9173.0 9174.0 9175.0 9176.0 9177.0 9178.0 9179.0 9180.0 9181.0 9182.0 9183.0 9184.0 9185.0 9186.0 9187.0 9188.0 9189.0 9190.0 9191.0 9192.0 9193.0 9194.0 9195.0 9196.0 9197.0 9198.0 9199.0 9200.0 9201.0 9202.0 9203.0 9204.0 9205.0 9206.0 9207.0 9208.0 9209.0 9210.0 9211.0 9212.0 9213.0 9214.0 9215.0 9216.0 9217.0 9218.0 9219.0 9220.0 9221.0 9222.0 9223.0 9224.0 9225.0 9226.0 9227.0 9228.0 9229.0 9230.0 9231.0 9232.0 9233.0 9234.0 9235.0 9236.0 9237.0 9238.0 9239.0 9240.0 9241.0 9242.0 9243.0 9244.0 9245.0 9246.0 9247.0 9248.0 9249.0 9250.0 9251.0 9252.0 9253.0 9254.0 9255.0 9256.0 9257.0 9258.0 9259.0 9260.0 9261.0 9262.0 9263.0 9264.0 9265.0 9266.0 9267.0 9268.0 9269.0 9270.0 9271.0 9272.0 9273.0 9274.0 9275.0 9276.0 9277.0 9278.0 9279.0 9280.0 9281.0 9282.0 9283.0 9284.0 9285.0 9286.0 9287.0 9288.0 9289.0 9290.0 9291.0 9292.0 9293.0 9294.0 9295.0 9296.0 9297.0 9298.0 9299.0 9300.0 9301.0 9302.0 9303.0 9304.0 9305.0 9306.0 9307.0 9308.0 9309.0 9310.0 9311.0 9312.0 9313.0 9314.0 9315.0 9316.0 9317.0 9318.0 9319.0 9320.0 9321.0 9322.0 9323.0 9324.0 9325.0 9326.0 9327.0 9328.0 9329.0 9330.0 9331.0 9332.0 9333.0 9334.0 9335.0 9336.0 9337.0 9338.0 9339.0 9340.0 9341.0 9342.0 9343.0 9344.0 9345.0 9346.0 9347.0 9348.0 9349.0 9350.0 9351.0 9352.0 9353.0 9354.0 9355.0 9356.0 9357.0 9358.0 9359.0 9360.0 9361.0 9362.0 9363.0 9364.0 9365.0 9366.0 9367.0 9368.0 9369.0 9370.0 9371.0 9372.0 9373.0 9374.0 9375.0 9376.0 9377.0 9378.0 9379.0 9380.0 9381.0 9382.0 9383.0 9384.0 9385.0 9386.0 9387.0 9388.0 9389.0 9390.0 9391.0 9392.0 9393.0 9394.0 9395.0 9396.0 9397.0 9398.0 9399.0 9400.0 9401.0 9402.0 9403.0 9404.0 9405.0 9406.0 9407.0 9408.0 9409.0 9410.0 9411.0 9412.0 9413.0 9414.0 9415.0 9416.0 9417.0 9418.0 9419.0 9420.0 9421.0 9422.0 9423.0 9424.0 9425.0 9426.0 9427.0 9428.0 9429.0 9430.0 9431.0 9432.0 9433.0 9434.0 9435.0 9436.0 9437.0 9438.0 9439.0 9440.0 9441.0 9442.0 9443.0 9444.0 9445.0 9446.0 9447.0 9448.0 9449.0 9450.0 9451.0 9452.0 9453.0 9454.0 9455.0 9456.0 9457.0 9458.0 9459.0 9460.0 9461.0 9462.0 9463.0 9464.0 9465.0 9466.0 9467.0 9468.0 9469.0 9470.0 9471.0 9472.0 9473.0 9474.0 9475.0 9476.0 9477.0 9478.0 9479.0 9480.0 9481.0 9482.0 9483.0 9484.0 9485.0 9486.0 9487.0 9488.0 9489.0 9490.0 9491.0 9492.0 9493.0 9494.0 9495.0 9496.0 9497.0 9498.0 9499.0 9500.0 9501.0 9502.0 9503.0 9504.0 9505.0 9506.0 9507.0 9508.0 9509.0 9510.0 9511.0 9512.0 9513.0 9514.0 9515.0 9516.0 9517.0 9518.0 9519.0 9520.0 9521.0 9522.0 9523.0 9524.0 9525.0 9526.0 9527.0 9528.0 9529.0 9530.0 9531.0 9532.0 9533.0 9534.0 9535.0 9536.0 9537.0 9538.0 9539.0 9540.0 9541.0 9542.0 9543.0 9544.0 9545.0 9546.0 9547.0 9548.0 9549.0 9550.0 9551.0 9552.0 9553.0 9554.0 9555.0 9556.0 9557.0 9558.0 9559.0 9560.0 9561.0 9562.0 9563.0 9564.0 9565.0 9566.0 9567.0 9568.0 9569.0 9570.0 9571.0 9572.0 9573.0 9574.0 9575.0 9576.0 9577.0 9578.0 9579.0 9580.0 9581.0 9582.0 9583.0 9584.0 9585.0 9586.0 9587.0 9588.0 9589.0 9590.0 9591.0 9592.0 9593.0 9594.0 9595.0 9596.0 9597.0 9598.0 9599.0 9600.0 9601.0 9602.0 9603.0 9604.0 9605.0 9606.0 9607.0 9608.0 9609.0 9610.0 9611.0 9612.0 9613.0 9614.0 9615.0 9616.0 9617.0 9618.0 9619.0 9620.0 9621.0 9622.0 9623.0 9624.0 9625.0 9626.0 9627.0 9628.0 9629.0 9630.0 9631.0 9632.0 9633.0 9634.0 9635.0 9636.0 9637.0 9638.0 9639.0 9640.0 9641.0 9642.0 9643.0 9644.0 9645.0 9646.0 9647.0 9648.0 9649.0 9650.0 9651.0 9652.0 9653.0 9654.0 9655.0 9656.0 9657.0 9658.0 9659.0 9660.0 9661.0 9662.0 9663.0 9664.0 9665.0 9666.0 9667.0 9668.0 9669.0 9670.0 9671.0 9672.0 9673.0 9674.0 9675.0 9676.0 9677.0 9678.0 9679.0 9680.0 9681.0 9682.0 9683.0 9684.0 9685.0 9686.0 9687.0 9688.0 9689.0 9690.0 9691.0 9692.0 9693.0 9694.0 9695.0 9696.0 9697.0 9698.0 9699.0 9700.0 9701.0 9702.0 9703.0 9704.0 9705.0 9706.0 9707.0 9708.0 9709.0 9710.0 9711.0 9712.0 9713.0 9714.0 9715.0 9716.0 9717.0 9718.0 9719.0 9720.0 9721.0 9722.0 9723.0 9724.0 9725.0 9726.0 9727.0 9728.0 9729.0 9730.0 9731.0 9732.0 9733.0 9734.0 9735.0 9736.0 9737.0 9738.0 9739.0 9740.0 9741.0 9742.0 9743.0 9744.0 9745.0 9746.0 9747.0 9748.0 9749.0 9750.0 9751.0 9752.0 9753.0 9754.0 9755.0 9756.0 9757.0 9758.0 9759.0 9760.0 9761.0 9762.0 9763.0 9764.0 9765.0 9766.0 9767.0 9768.0 9769.0 9770.0 9771.0 9772.0 9773.0 9774.0 9775.0 9776.0 9777.0 9778.0 9779.0 9780.0 9781.0 9782.0 9783.0 9784.0 9785.0 9786.0 9787.0 9788.0 9789.0 9790.0 9791.0 9792.0 9793.0 9794.0 9795.0 9796.0 9797.0 9798.0 9799.0 9800.0 9801.0 9802.0 9803.0 9804.0 9805.0 9806.0 9807.0 9808.0 9809.0 9810.0 9811.0 9812.0 9813.0 9814.0 9815.0 9816.0 9817.0 9818.0 9819.0 9820.0 9821.0 9822.0 9823.0 9824.0 9825.0 9826.0 9827.0 9828.0 9829.0 9830.0 9831.0 9832.0 9833.0 9834.0 9835.0 9836.0 9837.0 9838.0 9839.0 9840.0 9841.0 9842.0 9843.0 9844.0 9845.0 9846.0 9847.0 9848.0 9849.0 9850.0 9851.0 9852.0 9853.0 9854.0 9855.0 9856.0 9857.0 9858.0 9859.0 9860.0 9861.0 9862.0 9863.0 9864.0 9865.0 9866.0 9867.0 9868.0 9869.0 9870.0 9871.0 9872.0 9873.0 9874.0 9875.0 9876.0 9877.0 9878.0 9879.0 9880.0 9881.0 9882.0 9883.0 9884.0 9885.0 9886.0 9887.0 9888.0 9889.0 9890.0 9891.0 9892.0 9893.0 9894.0 9895.0 9896.0 9897.0 9898.0 9899.0 9900.0 9901.0 9902.0 9903.0 9904.0 9905.0 9906.0 9907.0 9908.0 9909.0 9910.0 9911.0 9912.0 9913.0 9914.0 9915.0 9916.0 9917.0 9918.0 9919.0 9920.0 9921.0 9922.0 9923.0 9924.0 9925.0 9926.0 9927.0 9928.0 9929.0 9930.0 9931.0 9932.0 9933.0 9934.0 9935.0 9936.0 9937.0 9938.0 9939.0 9940.0 9941.0 9942.0 9943.0 9944.0 9945.0 9946.0 9947.0 9948.0 9949.0 9950.0 9951.0 9952.0 9953.0 9954.0 9955.0 9956.0 9957.0 9958.0 9959.0 9960.0 9961.0 9962.0 9963.0 9964.0 9965.0 9966.0 9967.0 9968.0 9969.0 9970.0 9971.0 9972.0 9973.0 9974.0 9975.0 9976.0 9977.0 9978.0 9979.0 9980.0 9981.0 9982.0 9983.0 9984.0 9985.0 9986.0 9987.0 9988.0 9989.0 9990.0 9991.0 9992.0 9993.0 9994.0 9995.0 9996.0 9997.0 9998.0 9999.0 10000.0 10001.0 10002.0 10003.0 10004.0 10005.0 10006.0 10007.0 10008.0 10009.0 10010.0 10011.0 10012.0 10013.0 10014.0 10015.0 10016.0 10017.0 10018.0 10019.0 10020.0 10021.0 10022.0 10023.0 10024.0 10025.0 10026.0 10027.0 10028.0 10029.0 10030.0 10031.0 10032.0 10033.0 10034.0 10035.0 10036.0 10037.0 10038.0 10039.0 10040.0 10041.0 10042.0 10043.0 10044.0 10045.0 10046.0 10047.0 10048.0 10049.0 10050.0 10051.0 10052.0 10053.0 10054.0 10055.0 10056.0 10057.0 10058.0 10059.0 10060.0 10061.0 10062.0 10063.0 10064.0 10065.0 10066.0 10067.0 10068.0 10069.0 10070.0 10071.0 10072.0 10073.0 10074.0 10075.0 10076.0 10077.0 10078.0 10079.0 10080.0 10081.0 10082.0 10083.0 10084.0 10085.0 10086.0 10087.0 10088.0 10089.0 10090.0 10091.0 10092.0 10093.0 10094.0 10095.0 10096.0 10097.0 10098.0 10099.0 10100.0 10101.0 10102.0 10103.0 10104.0 10105.0 10106.0 10107.0 10108.0 10109.0 10110.0 10111.0 10112.0 10113.0 10114.0 10115.0 10116.0 10117.0 10118.0 10119.0 10120.0 10121.0 10122.0 10123.0 10124.0 10125.0 10126.0 10127.0 10128.0 10129.0 10130.0 10131.0 10132.0 10133.0 10134.0 10135.0 10136.0 10137.0 10138.0 10139.0 10140.0 10141.0 10142.0 10143.0 10144.0 10145.0 10146.0 10147.0 10148.0 10149.0 10150.0 10151.0 10152.0 10153.0 10154.0 10155.0 10156.0 10157.0 10158.0 10159.0 10160.0 10161.0 10162.0 10163.0 10164.0 10165.0 10166.0 10167.0 10168.0 10169.0 10170.0 10171.0 10172.0 10173.0 10174.0 10175.0 10176.0 10177.0 10178.0 10179.0 10180.0 10181.0 10182.0 10183.0 10184.0 10185.0 10186.0 10187.0 10188.0 10189.0 10190.0 10191.0 10192.0 10193.0 10194.0 10195.0 10196.0 10197.0 10198.0 10199.0 10200.0 10201.0 10202.0 10203.0 10204.0 10205.0 10206.0 10207.0 10208.0 10209.0 10210.0 10211.0 10212.0 10213.0 10214.0 10215.0 10216.0 10217.0 10218.0 10219.0 10220.0 10221.0 10222.0 10223.0 10224.0 10225.0 10226.0 10227.0 10228.0 10229.0 10230.0 10231.0 10232.0 10233.0 10234.0 10235.0 10236.0 10237.0 10238.0 10239.0 10240.0 10241.0 10242.0 10243.0 10244.0 10245.0 10246.0 10247.0 10248.0 10249.0 10250.0 10251.0 10252.0 10253.0 10254.0 10255.0 10256.0 10257.0 10258.0 10259.0 10260.0 10261.0 10262.0 10263.0 10264.0 10265.0 10266.0 10267.0 10268.0 10269.0 10270.0 10271.0 10272.0 10273.0 10274.0 10275.0 10276.0 10277.0 10278.0 10279.0 10280.0 10281.0 10282.0 10283.0 10284.0 10285.0 10286.0 10287.0 10288.0 10289.0 10290.0 10291.0 10292.0 10293.0 10294.0 10295.0 10296.0 10297.0 10298.0 10299.0 10300.0 10301.0 10302.0 10303.0 10304.0 10305.0 10306.0 10307.0 10308.0 10309.0 10310.0 10311.0 10312.0 10313.0 10314.0 10315.0 10316.0 10317.0 10318.0 10319.0 10320.0 10321.0 10322.0 10323.0 10324.0 10325.0 10326.0 10327.0 10328.0 10329.0 10330.0 10331.0 10332.0 10333.0 10334.0 10335.0 10336.0 10337.0 10338.0 10339.0 10340.0 10341.0 10342.0 10343.0 10344.0 10345.0 10346.0 10347.0 10348.0 10349.0 10350.0 10351.0 10352.0 10353.0 10354.0 10355.0 10356.0 10357.0 10358.0 10359.0 10360.0 10361.0 10362.0 10363.0 10364.0 10365.0 10366.0 10367.0 10368.0 10369.0 10370.0 10371.0 10372.0 10373.0 10374.0 10375.0 10376.0 10377.0 10378.0 10379.0 10380.0 10381.0 10382.0 10383.0 10384.0 10385.0 10386.0 10387.0 10388.0 10389.0 10390.0 10391.0 10392.0 10393.0 10394.0 10395.0 10396.0 10397.0 10398.0 10399.0 10400.0 10401.0 10402.0 10403.0 10404.0 10405.0 10406.0 10407.0 10408.0 10409.0 10410.0 10411.0 10412.0 10413.0 10414.0 10415.0 10416.0 10417.0 10418.0 10419.0 10420.0 10421.0 10422.0 10423.0 10424.0 10425.0 10426.0 10427.0 10428.0 10429.0 10430.0 10431.0 10432.0 10433.0 10434.0 10435.0 10436.0 10437.0 10438.0 10439.0 10440.0 10441.0 10442.0 10443.0 10444.0 10445.0 10446.0 10447.0 10448.0 10449.0 10450.0 10451.0 10452.0 10453.0 10454.0 10455.0 10456.0 10457.0 10458.0 10459.0 10460.0 10461.0 10462.0 10463.0 10464.0 10465.0 10466.0 10467.0 10468.0 10469.0 10470.0 10471.0 10472.0 10473.0 10474.0 10475.0 10476.0 10477.0 10478.0 10479.0 10480.0 10481.0 10482.0 10483.0 10484.0 10485.0 10486.0 10487.0 10488.0 10489.0 10490.0 10491.0 10492.0 10493.0 10494.0 10495.0 10496.0 10497.0 10498.0 10499.0 10500.0 10501.0 10502.0 10503.0 10504.0 10505.0 10506.0 10507.0 10508.0 10509.0 10510.0 10511.0 10512.0 10513.0 10514.0 10515.0 10516.0 10517.0 10518.0 10519.0 10520.0 10521.0 10522.0 10523.0 10524.0 10525.0 10526.0 10527.0 10528.0 10529.0 10530.0 10531.0 10532.0 10533.0 10534.0 10535.0 10536.0 10537.0 10538.0 10539.0 10540.0 10541.0 10542.0 10543.0 10544.0 10545.0 10546.0 10547.0 10548.0 10549.0 10550.0 10551.0 10552.0 10553.0 10554.0 10555.0 10556.0 10557.0 10558.0 10559.0 10560.0 10561.0 10562.0 10563.0 10564.0 10565.0 10566.0 10567.0 10568.0 10569.0 10570.0 10571.0 10572.0 10573.0 10574.0 10575.0 10576.0 10577.0 10578.0 10579.0 10580.0 10581.0 10582.0 10583.0 10584.0 10585.0 10586.0 10587.0 10588.0 10589.0 10590.0 10591.0 10592.0 10593.0 10594.0 10595.0 10596.0 10597.0 10598.0 10599.0 10600.0 10601.0 10602.0 10603.0 10604.0 10605.0 10606.0 10607.0 10608.0 10609.0 10610.0 10611.0 10612.0 10613.0 10614.0 10615.0 10616.0 10617.0 10618.0 10619.0 10620.0 10621.0 10622.0 10623.0 10624.0 10625.0 10626.0 10627.0 10628.0 10629.0 10630.0 10631.0 10632.0 10633.0 10634.0 10635.0 10636.0 10637.0 10638.0 10639.0 10640.0 10641.0 10642.0 10643.0 10644.0 10645.0 10646.0 10647.0 10648.0 10649.0 10650.0 10651.0 10652.0 10653.0 10654.0 10655.0 10656.0 10657.0 10658.0 10659.0 10660.0 10661.0 10662.0 10663.0 10664.0 10665.0 10666.0 10667.0 10668.0 10669.0 10670.0 10671.0 10672.0 10673.0 10674.0 10675.0 10676.0 10677.0 10678.0 10679.0 10680.0 10681.0 10682.0 10683.0 10684.0 10685.0 10686.0 10687.0 10688.0 10689.0 10690.0 10691.0 10692.0 10693.0 10694.0 10695.0 10696.0 10697.0 10698.0 10699.0 10700.0 10701.0 10702.0 10703.0 10704.0 10705.0 10706.0 10707.0 10708.0 10709.0 10710.0 10711.0 10712.0 10713.0 10714.0 10715.0 10716.0 10717.0 10718.0 10719.0 10720.0 10721.0 10722.0 10723.0 10724.0 10725.0 10726.0 10727.0 10728.0 10729.0 10730.0 10731.0 10732.0 10733.0 10734.0 10735.0 10736.0 10737.0 10738.0 10739.0 10740.0 10741.0 10742.0 10743.0 10744.0 10745.0 10746.0 10747.0 10748.0 10749.0 10750.0 10751.0 10752.0 10753.0 10754.0 10755.0 10756.0 10757.0 10758.0 10759.0 10760.0 10761.0 10762.0 10763.0 10764.0 10765.0 10766.0 10767.0 10768.0 10769.0 10770.0 10771.0 10772.0 10773.0 10774.0 10775.0 10776.0 10777.0 10778.0 10779.0 10780.0 10781.0 10782.0 10783.0 10784.0 10785.0 10786.0 10787.0 10788.0 10789.0 10790.0 10791.0 10792.0 10793.0 10794.0 10795.0 10796.0 10797.0 10798.0 10799.0 10800.0 10801.0 10802.0 10803.0 10804.0 10805.0 10806.0 10807.0 10808.0 10809.0 10810.0 10811.0 10812.0 10813.0 10814.0 10815.0 10816.0 10817.0 10818.0 10819.0 10820.0 10821.0 10822.0 10823.0 10824.0 10825.0 10826.0 10827.0 10828.0 10829.0 10830.0 10831.0 10832.0 10833.0 10834.0 10835.0 10836.0 10837.0 10838.0 10839.0 10840.0 10841.0 10842.0 10843.0 10844.0 10845.0 10846.0 10847.0 10848.0 10849.0 10850.0 10851.0 10852.0 10853.0 10854.0 10855.0 10856.0 10857.0 10858.0 10859.0 10860.0 10861.0 10862.0 10863.0 10864.0 10865.0 10866.0 10867.0 10868.0 10869.0 10870.0 10871.0 10872.0 10873.0 10874.0 10875.0 10876.0 10877.0 10878.0 10879.0 10880.0 10881.0 10882.0 10883.0 10884.0 10885.0 10886.0 10887.0 10888.0 10889.0 10890.0 10891.0 10892.0 10893.0 10894.0 10895.0 10896.0 10897.0 10898.0 10899.0 10900.0 10901.0 10902.0 10903.0 10904.0 10905.0 10906.0 10907.0 10908.0 10909.0 10910.0 10911.0 10912.0 10913.0 10914.0 10915.0 10916.0 10917.0 10918.0 10919.0 10920.0 10921.0 10922.0 10923.0 10924.0 10925.0 10926.0 10927.0 10928.0 10929.0 10930.0 10931.0 10932.0 10933.0 10934.0 10935.0 10936.0 10937.0 10938.0 10939.0 10940.0 10941.0 10942.0 10943.0 10944.0 10945.0 10946.0 10947.0 10948.0 10949.0 10950.0 10951.0 10952.0 10953.0 10954.0 10955.0 10956.0 10957.0 10958.0 10959.0 10960.0 10961.0 10962.0 10963.0 10964.0 10965.0 10966.0 10967.0 10968.0 10969.0 10970.0 10971.0 10972.0 10973.0 10974.0 10975.0 10976.0 10977.0 10978.0 10979.0 10980.0 10981.0 10982.0 10983.0 10984.0 10985.0 10986.0 10987.0 10988.0 10989.0 10990.0 10991.0 10992.0 10993.0 10994.0 10995.0 10996.0 10997.0 10998.0 10999.0 11000.0 11001.0 11002.0 11003.0 11004.0 11005.0 11006.0 11007.0 11008.0 11009.0 11010.0 11011.0 11012.0 11013.0 11014.0 11015.0 11016.0 11017.0 11018.0 11019.0 11020.0 11021.0 11022.0 11023.0 11024.0 11025.0 11026.0 11027.0 11028.0 11029.0 11030.0 11031.0 11032.0 11033.0 11034.0 11035.0 11036.0 11037.0 11038.0 11039.0 11040.0 11041.0 11042.0 11043.0 11044.0 11045.0 11046.0 11047.0 11048.0 11049.0 11050.0 11051.0 11052.0 11053.0 11054.0 11055.0 11056.0 11057.0 11058.0 11059.0 11060.0 11061.0 11062.0 11063.0 11064.0 11065.0 11066.0 11067.0 11068.0 11069.0 11070.0 11071.0 11072.0 11073.0 11074.0 11075.0 11076.0 11077.0 11078.0 11079.0 11080.0 11081.0 11082.0 11083.0 11084.0 11085.0 11086.0 11087.0 11088.0 11089.0 11090.0 11091.0 11092.0 11093.0 11094.0 11095.0 11096.0 11097.0 11098.0 11099.0 11100.0 11101.0 11102.0 11103.0 11104.0 11105.0 11106.0 11107.0 11108.0 11109.0 11110.0 11111.0 11112.0 11113.0 11114.0 11115.0 11116.0 11117.0 11118.0 11119.0 11120.0 11121.0 11122.0 11123.0 11124.0 11125.0 11126.0 11127.0 11128.0 11129.0 11130.0 11131.0 11132.0 11133.0 11134.0 11135.0 11136.0 11137.0 11138.0 11139.0 11140.0 11141.0 11142.0 11143.0 11144.0 11145.0 11146.0 11147.0 11148.0 11149.0 11150.0 11151.0 11152.0 11153.0 11154.0 11155.0 11156.0 11157.0 11158.0 11159.0 11160.0 11161.0 11162.0 11163.0 11164.0 11165.0 11166.0 11167.0 11168.0 11169.0 11170.0 11171.0 11172.0 11173.0 11174.0 11175.0 11176.0 11177.0 11178.0 11179.0 11180.0 11181.0 11182.0 11183.0 11184.0 11185.0 11186.0 11187.0 11188.0 11189.0 11190.0 11191.0 11192.0 11193.0 11194.0 11195.0 11196.0 11197.0 11198.0 11199.0 11200.0 11201.0 11202.0 11203.0 11204.0 11205.0 11206.0 11207.0 11208.0 11209.0 11210.0 11211.0 11212.0 11213.0 11214.0 11215.0 11216.0 11217.0 11218.0 11219.0 11220.0 11221.0 11222.0 11223.0 11224.0 11225.0 11226.0 11227.0 11228.0 11229.0 11230.0 11231.0 11232.0 11233.0 11234.0 11235.0 11236.0 11237.0 11238.0 11239.0 11240.0 11241.0 11242.0 11243.0 11244.0 11245.0 11246.0 11247.0 11248.0 11249.0 11250.0 11251.0 11252.0 11253.0 11254.0 11255.0 11256.0 11257.0 11258.0 11259.0 11260.0 11261.0 11262.0 11263.0 11264.0 11265.0 11266.0 11267.0 11268.0 11269.0 11270.0 11271.0 11272.0 11273.0 11274.0 11275.0 11276.0 11277.0 11278.0 11279.0 11280.0 11281.0 11282.0 11283.0 11284.0 11285.0 11286.0 11287.0 11288.0 11289.0 11290.0 11291.0 11292.0 11293.0 11294.0 11295.0 11296.0 11297.0 11298.0 11299.0 11300.0 11301.0 11302.0 11303.0 11304.0 11305.0 11306.0 11307.0 11308.0 11309.0 11310.0 11311.0 11312.0 11313.0 11314.0 11315.0 11316.0 11317.0 11318.0 11319.0 11320.0 11321.0 11322.0 11323.0 11324.0 11325.0 11326.0 11327.0 11328.0 11329.0 11330.0 11331.0 11332.0 11333.0 11334.0 11335.0 11336.0 11337.0 11338.0 11339.0 11340.0 11341.0 11342.0 11343.0 11344.0 11345.0 11346.0 11347.0 11348.0 11349.0 11350.0 11351.0 11352.0 11353.0 11354.0 11355.0 11356.0 11357.0 11358.0 11359.0 11360.0 11361.0 11362.0 11363.0 11364.0 11365.0 11366.0 11367.0 11368.0 11369.0 11370.0 11371.0 11372.0 11373.0 11374.0 11375.0 11376.0 11377.0 11378.0 11379.0 11380.0 11381.0 11382.0 11383.0 11384.0 11385.0 11386.0 11387.0 11388.0 11389.0 11390.0 11391.0 11392.0 11393.0 11394.0 11395.0 11396.0 11397.0 11398.0 11399.0 11400.0 11401.0 11402.0 11403.0 11404.0 11405.0 11406.0 11407.0 11408.0 11409.0 11410.0 11411.0 11412.0 11413.0 11414.0 11415.0 11416.0 11417.0 11418.0 11419.0 11420.0 11421.0 11422.0 11423.0 11424.0 11425.0 11426.0 11427.0 11428.0 11429.0 11430.0 11431.0 11432.0 11433.0 11434.0 11435.0 11436.0 11437.0 11438.0 11439.0 11440.0 11441.0 11442.0 11443.0 11444.0 11445.0 11446.0 11447.0 11448.0 11449.0 11450.0 11451.0 11452.0 11453.0 11454.0 11455.0 11456.0 11457.0 11458.0 11459.0 11460.0 11461.0 11462.0 11463.0 11464.0 11465.0 11466.0 11467.0 11468.0 11469.0 11470.0 11471.0 11472.0 11473.0 11474.0 11475.0 11476.0 11477.0 11478.0 11479.0 11480.0 11481.0 11482.0 11483.0 11484.0 11485.0 11486.0 11487.0 11488.0 11489.0 11490.0 11491.0 11492.0 11493.0 11494.0 11495.0 11496.0 11497.0 11498.0 11499.0 11500.0 11501.0 11502.0 11503.0 11504.0 11505.0 11506.0 11507.0 11508.0 11509.0 11510.0 11511.0 11512.0 11513.0 11514.0 11515.0 11516.0 11517.0 11518.0 11519.0 11520.0 11521.0 11522.0 11523.0 11524.0 11525.0 11526.0 11527.0 11528.0 11529.0 11530.0 11531.0 11532.0 11533.0 11534.0 11535.0 11536.0 11537.0 11538.0 11539.0 11540.0 11541.0 11542.0 11543.0 11544.0 11545.0 11546.0 11547.0 11548.0 11549.0 11550.0 11551.0 11552.0 11553.0 11554.0 11555.0 11556.0 11557.0 11558.0 11559.0 11560.0 11561.0 11562.0 11563.0 11564.0 11565.0 11566.0 11567.0 11568.0 11569.0 11570.0 11571.0 11572.0 11573.0 11574.0 11575.0 11576.0 11577.0 11578.0 11579.0 11580.0 11581.0 11582.0 11583.0 11584.0 11585.0 11586.0 11587.0 11588.0 11589.0 11590.0 11591.0 11592.0 11593.0 11594.0 11595.0 11596.0 11597.0 11598.0 11599.0 11600.0 11601.0 11602.0 11603.0 11604.0 11605.0 11606.0 11607.0 11608.0 11609.0 11610.0 11611.0 11612.0 11613.0 11614.0 11615.0 11616.0 11617.0 11618.0 11619.0 11620.0 11621.0 11622.0 11623.0 11624.0 11625.0 11626.0 11627.0 11628.0 11629.0 11630.0 11631.0 11632.0 11633.0 11634.0 11635.0 11636.0 11637.0 11638.0 11639.0 11640.0 11641.0 11642.0 11643.0 11644.0 11645.0 11646.0 11647.0 11648.0 11649.0 11650.0 11651.0 11652.0 11653.0 11654.0 11655.0 11656.0 11657.0 11658.0 11659.0 11660.0 11661.0 11662.0 11663.0 11664.0 11665.0 11666.0 11667.0 11668.0 11669.0 11670.0 11671.0 11672.0 11673.0 11674.0 11675.0 11676.0 11677.0 11678.0 11679.0 11680.0 11681.0 11682.0 11683.0 11684.0 11685.0 11686.0 11687.0 11688.0 11689.0 11690.0 11691.0 11692.0 11693.0 11694.0 11695.0 11696.0 11697.0 11698.0 11699.0 11700.0 11701.0 11702.0 11703.0 11704.0 11705.0 11706.0 11707.0 11708.0 11709.0 11710.0 11711.0 11712.0 11713.0 11714.0 11715.0 11716.0 11717.0 11718.0 11719.0 11720.0 11721.0 11722.0 11723.0 11724.0 11725.0 11726.0 11727.0 11728.0 11729.0 11730.0 11731.0 11732.0 11733.0 11734.0 11735.0 11736.0 11737.0 11738.0 11739.0 11740.0 11741.0 11742.0 11743.0 11744.0 11745.0 11746.0 11747.0 11748.0 11749.0 11750.0 11751.0 11752.0 11753.0 11754.0 11755.0 11756.0 11757.0 11758.0 11759.0 11760.0 11761.0 11762.0 11763.0 11764.0 11765.0 11766.0 11767.0 11768.0 11769.0 11770.0 11771.0 11772.0 11773.0 11774.0 11775.0 11776.0 11777.0 11778.0 11779.0 11780.0 11781.0 11782.0 11783.0 11784.0 11785.0 11786.0 11787.0 11788.0 11789.0 11790.0 11791.0 11792.0 11793.0 11794.0 11795.0 11796.0 11797.0 11798.0 11799.0 11800.0 11801.0 11802.0 11803.0 11804.0 11805.0 11806.0 11807.0 11808.0 11809.0 11810.0 11811.0 11812.0 11813.0 11814.0 11815.0 11816.0 11817.0 11818.0 11819.0 11820.0 11821.0 11822.0 11823.0 11824.0 11825.0 11826.0 11827.0 11828.0 11829.0 11830.0 11831.0 11832.0 11833.0 11834.0 11835.0 11836.0 11837.0 11838.0 11839.0 11840.0 11841.0 11842.0 11843.0 11844.0 11845.0 11846.0 11847.0 11848.0 11849.0 11850.0 11851.0 11852.0 11853.0 11854.0 11855.0 11856.0 11857.0 11858.0 11859.0 11860.0 11861.0 11862.0 11863.0 11864.0 11865.0 11866.0 11867.0 11868.0 11869.0 11870.0 11871.0 11872.0 11873.0 11874.0 11875.0 11876.0 11877.0 11878.0 11879.0 11880.0 11881.0 11882.0 11883.0 11884.0 11885.0 11886.0 11887.0 11888.0 11889.0 11890.0 11891.0 11892.0 11893.0 11894.0 11895.0 11896.0 11897.0 11898.0 11899.0 11900.0 11901.0 11902.0 11903.0 11904.0 11905.0 11906.0 11907.0 11908.0 11909.0 11910.0 11911.0 11912.0 11913.0 11914.0 11915.0 11916.0 11917.0 11918.0 11919.0 11920.0 11921.0 11922.0 11923.0 11924.0 11925.0 11926.0 11927.0 11928.0 11929.0 11930.0 11931.0 11932.0 11933.0 11934.0 11935.0 11936.0 11937.0 11938.0 11939.0 11940.0 11941.0 11942.0 11943.0 11944.0 11945.0 11946.0 11947.0 11948.0 11949.0 11950.0 11951.0 11952.0 11953.0 11954.0 11955.0 11956.0 11957.0 11958.0 11959.0 11960.0 11961.0 11962.0 11963.0 11964.0 11965.0 11966.0 11967.0 11968.0 11969.0 11970.0 11971.0 11972.0 11973.0 11974.0 11975.0 11976.0 11977.0 11978.0 11979.0 11980.0 11981.0 11982.0 11983.0 11984.0 11985.0 11986.0 11987.0 11988.0 11989.0 11990.0 11991.0 11992.0 11993.0 11994.0 11995.0 11996.0 11997.0 11998.0 11999.0 12000.0 12001.0 12002.0 12003.0 12004.0 12005.0 12006.0 12007.0 12008.0 12009.0 12010.0 12011.0 12012.0 12013.0 12014.0 12015.0 12016.0 12017.0 12018.0 12019.0 12020.0 12021.0 12022.0 12023.0 12024.0 12025.0 12026.0 12027.0 12028.0 12029.0 12030.0 12031.0 12032.0 12033.0 12034.0 12035.0 12036.0 12037.0 12038.0 12039.0 12040.0 12041.0 12042.0 12043.0 12044.0 12045.0 12046.0 12047.0 12048.0 12049.0 12050.0 12051.0 12052.0 12053.0 12054.0 12055.0 12056.0 12057.0 12058.0 12059.0 12060.0 12061.0 12062.0 12063.0 12064.0 12065.0 12066.0 12067.0 12068.0 12069.0 12070.0 12071.0 12072.0 12073.0 12074.0 12075.0 12076.0 12077.0 12078.0 12079.0 12080.0 12081.0 12082.0 12083.0 12084.0 12085.0 12086.0 12087.0 12088.0 12089.0 12090.0 12091.0 12092.0 12093.0 12094.0 12095.0 12096.0 12097.0 12098.0 12099.0 12100.0 12101.0 12102.0 12103.0 12104.0 12105.0 12106.0 12107.0 12108.0 12109.0 12110.0 12111.0 12112.0 12113.0 12114.0 12115.0 12116.0 12117.0 12118.0 12119.0 12120.0 12121.0 12122.0 12123.0 12124.0 12125.0 12126.0 12127.0 12128.0 12129.0 12130.0 12131.0 12132.0 12133.0 12134.0 12135.0 12136.0 12137.0 12138.0 12139.0 12140.0 12141.0 12142.0 12143.0 12144.0 12145.0 12146.0 12147.0 12148.0 12149.0 12150.0 12151.0 12152.0 12153.0 12154.0 12155.0 12156.0 12157.0 12158.0 12159.0 12160.0 12161.0 12162.0 12163.0 12164.0 12165.0 12166.0 12167.0 12168.0 12169.0 12170.0 12171.0 12172.0 12173.0 12174.0 12175.0 12176.0 12177.0 12178.0 12179.0 12180.0 12181.0 12182.0 12183.0 12184.0 12185.0 12186.0 12187.0 12188.0 12189.0 12190.0 12191.0 12192.0 12193.0 12194.0 12195.0 12196.0 12197.0 12198.0 12199.0 12200.0 12201.0 12202.0 12203.0 12204.0 12205.0 12206.0 12207.0 12208.0 12209.0 12210.0 12211.0 12212.0 12213.0 12214.0 12215.0 12216.0 12217.0 12218.0 12219.0 12220.0 12221.0 12222.0 12223.0 12224.0 12225.0 12226.0 12227.0 12228.0 12229.0 12230.0 12231.0 12232.0 12233.0 12234.0 12235.0 12236.0 12237.0 12238.0 12239.0 12240.0 12241.0 12242.0 12243.0 12244.0 12245.0 12246.0 12247.0 12248.0 12249.0 12250.0 12251.0 12252.0 12253.0 12254.0 12255.0 12256.0 12257.0 12258.0 12259.0 12260.0 12261.0 12262.0 12263.0 12264.0 12265.0 12266.0 12267.0 12268.0 12269.0 12270.0 12271.0 12272.0 12273.0 12274.0 12275.0 12276.0 12277.0 12278.0 12279.0 12280.0 12281.0 12282.0 12283.0 12284.0 12285.0 12286.0 12287.0 12288.0 12289.0 12290.0 12291.0 12292.0 12293.0 12294.0 12295.0 12296.0 12297.0 12298.0 12299.0 12300.0 12301.0 12302.0 12303.0 12304.0 12305.0 12306.0 12307.0 12308.0 12309.0 12310.0 12311.0 12312.0 12313.0 12314.0 12315.0 12316.0 12317.0 12318.0 12319.0 12320.0 12321.0 12322.0 12323.0 12324.0 12325.0 12326.0 12327.0 12328.0 12329.0 12330.0 12331.0 12332.0 12333.0 12334.0 12335.0 12336.0 12337.0 12338.0 12339.0 12340.0 12341.0 12342.0 12343.0 12344.0 12345.0 12346.0 12347.0 12348.0 12349.0 12350.0 12351.0 12352.0 12353.0 12354.0 12355.0 12356.0 12357.0 12358.0 12359.0 12360.0 12361.0 12362.0 12363.0 12364.0 12365.0 12366.0 12367.0 12368.0 12369.0 12370.0 12371.0 12372.0 12373.0 12374.0 12375.0 12376.0 12377.0 12378.0 12379.0 12380.0 12381.0 12382.0 12383.0 12384.0 12385.0 12386.0 12387.0 12388.0 12389.0 12390.0 12391.0 12392.0 12393.0 12394.0 12395.0 12396.0 12397.0 12398.0 12399.0 12400.0 12401.0 12402.0 12403.0 12404.0 12405.0 12406.0 12407.0 12408.0 12409.0 12410.0 12411.0 12412.0 12413.0 12414.0 12415.0 12416.0 12417.0 12418.0 12419.0 12420.0 12421.0 12422.0 12423.0 12424.0 12425.0 12426.0 12427.0 12428.0 12429.0 12430.0 12431.0 12432.0 12433.0 12434.0 12435.0 12436.0 12437.0 12438.0 12439.0 12440.0 12441.0 12442.0 12443.0 12444.0 12445.0 12446.0 12447.0 12448.0 12449.0 12450.0 12451.0 12452.0 12453.0 12454.0 12455.0 12456.0 12457.0 12458.0 12459.0 12460.0 12461.0 12462.0 12463.0 12464.0 12465.0 12466.0 12467.0 12468.0 12469.0 12470.0 12471.0 12472.0 12473.0 12474.0 12475.0 12476.0 12477.0 12478.0 12479.0 12480.0 12481.0 12482.0 12483.0 12484.0 12485.0 12486.0 12487.0 12488.0 12489.0 12490.0 12491.0 12492.0 12493.0 12494.0 12495.0 12496.0 12497.0 12498.0 12499.0 12500.0 12501.0 12502.0 12503.0 12504.0 12505.0 12506.0 12507.0 12508.0 12509.0 12510.0 12511.0 12512.0 12513.0 12514.0 12515.0 12516.0 12517.0 12518.0 12519.0 12520.0 12521.0 12522.0 12523.0 12524.0 12525.0 12526.0 12527.0 12528.0 12529.0 12530.0 12531.0 12532.0 12533.0 12534.0 12535.0 12536.0 12537.0 12538.0 12539.0 12540.0 12541.0 12542.0 12543.0 12544.0 12545.0 12546.0 12547.0 12548.0 12549.0 12550.0 12551.0 12552.0 12553.0 12554.0 12555.0 12556.0 12557.0 12558.0 12559.0 12560.0 12561.0 12562.0 12563.0 12564.0 12565.0 12566.0 12567.0 12568.0 12569.0 12570.0 12571.0 12572.0 12573.0 12574.0 12575.0 12576.0 12577.0 12578.0 12579.0 12580.0 12581.0 12582.0 12583.0 12584.0 12585.0 12586.0 12587.0 12588.0 12589.0 12590.0 12591.0 12592.0 12593.0 12594.0 12595.0 12596.0 12597.0 12598.0 12599.0 12600.0 12601.0 12602.0 12603.0 12604.0 12605.0 12606.0 12607.0 12608.0 12609.0 12610.0 12611.0 12612.0 12613.0 12614.0 12615.0 12616.0 12617.0 12618.0 12619.0 12620.0 12621.0 12622.0 12623.0 12624.0 12625.0 12626.0 12627.0 12628.0 12629.0 12630.0 12631.0 12632.0 12633.0 12634.0 12635.0 12636.0 12637.0 12638.0 12639.0 12640.0 12641.0 12642.0 12643.0 12644.0 12645.0 12646.0 12647.0 12648.0 12649.0 12650.0 12651.0 12652.0 12653.0 12654.0 12655.0 12656.0 12657.0 12658.0 12659.0 12660.0 12661.0 12662.0 12663.0 12664.0 12665.0 12666.0 12667.0 12668.0 12669.0 12670.0 12671.0 12672.0 12673.0 12674.0 12675.0 12676.0 12677.0 12678.0 12679.0 12680.0 12681.0 12682.0 12683.0 12684.0 12685.0 12686.0 12687.0 12688.0 12689.0 12690.0 12691.0 12692.0 12693.0 12694.0 12695.0 12696.0 12697.0 12698.0 12699.0 12700.0 12701.0 12702.0 12703.0 12704.0 12705.0 12706.0 12707.0 12708.0 12709.0 12710.0 12711.0 12712.0 12713.0 12714.0 12715.0 12716.0 12717.0 12718.0 12719.0 12720.0 12721.0 12722.0 12723.0 12724.0 12725.0 12726.0 12727.0 12728.0 12729.0 12730.0 12731.0 12732.0 12733.0 12734.0 12735.0 12736.0 12737.0 12738.0 12739.0 12740.0 12741.0 12742.0 12743.0 12744.0 12745.0 12746.0 12747.0 12748.0 12749.0 12750.0 12751.0 12752.0 12753.0 12754.0 12755.0 12756.0 12757.0 12758.0 12759.0 12760.0 12761.0 12762.0 12763.0 12764.0 12765.0 12766.0 12767.0 12768.0 12769.0 12770.0 12771.0 12772.0 12773.0 12774.0 12775.0 12776.0 12777.0 12778.0 12779.0 12780.0 12781.0 12782.0 12783.0 12784.0 12785.0 12786.0 12787.0 12788.0 12789.0 12790.0 12791.0 12792.0 12793.0 12794.0 12795.0 12796.0 12797.0 12798.0 12799.0 12800.0 12801.0 12802.0 12803.0 12804.0 12805.0 12806.0 12807.0 12808.0 12809.0 12810.0 12811.0 12812.0 12813.0 12814.0 12815.0 12816.0 12817.0 12818.0 12819.0 12820.0 12821.0 12822.0 12823.0 12824.0 12825.0 12826.0 12827.0 12828.0 12829.0 12830.0 12831.0 12832.0 12833.0 12834.0 12835.0 12836.0 12837.0 12838.0 12839.0 12840.0 12841.0 12842.0 12843.0 12844.0 12845.0 12846.0 12847.0 12848.0 12849.0 12850.0 12851.0 12852.0 12853.0 12854.0 12855.0 12856.0 12857.0 12858.0 12859.0 12860.0 12861.0 12862.0 12863.0 12864.0 12865.0 12866.0 12867.0 12868.0 12869.0 12870.0 12871.0 12872.0 12873.0 12874.0 12875.0 12876.0 12877.0 12878.0 12879.0 12880.0 12881.0 12882.0 12883.0 12884.0 12885.0 12886.0 12887.0 12888.0 12889.0 12890.0 12891.0 12892.0 12893.0 12894.0 12895.0 12896.0 12897.0 12898.0 12899.0 12900.0 12901.0 12902.0 12903.0 12904.0 12905.0 12906.0 12907.0 12908.0 12909.0 12910.0 12911.0 12912.0 12913.0 12914.0 12915.0 12916.0 12917.0 12918.0 12919.0 12920.0 12921.0 12922.0 12923.0 12924.0 12925.0 12926.0 12927.0 12928.0 12929.0 12930.0 12931.0 12932.0 12933.0 12934.0 12935.0 12936.0 12937.0 12938.0 12939.0 12940.0 12941.0 12942.0 12943.0 12944.0 12945.0 12946.0 12947.0 12948.0 12949.0 12950.0 12951.0 12952.0 12953.0 12954.0 12955.0 12956.0 12957.0 12958.0 12959.0 12960.0 12961.0 12962.0 12963.0 12964.0 12965.0 12966.0 12967.0 12968.0 12969.0 12970.0 12971.0 12972.0 12973.0 12974.0 12975.0 12976.0 12977.0 12978.0 12979.0 12980.0 12981.0 12982.0 12983.0 12984.0 12985.0 12986.0 12987.0 12988.0 12989.0 12990.0 12991.0 12992.0 12993.0 12994.0 12995.0 12996.0 12997.0 12998.0 12999.0 13000.0 13001.0 13002.0 13003.0 13004.0 13005.0 13006.0 13007.0 13008.0 13009.0 13010.0 13011.0 13012.0 13013.0 13014.0 13015.0 13016.0 13017.0 13018.0 13019.0 13020.0 13021.0 13022.0 13023.0 13024.0 13025.0 13026.0 13027.0 13028.0 13029.0 13030.0 13031.0 13032.0 13033.0 13034.0 13035.0 13036.0 13037.0 13038.0 13039.0 13040.0 13041.0 13042.0 13043.0 13044.0 13045.0 13046.0 13047.0 13048.0 13049.0 13050.0 13051.0 13052.0 13053.0 13054.0 13055.0 13056.0 13057.0 13058.0 13059.0 13060.0 13061.0 13062.0 13063.0 13064.0 13065.0 13066.0 13067.0 13068.0 13069.0 13070.0 13071.0 13072.0 13073.0 13074.0 13075.0 13076.0 13077.0 13078.0 13079.0 13080.0 13081.0 13082.0 13083.0 13084.0 13085.0 13086.0 13087.0 13088.0 13089.0 13090.0 13091.0 13092.0 13093.0 13094.0 13095.0 13096.0 13097.0 13098.0 13099.0 13100.0 13101.0 13102.0 13103.0 13104.0 13105.0 13106.0 13107.0 13108.0 13109.0 13110.0 13111.0 13112.0 13113.0 13114.0 13115.0 13116.0 13117.0 13118.0 13119.0 13120.0 13121.0 13122.0 13123.0 13124.0 13125.0 13126.0 13127.0 13128.0 13129.0 13130.0 13131.0 13132.0 13133.0 13134.0 13135.0 13136.0 13137.0 13138.0 13139.0 13140.0 13141.0 13142.0 13143.0 13144.0 13145.0 13146.0 13147.0 13148.0 13149.0 13150.0 13151.0 13152.0 13153.0 13154.0 13155.0 13156.0 13157.0 13158.0 13159.0 13160.0 13161.0 13162.0 13163.0 13164.0 13165.0 13166.0 13167.0 13168.0 13169.0 13170.0 13171.0 13172.0 13173.0 13174.0 13175.0 13176.0 13177.0 13178.0 13179.0 13180.0 13181.0 13182.0 13183.0 13184.0 13185.0 13186.0 13187.0 13188.0 13189.0 13190.0 13191.0 13192.0 13193.0 13194.0 13195.0 13196.0 13197.0 13198.0 13199.0 13200.0 13201.0 13202.0 13203.0 13204.0 13205.0 13206.0 13207.0 13208.0 13209.0 13210.0 13211.0 13212.0 13213.0 13214.0 13215.0 13216.0 13217.0 13218.0 13219.0 13220.0 13221.0 13222.0 13223.0 13224.0 13225.0 13226.0 13227.0 13228.0 13229.0 13230.0 13231.0 13232.0 13233.0 13234.0 13235.0 13236.0 13237.0 13238.0 13239.0 13240.0 13241.0 13242.0 13243.0 13244.0 13245.0 13246.0 13247.0 13248.0 13249.0 13250.0 13251.0 13252.0 13253.0 13254.0 13255.0 13256.0 13257.0 13258.0 13259.0 13260.0 13261.0 13262.0 13263.0 13264.0 13265.0 13266.0 13267.0 13268.0 13269.0 13270.0 13271.0 13272.0 13273.0 13274.0 13275.0 13276.0 13277.0 13278.0 13279.0 13280.0 13281.0 13282.0 13283.0 13284.0 13285.0 13286.0 13287.0 13288.0 13289.0 13290.0 13291.0 13292.0 13293.0 13294.0 13295.0 13296.0 13297.0 13298.0 13299.0 13300.0 13301.0 13302.0 13303.0 13304.0 13305.0 13306.0 13307.0 13308.0 13309.0 13310.0 13311.0 13312.0 13313.0 13314.0 13315.0 13316.0 13317.0 13318.0 13319.0 13320.0 13321.0 13322.0 13323.0 13324.0 13325.0 13326.0 13327.0 13328.0 13329.0 13330.0 13331.0 13332.0 13333.0 13334.0 13335.0 13336.0 13337.0 13338.0 13339.0 13340.0 13341.0 13342.0 13343.0 13344.0 13345.0 13346.0 13347.0 13348.0 13349.0 13350.0 13351.0 13352.0 13353.0 13354.0 13355.0 13356.0 13357.0 13358.0 13359.0 13360.0 13361.0 13362.0 13363.0 13364.0 13365.0 13366.0 13367.0 13368.0 13369.0 13370.0 13371.0 13372.0 13373.0 13374.0 13375.0 13376.0 13377.0 13378.0 13379.0 13380.0 13381.0 13382.0 13383.0 13384.0 13385.0 13386.0 13387.0 13388.0 13389.0 13390.0 13391.0 13392.0 13393.0 13394.0 13395.0 13396.0 13397.0 13398.0 13399.0 13400.0 13401.0 13402.0 13403.0 13404.0 13405.0 13406.0 13407.0 13408.0 13409.0 13410.0 13411.0 13412.0 13413.0 13414.0 13415.0 13416.0 13417.0 13418.0 13419.0 13420.0 13421.0 13422.0 13423.0 13424.0 13425.0 13426.0 13427.0 13428.0 13429.0 13430.0 13431.0 13432.0 13433.0 13434.0 13435.0 13436.0 13437.0 13438.0 13439.0 13440.0 13441.0 13442.0 13443.0 13444.0 13445.0 13446.0 13447.0 13448.0 13449.0 13450.0 13451.0 13452.0 13453.0 13454.0 13455.0 13456.0 13457.0 13458.0 13459.0 13460.0 13461.0 13462.0 13463.0 13464.0 13465.0 13466.0 13467.0 13468.0 13469.0 13470.0 13471.0 13472.0 13473.0 13474.0 13475.0 13476.0 13477.0 13478.0 13479.0 13480.0 13481.0 13482.0 13483.0 13484.0 13485.0 13486.0 13487.0 13488.0 13489.0 13490.0 13491.0 13492.0 13493.0 13494.0 13495.0 13496.0 13497.0 13498.0 13499.0 13500.0 13501.0 13502.0 13503.0 13504.0 13505.0 13506.0 13507.0 13508.0 13509.0 13510.0 13511.0 13512.0 13513.0 13514.0 13515.0 13516.0 13517.0 13518.0 13519.0 13520.0 13521.0 13522.0 13523.0 13524.0 13525.0 13526.0 13527.0 13528.0 13529.0 13530.0 13531.0 13532.0 13533.0 13534.0 13535.0 13536.0 13537.0 13538.0 13539.0 13540.0 13541.0 13542.0 13543.0 13544.0 13545.0 13546.0 13547.0 13548.0 13549.0 13550.0 13551.0 13552.0 13553.0 13554.0 13555.0 13556.0 13557.0 13558.0 13559.0 13560.0 13561.0 13562.0 13563.0 13564.0 13565.0 13566.0 13567.0 13568.0 13569.0 13570.0 13571.0 13572.0 13573.0 13574.0 13575.0 13576.0 13577.0 13578.0 13579.0 13580.0 13581.0 13582.0 13583.0 13584.0 13585.0 13586.0 13587.0 13588.0 13589.0 13590.0 13591.0 13592.0 13593.0 13594.0 13595.0 13596.0 13597.0 13598.0 13599.0 13600.0 13601.0 13602.0 13603.0 13604.0 13605.0 13606.0 13607.0 13608.0 13609.0 13610.0 13611.0 13612.0 13613.0 13614.0 13615.0 13616.0 13617.0 13618.0 13619.0 13620.0 13621.0 13622.0 13623.0 13624.0 13625.0 13626.0 13627.0 13628.0 13629.0 13630.0 13631.0 13632.0 13633.0 13634.0 13635.0 13636.0 13637.0 13638.0 13639.0 13640.0 13641.0 13642.0 13643.0 13644.0 13645.0 13646.0 13647.0 13648.0 13649.0 13650.0 13651.0 13652.0 13653.0 13654.0 13655.0 13656.0 13657.0 13658.0 13659.0 13660.0 13661.0 13662.0 13663.0 13664.0 13665.0 13666.0 13667.0 13668.0 13669.0 13670.0 13671.0 13672.0 13673.0 13674.0 13675.0 13676.0 13677.0 13678.0 13679.0 13680.0 13681.0 13682.0 13683.0 13684.0 13685.0 13686.0 13687.0 13688.0 13689.0 13690.0 13691.0 13692.0 13693.0 13694.0 13695.0 13696.0 13697.0 13698.0 13699.0 13700.0 13701.0 13702.0 13703.0 13704.0 13705.0 13706.0 13707.0 13708.0 13709.0 13710.0 13711.0 13712.0 13713.0 13714.0 13715.0 13716.0 13717.0 13718.0 13719.0 13720.0 13721.0 13722.0 13723.0 13724.0 13725.0 13726.0 13727.0 13728.0 13729.0 13730.0 13731.0 13732.0 13733.0 13734.0 13735.0 13736.0 13737.0 13738.0 13739.0 13740.0 13741.0 13742.0 13743.0 13744.0 13745.0 13746.0 13747.0 13748.0 13749.0 13750.0 13751.0 13752.0 13753.0 13754.0 13755.0 13756.0 13757.0 13758.0 13759.0 13760.0 13761.0 13762.0 13763.0 13764.0 13765.0 13766.0 13767.0 13768.0 13769.0 13770.0 13771.0 13772.0 13773.0 13774.0 13775.0 13776.0 13777.0 13778.0 13779.0 13780.0 13781.0 13782.0 13783.0 13784.0 13785.0 13786.0 13787.0 13788.0 13789.0 13790.0 13791.0 13792.0 13793.0 13794.0 13795.0 13796.0 13797.0 13798.0 13799.0 13800.0 13801.0 13802.0 13803.0 13804.0 13805.0 13806.0 13807.0 13808.0 13809.0 13810.0 13811.0 13812.0 13813.0 13814.0 13815.0 13816.0 13817.0 13818.0 13819.0 13820.0 13821.0 13822.0 13823.0 13824.0 13825.0 13826.0 13827.0 13828.0 13829.0 13830.0 13831.0 13832.0 13833.0 13834.0 13835.0 13836.0 13837.0 13838.0 13839.0 13840.0 13841.0 13842.0 13843.0 13844.0 13845.0 13846.0 13847.0 13848.0 13849.0 13850.0 13851.0 13852.0 13853.0 13854.0 13855.0 13856.0 13857.0 13858.0 13859.0 13860.0 13861.0 13862.0 13863.0 13864.0 13865.0 13866.0 13867.0 13868.0 13869.0 13870.0 13871.0 13872.0 13873.0 13874.0 13875.0 13876.0 13877.0 13878.0 13879.0 13880.0 13881.0 13882.0 13883.0 13884.0 13885.0 13886.0 13887.0 13888.0 13889.0 13890.0 13891.0 13892.0 13893.0 13894.0 13895.0 13896.0 13897.0 13898.0 13899.0 13900.0 13901.0 13902.0 13903.0 13904.0 13905.0 13906.0 13907.0 13908.0 13909.0 13910.0 13911.0 13912.0 13913.0 13914.0 13915.0 13916.0 13917.0 13918.0 13919.0 13920.0 13921.0 13922.0 13923.0 13924.0 13925.0 13926.0 13927.0 13928.0 13929.0 13930.0 13931.0 13932.0 13933.0 13934.0 13935.0 13936.0 13937.0 13938.0 13939.0 13940.0 13941.0 13942.0 13943.0 13944.0 13945.0 13946.0 13947.0 13948.0 13949.0 13950.0 13951.0 13952.0 13953.0 13954.0 13955.0 13956.0 13957.0 13958.0 13959.0 13960.0 13961.0 13962.0 13963.0 13964.0 13965.0 13966.0 13967.0 13968.0 13969.0 13970.0 13971.0 13972.0 13973.0 13974.0 13975.0 13976.0 13977.0 13978.0 13979.0 13980.0 13981.0 13982.0 13983.0 13984.0 13985.0 13986.0 13987.0 13988.0 13989.0 13990.0 13991.0 13992.0 13993.0 13994.0 13995.0 13996.0 13997.0 13998.0 13999.0 14000.0 14001.0 14002.0 14003.0 14004.0 14005.0 14006.0 14007.0 14008.0 14009.0 14010.0 14011.0 14012.0 14013.0 14014.0 14015.0 14016.0 14017.0 14018.0 14019.0 14020.0 14021.0 14022.0 14023.0 14024.0 14025.0 14026.0 14027.0 14028.0 14029.0 14030.0 14031.0 14032.0 14033.0 14034.0 14035.0 14036.0 14037.0 14038.0 14039.0 14040.0 14041.0 14042.0 14043.0 14044.0 14045.0 14046.0 14047.0 14048.0 14049.0 14050.0 14051.0 14052.0 14053.0 14054.0 14055.0 14056.0 14057.0 14058.0 14059.0 14060.0 14061.0 14062.0 14063.0 14064.0 14065.0 14066.0 14067.0 14068.0 14069.0 14070.0 14071.0 14072.0 14073.0 14074.0 14075.0 14076.0 14077.0 14078.0 14079.0 14080.0 14081.0 14082.0 14083.0 14084.0 14085.0 14086.0 14087.0 14088.0 14089.0 14090.0 14091.0 14092.0 14093.0 14094.0 14095.0 14096.0 14097.0 14098.0 14099.0 14100.0 14101.0 14102.0 14103.0 14104.0 14105.0 14106.0 14107.0 14108.0 14109.0 14110.0 14111.0 14112.0 14113.0 14114.0 14115.0 14116.0 14117.0 14118.0 14119.0 14120.0 14121.0 14122.0 14123.0 14124.0 14125.0 14126.0 14127.0 14128.0 14129.0 14130.0 14131.0 14132.0 14133.0 14134.0 14135.0 14136.0 14137.0 14138.0 14139.0 14140.0 14141.0 14142.0 14143.0 14144.0 14145.0 14146.0 14147.0 14148.0 14149.0 14150.0 14151.0 14152.0 14153.0 14154.0 14155.0 14156.0 14157.0 14158.0 14159.0 14160.0 14161.0 14162.0 14163.0 14164.0 14165.0 14166.0 14167.0 14168.0 14169.0 14170.0 14171.0 14172.0 14173.0 14174.0 14175.0 14176.0 14177.0 14178.0 14179.0 14180.0 14181.0 14182.0 14183.0 14184.0 14185.0 14186.0 14187.0 14188.0 14189.0 14190.0 14191.0 14192.0 14193.0 14194.0 14195.0 14196.0 14197.0 14198.0 14199.0 14200.0 14201.0 14202.0 14203.0 14204.0 14205.0 14206.0 14207.0 14208.0 14209.0 14210.0 14211.0 14212.0 14213.0 14214.0 14215.0 14216.0 14217.0 14218.0 14219.0 14220.0 14221.0 14222.0 14223.0 14224.0 14225.0 14226.0 14227.0 14228.0 14229.0 14230.0 14231.0 14232.0 14233.0 14234.0 14235.0 14236.0 14237.0 14238.0 14239.0 14240.0 14241.0 14242.0 14243.0 14244.0 14245.0 14246.0 14247.0 14248.0 14249.0 14250.0 14251.0 14252.0 14253.0 14254.0 14255.0 14256.0 14257.0 14258.0 14259.0 14260.0 14261.0 14262.0 14263.0 14264.0 14265.0 14266.0 14267.0 14268.0 14269.0 14270.0 14271.0 14272.0 14273.0 14274.0 14275.0 14276.0 14277.0 14278.0 14279.0 14280.0 14281.0 14282.0 14283.0 14284.0 14285.0 14286.0 14287.0 14288.0 14289.0 14290.0 14291.0 14292.0 14293.0 14294.0 14295.0 14296.0 14297.0 14298.0 14299.0 14300.0 14301.0 14302.0 14303.0 14304.0 14305.0 14306.0 14307.0 14308.0 14309.0 14310.0 14311.0 14312.0 14313.0 14314.0 14315.0 14316.0 14317.0 14318.0 14319.0 14320.0 14321.0 14322.0 14323.0 14324.0 14325.0 14326.0 14327.0 14328.0 14329.0 14330.0 14331.0 14332.0 14333.0 14334.0 14335.0 14336.0 14337.0 14338.0 14339.0 14340.0 14341.0 14342.0 14343.0 14344.0 14345.0 14346.0 14347.0 14348.0 14349.0 14350.0 14351.0 14352.0 14353.0 14354.0 14355.0 14356.0 14357.0 14358.0 14359.0 14360.0 14361.0 14362.0 14363.0 14364.0 14365.0 14366.0 14367.0 14368.0 14369.0 14370.0 14371.0 14372.0 14373.0 14374.0 14375.0 14376.0 14377.0 14378.0 14379.0 14380.0 14381.0 14382.0 14383.0 14384.0 14385.0 14386.0 14387.0 14388.0 14389.0 14390.0 14391.0 14392.0 14393.0 14394.0 14395.0 14396.0 14397.0 14398.0 14399.0 14400.0 14401.0 14402.0 14403.0 14404.0 14405.0 14406.0 14407.0 14408.0 14409.0 14410.0 14411.0 14412.0 14413.0 14414.0 14415.0 14416.0 14417.0 14418.0 14419.0 14420.0 14421.0 14422.0 14423.0 14424.0 14425.0 14426.0 14427.0 14428.0 14429.0 14430.0 14431.0 14432.0 14433.0 14434.0 14435.0 14436.0 14437.0 14438.0 14439.0 14440.0 14441.0 14442.0 14443.0 14444.0 14445.0 14446.0 14447.0 14448.0 14449.0 14450.0 14451.0 14452.0 14453.0 14454.0 14455.0 14456.0 14457.0 14458.0 14459.0 14460.0 14461.0 14462.0 14463.0 14464.0 14465.0 14466.0 14467.0 14468.0 14469.0 14470.0 14471.0 14472.0 14473.0 14474.0 14475.0 14476.0 14477.0 14478.0 14479.0 14480.0 14481.0 14482.0 14483.0 14484.0 14485.0 14486.0 14487.0 14488.0 14489.0 14490.0 14491.0 14492.0 14493.0 14494.0 14495.0 14496.0 14497.0 14498.0 14499.0 14500.0 14501.0 14502.0 14503.0 14504.0 14505.0 14506.0 14507.0 14508.0 14509.0 14510.0 14511.0 14512.0 14513.0 14514.0 14515.0 14516.0 14517.0 14518.0 14519.0 14520.0 14521.0 14522.0 14523.0 14524.0 14525.0 14526.0 14527.0 14528.0 14529.0 14530.0 14531.0 14532.0 14533.0 14534.0 14535.0 14536.0 14537.0 14538.0 14539.0 14540.0 14541.0 14542.0 14543.0 14544.0 14545.0 14546.0 14547.0 14548.0 14549.0 14550.0 14551.0 14552.0 14553.0 14554.0 14555.0 14556.0 14557.0 14558.0 14559.0 14560.0 14561.0 14562.0 14563.0 14564.0 14565.0 14566.0 14567.0 14568.0 14569.0 14570.0 14571.0 14572.0 14573.0 14574.0 14575.0 14576.0 14577.0 14578.0 14579.0 14580.0 14581.0 14582.0 14583.0 14584.0 14585.0 14586.0 14587.0 14588.0 14589.0 14590.0 14591.0 14592.0 14593.0 14594.0 14595.0 14596.0 14597.0 14598.0 14599.0 14600.0 14601.0 14602.0 14603.0 14604.0 14605.0 14606.0 14607.0 14608.0 14609.0 14610.0 14611.0 14612.0 14613.0 14614.0 14615.0 14616.0 14617.0 14618.0 14619.0 14620.0 14621.0 14622.0 14623.0 14624.0 14625.0 14626.0 14627.0 14628.0 14629.0 14630.0 14631.0 14632.0 14633.0 14634.0 14635.0 14636.0 14637.0 14638.0 14639.0 14640.0 14641.0 14642.0 14643.0 14644.0 14645.0 14646.0 14647.0 14648.0 14649.0 14650.0 14651.0 14652.0 14653.0 14654.0 14655.0 14656.0 14657.0 14658.0 14659.0 14660.0 14661.0 14662.0 14663.0 14664.0 14665.0 14666.0 14667.0 14668.0 14669.0 14670.0 14671.0 14672.0 14673.0 14674.0 14675.0 14676.0 14677.0 14678.0 14679.0 14680.0 14681.0 14682.0 14683.0 14684.0 14685.0 14686.0 14687.0 14688.0 14689.0 14690.0 14691.0 14692.0 14693.0 14694.0 14695.0 14696.0 14697.0 14698.0 14699.0 14700.0 14701.0 14702.0 14703.0 14704.0 14705.0 14706.0 14707.0 14708.0 14709.0 14710.0 14711.0 14712.0 14713.0 14714.0 14715.0 14716.0 14717.0 14718.0 14719.0 14720.0 14721.0 14722.0 14723.0 14724.0 14725.0 14726.0 14727.0 14728.0 14729.0 14730.0 14731.0 14732.0 14733.0 14734.0 14735.0 14736.0 14737.0 14738.0 14739.0 14740.0 14741.0 14742.0 14743.0 14744.0 14745.0 14746.0 14747.0 14748.0 14749.0 14750.0 14751.0 14752.0 14753.0 14754.0 14755.0 14756.0 14757.0 14758.0 14759.0 14760.0 14761.0 14762.0 14763.0 14764.0 14765.0 14766.0 14767.0 14768.0 14769.0 14770.0 14771.0 14772.0 14773.0 14774.0 14775.0 14776.0 14777.0 14778.0 14779.0 14780.0 14781.0 14782.0 14783.0 14784.0 14785.0 14786.0 14787.0 14788.0 14789.0 14790.0 14791.0 14792.0 14793.0 14794.0 14795.0 14796.0 14797.0 14798.0 14799.0 14800.0 14801.0 14802.0 14803.0 14804.0 14805.0 14806.0 14807.0 14808.0 14809.0 14810.0 14811.0 14812.0 14813.0 14814.0 14815.0 14816.0 14817.0 14818.0 14819.0 14820.0 14821.0 14822.0 14823.0 14824.0 14825.0 14826.0 14827.0 14828.0 14829.0 14830.0 14831.0 14832.0 14833.0 14834.0 14835.0 14836.0 14837.0 14838.0 14839.0 14840.0 14841.0 14842.0 14843.0 14844.0 14845.0 14846.0 14847.0 14848.0 14849.0 14850.0 14851.0 14852.0 14853.0 14854.0 14855.0 14856.0 14857.0 14858.0 14859.0 14860.0 14861.0 14862.0 14863.0 14864.0 14865.0 14866.0 14867.0 14868.0 14869.0 14870.0 14871.0 14872.0 14873.0 14874.0 14875.0 14876.0 14877.0 14878.0 14879.0 14880.0 14881.0 14882.0 14883.0 14884.0 14885.0 14886.0 14887.0 14888.0 14889.0 14890.0 14891.0 14892.0 14893.0 14894.0 14895.0 14896.0 14897.0 14898.0 14899.0 14900.0 14901.0 14902.0 14903.0 14904.0 14905.0 14906.0 14907.0 14908.0 14909.0 14910.0 14911.0 14912.0 14913.0 14914.0 14915.0 14916.0 14917.0 14918.0 14919.0 14920.0 14921.0 14922.0 14923.0 14924.0 14925.0 14926.0 14927.0 14928.0 14929.0 14930.0 14931.0 14932.0 14933.0 14934.0 14935.0 14936.0 14937.0 14938.0 14939.0 14940.0 14941.0 14942.0 14943.0 14944.0 14945.0 14946.0 14947.0 14948.0 14949.0 14950.0 14951.0 14952.0 14953.0 14954.0 14955.0 14956.0 14957.0 14958.0 14959.0 14960.0 14961.0 14962.0 14963.0 14964.0 14965.0 14966.0 14967.0 14968.0 14969.0 14970.0 14971.0 14972.0 14973.0 14974.0 14975.0 14976.0 14977.0 14978.0 14979.0 14980.0 14981.0 14982.0 14983.0 14984.0 14985.0 14986.0 14987.0 14988.0 14989.0 14990.0 14991.0 14992.0 14993.0 14994.0 14995.0 14996.0 14997.0 14998.0 14999.0 15000.0 15001.0 15002.0 15003.0 15004.0 15005.0 15006.0 15007.0 15008.0 15009.0 15010.0 15011.0 15012.0 15013.0 15014.0 15015.0 15016.0 15017.0 15018.0 15019.0 15020.0 15021.0 15022.0 15023.0 15024.0 15025.0 15026.0 15027.0 15028.0 15029.0 15030.0 15031.0 15032.0 15033.0 15034.0 15035.0 15036.0 15037.0 15038.0 15039.0 15040.0 15041.0 15042.0 15043.0 15044.0 15045.0 15046.0 15047.0 15048.0 15049.0 15050.0 15051.0 15052.0 15053.0 15054.0 15055.0 15056.0 15057.0 15058.0 15059.0 15060.0 15061.0 15062.0 15063.0 15064.0 15065.0 15066.0 15067.0 15068.0 15069.0 15070.0 15071.0 15072.0 15073.0 15074.0 15075.0 15076.0 15077.0 15078.0 15079.0 15080.0 15081.0 15082.0 15083.0 15084.0 15085.0 15086.0 15087.0 15088.0 15089.0 15090.0 15091.0 15092.0 15093.0 15094.0 15095.0 15096.0 15097.0 15098.0 15099.0 15100.0 15101.0 15102.0 15103.0 15104.0 15105.0 15106.0 15107.0 15108.0 15109.0 15110.0 15111.0 15112.0 15113.0 15114.0 15115.0 15116.0 15117.0 15118.0 15119.0 15120.0 15121.0 15122.0 15123.0 15124.0 15125.0 15126.0 15127.0 15128.0 15129.0 15130.0 15131.0 15132.0 15133.0 15134.0 15135.0 15136.0 15137.0 15138.0 15139.0 15140.0 15141.0 15142.0 15143.0 15144.0 15145.0 15146.0 15147.0 15148.0 15149.0 15150.0 15151.0 15152.0 15153.0 15154.0 15155.0 15156.0 15157.0 15158.0 15159.0 15160.0 15161.0 15162.0 15163.0 15164.0 15165.0 15166.0 15167.0 15168.0 15169.0 15170.0 15171.0 15172.0 15173.0 15174.0 15175.0 15176.0 15177.0 15178.0 15179.0 15180.0 15181.0 15182.0 15183.0 15184.0 15185.0 15186.0 15187.0 15188.0 15189.0 15190.0 15191.0 15192.0 15193.0 15194.0 15195.0 15196.0 15197.0 15198.0 15199.0 15200.0 15201.0 15202.0 15203.0 15204.0 15205.0 15206.0 15207.0 15208.0 15209.0 15210.0 15211.0 15212.0 15213.0 15214.0 15215.0 15216.0 15217.0 15218.0 15219.0 15220.0 15221.0 15222.0 15223.0 15224.0 15225.0 15226.0 15227.0 15228.0 15229.0 15230.0 15231.0 15232.0 15233.0 15234.0 15235.0 15236.0 15237.0 15238.0 15239.0 15240.0 15241.0 15242.0 15243.0 15244.0 15245.0 15246.0 15247.0 15248.0 15249.0 15250.0 15251.0 15252.0 15253.0 15254.0 15255.0 15256.0 15257.0 15258.0 15259.0 15260.0 15261.0 15262.0 15263.0 15264.0 15265.0 15266.0 15267.0 15268.0 15269.0 15270.0 15271.0 15272.0 15273.0 15274.0 15275.0 15276.0 15277.0 15278.0 15279.0 15280.0 15281.0 15282.0 15283.0 15284.0 15285.0 15286.0 15287.0 15288.0 15289.0 15290.0 15291.0 15292.0 15293.0 15294.0 15295.0 15296.0 15297.0 15298.0 15299.0 15300.0 15301.0 15302.0 15303.0 15304.0 15305.0 15306.0 15307.0 15308.0 15309.0 15310.0 15311.0 15312.0 15313.0 15314.0 15315.0 15316.0 15317.0 15318.0 15319.0 15320.0 15321.0 15322.0 15323.0 15324.0 15325.0 15326.0 15327.0 15328.0 15329.0 15330.0 15331.0 15332.0 15333.0 15334.0 15335.0 15336.0 15337.0 15338.0 15339.0 15340.0 15341.0 15342.0 15343.0 15344.0 15345.0 15346.0 15347.0 15348.0 15349.0 15350.0 15351.0 15352.0 15353.0 15354.0 15355.0 15356.0 15357.0 15358.0 15359.0 15360.0 15361.0 15362.0 15363.0 15364.0 15365.0 15366.0 15367.0 15368.0 15369.0 15370.0 15371.0 15372.0 15373.0 15374.0 15375.0 15376.0 15377.0 15378.0 15379.0 15380.0 15381.0 15382.0 15383.0 15384.0 15385.0 15386.0 15387.0 15388.0 15389.0 15390.0 15391.0 15392.0 15393.0 15394.0 15395.0 15396.0 15397.0 15398.0 15399.0 15400.0 15401.0 15402.0 15403.0 15404.0 15405.0 15406.0 15407.0 15408.0 15409.0 15410.0 15411.0 15412.0 15413.0 15414.0 15415.0 15416.0 15417.0 15418.0 15419.0 15420.0 15421.0 15422.0 15423.0 15424.0 15425.0 15426.0 15427.0 15428.0 15429.0 15430.0 15431.0 15432.0 15433.0 15434.0 15435.0 15436.0 15437.0 15438.0 15439.0 15440.0 15441.0 15442.0 15443.0 15444.0 15445.0 15446.0 15447.0 15448.0 15449.0 15450.0 15451.0 15452.0 15453.0 15454.0 15455.0 15456.0 15457.0 15458.0 15459.0 15460.0 15461.0 15462.0 15463.0 15464.0 15465.0 15466.0 15467.0 15468.0 15469.0 15470.0 15471.0 15472.0 15473.0 15474.0 15475.0 15476.0 15477.0 15478.0 15479.0 15480.0 15481.0 15482.0 15483.0 15484.0 15485.0 15486.0 15487.0 15488.0 15489.0 15490.0 15491.0 15492.0 15493.0 15494.0 15495.0 15496.0 15497.0 15498.0 15499.0 15500.0 15501.0 15502.0 15503.0 15504.0 15505.0 15506.0 15507.0 15508.0 15509.0 15510.0 15511.0 15512.0 15513.0 15514.0 15515.0 15516.0 15517.0 15518.0 15519.0 15520.0 15521.0 15522.0 15523.0 15524.0 15525.0 15526.0 15527.0 15528.0 15529.0 15530.0 15531.0 15532.0 15533.0 15534.0 15535.0 15536.0 15537.0 15538.0 15539.0 15540.0 15541.0 15542.0 15543.0 15544.0 15545.0 15546.0 15547.0 15548.0 15549.0 15550.0 15551.0 15552.0 15553.0 15554.0 15555.0 15556.0 15557.0 15558.0 15559.0 15560.0 15561.0 15562.0 15563.0 15564.0 15565.0 15566.0 15567.0 15568.0 15569.0 15570.0 15571.0 15572.0 15573.0 15574.0 15575.0 15576.0 15577.0 15578.0 15579.0 15580.0 15581.0 15582.0 15583.0 15584.0 15585.0 15586.0 15587.0 15588.0 15589.0 15590.0 15591.0 15592.0 15593.0 15594.0 15595.0 15596.0 15597.0 15598.0 15599.0 15600.0 15601.0 15602.0 15603.0 15604.0 15605.0 15606.0 15607.0 15608.0 15609.0 15610.0 15611.0 15612.0 15613.0 15614.0 15615.0 15616.0 15617.0 15618.0 15619.0 15620.0 15621.0 15622.0 15623.0 15624.0 15625.0 15626.0 15627.0 15628.0 15629.0 15630.0 15631.0 15632.0 15633.0 15634.0 15635.0 15636.0 15637.0 15638.0 15639.0 15640.0 15641.0 15642.0 15643.0 15644.0 15645.0 15646.0 15647.0 15648.0 15649.0 15650.0 15651.0 15652.0 15653.0 15654.0 15655.0 15656.0 15657.0 15658.0 15659.0 15660.0 15661.0 15662.0 15663.0 15664.0 15665.0 15666.0 15667.0 15668.0 15669.0 15670.0 15671.0 15672.0 15673.0 15674.0 15675.0 15676.0 15677.0 15678.0 15679.0 15680.0 15681.0 15682.0 15683.0 15684.0 15685.0 15686.0 15687.0 15688.0 15689.0 15690.0 15691.0 15692.0 15693.0 15694.0 15695.0 15696.0 15697.0 15698.0 15699.0 15700.0 15701.0 15702.0 15703.0 15704.0 15705.0 15706.0 15707.0 15708.0 15709.0 15710.0 15711.0 15712.0 15713.0 15714.0 15715.0 15716.0 15717.0 15718.0 15719.0 15720.0 15721.0 15722.0 15723.0 15724.0 15725.0 15726.0 15727.0 15728.0 15729.0 15730.0 15731.0 15732.0 15733.0 15734.0 15735.0 15736.0 15737.0 15738.0 15739.0 15740.0 15741.0 15742.0 15743.0 15744.0 15745.0 15746.0 15747.0 15748.0 15749.0 15750.0 15751.0 15752.0 15753.0 15754.0 15755.0 15756.0 15757.0 15758.0 15759.0 15760.0 15761.0 15762.0 15763.0 15764.0 15765.0 15766.0 15767.0 15768.0 15769.0 15770.0 15771.0 15772.0 15773.0 15774.0 15775.0 15776.0 15777.0 15778.0 15779.0 15780.0 15781.0 15782.0 15783.0 15784.0 15785.0 15786.0 15787.0 15788.0 15789.0 15790.0 15791.0 15792.0 15793.0 15794.0 15795.0 15796.0 15797.0 15798.0 15799.0 15800.0 15801.0 15802.0 15803.0 15804.0 15805.0 15806.0 15807.0 15808.0 15809.0 15810.0 15811.0 15812.0 15813.0 15814.0 15815.0 15816.0 15817.0 15818.0 15819.0 15820.0 15821.0 15822.0 15823.0 15824.0 15825.0 15826.0 15827.0 15828.0 15829.0 15830.0 15831.0 15832.0 15833.0 15834.0 15835.0 15836.0 15837.0 15838.0 15839.0 15840.0 15841.0 15842.0 15843.0 15844.0 15845.0 15846.0 15847.0 15848.0 15849.0 15850.0 15851.0 15852.0 15853.0 15854.0 15855.0 15856.0 15857.0 15858.0 15859.0 15860.0 15861.0 15862.0 15863.0 15864.0 15865.0 15866.0 15867.0 15868.0 15869.0 15870.0 15871.0 15872.0 15873.0 15874.0 15875.0 15876.0 15877.0 15878.0 15879.0 15880.0 15881.0 15882.0 15883.0 15884.0 15885.0 15886.0 15887.0 15888.0 15889.0 15890.0 15891.0 15892.0 15893.0 15894.0 15895.0 15896.0 15897.0 15898.0 15899.0 15900.0 15901.0 15902.0 15903.0 15904.0 15905.0 15906.0 15907.0 15908.0 15909.0 15910.0 15911.0 15912.0 15913.0 15914.0 15915.0 15916.0 15917.0 15918.0 15919.0 15920.0 15921.0 15922.0 15923.0 15924.0 15925.0 15926.0 15927.0 15928.0 15929.0 15930.0 15931.0 15932.0 15933.0 15934.0 15935.0 15936.0 15937.0 15938.0 15939.0 15940.0 15941.0 15942.0 15943.0 15944.0 15945.0 15946.0 15947.0 15948.0 15949.0 15950.0 15951.0 15952.0 15953.0 15954.0 15955.0 15956.0 15957.0 15958.0 15959.0 15960.0 15961.0 15962.0 15963.0 15964.0 15965.0 15966.0 15967.0 15968.0 15969.0 15970.0 15971.0 15972.0 15973.0 15974.0 15975.0 15976.0 15977.0 15978.0 15979.0 15980.0 15981.0 15982.0 15983.0 15984.0 15985.0 15986.0 15987.0 15988.0 15989.0 15990.0 15991.0 15992.0 15993.0 15994.0 15995.0 15996.0 15997.0 15998.0 15999.0 16000.0 16001.0 16002.0 16003.0 16004.0 16005.0 16006.0 16007.0 16008.0 16009.0 16010.0 16011.0 16012.0 16013.0 16014.0 16015.0 16016.0 16017.0 16018.0 16019.0 16020.0 16021.0 16022.0 16023.0 16024.0 16025.0 16026.0 16027.0 16028.0 16029.0 16030.0 16031.0 16032.0 16033.0 16034.0 16035.0 16036.0 16037.0 16038.0 16039.0 16040.0 16041.0 16042.0 16043.0 16044.0 16045.0 16046.0 16047.0 16048.0 16049.0 16050.0 16051.0 16052.0 16053.0 16054.0 16055.0 16056.0 16057.0 16058.0 16059.0 16060.0 16061.0 16062.0 16063.0 16064.0 16065.0 16066.0 16067.0 16068.0 16069.0 16070.0 16071.0 16072.0 16073.0 16074.0 16075.0 16076.0 16077.0 16078.0 16079.0 16080.0 16081.0 16082.0 16083.0 16084.0 16085.0 16086.0 16087.0 16088.0 16089.0 16090.0 16091.0 16092.0 16093.0 16094.0 16095.0 16096.0 16097.0 16098.0 16099.0 16100.0 16101.0 16102.0 16103.0 16104.0 16105.0 16106.0 16107.0 16108.0 16109.0 16110.0 16111.0 16112.0 16113.0 16114.0 16115.0 16116.0 16117.0 16118.0 16119.0 16120.0 16121.0 16122.0 16123.0 16124.0 16125.0 16126.0 16127.0 16128.0 16129.0 16130.0 16131.0 16132.0 16133.0 16134.0 16135.0 16136.0 16137.0 16138.0 16139.0 16140.0 16141.0 16142.0 16143.0 16144.0 16145.0 16146.0 16147.0 16148.0 16149.0 16150.0 16151.0 16152.0 16153.0 16154.0 16155.0 16156.0 16157.0 16158.0 16159.0 16160.0 16161.0 16162.0 16163.0 16164.0 16165.0 16166.0 16167.0 16168.0 16169.0 16170.0 16171.0 16172.0 16173.0 16174.0 16175.0 16176.0 16177.0 16178.0 16179.0 16180.0 16181.0 16182.0 16183.0 16184.0 16185.0 16186.0 16187.0 16188.0 16189.0 16190.0 16191.0 16192.0 16193.0 16194.0 16195.0 16196.0 16197.0 16198.0 16199.0 16200.0 16201.0 16202.0 16203.0 16204.0 16205.0 16206.0 16207.0 16208.0 16209.0 16210.0 16211.0 16212.0 16213.0 16214.0 16215.0 16216.0 16217.0 16218.0 16219.0 16220.0 16221.0 16222.0 16223.0 16224.0 16225.0 16226.0 16227.0 16228.0 16229.0 16230.0 16231.0 16232.0 16233.0 16234.0 16235.0 16236.0 16237.0 16238.0 16239.0 16240.0 16241.0 16242.0 16243.0 16244.0 16245.0 16246.0 16247.0 16248.0 16249.0 16250.0 16251.0 16252.0 16253.0 16254.0 16255.0 16256.0 16257.0 16258.0 16259.0 16260.0 16261.0 16262.0 16263.0 16264.0 16265.0 16266.0 16267.0 16268.0 16269.0 16270.0 16271.0 16272.0 16273.0 16274.0 16275.0 16276.0 16277.0 16278.0 16279.0 16280.0 16281.0 16282.0 16283.0 16284.0 16285.0 16286.0 16287.0 16288.0 16289.0 16290.0 16291.0 16292.0 16293.0 16294.0 16295.0 16296.0 16297.0 16298.0 16299.0 16300.0 16301.0 16302.0 16303.0 16304.0 16305.0 16306.0 16307.0 16308.0 16309.0 16310.0 16311.0 16312.0 16313.0 16314.0 16315.0 16316.0 16317.0 16318.0 16319.0 16320.0 16321.0 16322.0 16323.0 16324.0 16325.0 16326.0 16327.0 16328.0 16329.0 16330.0 16331.0 16332.0 16333.0 16334.0 16335.0 16336.0 16337.0 16338.0 16339.0 16340.0 16341.0 16342.0 16343.0 16344.0 16345.0 16346.0 16347.0 16348.0 16349.0 16350.0 16351.0 16352.0 16353.0 16354.0 16355.0 16356.0 16357.0 16358.0 16359.0 16360.0 16361.0 16362.0 16363.0 16364.0 16365.0 16366.0 16367.0 16368.0 16369.0 16370.0 16371.0 16372.0 16373.0 16374.0 16375.0 16376.0 16377.0 16378.0 16379.0 16380.0 16381.0 16382.0 16383.0 16384.0 16385.0 16386.0 16387.0 16388.0 16389.0 16390.0 16391.0 16392.0 16393.0 16394.0 16395.0 16396.0 16397.0 16398.0 16399.0 16400.0 16401.0 16402.0 16403.0 16404.0 16405.0 16406.0 16407.0 16408.0 16409.0 16410.0 16411.0 16412.0 16413.0 16414.0 16415.0 16416.0 16417.0 16418.0 16419.0 16420.0 16421.0 16422.0 16423.0 16424.0 16425.0 16426.0 16427.0 16428.0 16429.0 16430.0 16431.0 16432.0 16433.0 16434.0 16435.0 16436.0 16437.0 16438.0 16439.0 16440.0 16441.0 16442.0 16443.0 16444.0 16445.0 16446.0 16447.0 16448.0 16449.0 16450.0 16451.0 16452.0 16453.0 16454.0 16455.0 16456.0 16457.0 16458.0 16459.0 16460.0 16461.0 16462.0 16463.0 16464.0 16465.0 16466.0 16467.0 16468.0 16469.0 16470.0 16471.0 16472.0 16473.0 16474.0 16475.0 16476.0 16477.0 16478.0 16479.0 16480.0 16481.0 16482.0 16483.0 16484.0 16485.0 16486.0 16487.0 16488.0 16489.0 16490.0 16491.0 16492.0 16493.0 16494.0 16495.0 16496.0 16497.0 16498.0 16499.0 16500.0 16501.0 16502.0 16503.0 16504.0 16505.0 16506.0 16507.0 16508.0 16509.0 16510.0 16511.0 16512.0 16513.0 16514.0 16515.0 16516.0 16517.0 16518.0 16519.0 16520.0 16521.0 16522.0 16523.0 16524.0 16525.0 16526.0 16527.0 16528.0 16529.0 16530.0 16531.0 16532.0 16533.0 16534.0 16535.0 16536.0 16537.0 16538.0 16539.0 16540.0 16541.0 16542.0 16543.0 16544.0 16545.0 16546.0 16547.0 16548.0 16549.0 16550.0 16551.0 16552.0 16553.0 16554.0 16555.0 16556.0 16557.0 16558.0 16559.0 16560.0 16561.0 16562.0 16563.0 16564.0 16565.0 16566.0 16567.0 16568.0 16569.0 16570.0 16571.0 16572.0 16573.0 16574.0 16575.0 16576.0 16577.0 16578.0 16579.0 16580.0 16581.0 16582.0 16583.0 16584.0 16585.0 16586.0 16587.0 16588.0 16589.0 16590.0 16591.0 16592.0 16593.0 16594.0 16595.0 16596.0 16597.0 16598.0 16599.0 16600.0 16601.0 16602.0 16603.0 16604.0 16605.0 16606.0 16607.0 16608.0 16609.0 16610.0 16611.0 16612.0 16613.0 16614.0 16615.0 16616.0 16617.0 16618.0 16619.0 16620.0 16621.0 16622.0 16623.0 16624.0 16625.0 16626.0 16627.0 16628.0 16629.0 16630.0 16631.0 16632.0 16633.0 16634.0 16635.0 16636.0 16637.0 16638.0 16639.0 16640.0 16641.0 16642.0 16643.0 16644.0 16645.0 16646.0 16647.0 16648.0 16649.0 16650.0 16651.0 16652.0 16653.0 16654.0 16655.0 16656.0 16657.0 16658.0 16659.0 16660.0 16661.0 16662.0 16663.0 16664.0 16665.0 16666.0 16667.0 16668.0 16669.0 16670.0 16671.0 16672.0 16673.0 16674.0 16675.0 16676.0 16677.0 16678.0 16679.0 16680.0 16681.0 16682.0 16683.0 16684.0 16685.0 16686.0 16687.0 16688.0 16689.0 16690.0 16691.0 16692.0 16693.0 16694.0 16695.0 16696.0 16697.0 16698.0 16699.0 16700.0 16701.0 16702.0 16703.0 16704.0 16705.0 16706.0 16707.0 16708.0 16709.0 16710.0 16711.0 16712.0 16713.0 16714.0 16715.0 16716.0 16717.0 16718.0 16719.0 16720.0 16721.0 16722.0 16723.0 16724.0 16725.0 16726.0 16727.0 16728.0 16729.0 16730.0 16731.0 16732.0 16733.0 16734.0 16735.0 16736.0 16737.0 16738.0 16739.0 16740.0 16741.0 16742.0 16743.0 16744.0 16745.0 16746.0 16747.0 16748.0 16749.0 16750.0 16751.0 16752.0 16753.0 16754.0 16755.0 16756.0 16757.0 16758.0 16759.0 16760.0 16761.0 16762.0 16763.0 16764.0 16765.0 16766.0 16767.0 16768.0 16769.0 16770.0 16771.0 16772.0 16773.0 16774.0 16775.0 16776.0 16777.0 16778.0 16779.0 16780.0 16781.0 16782.0 16783.0 16784.0 16785.0 16786.0 16787.0 16788.0 16789.0 16790.0 16791.0 16792.0 16793.0 16794.0 16795.0 16796.0 16797.0 16798.0 16799.0 16800.0 16801.0 16802.0 16803.0 16804.0 16805.0 16806.0 16807.0 16808.0 16809.0 16810.0 16811.0 16812.0 16813.0 16814.0 16815.0 16816.0 16817.0 16818.0 16819.0 16820.0 16821.0 16822.0 16823.0 16824.0 16825.0 16826.0 16827.0 16828.0 16829.0 16830.0 16831.0 16832.0 16833.0 16834.0 16835.0 16836.0 16837.0 16838.0 16839.0 16840.0 16841.0 16842.0 16843.0 16844.0 16845.0 16846.0 16847.0 16848.0 16849.0 16850.0 16851.0 16852.0 16853.0 16854.0 16855.0 16856.0 16857.0 16858.0 16859.0 16860.0 16861.0 16862.0 16863.0 16864.0 16865.0 16866.0 16867.0 16868.0 16869.0 16870.0 16871.0 16872.0 16873.0 16874.0 16875.0 16876.0 16877.0 16878.0 16879.0 16880.0 16881.0 16882.0 16883.0 16884.0 16885.0 16886.0 16887.0 16888.0 16889.0 16890.0 16891.0 16892.0 16893.0 16894.0 16895.0 16896.0 16897.0 16898.0 16899.0 16900.0 16901.0 16902.0 16903.0 16904.0 16905.0 16906.0 16907.0 16908.0 16909.0 16910.0 16911.0 16912.0 16913.0 16914.0 16915.0 16916.0 16917.0 16918.0 16919.0 16920.0 16921.0 16922.0 16923.0 16924.0 16925.0 16926.0 16927.0 16928.0 16929.0 16930.0 16931.0 16932.0 16933.0 16934.0 16935.0 16936.0 16937.0 16938.0 16939.0 16940.0 16941.0 16942.0 16943.0 16944.0 16945.0 16946.0 16947.0 16948.0 16949.0 16950.0 16951.0 16952.0 16953.0 16954.0 16955.0 16956.0 16957.0 16958.0 16959.0 16960.0 16961.0 16962.0 16963.0 16964.0 16965.0 16966.0 16967.0 16968.0 16969.0 16970.0 16971.0 16972.0 16973.0 16974.0 16975.0 16976.0 16977.0 16978.0 16979.0 16980.0 16981.0 16982.0 16983.0 16984.0 16985.0 16986.0 16987.0 16988.0 16989.0 16990.0 16991.0 16992.0 16993.0 16994.0 16995.0 16996.0 16997.0 16998.0 16999.0 17000.0 17001.0 17002.0 17003.0 17004.0 17005.0 17006.0 17007.0 17008.0 17009.0 17010.0 17011.0 17012.0 17013.0 17014.0 17015.0 17016.0 17017.0 17018.0 17019.0 17020.0 17021.0 17022.0 17023.0 17024.0 17025.0 17026.0 17027.0 17028.0 17029.0 17030.0 17031.0 17032.0 17033.0 17034.0 17035.0 17036.0 17037.0 17038.0 17039.0 17040.0 17041.0 17042.0 17043.0 17044.0 17045.0 17046.0 17047.0 17048.0 17049.0 17050.0 17051.0 17052.0 17053.0 17054.0 17055.0 17056.0 17057.0 17058.0 17059.0 17060.0 17061.0 17062.0 17063.0 17064.0 17065.0 17066.0 17067.0 17068.0 17069.0 17070.0 17071.0 17072.0 17073.0 17074.0 17075.0 17076.0 17077.0 17078.0 17079.0 17080.0 17081.0 17082.0 17083.0 17084.0 17085.0 17086.0 17087.0 17088.0 17089.0 17090.0 17091.0 17092.0 17093.0 17094.0 17095.0 17096.0 17097.0 17098.0 17099.0 17100.0 17101.0 17102.0 17103.0 17104.0 17105.0 17106.0 17107.0 17108.0 17109.0 17110.0 17111.0 17112.0 17113.0 17114.0 17115.0 17116.0 17117.0 17118.0 17119.0 17120.0 17121.0 17122.0 17123.0 17124.0 17125.0 17126.0 17127.0 17128.0 17129.0 17130.0 17131.0 17132.0 17133.0 17134.0 17135.0 17136.0 17137.0 17138.0 17139.0 17140.0 17141.0 17142.0 17143.0 17144.0 17145.0 17146.0 17147.0 17148.0 17149.0 17150.0 17151.0 17152.0 17153.0 17154.0 17155.0 17156.0 17157.0 17158.0 17159.0 17160.0 17161.0 17162.0 17163.0 17164.0 17165.0 17166.0 17167.0 17168.0 17169.0 17170.0 17171.0 17172.0 17173.0 17174.0 17175.0 17176.0 17177.0 17178.0 17179.0 17180.0 17181.0 17182.0 17183.0 17184.0 17185.0 17186.0 17187.0 17188.0 17189.0 17190.0 17191.0 17192.0 17193.0 17194.0 17195.0 17196.0 17197.0 17198.0 17199.0 17200.0 17201.0 17202.0 17203.0 17204.0 17205.0 17206.0 17207.0 17208.0 17209.0 17210.0 17211.0 17212.0 17213.0 17214.0 17215.0 17216.0 17217.0 17218.0 17219.0 17220.0 17221.0 17222.0 17223.0 17224.0 17225.0 17226.0 17227.0 17228.0 17229.0 17230.0 17231.0 17232.0 17233.0 17234.0 17235.0 17236.0 17237.0 17238.0 17239.0 17240.0 17241.0 17242.0 17243.0 17244.0 17245.0 17246.0 17247.0 17248.0 17249.0 17250.0 17251.0 17252.0 17253.0 17254.0 17255.0 17256.0 17257.0 17258.0 17259.0 17260.0 17261.0 17262.0 17263.0 17264.0 17265.0 17266.0 17267.0 17268.0 17269.0 17270.0 17271.0 17272.0 17273.0 17274.0 17275.0 17276.0 17277.0 17278.0 17279.0 17280.0 17281.0 17282.0 17283.0 17284.0 17285.0 17286.0 17287.0 17288.0 17289.0 17290.0 17291.0 17292.0 17293.0 17294.0 17295.0 17296.0 17297.0 17298.0 17299.0 17300.0 17301.0 17302.0 17303.0 17304.0 17305.0 17306.0 17307.0 17308.0 17309.0 17310.0 17311.0 17312.0 17313.0 17314.0 17315.0 17316.0 17317.0 17318.0 17319.0 17320.0 17321.0 17322.0 17323.0 17324.0 17325.0 17326.0 17327.0 17328.0 17329.0 17330.0 17331.0 17332.0 17333.0 17334.0 17335.0 17336.0 17337.0 17338.0 17339.0 17340.0 17341.0 17342.0 17343.0 17344.0 17345.0 17346.0 17347.0 17348.0 17349.0 17350.0 17351.0 17352.0 17353.0 17354.0 17355.0 17356.0 17357.0 17358.0 17359.0 17360.0 17361.0 17362.0 17363.0 17364.0 17365.0 17366.0 17367.0 17368.0 17369.0 17370.0 17371.0 17372.0 17373.0 17374.0 17375.0 17376.0 17377.0 17378.0 17379.0 17380.0 17381.0 17382.0 17383.0 17384.0 17385.0 17386.0 17387.0 17388.0 17389.0 17390.0 17391.0 17392.0 17393.0 17394.0 17395.0 17396.0 17397.0 17398.0 17399.0 17400.0 17401.0 17402.0 17403.0 17404.0 17405.0 17406.0 17407.0 17408.0 17409.0 17410.0 17411.0 17412.0 17413.0 17414.0 17415.0 17416.0 17417.0 17418.0 17419.0 17420.0 17421.0 17422.0 17423.0 17424.0 17425.0 17426.0 17427.0 17428.0 17429.0 17430.0 17431.0 17432.0 17433.0 17434.0 17435.0 17436.0 17437.0 17438.0 17439.0 17440.0 17441.0 17442.0 17443.0 17444.0 17445.0 17446.0 17447.0 17448.0 17449.0 17450.0 17451.0 17452.0 17453.0 17454.0 17455.0 17456.0 17457.0 17458.0 17459.0 17460.0 17461.0 17462.0 17463.0 17464.0 17465.0 17466.0 17467.0 17468.0 17469.0 17470.0 17471.0 17472.0 17473.0 17474.0 17475.0 17476.0 17477.0 17478.0 17479.0 17480.0 17481.0 17482.0 17483.0 17484.0 17485.0 17486.0 17487.0 17488.0 17489.0 17490.0 17491.0 17492.0 17493.0 17494.0 17495.0 17496.0 17497.0 17498.0 17499.0 17500.0 17501.0 17502.0 17503.0 17504.0 17505.0 17506.0 17507.0 17508.0 17509.0 17510.0 17511.0 17512.0 17513.0 17514.0 17515.0 17516.0 17517.0 17518.0 17519.0 17520.0 17521.0 17522.0 17523.0 17524.0 17525.0 17526.0 17527.0 17528.0 17529.0 17530.0 17531.0 17532.0 17533.0 17534.0 17535.0 17536.0 17537.0 17538.0 17539.0 17540.0 17541.0 17542.0 17543.0 17544.0 17545.0 17546.0 17547.0 17548.0 17549.0 17550.0 17551.0 17552.0 17553.0 17554.0 17555.0 17556.0 17557.0 17558.0 17559.0 17560.0 17561.0 17562.0 17563.0 17564.0 17565.0 17566.0 17567.0 17568.0 17569.0 17570.0 17571.0 17572.0 17573.0 17574.0 17575.0 17576.0 17577.0 17578.0 17579.0 17580.0 17581.0 17582.0 17583.0 17584.0 17585.0 17586.0 17587.0 17588.0 17589.0 17590.0 17591.0 17592.0 17593.0 17594.0 17595.0 17596.0 17597.0 17598.0 17599.0 17600.0 17601.0 17602.0 17603.0 17604.0 17605.0 17606.0 17607.0 17608.0 17609.0 17610.0 17611.0 17612.0 17613.0 17614.0 17615.0 17616.0 17617.0 17618.0 17619.0 17620.0 17621.0 17622.0 17623.0 17624.0 17625.0 17626.0 17627.0 17628.0 17629.0 17630.0 17631.0 17632.0 17633.0 17634.0 17635.0 17636.0 17637.0 17638.0 17639.0 17640.0 17641.0 17642.0 17643.0 17644.0 17645.0 17646.0 17647.0 17648.0 17649.0 17650.0 17651.0 17652.0 17653.0 17654.0 17655.0 17656.0 17657.0 17658.0 17659.0 17660.0 17661.0 17662.0 17663.0 17664.0 17665.0 17666.0 17667.0 17668.0 17669.0 17670.0 17671.0 17672.0 17673.0 17674.0 17675.0 17676.0 17677.0 17678.0 17679.0 17680.0 17681.0 17682.0 17683.0 17684.0 17685.0 17686.0 17687.0 17688.0 17689.0 17690.0 17691.0 17692.0 17693.0 17694.0 17695.0 17696.0 17697.0 17698.0 17699.0 17700.0 17701.0 17702.0 17703.0 17704.0 17705.0 17706.0 17707.0 17708.0 17709.0 17710.0 17711.0 17712.0 17713.0 17714.0 17715.0 17716.0 17717.0 17718.0 17719.0 17720.0 17721.0 17722.0 17723.0 17724.0 17725.0 17726.0 17727.0 17728.0 17729.0 17730.0 17731.0 17732.0 17733.0 17734.0 17735.0 17736.0 17737.0 17738.0 17739.0 17740.0 17741.0 17742.0 17743.0 17744.0 17745.0 17746.0 17747.0 17748.0 17749.0 17750.0 17751.0 17752.0 17753.0 17754.0 17755.0 17756.0 17757.0 17758.0 17759.0 17760.0 17761.0 17762.0 17763.0 17764.0 17765.0 17766.0 17767.0 17768.0 17769.0 17770.0 17771.0 17772.0 17773.0 17774.0 17775.0 17776.0 17777.0 17778.0 17779.0 17780.0 17781.0 17782.0 17783.0 17784.0 17785.0 17786.0 17787.0 17788.0 17789.0 17790.0 17791.0 17792.0 17793.0 17794.0 17795.0 17796.0 17797.0 17798.0 17799.0 17800.0 17801.0 17802.0 17803.0 17804.0 17805.0 17806.0 17807.0 17808.0 17809.0 17810.0 17811.0 17812.0 17813.0 17814.0 17815.0 17816.0 17817.0 17818.0 17819.0 17820.0 17821.0 17822.0 17823.0 17824.0 17825.0 17826.0 17827.0 17828.0 17829.0 17830.0 17831.0 17832.0 17833.0 17834.0 17835.0 17836.0 17837.0 17838.0 17839.0 17840.0 17841.0 17842.0 17843.0 17844.0 17845.0 17846.0 17847.0 17848.0 17849.0 17850.0 17851.0 17852.0 17853.0 17854.0 17855.0 17856.0 17857.0 17858.0 17859.0 17860.0 17861.0 17862.0 17863.0 17864.0 17865.0 17866.0 17867.0 17868.0 17869.0 17870.0 17871.0 17872.0 17873.0 17874.0 17875.0 17876.0 17877.0 17878.0 17879.0 17880.0 17881.0 17882.0 17883.0 17884.0 17885.0 17886.0 17887.0 17888.0 17889.0 17890.0 17891.0 17892.0 17893.0 17894.0 17895.0 17896.0 17897.0 17898.0 17899.0 17900.0 17901.0 17902.0 17903.0 17904.0 17905.0 17906.0 17907.0 17908.0 17909.0 17910.0 17911.0 17912.0 17913.0 17914.0 17915.0 17916.0 17917.0 17918.0 17919.0 17920.0 17921.0 17922.0 17923.0 17924.0 17925.0 17926.0 17927.0 17928.0 17929.0 17930.0 17931.0 17932.0 17933.0 17934.0 17935.0 17936.0 17937.0 17938.0 17939.0 17940.0 17941.0 17942.0 17943.0 17944.0 17945.0 17946.0 17947.0 17948.0 17949.0 17950.0 17951.0 17952.0 17953.0 17954.0 17955.0 17956.0 17957.0 17958.0 17959.0 17960.0 17961.0 17962.0 17963.0 17964.0 17965.0 17966.0 17967.0 17968.0 17969.0 17970.0 17971.0 17972.0 17973.0 17974.0 17975.0 17976.0 17977.0 17978.0 17979.0 17980.0 17981.0 17982.0 17983.0 17984.0 17985.0 17986.0 17987.0 17988.0 17989.0 17990.0 17991.0 17992.0 17993.0 17994.0 17995.0 17996.0 17997.0 17998.0 17999.0 18000.0 18001.0 18002.0 18003.0 18004.0 18005.0 18006.0 18007.0 18008.0 18009.0 18010.0 18011.0 18012.0 18013.0 18014.0 18015.0 18016.0 18017.0 18018.0 18019.0 18020.0 18021.0 18022.0 18023.0 18024.0 18025.0 18026.0 18027.0 18028.0 18029.0 18030.0 18031.0 18032.0 18033.0 18034.0 18035.0 18036.0 18037.0 18038.0 18039.0 18040.0 18041.0 18042.0 18043.0 18044.0 18045.0 18046.0 18047.0 18048.0 18049.0 18050.0 18051.0 18052.0 18053.0 18054.0 18055.0 18056.0 18057.0 18058.0 18059.0 18060.0 18061.0 18062.0 18063.0 18064.0 18065.0 18066.0 18067.0 18068.0 18069.0 18070.0 18071.0 18072.0 18073.0 18074.0 18075.0 18076.0 18077.0 18078.0 18079.0 18080.0 18081.0 18082.0 18083.0 18084.0 18085.0 18086.0 18087.0 18088.0 18089.0 18090.0 18091.0 18092.0 18093.0 18094.0 18095.0 18096.0 18097.0 18098.0 18099.0 18100.0 18101.0 18102.0 18103.0 18104.0 18105.0 18106.0 18107.0 18108.0 18109.0 18110.0 18111.0 18112.0 18113.0 18114.0 18115.0 18116.0 18117.0 18118.0 18119.0 18120.0 18121.0 18122.0 18123.0 18124.0 18125.0 18126.0 18127.0 18128.0 18129.0 18130.0 18131.0 18132.0 18133.0 18134.0 18135.0 18136.0 18137.0 18138.0 18139.0 18140.0 18141.0 18142.0 18143.0 18144.0 18145.0 18146.0 18147.0 18148.0 18149.0 18150.0 18151.0 18152.0 18153.0 18154.0 18155.0 18156.0 18157.0 18158.0 18159.0 18160.0 18161.0 18162.0 18163.0 18164.0 18165.0 18166.0 18167.0 18168.0 18169.0 18170.0 18171.0 18172.0 18173.0 18174.0 18175.0 18176.0 18177.0 18178.0 18179.0 18180.0 18181.0 18182.0 18183.0 18184.0 18185.0 18186.0 18187.0 18188.0 18189.0 18190.0 18191.0 18192.0 18193.0 18194.0 18195.0 18196.0 18197.0 18198.0 18199.0 18200.0 18201.0 18202.0 18203.0 18204.0 18205.0 18206.0 18207.0 18208.0 18209.0 18210.0 18211.0 18212.0 18213.0 18214.0 18215.0 18216.0 18217.0 18218.0 18219.0 18220.0 18221.0 18222.0 18223.0 18224.0 18225.0 18226.0 18227.0 18228.0 18229.0 18230.0 18231.0 18232.0 18233.0 18234.0 18235.0 18236.0 18237.0 18238.0 18239.0 18240.0 18241.0 18242.0 18243.0 18244.0 18245.0 18246.0 18247.0 18248.0 18249.0 18250.0 18251.0 18252.0 18253.0 18254.0 18255.0 18256.0 18257.0 18258.0 18259.0 18260.0 18261.0 18262.0 18263.0 18264.0 18265.0 18266.0 18267.0 18268.0 18269.0 18270.0 18271.0 18272.0 18273.0 18274.0 18275.0 18276.0 18277.0 18278.0 18279.0 18280.0 18281.0 18282.0 18283.0 18284.0 18285.0 18286.0 18287.0 18288.0 18289.0 18290.0 18291.0 18292.0 18293.0 18294.0 18295.0 18296.0 18297.0 18298.0 18299.0 18300.0 18301.0 18302.0 18303.0 18304.0 18305.0 18306.0 18307.0 18308.0 18309.0 18310.0 18311.0 18312.0 18313.0 18314.0 18315.0 18316.0 18317.0 18318.0 18319.0 18320.0 18321.0 18322.0 18323.0 18324.0 18325.0 18326.0 18327.0 18328.0 18329.0 18330.0 18331.0 18332.0 18333.0 18334.0 18335.0 18336.0 18337.0 18338.0 18339.0 18340.0 18341.0 18342.0 18343.0 18344.0 18345.0 18346.0 18347.0 18348.0 18349.0 18350.0 18351.0 18352.0 18353.0 18354.0 18355.0 18356.0 18357.0 18358.0 18359.0 18360.0 18361.0 18362.0 18363.0 18364.0 18365.0 18366.0 18367.0 18368.0 18369.0 18370.0 18371.0 18372.0 18373.0 18374.0 18375.0 18376.0 18377.0 18378.0 18379.0 18380.0 18381.0 18382.0 18383.0 18384.0 18385.0 18386.0 18387.0 18388.0 18389.0 18390.0 18391.0 18392.0 18393.0 18394.0 18395.0 18396.0 18397.0 18398.0 18399.0 18400.0 18401.0 18402.0 18403.0 18404.0 18405.0 18406.0 18407.0 18408.0 18409.0 18410.0 18411.0 18412.0 18413.0 18414.0 18415.0 18416.0 18417.0 18418.0 18419.0 18420.0 18421.0 18422.0 18423.0 18424.0 18425.0 18426.0 18427.0 18428.0 18429.0 18430.0 18431.0 18432.0 18433.0 18434.0 18435.0 18436.0 18437.0 18438.0 18439.0 18440.0 18441.0 18442.0 18443.0 18444.0 18445.0 18446.0 18447.0 18448.0 18449.0 18450.0 18451.0 18452.0 18453.0 18454.0 18455.0 18456.0 18457.0 18458.0 18459.0 18460.0 18461.0 18462.0 18463.0 18464.0 18465.0 18466.0 18467.0 18468.0 18469.0 18470.0 18471.0 18472.0 18473.0 18474.0 18475.0 18476.0 18477.0 18478.0 18479.0 18480.0 18481.0 18482.0 18483.0 18484.0 18485.0 18486.0 18487.0 18488.0 18489.0 18490.0 18491.0 18492.0 18493.0 18494.0 18495.0 18496.0 18497.0 18498.0 18499.0 18500.0 18501.0 18502.0 18503.0 18504.0 18505.0 18506.0 18507.0 18508.0 18509.0 18510.0 18511.0 18512.0 18513.0 18514.0 18515.0 18516.0 18517.0 18518.0 18519.0 18520.0 18521.0 18522.0 18523.0 18524.0 18525.0 18526.0 18527.0 18528.0 18529.0 18530.0 18531.0 18532.0 18533.0 18534.0 18535.0 18536.0 18537.0 18538.0 18539.0 18540.0 18541.0 18542.0 18543.0 18544.0 18545.0 18546.0 18547.0 18548.0 18549.0 18550.0 18551.0 18552.0 18553.0 18554.0 18555.0 18556.0 18557.0 18558.0 18559.0 18560.0 18561.0 18562.0 18563.0 18564.0 18565.0 18566.0 18567.0 18568.0 18569.0 18570.0 18571.0 18572.0 18573.0 18574.0 18575.0 18576.0 18577.0 18578.0 18579.0 18580.0 18581.0 18582.0 18583.0 18584.0 18585.0 18586.0 18587.0 18588.0 18589.0 18590.0 18591.0 18592.0 18593.0 18594.0 18595.0 18596.0 18597.0 18598.0 18599.0 18600.0 18601.0 18602.0 18603.0 18604.0 18605.0 18606.0 18607.0 18608.0 18609.0 18610.0 18611.0 18612.0 18613.0 18614.0 18615.0 18616.0 18617.0 18618.0 18619.0 18620.0 18621.0 18622.0 18623.0 18624.0 18625.0 18626.0 18627.0 18628.0 18629.0 18630.0 18631.0 18632.0 18633.0 18634.0 18635.0 18636.0 18637.0 18638.0 18639.0 18640.0 18641.0 18642.0 18643.0 18644.0 18645.0 18646.0 18647.0 18648.0 18649.0 18650.0 18651.0 18652.0 18653.0 18654.0 18655.0 18656.0 18657.0 18658.0 18659.0 18660.0 18661.0 18662.0 18663.0 18664.0 18665.0 18666.0 18667.0 18668.0 18669.0 18670.0 18671.0 18672.0 18673.0 18674.0 18675.0 18676.0 18677.0 18678.0 18679.0 18680.0 18681.0 18682.0 18683.0 18684.0 18685.0 18686.0 18687.0 18688.0 18689.0 18690.0 18691.0 18692.0 18693.0 18694.0 18695.0 18696.0 18697.0 18698.0 18699.0 18700.0 18701.0 18702.0 18703.0 18704.0 18705.0 18706.0 18707.0 18708.0 18709.0 18710.0 18711.0 18712.0 18713.0 18714.0 18715.0 18716.0 18717.0 18718.0 18719.0 18720.0 18721.0 18722.0 18723.0 18724.0 18725.0 18726.0 18727.0 18728.0 18729.0 18730.0 18731.0 18732.0 18733.0 18734.0 18735.0 18736.0 18737.0 18738.0 18739.0 18740.0 18741.0 18742.0 18743.0 18744.0 18745.0 18746.0 18747.0 18748.0 18749.0 18750.0 18751.0 18752.0 18753.0 18754.0 18755.0 18756.0 18757.0 18758.0 18759.0 18760.0 18761.0 18762.0 18763.0 18764.0 18765.0 18766.0 18767.0 18768.0 18769.0 18770.0 18771.0 18772.0 18773.0 18774.0 18775.0 18776.0 18777.0 18778.0 18779.0 18780.0 18781.0 18782.0 18783.0 18784.0 18785.0 18786.0 18787.0 18788.0 18789.0 18790.0 18791.0 18792.0 18793.0 18794.0 18795.0 18796.0 18797.0 18798.0 18799.0 18800.0 18801.0 18802.0 18803.0 18804.0 18805.0 18806.0 18807.0 18808.0 18809.0 18810.0 18811.0 18812.0 18813.0 18814.0 18815.0 18816.0 18817.0 18818.0 18819.0 18820.0 18821.0 18822.0 18823.0 18824.0 18825.0 18826.0 18827.0 18828.0 18829.0 18830.0 18831.0 18832.0 18833.0 18834.0 18835.0 18836.0 18837.0 18838.0 18839.0 18840.0 18841.0 18842.0 18843.0 18844.0 18845.0 18846.0 18847.0 18848.0 18849.0 18850.0 18851.0 18852.0 18853.0 18854.0 18855.0 18856.0 18857.0 18858.0 18859.0 18860.0 18861.0 18862.0 18863.0 18864.0 18865.0 18866.0 18867.0 18868.0 18869.0 18870.0 18871.0 18872.0 18873.0 18874.0 18875.0 18876.0 18877.0 18878.0 18879.0 18880.0 18881.0 18882.0 18883.0 18884.0 18885.0 18886.0 18887.0 18888.0 18889.0 18890.0 18891.0 18892.0 18893.0 18894.0 18895.0 18896.0 18897.0 18898.0 18899.0 18900.0 18901.0 18902.0 18903.0 18904.0 18905.0 18906.0 18907.0 18908.0 18909.0 18910.0 18911.0 18912.0 18913.0 18914.0 18915.0 18916.0 18917.0 18918.0 18919.0 18920.0 18921.0 18922.0 18923.0 18924.0 18925.0 18926.0 18927.0 18928.0 18929.0 18930.0 18931.0 18932.0 18933.0 18934.0 18935.0 18936.0 18937.0 18938.0 18939.0 18940.0 18941.0 18942.0 18943.0 18944.0 18945.0 18946.0 18947.0 18948.0 18949.0 18950.0 18951.0 18952.0 18953.0 18954.0 18955.0 18956.0 18957.0 18958.0 18959.0 18960.0 18961.0 18962.0 18963.0 18964.0 18965.0 18966.0 18967.0 18968.0 18969.0 18970.0 18971.0 18972.0 18973.0 18974.0 18975.0 18976.0 18977.0 18978.0 18979.0 18980.0 18981.0 18982.0 18983.0 18984.0 18985.0 18986.0 18987.0 18988.0 18989.0 18990.0 18991.0 18992.0 18993.0 18994.0 18995.0 18996.0 18997.0 18998.0 18999.0 19000.0 19001.0 19002.0 19003.0 19004.0 19005.0 19006.0 19007.0 19008.0 19009.0 19010.0 19011.0 19012.0 19013.0 19014.0 19015.0 19016.0 19017.0 19018.0 19019.0 19020.0 19021.0 19022.0 19023.0 19024.0 19025.0 19026.0 19027.0 19028.0 19029.0 19030.0 19031.0 19032.0 19033.0 19034.0 19035.0 19036.0 19037.0 19038.0 19039.0 19040.0 19041.0 19042.0 19043.0 19044.0 19045.0 19046.0 19047.0 19048.0 19049.0 19050.0 19051.0 19052.0 19053.0 19054.0 19055.0 19056.0 19057.0 19058.0 19059.0 19060.0 19061.0 19062.0 19063.0 19064.0 19065.0 19066.0 19067.0 19068.0 19069.0 19070.0 19071.0 19072.0 19073.0 19074.0 19075.0 19076.0 19077.0 19078.0 19079.0 19080.0 19081.0 19082.0 19083.0 19084.0 19085.0 19086.0 19087.0 19088.0 19089.0 19090.0 19091.0 19092.0 19093.0 19094.0 19095.0 19096.0 19097.0 19098.0 19099.0 19100.0 19101.0 19102.0 19103.0 19104.0 19105.0 19106.0 19107.0 19108.0 19109.0 19110.0 19111.0 19112.0 19113.0 19114.0 19115.0 19116.0 19117.0 19118.0 19119.0 19120.0 19121.0 19122.0 19123.0 19124.0 19125.0 19126.0 19127.0 19128.0 19129.0 19130.0 19131.0 19132.0 19133.0 19134.0 19135.0 19136.0 19137.0 19138.0 19139.0 19140.0 19141.0 19142.0 19143.0 19144.0 19145.0 19146.0 19147.0 19148.0 19149.0 19150.0 19151.0 19152.0 19153.0 19154.0 19155.0 19156.0 19157.0 19158.0 19159.0 19160.0 19161.0 19162.0 19163.0 19164.0 19165.0 19166.0 19167.0 19168.0 19169.0 19170.0 19171.0 19172.0 19173.0 19174.0 19175.0 19176.0 19177.0 19178.0 19179.0 19180.0 19181.0 19182.0 19183.0 19184.0 19185.0 19186.0 19187.0 19188.0 19189.0 19190.0 19191.0 19192.0 19193.0 19194.0 19195.0 19196.0 19197.0 19198.0 19199.0 19200.0 19201.0 19202.0 19203.0 19204.0 19205.0 19206.0 19207.0 19208.0 19209.0 19210.0 19211.0 19212.0 19213.0 19214.0 19215.0 19216.0 19217.0 19218.0 19219.0 19220.0 19221.0 19222.0 19223.0 19224.0 19225.0 19226.0 19227.0 19228.0 19229.0 19230.0 19231.0 19232.0 19233.0 19234.0 19235.0 19236.0 19237.0 19238.0 19239.0 19240.0 19241.0 19242.0 19243.0 19244.0 19245.0 19246.0 19247.0 19248.0 19249.0 19250.0 19251.0 19252.0 19253.0 19254.0 19255.0 19256.0 19257.0 19258.0 19259.0 19260.0 19261.0 19262.0 19263.0 19264.0 19265.0 19266.0 19267.0 19268.0 19269.0 19270.0 19271.0 19272.0 19273.0 19274.0 19275.0 19276.0 19277.0 19278.0 19279.0 19280.0 19281.0 19282.0 19283.0 19284.0 19285.0 19286.0 19287.0 19288.0 19289.0 19290.0 19291.0 19292.0 19293.0 19294.0 19295.0 19296.0 19297.0 19298.0 19299.0 19300.0 19301.0 19302.0 19303.0 19304.0 19305.0 19306.0 19307.0 19308.0 19309.0 19310.0 19311.0 19312.0 19313.0 19314.0 19315.0 19316.0 19317.0 19318.0 19319.0 19320.0 19321.0 19322.0 19323.0 19324.0 19325.0 19326.0 19327.0 19328.0 19329.0 19330.0 19331.0 19332.0 19333.0 19334.0 19335.0 19336.0 19337.0 19338.0 19339.0 19340.0 19341.0 19342.0 19343.0 19344.0 19345.0 19346.0 19347.0 19348.0 19349.0 19350.0 19351.0 19352.0 19353.0 19354.0 19355.0 19356.0 19357.0 19358.0 19359.0 19360.0 19361.0 19362.0 19363.0 19364.0 19365.0 19366.0 19367.0 19368.0 19369.0 19370.0 19371.0 19372.0 19373.0 19374.0 19375.0 19376.0 19377.0 19378.0 19379.0 19380.0 19381.0 19382.0 19383.0 19384.0 19385.0 19386.0 19387.0 19388.0 19389.0 19390.0 19391.0 19392.0 19393.0 19394.0 19395.0 19396.0 19397.0 19398.0 19399.0 19400.0 19401.0 19402.0 19403.0 19404.0 19405.0 19406.0 19407.0 19408.0 19409.0 19410.0 19411.0 19412.0 19413.0 19414.0 19415.0 19416.0 19417.0 19418.0 19419.0 19420.0 19421.0 19422.0 19423.0 19424.0 19425.0 19426.0 19427.0 19428.0 19429.0 19430.0 19431.0 19432.0 19433.0 19434.0 19435.0 19436.0 19437.0 19438.0 19439.0 19440.0 19441.0 19442.0 19443.0 19444.0 19445.0 19446.0 19447.0 19448.0 19449.0 19450.0 19451.0 19452.0 19453.0 19454.0 19455.0 19456.0 19457.0 19458.0 19459.0 19460.0 19461.0 19462.0 19463.0 19464.0 19465.0 19466.0 19467.0 19468.0 19469.0 19470.0 19471.0 19472.0 19473.0 19474.0 19475.0 19476.0 19477.0 19478.0 19479.0 19480.0 19481.0 19482.0 19483.0 19484.0 19485.0 19486.0 19487.0 19488.0 19489.0 19490.0 19491.0 19492.0 19493.0 19494.0 19495.0 19496.0 19497.0 19498.0 19499.0 19500.0 19501.0 19502.0 19503.0 19504.0 19505.0 19506.0 19507.0 19508.0 19509.0 19510.0 19511.0 19512.0 19513.0 19514.0 19515.0 19516.0 19517.0 19518.0 19519.0 19520.0 19521.0 19522.0 19523.0 19524.0 19525.0 19526.0 19527.0 19528.0 19529.0 19530.0 19531.0 19532.0 19533.0 19534.0 19535.0 19536.0 19537.0 19538.0 19539.0 19540.0 19541.0 19542.0 19543.0 19544.0 19545.0 19546.0 19547.0 19548.0 19549.0 19550.0 19551.0 19552.0 19553.0 19554.0 19555.0 19556.0 19557.0 19558.0 19559.0 19560.0 19561.0 19562.0 19563.0 19564.0 19565.0 19566.0 19567.0 19568.0 19569.0 19570.0 19571.0 19572.0 19573.0 19574.0 19575.0 19576.0 19577.0 19578.0 19579.0 19580.0 19581.0 19582.0 19583.0 19584.0 19585.0 19586.0 19587.0 19588.0 19589.0 19590.0 19591.0 19592.0 19593.0 19594.0 19595.0 19596.0 19597.0 19598.0 19599.0 19600.0 19601.0 19602.0 19603.0 19604.0 19605.0 19606.0 19607.0 19608.0 19609.0 19610.0 19611.0 19612.0 19613.0 19614.0 19615.0 19616.0 19617.0 19618.0 19619.0 19620.0 19621.0 19622.0 19623.0 19624.0 19625.0 19626.0 19627.0 19628.0 19629.0 19630.0 19631.0 19632.0 19633.0 19634.0 19635.0 19636.0 19637.0 19638.0 19639.0 19640.0 19641.0 19642.0 19643.0 19644.0 19645.0 19646.0 19647.0 19648.0 19649.0 19650.0 19651.0 19652.0 19653.0 19654.0 19655.0 19656.0 19657.0 19658.0 19659.0 19660.0 19661.0 19662.0 19663.0 19664.0 19665.0 19666.0 19667.0 19668.0 19669.0 19670.0 19671.0 19672.0 19673.0 19674.0 19675.0 19676.0 19677.0 19678.0 19679.0 19680.0 19681.0 19682.0 19683.0 19684.0 19685.0 19686.0 19687.0 19688.0 19689.0 19690.0 19691.0 19692.0 19693.0 19694.0 19695.0 19696.0 19697.0 19698.0 19699.0 19700.0 19701.0 19702.0 19703.0 19704.0 19705.0 19706.0 19707.0 19708.0 19709.0 19710.0 19711.0 19712.0 19713.0 19714.0 19715.0 19716.0 19717.0 19718.0 19719.0 19720.0 19721.0 19722.0 19723.0 19724.0 19725.0 19726.0 19727.0 19728.0 19729.0 19730.0 19731.0 19732.0 19733.0 19734.0 19735.0 19736.0 19737.0 19738.0 19739.0 19740.0 19741.0 19742.0 19743.0 19744.0 19745.0 19746.0 19747.0 19748.0 19749.0 19750.0 19751.0 19752.0 19753.0 19754.0 19755.0 19756.0 19757.0 19758.0 19759.0 19760.0 19761.0 19762.0 19763.0 19764.0 19765.0 19766.0 19767.0 19768.0 19769.0 19770.0 19771.0 19772.0 19773.0 19774.0 19775.0 19776.0 19777.0 19778.0 19779.0 19780.0 19781.0 19782.0 19783.0 19784.0 19785.0 19786.0 19787.0 19788.0 19789.0 19790.0 19791.0 19792.0 19793.0 19794.0 19795.0 19796.0 19797.0 19798.0 19799.0 19800.0 19801.0 19802.0 19803.0 19804.0 19805.0 19806.0 19807.0 19808.0 19809.0 19810.0 19811.0 19812.0 19813.0 19814.0 19815.0 19816.0 19817.0 19818.0 19819.0 19820.0 19821.0 19822.0 19823.0 19824.0 19825.0 19826.0 19827.0 19828.0 19829.0 19830.0 19831.0 19832.0 19833.0 19834.0 19835.0 19836.0 19837.0 19838.0 19839.0 19840.0 19841.0 19842.0 19843.0 19844.0 19845.0 19846.0 19847.0 19848.0 19849.0 19850.0 19851.0 19852.0 19853.0 19854.0 19855.0 19856.0 19857.0 19858.0 19859.0 19860.0 19861.0 19862.0 19863.0 19864.0 19865.0 19866.0 19867.0 19868.0 19869.0 19870.0 19871.0 19872.0 19873.0 19874.0 19875.0 19876.0 19877.0 19878.0 19879.0 19880.0 19881.0 19882.0 19883.0 19884.0 19885.0 19886.0 19887.0 19888.0 19889.0 19890.0 19891.0 19892.0 19893.0 19894.0 19895.0 19896.0 19897.0 19898.0 19899.0 19900.0 19901.0 19902.0 19903.0 19904.0 19905.0 19906.0 19907.0 19908.0 19909.0 19910.0 19911.0 19912.0 19913.0 19914.0 19915.0 19916.0 19917.0 19918.0 19919.0 19920.0 19921.0 19922.0 19923.0 19924.0 19925.0 19926.0 19927.0 19928.0 19929.0 19930.0 19931.0 19932.0 19933.0 19934.0 19935.0 19936.0 19937.0 19938.0 19939.0 19940.0 19941.0 19942.0 19943.0 19944.0 19945.0 19946.0 19947.0 19948.0 19949.0 19950.0 19951.0 19952.0 19953.0 19954.0 19955.0 19956.0 19957.0 19958.0 19959.0 19960.0 19961.0 19962.0 19963.0 19964.0 19965.0 19966.0 19967.0 19968.0 19969.0 19970.0 19971.0 19972.0 19973.0 19974.0 19975.0 19976.0 19977.0 19978.0 19979.0 19980.0 19981.0 19982.0 19983.0 19984.0 19985.0 19986.0 19987.0 19988.0 19989.0 19990.0 19991.0 19992.0 19993.0 19994.0 19995.0 19996.0 19997.0 19998.0 19999.0 20000.0 20001.0 20002.0 20003.0 20004.0 20005.0 20006.0 20007.0 20008.0 20009.0 20010.0 20011.0 20012.0 20013.0 20014.0 20015.0 20016.0 20017.0 20018.0 20019.0 20020.0 20021.0 20022.0 20023.0 20024.0 20025.0 20026.0 20027.0 20028.0 20029.0 20030.0 20031.0 20032.0 20033.0 20034.0 20035.0 20036.0 20037.0 20038.0 20039.0 20040.0 20041.0 20042.0 20043.0 20044.0 20045.0 20046.0 20047.0 20048.0 20049.0 20050.0 20051.0 20052.0 20053.0 20054.0 20055.0 20056.0 20057.0 20058.0 20059.0 20060.0 20061.0 20062.0 20063.0 20064.0 20065.0 20066.0 20067.0 20068.0 20069.0 20070.0 20071.0 20072.0 20073.0 20074.0 20075.0 20076.0 20077.0 20078.0 20079.0 20080.0 20081.0 20082.0 20083.0 20084.0 20085.0 20086.0 20087.0 20088.0 20089.0 20090.0 20091.0 20092.0 20093.0 20094.0 20095.0 20096.0 20097.0 20098.0 20099.0 20100.0 20101.0 20102.0 20103.0 20104.0 20105.0 20106.0 20107.0 20108.0 20109.0 20110.0 20111.0 20112.0 20113.0 20114.0 20115.0 20116.0 20117.0 20118.0 20119.0 20120.0 20121.0 20122.0 20123.0 20124.0 20125.0 20126.0 20127.0 20128.0 20129.0 20130.0 20131.0 20132.0 20133.0 20134.0 20135.0 20136.0 20137.0 20138.0 20139.0 20140.0 20141.0 20142.0 20143.0 20144.0 20145.0 20146.0 20147.0 20148.0 20149.0 20150.0 20151.0 20152.0 20153.0 20154.0 20155.0 20156.0 20157.0 20158.0 20159.0 20160.0 20161.0 20162.0 20163.0 20164.0 20165.0 20166.0 20167.0 20168.0 20169.0 20170.0 20171.0 20172.0 20173.0 20174.0 20175.0 20176.0 20177.0 20178.0 20179.0 20180.0 20181.0 20182.0 20183.0 20184.0 20185.0 20186.0 20187.0 20188.0 20189.0 20190.0 20191.0 20192.0 20193.0 20194.0 20195.0 20196.0 20197.0 20198.0 20199.0 20200.0 20201.0 20202.0 20203.0 20204.0 20205.0 20206.0 20207.0 20208.0 20209.0 20210.0 20211.0 20212.0 20213.0 20214.0 20215.0 20216.0 20217.0 20218.0 20219.0 20220.0 20221.0 20222.0 20223.0 20224.0 20225.0 20226.0 20227.0 20228.0 20229.0 20230.0 20231.0 20232.0 20233.0 20234.0 20235.0 20236.0 20237.0 20238.0 20239.0 20240.0 20241.0 20242.0 20243.0 20244.0 20245.0 20246.0 20247.0 20248.0 20249.0 20250.0 20251.0 20252.0 20253.0 20254.0 20255.0 20256.0 20257.0 20258.0 20259.0 20260.0 20261.0 20262.0 20263.0 20264.0 20265.0 20266.0 20267.0 20268.0 20269.0 20270.0 20271.0 20272.0 20273.0 20274.0 20275.0 20276.0 20277.0 20278.0 20279.0 20280.0 20281.0 20282.0 20283.0 20284.0 20285.0 20286.0 20287.0 20288.0 20289.0 20290.0 20291.0 20292.0 20293.0 20294.0 20295.0 20296.0 20297.0 20298.0 20299.0 20300.0 20301.0 20302.0 20303.0 20304.0 20305.0 20306.0 20307.0 20308.0 20309.0 20310.0 20311.0 20312.0 20313.0 20314.0 20315.0 20316.0 20317.0 20318.0 20319.0 20320.0 20321.0 20322.0 20323.0 20324.0 20325.0 20326.0 20327.0 20328.0 20329.0 20330.0 20331.0 20332.0 20333.0 20334.0 20335.0 20336.0 20337.0 20338.0 20339.0 20340.0 20341.0 20342.0 20343.0 20344.0 20345.0 20346.0 20347.0 20348.0 20349.0 20350.0 20351.0 20352.0 20353.0 20354.0 20355.0 20356.0 20357.0 20358.0 20359.0 20360.0 20361.0 20362.0 20363.0 20364.0 20365.0 20366.0 20367.0 20368.0 20369.0 20370.0 20371.0 20372.0 20373.0 20374.0 20375.0 20376.0 20377.0 20378.0 20379.0 20380.0 20381.0 20382.0 20383.0 20384.0 20385.0 20386.0 20387.0 20388.0 20389.0 20390.0 20391.0 20392.0 20393.0 20394.0 20395.0 20396.0 20397.0 20398.0 20399.0 20400.0 20401.0 20402.0 20403.0 20404.0 20405.0 20406.0 20407.0 20408.0 20409.0 20410.0 20411.0 20412.0 20413.0 20414.0 20415.0 20416.0 20417.0 20418.0 20419.0 20420.0 20421.0 20422.0 20423.0 20424.0 20425.0 20426.0 20427.0 20428.0 20429.0 20430.0 20431.0 20432.0 20433.0 20434.0 20435.0 20436.0 20437.0 20438.0 20439.0 20440.0 20441.0 20442.0 20443.0 20444.0 20445.0 20446.0 20447.0 20448.0 20449.0 20450.0 20451.0 20452.0 20453.0 20454.0 20455.0 20456.0 20457.0 20458.0 20459.0 20460.0 20461.0 20462.0 20463.0 20464.0 20465.0 20466.0 20467.0 20468.0 20469.0 20470.0 20471.0 20472.0 20473.0 20474.0 20475.0 20476.0 20477.0 20478.0 20479.0 20480.0 20481.0 20482.0 20483.0 20484.0 20485.0 20486.0 20487.0 20488.0 20489.0 20490.0 20491.0 20492.0 20493.0 20494.0 20495.0 20496.0 20497.0 20498.0 20499.0 20500.0 20501.0 20502.0 20503.0 20504.0 20505.0 20506.0 20507.0 20508.0 20509.0 20510.0 20511.0 20512.0 20513.0 20514.0 20515.0 20516.0 20517.0 20518.0 20519.0 20520.0 20521.0 20522.0 20523.0 20524.0 20525.0 20526.0 20527.0 20528.0 20529.0 20530.0 20531.0 20532.0 20533.0 20534.0 20535.0 20536.0 20537.0 20538.0 20539.0 20540.0 20541.0 20542.0 20543.0 20544.0 20545.0 20546.0 20547.0 20548.0 20549.0 20550.0 20551.0 20552.0 20553.0 20554.0 20555.0 20556.0 20557.0 20558.0 20559.0 20560.0 20561.0 20562.0 20563.0 20564.0 20565.0 20566.0 20567.0 20568.0 20569.0 20570.0 20571.0 20572.0 20573.0 20574.0 20575.0 20576.0 20577.0 20578.0 20579.0 20580.0 20581.0 20582.0 20583.0 20584.0 20585.0 20586.0 20587.0 20588.0 20589.0 20590.0 20591.0 20592.0 20593.0 20594.0 20595.0 20596.0 20597.0 20598.0 20599.0 20600.0 20601.0 20602.0 20603.0 20604.0 20605.0 20606.0 20607.0 20608.0 20609.0 20610.0 20611.0 20612.0 20613.0 20614.0 20615.0 20616.0 20617.0 20618.0 20619.0 20620.0 20621.0 20622.0 20623.0 20624.0 20625.0 20626.0 20627.0 20628.0 20629.0 20630.0 20631.0 20632.0 20633.0 20634.0 20635.0 20636.0 20637.0 20638.0 20639.0 20640.0 20641.0 20642.0 20643.0 20644.0 20645.0 20646.0 20647.0 20648.0 20649.0 20650.0 20651.0 20652.0 20653.0 20654.0 20655.0 20656.0 20657.0 20658.0 20659.0 20660.0 20661.0 20662.0 20663.0 20664.0 20665.0 20666.0 20667.0 20668.0 20669.0 20670.0 20671.0 20672.0 20673.0 20674.0 20675.0 20676.0 20677.0 20678.0 20679.0 20680.0 20681.0 20682.0 20683.0 20684.0 20685.0 20686.0 20687.0 20688.0 20689.0 20690.0 20691.0 20692.0 20693.0 20694.0 20695.0 20696.0 20697.0 20698.0 20699.0 20700.0 20701.0 20702.0 20703.0 20704.0 20705.0 20706.0 20707.0 20708.0 20709.0 20710.0 20711.0 20712.0 20713.0 20714.0 20715.0 20716.0 20717.0 20718.0 20719.0 20720.0 20721.0 20722.0 20723.0 20724.0 20725.0 20726.0 20727.0 20728.0 20729.0 20730.0 20731.0 20732.0 20733.0 20734.0 20735.0 20736.0 20737.0 20738.0 20739.0 20740.0 20741.0 20742.0 20743.0 20744.0 20745.0 20746.0 20747.0 20748.0 20749.0 20750.0 20751.0 20752.0 20753.0 20754.0 20755.0 20756.0 20757.0 20758.0 20759.0 20760.0 20761.0 20762.0 20763.0 20764.0 20765.0 20766.0 20767.0 20768.0 20769.0 20770.0 20771.0 20772.0 20773.0 20774.0 20775.0 20776.0 20777.0 20778.0 20779.0 20780.0 20781.0 20782.0 20783.0 20784.0 20785.0 20786.0 20787.0 20788.0 20789.0 20790.0 20791.0 20792.0 20793.0 20794.0 20795.0 20796.0 20797.0 20798.0 20799.0 20800.0 20801.0 20802.0 20803.0 20804.0 20805.0 20806.0 20807.0 20808.0 20809.0 20810.0 20811.0 20812.0 20813.0 20814.0 20815.0 20816.0 20817.0 20818.0 20819.0 20820.0 20821.0 20822.0 20823.0 20824.0 20825.0 20826.0 20827.0 20828.0 20829.0 20830.0 20831.0 20832.0 20833.0 20834.0 20835.0 20836.0 20837.0 20838.0 20839.0 20840.0 20841.0 20842.0 20843.0 20844.0 20845.0 20846.0 20847.0 20848.0 20849.0 20850.0 20851.0 20852.0 20853.0 20854.0 20855.0 20856.0 20857.0 20858.0 20859.0 20860.0 20861.0 20862.0 20863.0 20864.0 20865.0 20866.0 20867.0 20868.0 20869.0 20870.0 20871.0 20872.0 20873.0 20874.0 20875.0 20876.0 20877.0 20878.0 20879.0 20880.0 20881.0 20882.0 20883.0 20884.0 20885.0 20886.0 20887.0 20888.0 20889.0 20890.0 20891.0 20892.0 20893.0 20894.0 20895.0 20896.0 20897.0 20898.0 20899.0 20900.0 20901.0 20902.0 20903.0 20904.0 20905.0 20906.0 20907.0 20908.0 20909.0 20910.0 20911.0 20912.0 20913.0 20914.0 20915.0 20916.0 20917.0 20918.0 20919.0 20920.0 20921.0 20922.0 20923.0 20924.0 20925.0 20926.0 20927.0 20928.0 20929.0 20930.0 20931.0 20932.0 20933.0 20934.0 20935.0 20936.0 20937.0 20938.0 20939.0 20940.0 20941.0 20942.0 20943.0 20944.0 20945.0 20946.0 20947.0 20948.0 20949.0 20950.0 20951.0 20952.0 20953.0 20954.0 20955.0 20956.0 20957.0 20958.0 20959.0 20960.0 20961.0 20962.0 20963.0 20964.0 20965.0 20966.0 20967.0 20968.0 20969.0 20970.0 20971.0 20972.0 20973.0 20974.0 20975.0 20976.0 20977.0 20978.0 20979.0 20980.0 20981.0 20982.0 20983.0 20984.0 20985.0 20986.0 20987.0 20988.0 20989.0 20990.0 20991.0 20992.0 20993.0 20994.0 20995.0 20996.0 20997.0 20998.0 20999.0 21000.0 21001.0 21002.0 21003.0 21004.0 21005.0 21006.0 21007.0 21008.0 21009.0 21010.0 21011.0 21012.0 21013.0 21014.0 21015.0 21016.0 21017.0 21018.0 21019.0 21020.0 21021.0 21022.0 21023.0 21024.0 21025.0 21026.0 21027.0 21028.0 21029.0 21030.0 21031.0 21032.0 21033.0 21034.0 21035.0 21036.0 21037.0 21038.0 21039.0 21040.0 21041.0 21042.0 21043.0 21044.0 21045.0 21046.0 21047.0 21048.0 21049.0 21050.0 21051.0 21052.0 21053.0 21054.0 21055.0 21056.0 21057.0 21058.0 21059.0 21060.0 21061.0 21062.0 21063.0 21064.0 21065.0 21066.0 21067.0 21068.0 21069.0 21070.0 21071.0 21072.0 21073.0 21074.0 21075.0 21076.0 21077.0 21078.0 21079.0 21080.0 21081.0 21082.0 21083.0 21084.0 21085.0 21086.0 21087.0 21088.0 21089.0 21090.0 21091.0 21092.0 21093.0 21094.0 21095.0 21096.0 21097.0 21098.0 21099.0 21100.0 21101.0 21102.0 21103.0 21104.0 21105.0 21106.0 21107.0 21108.0 21109.0 21110.0 21111.0 21112.0 21113.0 21114.0 21115.0 21116.0 21117.0 21118.0 21119.0 21120.0 21121.0 21122.0 21123.0 21124.0 21125.0 21126.0 21127.0 21128.0 21129.0 21130.0 21131.0 21132.0 21133.0 21134.0 21135.0 21136.0 21137.0 21138.0 21139.0 21140.0 21141.0 21142.0 21143.0 21144.0 21145.0 21146.0 21147.0 21148.0 21149.0 21150.0 21151.0 21152.0 21153.0 21154.0 21155.0 21156.0 21157.0 21158.0 21159.0 21160.0 21161.0 21162.0 21163.0 21164.0 21165.0 21166.0 21167.0 21168.0 21169.0 21170.0 21171.0 21172.0 21173.0 21174.0 21175.0 21176.0 21177.0 21178.0 21179.0 21180.0 21181.0 21182.0 21183.0 21184.0 21185.0 21186.0 21187.0 21188.0 21189.0 21190.0 21191.0 21192.0 21193.0 21194.0 21195.0 21196.0 21197.0 21198.0 21199.0 21200.0 21201.0 21202.0 21203.0 21204.0 21205.0 21206.0 21207.0 21208.0 21209.0 21210.0 21211.0 21212.0 21213.0 21214.0 21215.0 21216.0 21217.0 21218.0 21219.0 21220.0 21221.0 21222.0 21223.0 21224.0 21225.0 21226.0 21227.0 21228.0 21229.0 21230.0 21231.0 21232.0 21233.0 21234.0 21235.0 21236.0 21237.0 21238.0 21239.0 21240.0 21241.0 21242.0 21243.0 21244.0 21245.0 21246.0 21247.0 21248.0 21249.0 21250.0 21251.0 21252.0 21253.0 21254.0 21255.0 21256.0 21257.0 21258.0 21259.0 21260.0 21261.0 21262.0 21263.0 21264.0 21265.0 21266.0 21267.0 21268.0 21269.0 21270.0 21271.0 21272.0 21273.0 21274.0 21275.0 21276.0 21277.0 21278.0 21279.0 21280.0 21281.0 21282.0 21283.0 21284.0 21285.0 21286.0 21287.0 21288.0 21289.0 21290.0 21291.0 21292.0 21293.0 21294.0 21295.0 21296.0 21297.0 21298.0 21299.0 21300.0 21301.0 21302.0 21303.0 21304.0 21305.0 21306.0 21307.0 21308.0 21309.0 21310.0 21311.0 21312.0 21313.0 21314.0 21315.0 21316.0 21317.0 21318.0 21319.0 21320.0 21321.0 21322.0 21323.0 21324.0 21325.0 21326.0 21327.0 21328.0 21329.0 21330.0 21331.0 21332.0 21333.0 21334.0 21335.0 21336.0 21337.0 21338.0 21339.0 21340.0 21341.0 21342.0 21343.0 21344.0 21345.0 21346.0 21347.0 21348.0 21349.0 21350.0 21351.0 21352.0 21353.0 21354.0 21355.0 21356.0 21357.0 21358.0 21359.0 21360.0 21361.0 21362.0 21363.0 21364.0 21365.0 21366.0 21367.0 21368.0 21369.0 21370.0 21371.0 21372.0 21373.0 21374.0 21375.0 21376.0 21377.0 21378.0 21379.0 21380.0 21381.0 21382.0 21383.0 21384.0 21385.0 21386.0 21387.0 21388.0 21389.0 21390.0 21391.0 21392.0 21393.0 21394.0 21395.0 21396.0 21397.0 21398.0 21399.0 21400.0 21401.0 21402.0 21403.0 21404.0 21405.0 21406.0 21407.0 21408.0 21409.0 21410.0 21411.0 21412.0 21413.0 21414.0 21415.0 21416.0 21417.0 21418.0 21419.0 21420.0 21421.0 21422.0 21423.0 21424.0 21425.0 21426.0 21427.0 21428.0 21429.0 21430.0 21431.0 21432.0 21433.0 21434.0 21435.0 21436.0 21437.0 21438.0 21439.0 21440.0 21441.0 21442.0 21443.0 21444.0 21445.0 21446.0 21447.0 21448.0 21449.0 21450.0 21451.0 21452.0 21453.0 21454.0 21455.0 21456.0 21457.0 21458.0 21459.0 21460.0 21461.0 21462.0 21463.0 21464.0 21465.0 21466.0 21467.0 21468.0 21469.0 21470.0 21471.0 21472.0 21473.0 21474.0 21475.0 21476.0 21477.0 21478.0 21479.0 21480.0 21481.0 21482.0 21483.0 21484.0 21485.0 21486.0 21487.0 21488.0 21489.0 21490.0 21491.0 21492.0 21493.0 21494.0 21495.0 21496.0 21497.0 21498.0 21499.0 21500.0 21501.0 21502.0 21503.0 21504.0 21505.0 21506.0 21507.0 21508.0 21509.0 21510.0 21511.0 21512.0 21513.0 21514.0 21515.0 21516.0 21517.0 21518.0 21519.0 21520.0 21521.0 21522.0 21523.0 21524.0 21525.0 21526.0 21527.0 21528.0 21529.0 21530.0 21531.0 21532.0 21533.0 21534.0 21535.0 21536.0 21537.0 21538.0 21539.0 21540.0 21541.0 21542.0 21543.0 21544.0 21545.0 21546.0 21547.0 21548.0 21549.0 21550.0 21551.0 21552.0 21553.0 21554.0 21555.0 21556.0 21557.0 21558.0 21559.0 21560.0 21561.0 21562.0 21563.0 21564.0 21565.0 21566.0 21567.0 21568.0 21569.0 21570.0 21571.0 21572.0 21573.0 21574.0 21575.0 21576.0 21577.0 21578.0 21579.0 21580.0 21581.0 21582.0 21583.0 21584.0 21585.0 21586.0 21587.0 21588.0 21589.0 21590.0 21591.0 21592.0 21593.0 21594.0 21595.0 21596.0 21597.0 21598.0 21599.0 21600.0 21601.0 21602.0 21603.0 21604.0 21605.0 21606.0 21607.0 21608.0 21609.0 21610.0 21611.0 21612.0 21613.0 21614.0 21615.0 21616.0 21617.0 21618.0 21619.0 21620.0 21621.0 21622.0 21623.0 21624.0 21625.0 21626.0 21627.0 21628.0 21629.0 21630.0 21631.0 21632.0 21633.0 21634.0 21635.0 21636.0 21637.0 21638.0 21639.0 21640.0 21641.0 21642.0 21643.0 21644.0 21645.0 21646.0 21647.0 21648.0 21649.0 21650.0 21651.0 21652.0 21653.0 21654.0 21655.0 21656.0 21657.0 21658.0 21659.0 21660.0 21661.0 21662.0 21663.0 21664.0 21665.0 21666.0 21667.0 21668.0 21669.0 21670.0 21671.0 21672.0 21673.0 21674.0 21675.0 21676.0 21677.0 21678.0 21679.0 21680.0 21681.0 21682.0 21683.0 21684.0 21685.0 21686.0 21687.0 21688.0 21689.0 21690.0 21691.0 21692.0 21693.0 21694.0 21695.0 21696.0 21697.0 21698.0 21699.0 21700.0 21701.0 21702.0 21703.0 21704.0 21705.0 21706.0 21707.0 21708.0 21709.0 21710.0 21711.0 21712.0 21713.0 21714.0 21715.0 21716.0 21717.0 21718.0 21719.0 21720.0 21721.0 21722.0 21723.0 21724.0 21725.0 21726.0 21727.0 21728.0 21729.0 21730.0 21731.0 21732.0 21733.0 21734.0 21735.0 21736.0 21737.0 21738.0 21739.0 21740.0 21741.0 21742.0 21743.0 21744.0 21745.0 21746.0 21747.0 21748.0 21749.0 21750.0 21751.0 21752.0 21753.0 21754.0 21755.0 21756.0 21757.0 21758.0 21759.0 21760.0 21761.0 21762.0 21763.0 21764.0 21765.0 21766.0 21767.0 21768.0 21769.0 21770.0 21771.0 21772.0 21773.0 21774.0 21775.0 21776.0 21777.0 21778.0 21779.0 21780.0 21781.0 21782.0 21783.0 21784.0 21785.0 21786.0 21787.0 21788.0 21789.0 21790.0 21791.0 21792.0 21793.0 21794.0 21795.0 21796.0 21797.0 21798.0 21799.0 21800.0 21801.0 21802.0 21803.0 21804.0 21805.0 21806.0 21807.0 21808.0 21809.0 21810.0 21811.0 21812.0 21813.0 21814.0 21815.0 21816.0 21817.0 21818.0 21819.0 21820.0 21821.0 21822.0 21823.0 21824.0 21825.0 21826.0 21827.0 21828.0 21829.0 21830.0 21831.0 21832.0 21833.0 21834.0 21835.0 21836.0 21837.0 21838.0 21839.0 21840.0 21841.0 21842.0 21843.0 21844.0 21845.0 21846.0 21847.0 21848.0 21849.0 21850.0 21851.0 21852.0 21853.0 21854.0 21855.0 21856.0 21857.0 21858.0 21859.0 21860.0 21861.0 21862.0 21863.0 21864.0 21865.0 21866.0 21867.0 21868.0 21869.0 21870.0 21871.0 21872.0 21873.0 21874.0 21875.0 21876.0 21877.0 21878.0 21879.0 21880.0 21881.0 21882.0 21883.0 21884.0 21885.0 21886.0 21887.0 21888.0 21889.0 21890.0 21891.0 21892.0 21893.0 21894.0 21895.0 21896.0 21897.0 21898.0 21899.0 21900.0 21901.0 21902.0 21903.0 21904.0 21905.0 21906.0 21907.0 21908.0 21909.0 21910.0 21911.0 21912.0 21913.0 21914.0 21915.0 21916.0 21917.0 21918.0 21919.0 21920.0 21921.0 21922.0 21923.0 21924.0 21925.0 21926.0 21927.0 21928.0 21929.0 21930.0 21931.0 21932.0 21933.0 21934.0 21935.0 21936.0 21937.0 21938.0 21939.0 21940.0 21941.0 21942.0 21943.0 21944.0 21945.0 21946.0 21947.0 21948.0 21949.0 21950.0 21951.0 21952.0 21953.0 21954.0 21955.0 21956.0 21957.0 21958.0 21959.0 21960.0 21961.0 21962.0 21963.0 21964.0 21965.0 21966.0 21967.0 21968.0 21969.0 21970.0 21971.0 21972.0 21973.0 21974.0 21975.0 21976.0 21977.0 21978.0 21979.0 21980.0 21981.0 21982.0 21983.0 21984.0 21985.0 21986.0 21987.0 21988.0 21989.0 21990.0 21991.0 21992.0 21993.0 21994.0 21995.0 21996.0 21997.0 21998.0 21999.0 22000.0 22001.0 22002.0 22003.0 22004.0 22005.0 22006.0 22007.0 22008.0 22009.0 22010.0 22011.0 22012.0 22013.0 22014.0 22015.0 22016.0 22017.0 22018.0 22019.0 22020.0 22021.0 22022.0 22023.0 22024.0 22025.0 22026.0 22027.0 22028.0 22029.0 22030.0 22031.0 22032.0 22033.0 22034.0 22035.0 22036.0 22037.0 22038.0 22039.0 22040.0 22041.0 22042.0 22043.0 22044.0 22045.0 22046.0 22047.0 22048.0 22049.0 22050.0 22051.0 22052.0 22053.0 22054.0 22055.0 22056.0 22057.0 22058.0 22059.0 22060.0 22061.0 22062.0 22063.0 22064.0 22065.0 22066.0 22067.0 22068.0 22069.0 22070.0 22071.0 22072.0 22073.0 22074.0 22075.0 22076.0 22077.0 22078.0 22079.0 22080.0 22081.0 22082.0 22083.0 22084.0 22085.0 22086.0 22087.0 22088.0 22089.0 22090.0 22091.0 22092.0 22093.0 22094.0 22095.0 22096.0 22097.0 22098.0 22099.0 22100.0 22101.0 22102.0 22103.0 22104.0 22105.0 22106.0 22107.0 22108.0 22109.0 22110.0 22111.0 22112.0 22113.0 22114.0 22115.0 22116.0 22117.0 22118.0 22119.0 22120.0 22121.0 22122.0 22123.0 22124.0 22125.0 22126.0 22127.0 22128.0 22129.0 22130.0 22131.0 22132.0 22133.0 22134.0 22135.0 22136.0 22137.0 22138.0 22139.0 22140.0 22141.0 22142.0 22143.0 22144.0 22145.0 22146.0 22147.0 22148.0 22149.0 22150.0 22151.0 22152.0 22153.0 22154.0 22155.0 22156.0 22157.0 22158.0 22159.0 22160.0 22161.0 22162.0 22163.0 22164.0 22165.0 22166.0 22167.0 22168.0 22169.0 22170.0 22171.0 22172.0 22173.0 22174.0 22175.0 22176.0 22177.0 22178.0 22179.0 22180.0 22181.0 22182.0 22183.0 22184.0 22185.0 22186.0 22187.0 22188.0 22189.0 22190.0 22191.0 22192.0 22193.0 22194.0 22195.0 22196.0 22197.0 22198.0 22199.0 22200.0 22201.0 22202.0 22203.0 22204.0 22205.0 22206.0 22207.0 22208.0 22209.0 22210.0 22211.0 22212.0 22213.0 22214.0 22215.0 22216.0 22217.0 22218.0 22219.0 22220.0 22221.0 22222.0 22223.0 22224.0 22225.0 22226.0 22227.0 22228.0 22229.0 22230.0 22231.0 22232.0 22233.0 22234.0 22235.0 22236.0 22237.0 22238.0 22239.0 22240.0 22241.0 22242.0 22243.0 22244.0 22245.0 22246.0 22247.0 22248.0 22249.0 22250.0 22251.0 22252.0 22253.0 22254.0 22255.0 22256.0 22257.0 22258.0 22259.0 22260.0 22261.0 22262.0 22263.0 22264.0 22265.0 22266.0 22267.0 22268.0 22269.0 22270.0 22271.0 22272.0 22273.0 22274.0 22275.0 22276.0 22277.0 22278.0 22279.0 22280.0 22281.0 22282.0 22283.0 22284.0 22285.0 22286.0 22287.0 22288.0 22289.0 22290.0 22291.0 22292.0 22293.0 22294.0 22295.0 22296.0 22297.0 22298.0 22299.0 22300.0 22301.0 22302.0 22303.0 22304.0 22305.0 22306.0 22307.0 22308.0 22309.0 22310.0 22311.0 22312.0 22313.0 22314.0 22315.0 22316.0 22317.0 22318.0 22319.0 22320.0 22321.0 22322.0 22323.0 22324.0 22325.0 22326.0 22327.0 22328.0 22329.0 22330.0 22331.0 22332.0 22333.0 22334.0 22335.0 22336.0 22337.0 22338.0 22339.0 22340.0 22341.0 22342.0 22343.0 22344.0 22345.0 22346.0 22347.0 22348.0 22349.0 22350.0 22351.0 22352.0 22353.0 22354.0 22355.0 22356.0 22357.0 22358.0 22359.0 22360.0 22361.0 22362.0 22363.0 22364.0 22365.0 22366.0 22367.0 22368.0 22369.0 22370.0 22371.0 22372.0 22373.0 22374.0 22375.0 22376.0 22377.0 22378.0 22379.0 22380.0 22381.0 22382.0 22383.0 22384.0 22385.0 22386.0 22387.0 22388.0 22389.0 22390.0 22391.0 22392.0 22393.0 22394.0 22395.0 22396.0 22397.0 22398.0 22399.0 22400.0 22401.0 22402.0 22403.0 22404.0 22405.0 22406.0 22407.0 22408.0 22409.0 22410.0 22411.0 22412.0 22413.0 22414.0 22415.0 22416.0 22417.0 22418.0 22419.0 22420.0 22421.0 22422.0 22423.0 22424.0 22425.0 22426.0 22427.0 22428.0 22429.0 22430.0 22431.0 22432.0 22433.0 22434.0 22435.0 22436.0 22437.0 22438.0 22439.0 22440.0 22441.0 22442.0 22443.0 22444.0 22445.0 22446.0 22447.0 22448.0 22449.0 22450.0 22451.0 22452.0 22453.0 22454.0 22455.0 22456.0 22457.0 22458.0 22459.0 22460.0 22461.0 22462.0 22463.0 22464.0 22465.0 22466.0 22467.0 22468.0 22469.0 22470.0 22471.0 22472.0 22473.0 22474.0 22475.0 22476.0 22477.0 22478.0 22479.0 22480.0 22481.0 22482.0 22483.0 22484.0 22485.0 22486.0 22487.0 22488.0 22489.0 22490.0 22491.0 22492.0 22493.0 22494.0 22495.0 22496.0 22497.0 22498.0 22499.0 22500.0 22501.0 22502.0 22503.0 22504.0 22505.0 22506.0 22507.0 22508.0 22509.0 22510.0 22511.0 22512.0 22513.0 22514.0 22515.0 22516.0 22517.0 22518.0 22519.0 22520.0 22521.0 22522.0 22523.0 22524.0 22525.0 22526.0 22527.0 22528.0 22529.0 22530.0 22531.0 22532.0 22533.0 22534.0 22535.0 22536.0 22537.0 22538.0 22539.0 22540.0 22541.0 22542.0 22543.0 22544.0 22545.0 22546.0 22547.0 22548.0 22549.0 22550.0 22551.0 22552.0 22553.0 22554.0 22555.0 22556.0 22557.0 22558.0 22559.0 22560.0 22561.0 22562.0 22563.0 22564.0 22565.0 22566.0 22567.0 22568.0 22569.0 22570.0 22571.0 22572.0 22573.0 22574.0 22575.0 22576.0 22577.0 22578.0 22579.0 22580.0 22581.0 22582.0 22583.0 22584.0 22585.0 22586.0 22587.0 22588.0 22589.0 22590.0 22591.0 22592.0 22593.0 22594.0 22595.0 22596.0 22597.0 22598.0 22599.0 22600.0 22601.0 22602.0 22603.0 22604.0 22605.0 22606.0 22607.0 22608.0 22609.0 22610.0 22611.0 22612.0 22613.0 22614.0 22615.0 22616.0 22617.0 22618.0 22619.0 22620.0 22621.0 22622.0 22623.0 22624.0 22625.0 22626.0 22627.0 22628.0 22629.0 22630.0 22631.0 22632.0 22633.0 22634.0 22635.0 22636.0 22637.0 22638.0 22639.0 22640.0 22641.0 22642.0 22643.0 22644.0 22645.0 22646.0 22647.0 22648.0 22649.0 22650.0 22651.0 22652.0 22653.0 22654.0 22655.0 22656.0 22657.0 22658.0 22659.0 22660.0 22661.0 22662.0 22663.0 22664.0 22665.0 22666.0 22667.0 22668.0 22669.0 22670.0 22671.0 22672.0 22673.0 22674.0 22675.0 22676.0 22677.0 22678.0 22679.0 22680.0 22681.0 22682.0 22683.0 22684.0 22685.0 22686.0 22687.0 22688.0 22689.0 22690.0 22691.0 22692.0 22693.0 22694.0 22695.0 22696.0 22697.0 22698.0 22699.0 22700.0 22701.0 22702.0 22703.0 22704.0 22705.0 22706.0 22707.0 22708.0 22709.0 22710.0 22711.0 22712.0 22713.0 22714.0 22715.0 22716.0 22717.0 22718.0 22719.0 22720.0 22721.0 22722.0 22723.0 22724.0 22725.0 22726.0 22727.0 22728.0 22729.0 22730.0 22731.0 22732.0 22733.0 22734.0 22735.0 22736.0 22737.0 22738.0 22739.0 22740.0 22741.0 22742.0 22743.0 22744.0 22745.0 22746.0 22747.0 22748.0 22749.0 22750.0 22751.0 22752.0 22753.0 22754.0 22755.0 22756.0 22757.0 22758.0 22759.0 22760.0 22761.0 22762.0 22763.0 22764.0 22765.0 22766.0 22767.0 22768.0 22769.0 22770.0 22771.0 22772.0 22773.0 22774.0 22775.0 22776.0 22777.0 22778.0 22779.0 22780.0 22781.0 22782.0 22783.0 22784.0 22785.0 22786.0 22787.0 22788.0 22789.0 22790.0 22791.0 22792.0 22793.0 22794.0 22795.0 22796.0 22797.0 22798.0 22799.0 22800.0 22801.0 22802.0 22803.0 22804.0 22805.0 22806.0 22807.0 22808.0 22809.0 22810.0 22811.0 22812.0 22813.0 22814.0 22815.0 22816.0 22817.0 22818.0 22819.0 22820.0 22821.0 22822.0 22823.0 22824.0 22825.0 22826.0 22827.0 22828.0 22829.0 22830.0 22831.0 22832.0 22833.0 22834.0 22835.0 22836.0 22837.0 22838.0 22839.0 22840.0 22841.0 22842.0 22843.0 22844.0 22845.0 22846.0 22847.0 22848.0 22849.0 22850.0 22851.0 22852.0 22853.0 22854.0 22855.0 22856.0 22857.0 22858.0 22859.0 22860.0 22861.0 22862.0 22863.0 22864.0 22865.0 22866.0 22867.0 22868.0 22869.0 22870.0 22871.0 22872.0 22873.0 22874.0 22875.0 22876.0 22877.0 22878.0 22879.0 22880.0 22881.0 22882.0 22883.0 22884.0 22885.0 22886.0 22887.0 22888.0 22889.0 22890.0 22891.0 22892.0 22893.0 22894.0 22895.0 22896.0 22897.0 22898.0 22899.0 22900.0 22901.0 22902.0 22903.0 22904.0 22905.0 22906.0 22907.0 22908.0 22909.0 22910.0 22911.0 22912.0 22913.0 22914.0 22915.0 22916.0 22917.0 22918.0 22919.0 22920.0 22921.0 22922.0 22923.0 22924.0 22925.0 22926.0 22927.0 22928.0 22929.0 22930.0 22931.0 22932.0 22933.0 22934.0 22935.0 22936.0 22937.0 22938.0 22939.0 22940.0 22941.0 22942.0 22943.0 22944.0 22945.0 22946.0 22947.0 22948.0 22949.0 22950.0 22951.0 22952.0 22953.0 22954.0 22955.0 22956.0 22957.0 22958.0 22959.0 22960.0 22961.0 22962.0 22963.0 22964.0 22965.0 22966.0 22967.0 22968.0 22969.0 22970.0 22971.0 22972.0 22973.0 22974.0 22975.0 22976.0 22977.0 22978.0 22979.0 22980.0 22981.0 22982.0 22983.0 22984.0 22985.0 22986.0 22987.0 22988.0 22989.0 22990.0 22991.0 22992.0 22993.0 22994.0 22995.0 22996.0 22997.0 22998.0 22999.0 23000.0 23001.0 23002.0 23003.0 23004.0 23005.0 23006.0 23007.0 23008.0 23009.0 23010.0 23011.0 23012.0 23013.0 23014.0 23015.0 23016.0 23017.0 23018.0 23019.0 23020.0 23021.0 23022.0 23023.0 23024.0 23025.0 23026.0 23027.0 23028.0 23029.0 23030.0 23031.0 23032.0 23033.0 23034.0 23035.0 23036.0 23037.0 23038.0 23039.0 23040.0 23041.0 23042.0 23043.0 23044.0 23045.0 23046.0 23047.0 23048.0 23049.0 23050.0 23051.0 23052.0 23053.0 23054.0 23055.0 23056.0 23057.0 23058.0 23059.0 23060.0 23061.0 23062.0 23063.0 23064.0 23065.0 23066.0 23067.0 23068.0 23069.0 23070.0 23071.0 23072.0 23073.0 23074.0 23075.0 23076.0 23077.0 23078.0 23079.0 23080.0 23081.0 23082.0 23083.0 23084.0 23085.0 23086.0 23087.0 23088.0 23089.0 23090.0 23091.0 23092.0 23093.0 23094.0 23095.0 23096.0 23097.0 23098.0 23099.0 23100.0 23101.0 23102.0 23103.0 23104.0 23105.0 23106.0 23107.0 23108.0 23109.0 23110.0 23111.0 23112.0 23113.0 23114.0 23115.0 23116.0 23117.0 23118.0 23119.0 23120.0 23121.0 23122.0 23123.0 23124.0 23125.0 23126.0 23127.0 23128.0 23129.0 23130.0 23131.0 23132.0 23133.0 23134.0 23135.0 23136.0 23137.0 23138.0 23139.0 23140.0 23141.0 23142.0 23143.0 23144.0 23145.0 23146.0 23147.0 23148.0 23149.0 23150.0 23151.0 23152.0 23153.0 23154.0 23155.0 23156.0 23157.0 23158.0 23159.0 23160.0 23161.0 23162.0 23163.0 23164.0 23165.0 23166.0 23167.0 23168.0 23169.0 23170.0 23171.0 23172.0 23173.0 23174.0 23175.0 23176.0 23177.0 23178.0 23179.0 23180.0 23181.0 23182.0 23183.0 23184.0 23185.0 23186.0 23187.0 23188.0 23189.0 23190.0 23191.0 23192.0 23193.0 23194.0 23195.0 23196.0 23197.0 23198.0 23199.0 23200.0 23201.0 23202.0 23203.0 23204.0 23205.0 23206.0 23207.0 23208.0 23209.0 23210.0 23211.0 23212.0 23213.0 23214.0 23215.0 23216.0 23217.0 23218.0 23219.0 23220.0 23221.0 23222.0 23223.0 23224.0 23225.0 23226.0 23227.0 23228.0 23229.0 23230.0 23231.0 23232.0 23233.0 23234.0 23235.0 23236.0 23237.0 23238.0 23239.0 23240.0 23241.0 23242.0 23243.0 23244.0 23245.0 23246.0 23247.0 23248.0 23249.0 23250.0 23251.0 23252.0 23253.0 23254.0 23255.0 23256.0 23257.0 23258.0 23259.0 23260.0 23261.0 23262.0 23263.0 23264.0 23265.0 23266.0 23267.0 23268.0 23269.0 23270.0 23271.0 23272.0 23273.0 23274.0 23275.0 23276.0 23277.0 23278.0 23279.0 23280.0 23281.0 23282.0 23283.0 23284.0 23285.0 23286.0 23287.0 23288.0 23289.0 23290.0 23291.0 23292.0 23293.0 23294.0 23295.0 23296.0 23297.0 23298.0 23299.0 23300.0 23301.0 23302.0 23303.0 23304.0 23305.0 23306.0 23307.0 23308.0 23309.0 23310.0 23311.0 23312.0 23313.0 23314.0 23315.0 23316.0 23317.0 23318.0 23319.0 23320.0 23321.0 23322.0 23323.0 23324.0 23325.0 23326.0 23327.0 23328.0 23329.0 23330.0 23331.0 23332.0 23333.0 23334.0 23335.0 23336.0 23337.0 23338.0 23339.0 23340.0 23341.0 23342.0 23343.0 23344.0 23345.0 23346.0 23347.0 23348.0 23349.0 23350.0 23351.0 23352.0 23353.0 23354.0 23355.0 23356.0 23357.0 23358.0 23359.0 23360.0 23361.0 23362.0 23363.0 23364.0 23365.0 23366.0 23367.0 23368.0 23369.0 23370.0 23371.0 23372.0 23373.0 23374.0 23375.0 23376.0 23377.0 23378.0 23379.0 23380.0 23381.0 23382.0 23383.0 23384.0 23385.0 23386.0 23387.0 23388.0 23389.0 23390.0 23391.0 23392.0 23393.0 23394.0 23395.0 23396.0 23397.0 23398.0 23399.0 23400.0 23401.0 23402.0 23403.0 23404.0 23405.0 23406.0 23407.0 23408.0 23409.0 23410.0 23411.0 23412.0 23413.0 23414.0 23415.0 23416.0 23417.0 23418.0 23419.0 23420.0 23421.0 23422.0 23423.0 23424.0 23425.0 23426.0 23427.0 23428.0 23429.0 23430.0 23431.0 23432.0 23433.0 23434.0 23435.0 23436.0 23437.0 23438.0 23439.0 23440.0 23441.0 23442.0 23443.0 23444.0 23445.0 23446.0 23447.0 23448.0 23449.0 23450.0 23451.0 23452.0 23453.0 23454.0 23455.0 23456.0 23457.0 23458.0 23459.0 23460.0 23461.0 23462.0 23463.0 23464.0 23465.0 23466.0 23467.0 23468.0 23469.0 23470.0 23471.0 23472.0 23473.0 23474.0 23475.0 23476.0 23477.0 23478.0 23479.0 23480.0 23481.0 23482.0 23483.0 23484.0 23485.0 23486.0 23487.0 23488.0 23489.0 23490.0 23491.0 23492.0 23493.0 23494.0 23495.0 23496.0 23497.0 23498.0 23499.0 23500.0 23501.0 23502.0 23503.0 23504.0 23505.0 23506.0 23507.0 23508.0 23509.0 23510.0 23511.0 23512.0 23513.0 23514.0 23515.0 23516.0 23517.0 23518.0 23519.0 23520.0 23521.0 23522.0 23523.0 23524.0 23525.0 23526.0 23527.0 23528.0 23529.0 23530.0 23531.0 23532.0 23533.0 23534.0 23535.0 23536.0 23537.0 23538.0 23539.0 23540.0 23541.0 23542.0 23543.0 23544.0 23545.0 23546.0 23547.0 23548.0 23549.0 23550.0 23551.0 23552.0 23553.0 23554.0 23555.0 23556.0 23557.0 23558.0 23559.0 23560.0 23561.0 23562.0 23563.0 23564.0 23565.0 23566.0 23567.0 23568.0 23569.0 23570.0 23571.0 23572.0 23573.0 23574.0 23575.0 23576.0 23577.0 23578.0 23579.0 23580.0 23581.0 23582.0 23583.0 23584.0 23585.0 23586.0 23587.0 23588.0 23589.0 23590.0 23591.0 23592.0 23593.0 23594.0 23595.0 23596.0 23597.0 23598.0 23599.0 23600.0 23601.0 23602.0 23603.0 23604.0 23605.0 23606.0 23607.0 23608.0 23609.0 23610.0 23611.0 23612.0 23613.0 23614.0 23615.0 23616.0 23617.0 23618.0 23619.0 23620.0 23621.0 23622.0 23623.0 23624.0 23625.0 23626.0 23627.0 23628.0 23629.0 23630.0 23631.0 23632.0 23633.0 23634.0 23635.0 23636.0 23637.0 23638.0 23639.0 23640.0 23641.0 23642.0 23643.0 23644.0 23645.0 23646.0 23647.0 23648.0 23649.0 23650.0 23651.0 23652.0 23653.0 23654.0 23655.0 23656.0 23657.0 23658.0 23659.0 23660.0 23661.0 23662.0 23663.0 23664.0 23665.0 23666.0 23667.0 23668.0 23669.0 23670.0 23671.0 23672.0 23673.0 23674.0 23675.0 23676.0 23677.0 23678.0 23679.0 23680.0 23681.0 23682.0 23683.0 23684.0 23685.0 23686.0 23687.0 23688.0 23689.0 23690.0 23691.0 23692.0 23693.0 23694.0 23695.0 23696.0 23697.0 23698.0 23699.0 23700.0 23701.0 23702.0 23703.0 23704.0 23705.0 23706.0 23707.0 23708.0 23709.0 23710.0 23711.0 23712.0 23713.0 23714.0 23715.0 23716.0 23717.0 23718.0 23719.0 23720.0 23721.0 23722.0 23723.0 23724.0 23725.0 23726.0 23727.0 23728.0 23729.0 23730.0 23731.0 23732.0 23733.0 23734.0 23735.0 23736.0 23737.0 23738.0 23739.0 23740.0 23741.0 23742.0 23743.0 23744.0 23745.0 23746.0 23747.0 23748.0 23749.0 23750.0 23751.0 23752.0 23753.0 23754.0 23755.0 23756.0 23757.0 23758.0 23759.0 23760.0 23761.0 23762.0 23763.0 23764.0 23765.0 23766.0 23767.0 23768.0 23769.0 23770.0 23771.0 23772.0 23773.0 23774.0 23775.0 23776.0 23777.0 23778.0 23779.0 23780.0 23781.0 23782.0 23783.0 23784.0 23785.0 23786.0 23787.0 23788.0 23789.0 23790.0 23791.0 23792.0 23793.0 23794.0 23795.0 23796.0 23797.0 23798.0 23799.0 23800.0 23801.0 23802.0 23803.0 23804.0 23805.0 23806.0 23807.0 23808.0 23809.0 23810.0 23811.0 23812.0 23813.0 23814.0 23815.0 23816.0 23817.0 23818.0 23819.0 23820.0 23821.0 23822.0 23823.0 23824.0 23825.0 23826.0 23827.0 23828.0 23829.0 23830.0 23831.0 23832.0 23833.0 23834.0 23835.0 23836.0 23837.0 23838.0 23839.0 23840.0 23841.0 23842.0 23843.0 23844.0 23845.0 23846.0 23847.0 23848.0 23849.0 23850.0 23851.0 23852.0 23853.0 23854.0 23855.0 23856.0 23857.0 23858.0 23859.0 23860.0 23861.0 23862.0 23863.0 23864.0 23865.0 23866.0 23867.0 23868.0 23869.0 23870.0 23871.0 23872.0 23873.0 23874.0 23875.0 23876.0 23877.0 23878.0 23879.0 23880.0 23881.0 23882.0 23883.0 23884.0 23885.0 23886.0 23887.0 23888.0 23889.0 23890.0 23891.0 23892.0 23893.0 23894.0 23895.0 23896.0 23897.0 23898.0 23899.0 23900.0 23901.0 23902.0 23903.0 23904.0 23905.0 23906.0 23907.0 23908.0 23909.0 23910.0 23911.0 23912.0 23913.0 23914.0 23915.0 23916.0 23917.0 23918.0 23919.0 23920.0 23921.0 23922.0 23923.0 23924.0 23925.0 23926.0 23927.0 23928.0 23929.0 23930.0 23931.0 23932.0 23933.0 23934.0 23935.0 23936.0 23937.0 23938.0 23939.0 23940.0 23941.0 23942.0 23943.0 23944.0 23945.0 23946.0 23947.0 23948.0 23949.0 23950.0 23951.0 23952.0 23953.0 23954.0 23955.0 23956.0 23957.0 23958.0 23959.0 23960.0 23961.0 23962.0 23963.0 23964.0 23965.0 23966.0 23967.0 23968.0 23969.0 23970.0 23971.0 23972.0 23973.0 23974.0 23975.0 23976.0 23977.0 23978.0 23979.0 23980.0 23981.0 23982.0 23983.0 23984.0 23985.0 23986.0 23987.0 23988.0 23989.0 23990.0 23991.0 23992.0 23993.0 23994.0 23995.0 23996.0 23997.0 23998.0 23999.0 24000.0 24001.0 24002.0 24003.0 24004.0 24005.0 24006.0 24007.0 24008.0 24009.0 24010.0 24011.0 24012.0 24013.0 24014.0 24015.0 24016.0 24017.0 24018.0 24019.0 24020.0 24021.0 24022.0 24023.0 24024.0 24025.0 24026.0 24027.0 24028.0 24029.0 24030.0 24031.0 24032.0 24033.0 24034.0 24035.0 24036.0 24037.0 24038.0 24039.0 24040.0 24041.0 24042.0 24043.0 24044.0 24045.0 24046.0 24047.0 24048.0 24049.0 24050.0 24051.0 24052.0 24053.0 24054.0 24055.0 24056.0 24057.0 24058.0 24059.0 24060.0 24061.0 24062.0 24063.0 24064.0 24065.0 24066.0 24067.0 24068.0 24069.0 24070.0 24071.0 24072.0 24073.0 24074.0 24075.0 24076.0 24077.0 24078.0 24079.0 24080.0 24081.0 24082.0 24083.0 24084.0 24085.0 24086.0 24087.0 24088.0 24089.0 24090.0 24091.0 24092.0 24093.0 24094.0 24095.0 24096.0 24097.0 24098.0 24099.0 24100.0 24101.0 24102.0 24103.0 24104.0 24105.0 24106.0 24107.0 24108.0 24109.0 24110.0 24111.0 24112.0 24113.0 24114.0 24115.0 24116.0 24117.0 24118.0 24119.0 24120.0 24121.0 24122.0 24123.0 24124.0 24125.0 24126.0 24127.0 24128.0 24129.0 24130.0 24131.0 24132.0 24133.0 24134.0 24135.0 24136.0 24137.0 24138.0 24139.0 24140.0 24141.0 24142.0 24143.0 24144.0 24145.0 24146.0 24147.0 24148.0 24149.0 24150.0 24151.0 24152.0 24153.0 24154.0 24155.0 24156.0 24157.0 24158.0 24159.0 24160.0 24161.0 24162.0 24163.0 24164.0 24165.0 24166.0 24167.0 24168.0 24169.0 24170.0 24171.0 24172.0 24173.0 24174.0 24175.0 24176.0 24177.0 24178.0 24179.0 24180.0 24181.0 24182.0 24183.0 24184.0 24185.0 24186.0 24187.0 24188.0 24189.0 24190.0 24191.0 24192.0 24193.0 24194.0 24195.0 24196.0 24197.0 24198.0 24199.0 24200.0 24201.0 24202.0 24203.0 24204.0 24205.0 24206.0 24207.0 24208.0 24209.0 24210.0 24211.0 24212.0 24213.0 24214.0 24215.0 24216.0 24217.0 24218.0 24219.0 24220.0 24221.0 24222.0 24223.0 24224.0 24225.0 24226.0 24227.0 24228.0 24229.0 24230.0 24231.0 24232.0 24233.0 24234.0 24235.0 24236.0 24237.0 24238.0 24239.0 24240.0 24241.0 24242.0 24243.0 24244.0 24245.0 24246.0 24247.0 24248.0 24249.0 24250.0 24251.0 24252.0 24253.0 24254.0 24255.0 24256.0 24257.0 24258.0 24259.0 24260.0 24261.0 24262.0 24263.0 24264.0 24265.0 24266.0 24267.0 24268.0 24269.0 24270.0 24271.0 24272.0 24273.0 24274.0 24275.0 24276.0 24277.0 24278.0 24279.0 24280.0 24281.0 24282.0 24283.0 24284.0 24285.0 24286.0 24287.0 24288.0 24289.0 24290.0 24291.0 24292.0 24293.0 24294.0 24295.0 24296.0 24297.0 24298.0 24299.0 24300.0 24301.0 24302.0 24303.0 24304.0 24305.0 24306.0 24307.0 24308.0 24309.0 24310.0 24311.0 24312.0 24313.0 24314.0 24315.0 24316.0 24317.0 24318.0 24319.0 24320.0 24321.0 24322.0 24323.0 24324.0 24325.0 24326.0 24327.0 24328.0 24329.0 24330.0 24331.0 24332.0 24333.0 24334.0 24335.0 24336.0 24337.0 24338.0 24339.0 24340.0 24341.0 24342.0 24343.0 24344.0 24345.0 24346.0 24347.0 24348.0 24349.0 24350.0 24351.0 24352.0 24353.0 24354.0 24355.0 24356.0 24357.0 24358.0 24359.0 24360.0 24361.0 24362.0 24363.0 24364.0 24365.0 24366.0 24367.0 24368.0 24369.0 24370.0 24371.0 24372.0 24373.0 24374.0 24375.0 24376.0 24377.0 24378.0 24379.0 24380.0 24381.0 24382.0 24383.0 24384.0 24385.0 24386.0 24387.0 24388.0 24389.0 24390.0 24391.0 24392.0 24393.0 24394.0 24395.0 24396.0 24397.0 24398.0 24399.0 24400.0 24401.0 24402.0 24403.0 24404.0 24405.0 24406.0 24407.0 24408.0 24409.0 24410.0 24411.0 24412.0 24413.0 24414.0 24415.0 24416.0 24417.0 24418.0 24419.0 24420.0 24421.0 24422.0 24423.0 24424.0 24425.0 24426.0 24427.0 24428.0 24429.0 24430.0 24431.0 24432.0 24433.0 24434.0 24435.0 24436.0 24437.0 24438.0 24439.0 24440.0 24441.0 24442.0 24443.0 24444.0 24445.0 24446.0 24447.0 24448.0 24449.0 24450.0 24451.0 24452.0 24453.0 24454.0 24455.0 24456.0 24457.0 24458.0 24459.0 24460.0 24461.0 24462.0 24463.0 24464.0 24465.0 24466.0 24467.0 24468.0 24469.0 24470.0 24471.0 24472.0 24473.0 24474.0 24475.0 24476.0 24477.0 24478.0 24479.0 24480.0 24481.0 24482.0 24483.0 24484.0 24485.0 24486.0 24487.0 24488.0 24489.0 24490.0 24491.0 24492.0 24493.0 24494.0 24495.0 24496.0 24497.0 24498.0 24499.0 24500.0 24501.0 24502.0 24503.0 24504.0 24505.0 24506.0 24507.0 24508.0 24509.0 24510.0 24511.0 24512.0 24513.0 24514.0 24515.0 24516.0 24517.0 24518.0 24519.0 24520.0 24521.0 24522.0 24523.0 24524.0 24525.0 24526.0 24527.0 24528.0 24529.0 24530.0 24531.0 24532.0 24533.0 24534.0 24535.0 24536.0 24537.0 24538.0 24539.0 24540.0 24541.0 24542.0 24543.0 24544.0 24545.0 24546.0 24547.0 24548.0 24549.0 24550.0 24551.0 24552.0 24553.0 24554.0 24555.0 24556.0 24557.0 24558.0 24559.0 24560.0 24561.0 24562.0 24563.0 24564.0 24565.0 24566.0 24567.0 24568.0 24569.0 24570.0 24571.0 24572.0 24573.0 24574.0 24575.0 24576.0 24577.0 24578.0 24579.0 24580.0 24581.0 24582.0 24583.0 24584.0 24585.0 24586.0 24587.0 24588.0 24589.0 24590.0 24591.0 24592.0 24593.0 24594.0 24595.0 24596.0 24597.0 24598.0 24599.0 24600.0 24601.0 24602.0 24603.0 24604.0 24605.0 24606.0 24607.0 24608.0 24609.0 24610.0 24611.0 24612.0 24613.0 24614.0 24615.0 24616.0 24617.0 24618.0 24619.0 24620.0 24621.0 24622.0 24623.0 24624.0 24625.0 24626.0 24627.0 24628.0 24629.0 24630.0 24631.0 24632.0 24633.0 24634.0 24635.0 24636.0 24637.0 24638.0 24639.0 24640.0 24641.0 24642.0 24643.0 24644.0 24645.0 24646.0 24647.0 24648.0 24649.0 24650.0 24651.0 24652.0 24653.0 24654.0 24655.0 24656.0 24657.0 24658.0 24659.0 24660.0 24661.0 24662.0 24663.0 24664.0 24665.0 24666.0 24667.0 24668.0 24669.0 24670.0 24671.0 24672.0 24673.0 24674.0 24675.0 24676.0 24677.0 24678.0 24679.0 24680.0 24681.0 24682.0 24683.0 24684.0 24685.0 24686.0 24687.0 24688.0 24689.0 24690.0 24691.0 24692.0 24693.0 24694.0 24695.0 24696.0 24697.0 24698.0 24699.0 24700.0 24701.0 24702.0 24703.0 24704.0 24705.0 24706.0 24707.0 24708.0 24709.0 24710.0 24711.0 24712.0 24713.0 24714.0 24715.0 24716.0 24717.0 24718.0 24719.0 24720.0 24721.0 24722.0 24723.0 24724.0 24725.0 24726.0 24727.0 24728.0 24729.0 24730.0 24731.0 24732.0 24733.0 24734.0 24735.0 24736.0 24737.0 24738.0 24739.0 24740.0 24741.0 24742.0 24743.0 24744.0 24745.0 24746.0 24747.0 24748.0 24749.0 24750.0 24751.0 24752.0 24753.0 24754.0 24755.0 24756.0 24757.0 24758.0 24759.0 24760.0 24761.0 24762.0 24763.0 24764.0 24765.0 24766.0 24767.0 24768.0 24769.0 24770.0 24771.0 24772.0 24773.0 24774.0 24775.0 24776.0 24777.0 24778.0 24779.0 24780.0 24781.0 24782.0 24783.0 24784.0 24785.0 24786.0 24787.0 24788.0 24789.0 24790.0 24791.0 24792.0 24793.0 24794.0 24795.0 24796.0 24797.0 24798.0 24799.0 24800.0 24801.0 24802.0 24803.0 24804.0 24805.0 24806.0 24807.0 24808.0 24809.0 24810.0 24811.0 24812.0 24813.0 24814.0 24815.0 24816.0 24817.0 24818.0 24819.0 24820.0 24821.0 24822.0 24823.0 24824.0 24825.0 24826.0 24827.0 24828.0 24829.0 24830.0 24831.0 24832.0 24833.0 24834.0 24835.0 24836.0 24837.0 24838.0 24839.0 24840.0 24841.0 24842.0 24843.0 24844.0 24845.0 24846.0 24847.0 24848.0 24849.0 24850.0 24851.0 24852.0 24853.0 24854.0 24855.0 24856.0 24857.0 24858.0 24859.0 24860.0 24861.0 24862.0 24863.0 24864.0 24865.0 24866.0 24867.0 24868.0 24869.0 24870.0 24871.0 24872.0 24873.0 24874.0 24875.0 24876.0 24877.0 24878.0 24879.0 24880.0 24881.0 24882.0 24883.0 24884.0 24885.0 24886.0 24887.0 24888.0 24889.0 24890.0 24891.0 24892.0 24893.0 24894.0 24895.0 24896.0 24897.0 24898.0 24899.0 24900.0 24901.0 24902.0 24903.0 24904.0 24905.0 24906.0 24907.0 24908.0 24909.0 24910.0 24911.0 24912.0 24913.0 24914.0 24915.0 24916.0 24917.0 24918.0 24919.0 24920.0 24921.0 24922.0 24923.0 24924.0 24925.0 24926.0 24927.0 24928.0 24929.0 24930.0 24931.0 24932.0 24933.0 24934.0 24935.0 24936.0 24937.0 24938.0 24939.0 24940.0 24941.0 24942.0 24943.0 24944.0 24945.0 24946.0 24947.0 24948.0 24949.0 24950.0 24951.0 24952.0 24953.0 24954.0 24955.0 24956.0 24957.0 24958.0 24959.0 24960.0 24961.0 24962.0 24963.0 24964.0 24965.0 24966.0 24967.0 24968.0 24969.0 24970.0 24971.0 24972.0 24973.0 24974.0 24975.0 24976.0 24977.0 24978.0 24979.0 24980.0 24981.0 24982.0 24983.0 24984.0 24985.0 24986.0 24987.0 24988.0 24989.0 24990.0 24991.0 24992.0 24993.0 24994.0 24995.0 24996.0 24997.0 24998.0 24999.0 25000.0 25001.0 25002.0 25003.0 25004.0 25005.0 25006.0 25007.0 25008.0 25009.0 25010.0 25011.0 25012.0 25013.0 25014.0 25015.0 25016.0 25017.0 25018.0 25019.0 25020.0 25021.0 25022.0 25023.0 25024.0 25025.0 25026.0 25027.0 25028.0 25029.0 25030.0 25031.0 25032.0 25033.0 25034.0 25035.0 25036.0 25037.0 25038.0 25039.0 25040.0 25041.0 25042.0 25043.0 25044.0 25045.0 25046.0 25047.0 25048.0 25049.0 25050.0 25051.0 25052.0 25053.0 25054.0 25055.0 25056.0 25057.0 25058.0 25059.0 25060.0 25061.0 25062.0 25063.0 25064.0 25065.0 25066.0 25067.0 25068.0 25069.0 25070.0 25071.0 25072.0 25073.0 25074.0 25075.0 25076.0 25077.0 25078.0 25079.0 25080.0 25081.0 25082.0 25083.0 25084.0 25085.0 25086.0 25087.0 25088.0 25089.0 25090.0 25091.0 25092.0 25093.0 25094.0 25095.0 25096.0 25097.0 25098.0 25099.0 25100.0 25101.0 25102.0 25103.0 25104.0 25105.0 25106.0 25107.0 25108.0 25109.0 25110.0 25111.0 25112.0 25113.0 25114.0 25115.0 25116.0 25117.0 25118.0 25119.0 25120.0 25121.0 25122.0 25123.0 25124.0 25125.0 25126.0 25127.0 25128.0 25129.0 25130.0 25131.0 25132.0 25133.0 25134.0 25135.0 25136.0 25137.0 25138.0 25139.0 25140.0 25141.0 25142.0 25143.0 25144.0 25145.0 25146.0 25147.0 25148.0 25149.0 25150.0 25151.0 25152.0 25153.0 25154.0 25155.0 25156.0 25157.0 25158.0 25159.0 25160.0 25161.0 25162.0 25163.0 25164.0 25165.0 25166.0 25167.0 25168.0 25169.0 25170.0 25171.0 25172.0 25173.0 25174.0 25175.0 25176.0 25177.0 25178.0 25179.0 25180.0 25181.0 25182.0 25183.0 25184.0 25185.0 25186.0 25187.0 25188.0 25189.0 25190.0 25191.0 25192.0 25193.0 25194.0 25195.0 25196.0 25197.0 25198.0 25199.0 25200.0 25201.0 25202.0 25203.0 25204.0 25205.0 25206.0 25207.0 25208.0 25209.0 25210.0 25211.0 25212.0 25213.0 25214.0 25215.0 25216.0 25217.0 25218.0 25219.0 25220.0 25221.0 25222.0 25223.0 25224.0 25225.0 25226.0 25227.0 25228.0 25229.0 25230.0 25231.0 25232.0 25233.0 25234.0 25235.0 25236.0 25237.0 25238.0 25239.0 25240.0 25241.0 25242.0 25243.0 25244.0 25245.0 25246.0 25247.0 25248.0 25249.0 25250.0 25251.0 25252.0 25253.0 25254.0 25255.0 25256.0 25257.0 25258.0 25259.0 25260.0 25261.0 25262.0 25263.0 25264.0 25265.0 25266.0 25267.0 25268.0 25269.0 25270.0 25271.0 25272.0 25273.0 25274.0 25275.0 25276.0 25277.0 25278.0 25279.0 25280.0 25281.0 25282.0 25283.0 25284.0 25285.0 25286.0 25287.0 25288.0 25289.0 25290.0 25291.0 25292.0 25293.0 25294.0 25295.0 25296.0 25297.0 25298.0 25299.0 25300.0 25301.0 25302.0 25303.0 25304.0 25305.0 25306.0 25307.0 25308.0 25309.0 25310.0 25311.0 25312.0 25313.0 25314.0 25315.0 25316.0 25317.0 25318.0 25319.0 25320.0 25321.0 25322.0 25323.0 25324.0 25325.0 25326.0 25327.0 25328.0 25329.0 25330.0 25331.0 25332.0 25333.0 25334.0 25335.0 25336.0 25337.0 25338.0 25339.0 25340.0 25341.0 25342.0 25343.0 25344.0 25345.0 25346.0 25347.0 25348.0 25349.0 25350.0 25351.0 25352.0 25353.0 25354.0 25355.0 25356.0 25357.0 25358.0 25359.0 25360.0 25361.0 25362.0 25363.0 25364.0 25365.0 25366.0 25367.0 25368.0 25369.0 25370.0 25371.0 25372.0 25373.0 25374.0 25375.0 25376.0 25377.0 25378.0 25379.0 25380.0 25381.0 25382.0 25383.0 25384.0 25385.0 25386.0 25387.0 25388.0 25389.0 25390.0 25391.0 25392.0 25393.0 25394.0 25395.0 25396.0 25397.0 25398.0 25399.0 25400.0 25401.0 25402.0 25403.0 25404.0 25405.0 25406.0 25407.0 25408.0 25409.0 25410.0 25411.0 25412.0 25413.0 25414.0 25415.0 25416.0 25417.0 25418.0 25419.0 25420.0 25421.0 25422.0 25423.0 25424.0 25425.0 25426.0 25427.0 25428.0 25429.0 25430.0 25431.0 25432.0 25433.0 25434.0 25435.0 25436.0 25437.0 25438.0 25439.0 25440.0 25441.0 25442.0 25443.0 25444.0 25445.0 25446.0 25447.0 25448.0 25449.0 25450.0 25451.0 25452.0 25453.0 25454.0 25455.0 25456.0 25457.0 25458.0 25459.0 25460.0 25461.0 25462.0 25463.0 25464.0 25465.0 25466.0 25467.0 25468.0 25469.0 25470.0 25471.0 25472.0 25473.0 25474.0 25475.0 25476.0 25477.0 25478.0 25479.0 25480.0 25481.0 25482.0 25483.0 25484.0 25485.0 25486.0 25487.0 25488.0 25489.0 25490.0 25491.0 25492.0 25493.0 25494.0 25495.0 25496.0 25497.0 25498.0 25499.0 25500.0 25501.0 25502.0 25503.0 25504.0 25505.0 25506.0 25507.0 25508.0 25509.0 25510.0 25511.0 25512.0 25513.0 25514.0 25515.0 25516.0 25517.0 25518.0 25519.0 25520.0 25521.0 25522.0 25523.0 25524.0 25525.0 25526.0 25527.0 25528.0 25529.0 25530.0 25531.0 25532.0 25533.0 25534.0 25535.0 25536.0 25537.0 25538.0 25539.0 25540.0 25541.0 25542.0 25543.0 25544.0 25545.0 25546.0 25547.0 25548.0 25549.0 25550.0 25551.0 25552.0 25553.0 25554.0 25555.0 25556.0 25557.0 25558.0 25559.0 25560.0 25561.0 25562.0 25563.0 25564.0 25565.0 25566.0 25567.0 25568.0 25569.0 25570.0 25571.0 25572.0 25573.0 25574.0 25575.0 25576.0 25577.0 25578.0 25579.0 25580.0 25581.0 25582.0 25583.0 25584.0 25585.0 25586.0 25587.0 25588.0 25589.0 25590.0 25591.0 25592.0 25593.0 25594.0 25595.0 25596.0 25597.0 25598.0 25599.0 25600.0 25601.0 25602.0 25603.0 25604.0 25605.0 25606.0 25607.0 25608.0 25609.0 25610.0 25611.0 25612.0 25613.0 25614.0 25615.0 25616.0 25617.0 25618.0 25619.0 25620.0 25621.0 25622.0 25623.0 25624.0 25625.0 25626.0 25627.0 25628.0 25629.0 25630.0 25631.0 25632.0 25633.0 25634.0 25635.0 25636.0 25637.0 25638.0 25639.0 25640.0 25641.0 25642.0 25643.0 25644.0 25645.0 25646.0 25647.0 25648.0 25649.0 25650.0 25651.0 25652.0 25653.0 25654.0 25655.0 25656.0 25657.0 25658.0 25659.0 25660.0 25661.0 25662.0 25663.0 25664.0 25665.0 25666.0 25667.0 25668.0 25669.0 25670.0 25671.0 25672.0 25673.0 25674.0 25675.0 25676.0 25677.0 25678.0 25679.0 25680.0 25681.0 25682.0 25683.0 25684.0 25685.0 25686.0 25687.0 25688.0 25689.0 25690.0 25691.0 25692.0 25693.0 25694.0 25695.0 25696.0 25697.0 25698.0 25699.0 25700.0 25701.0 25702.0 25703.0 25704.0 25705.0 25706.0 25707.0 25708.0 25709.0 25710.0 25711.0 25712.0 25713.0 25714.0 25715.0 25716.0 25717.0 25718.0 25719.0 25720.0 25721.0 25722.0 25723.0 25724.0 25725.0 25726.0 25727.0 25728.0 25729.0 25730.0 25731.0 25732.0 25733.0 25734.0 25735.0 25736.0 25737.0 25738.0 25739.0 25740.0 25741.0 25742.0 25743.0 25744.0 25745.0 25746.0 25747.0 25748.0 25749.0 25750.0 25751.0 25752.0 25753.0 25754.0 25755.0 25756.0 25757.0 25758.0 25759.0 25760.0 25761.0 25762.0 25763.0 25764.0 25765.0 25766.0 25767.0 25768.0 25769.0 25770.0 25771.0 25772.0 25773.0 25774.0 25775.0 25776.0 25777.0 25778.0 25779.0 25780.0 25781.0 25782.0 25783.0 25784.0 25785.0 25786.0 25787.0 25788.0 25789.0 25790.0 25791.0 25792.0 25793.0 25794.0 25795.0 25796.0 25797.0 25798.0 25799.0 25800.0 25801.0 25802.0 25803.0 25804.0 25805.0 25806.0 25807.0 25808.0 25809.0 25810.0 25811.0 25812.0 25813.0 25814.0 25815.0 25816.0 25817.0 25818.0 25819.0 25820.0 25821.0 25822.0 25823.0 25824.0 25825.0 25826.0 25827.0 25828.0 25829.0 25830.0 25831.0 25832.0 25833.0 25834.0 25835.0 25836.0 25837.0 25838.0 25839.0 25840.0 25841.0 25842.0 25843.0 25844.0 25845.0 25846.0 25847.0 25848.0 25849.0 25850.0 25851.0 25852.0 25853.0 25854.0 25855.0 25856.0 25857.0 25858.0 25859.0 25860.0 25861.0 25862.0 25863.0 25864.0 25865.0 25866.0 25867.0 25868.0 25869.0 25870.0 25871.0 25872.0 25873.0 25874.0 25875.0 25876.0 25877.0 25878.0 25879.0 25880.0 25881.0 25882.0 25883.0 25884.0 25885.0 25886.0 25887.0 25888.0 25889.0 25890.0 25891.0 25892.0 25893.0 25894.0 25895.0 25896.0 25897.0 25898.0 25899.0 25900.0 25901.0 25902.0 25903.0 25904.0 25905.0 25906.0 25907.0 25908.0 25909.0 25910.0 25911.0 25912.0 25913.0 25914.0 25915.0 25916.0 25917.0 25918.0 25919.0 25920.0 25921.0 25922.0 25923.0 25924.0 25925.0 25926.0 25927.0 25928.0 25929.0 25930.0 25931.0 25932.0 25933.0 25934.0 25935.0 25936.0 25937.0 25938.0 25939.0 25940.0 25941.0 25942.0 25943.0 25944.0 25945.0 25946.0 25947.0 25948.0 25949.0 25950.0 25951.0 25952.0 25953.0 25954.0 25955.0 25956.0 25957.0 25958.0 25959.0 25960.0 25961.0 25962.0 25963.0 25964.0 25965.0 25966.0 25967.0 25968.0 25969.0 25970.0 25971.0 25972.0 25973.0 25974.0 25975.0 25976.0 25977.0 25978.0 25979.0 25980.0 25981.0 25982.0 25983.0 25984.0 25985.0 25986.0 25987.0 25988.0 25989.0 25990.0 25991.0 25992.0 25993.0 25994.0 25995.0 25996.0 25997.0 25998.0 25999.0 26000.0 26001.0 26002.0 26003.0 26004.0 26005.0 26006.0 26007.0 26008.0 26009.0 26010.0 26011.0 26012.0 26013.0 26014.0 26015.0 26016.0 26017.0 26018.0 26019.0 26020.0 26021.0 26022.0 26023.0 26024.0 26025.0 26026.0 26027.0 26028.0 26029.0 26030.0 26031.0 26032.0 26033.0 26034.0 26035.0 26036.0 26037.0 26038.0 26039.0 26040.0 26041.0 26042.0 26043.0 26044.0 26045.0 26046.0 26047.0 26048.0 26049.0 26050.0 26051.0 26052.0 26053.0 26054.0 26055.0 26056.0 26057.0 26058.0 26059.0 26060.0 26061.0 26062.0 26063.0 26064.0 26065.0 26066.0 26067.0 26068.0 26069.0 26070.0 26071.0 26072.0 26073.0 26074.0 26075.0 26076.0 26077.0 26078.0 26079.0 26080.0 26081.0 26082.0 26083.0 26084.0 26085.0 26086.0 26087.0 26088.0 26089.0 26090.0 26091.0 26092.0 26093.0 26094.0 26095.0 26096.0 26097.0 26098.0 26099.0 26100.0 26101.0 26102.0 26103.0 26104.0 26105.0 26106.0 26107.0 26108.0 26109.0 26110.0 26111.0 26112.0 26113.0 26114.0 26115.0 26116.0 26117.0 26118.0 26119.0 26120.0 26121.0 26122.0 26123.0 26124.0 26125.0 26126.0 26127.0 26128.0 26129.0 26130.0 26131.0 26132.0 26133.0 26134.0 26135.0 26136.0 26137.0 26138.0 26139.0 26140.0 26141.0 26142.0 26143.0 26144.0 26145.0 26146.0 26147.0 26148.0 26149.0 26150.0 26151.0 26152.0 26153.0 26154.0 26155.0 26156.0 26157.0 26158.0 26159.0 26160.0 26161.0 26162.0 26163.0 26164.0 26165.0 26166.0 26167.0 26168.0 26169.0 26170.0 26171.0 26172.0 26173.0 26174.0 26175.0 26176.0 26177.0 26178.0 26179.0 26180.0 26181.0 26182.0 26183.0 26184.0 26185.0 26186.0 26187.0 26188.0 26189.0 26190.0 26191.0 26192.0 26193.0 26194.0 26195.0 26196.0 26197.0 26198.0 26199.0 26200.0 26201.0 26202.0 26203.0 26204.0 26205.0 26206.0 26207.0 26208.0 26209.0 26210.0 26211.0 26212.0 26213.0 26214.0 26215.0 26216.0 26217.0 26218.0 26219.0 26220.0 26221.0 26222.0 26223.0 26224.0 26225.0 26226.0 26227.0 26228.0 26229.0 26230.0 26231.0 26232.0 26233.0 26234.0 26235.0 26236.0 26237.0 26238.0 26239.0 26240.0 26241.0 26242.0 26243.0 26244.0 26245.0 26246.0 26247.0 26248.0 26249.0 26250.0 26251.0 26252.0 26253.0 26254.0 26255.0 26256.0 26257.0 26258.0 26259.0 26260.0 26261.0 26262.0 26263.0 26264.0 26265.0 26266.0 26267.0 26268.0 26269.0 26270.0 26271.0 26272.0 26273.0 26274.0 26275.0 26276.0 26277.0 26278.0 26279.0 26280.0 26281.0 26282.0 26283.0 26284.0 26285.0 26286.0 26287.0 26288.0 26289.0 26290.0 26291.0 26292.0 26293.0 26294.0 26295.0 26296.0 26297.0 26298.0 26299.0 26300.0 26301.0 26302.0 26303.0 26304.0 26305.0 26306.0 26307.0 26308.0 26309.0 26310.0 26311.0 26312.0 26313.0 26314.0 26315.0 26316.0 26317.0 26318.0 26319.0 26320.0 26321.0 26322.0 26323.0 26324.0 26325.0 26326.0 26327.0 26328.0 26329.0 26330.0 26331.0 26332.0 26333.0 26334.0 26335.0 26336.0 26337.0 26338.0 26339.0 26340.0 26341.0 26342.0 26343.0 26344.0 26345.0 26346.0 26347.0 26348.0 26349.0 26350.0 26351.0 26352.0 26353.0 26354.0 26355.0 26356.0 26357.0 26358.0 26359.0 26360.0 26361.0 26362.0 26363.0 26364.0 26365.0 26366.0 26367.0 26368.0 26369.0 26370.0 26371.0 26372.0 26373.0 26374.0 26375.0 26376.0 26377.0 26378.0 26379.0 26380.0 26381.0 26382.0 26383.0 26384.0 26385.0 26386.0 26387.0 26388.0 26389.0 26390.0 26391.0 26392.0 26393.0 26394.0 26395.0 26396.0 26397.0 26398.0 26399.0 26400.0 26401.0 26402.0 26403.0 26404.0 26405.0 26406.0 26407.0 26408.0 26409.0 26410.0 26411.0 26412.0 26413.0 26414.0 26415.0 26416.0 26417.0 26418.0 26419.0 26420.0 26421.0 26422.0 26423.0 26424.0 26425.0 26426.0 26427.0 26428.0 26429.0 26430.0 26431.0 26432.0 26433.0 26434.0 26435.0 26436.0 26437.0 26438.0 26439.0 26440.0 26441.0 26442.0 26443.0 26444.0 26445.0 26446.0 26447.0 26448.0 26449.0 26450.0 26451.0 26452.0 26453.0 26454.0 26455.0 26456.0 26457.0 26458.0 26459.0 26460.0 26461.0 26462.0 26463.0 26464.0 26465.0 26466.0 26467.0 26468.0 26469.0 26470.0 26471.0 26472.0 26473.0 26474.0 26475.0 26476.0 26477.0 26478.0 26479.0 26480.0 26481.0 26482.0 26483.0 26484.0 26485.0 26486.0 26487.0 26488.0 26489.0 26490.0 26491.0 26492.0 26493.0 26494.0 26495.0 26496.0 26497.0 26498.0 26499.0 26500.0 26501.0 26502.0 26503.0 26504.0 26505.0 26506.0 26507.0 26508.0 26509.0 26510.0 26511.0 26512.0 26513.0 26514.0 26515.0 26516.0 26517.0 26518.0 26519.0 26520.0 26521.0 26522.0 26523.0 26524.0 26525.0 26526.0 26527.0 26528.0 26529.0 26530.0 26531.0 26532.0 26533.0 26534.0 26535.0 26536.0 26537.0 26538.0 26539.0 26540.0 26541.0 26542.0 26543.0 26544.0 26545.0 26546.0 26547.0 26548.0 26549.0 26550.0 26551.0 26552.0 26553.0 26554.0 26555.0 26556.0 26557.0 26558.0 26559.0 26560.0 26561.0 26562.0 26563.0 26564.0 26565.0 26566.0 26567.0 26568.0 26569.0 26570.0 26571.0 26572.0 26573.0 26574.0 26575.0 26576.0 26577.0 26578.0 26579.0 26580.0 26581.0 26582.0 26583.0 26584.0 26585.0 26586.0 26587.0 26588.0 26589.0 26590.0 26591.0 26592.0 26593.0 26594.0 26595.0 26596.0 26597.0 26598.0 26599.0 26600.0 26601.0 26602.0 26603.0 26604.0 26605.0 26606.0 26607.0 26608.0 26609.0 26610.0 26611.0 26612.0 26613.0 26614.0 26615.0 26616.0 26617.0 26618.0 26619.0 26620.0 26621.0 26622.0 26623.0 26624.0 26625.0 26626.0 26627.0 26628.0 26629.0 26630.0 26631.0 26632.0 26633.0 26634.0 26635.0 26636.0 26637.0 26638.0 26639.0 26640.0 26641.0 26642.0 26643.0 26644.0 26645.0 26646.0 26647.0 26648.0 26649.0 26650.0 26651.0 26652.0 26653.0 26654.0 26655.0 26656.0 26657.0 26658.0 26659.0 26660.0 26661.0 26662.0 26663.0 26664.0 26665.0 26666.0 26667.0 26668.0 26669.0 26670.0 26671.0 26672.0 26673.0 26674.0 26675.0 26676.0 26677.0 26678.0 26679.0 26680.0 26681.0 26682.0 26683.0 26684.0 26685.0 26686.0 26687.0 26688.0 26689.0 26690.0 26691.0 26692.0 26693.0 26694.0 26695.0 26696.0 26697.0 26698.0 26699.0 26700.0 26701.0 26702.0 26703.0 26704.0 26705.0 26706.0 26707.0 26708.0 26709.0 26710.0 26711.0 26712.0 26713.0 26714.0 26715.0 26716.0 26717.0 26718.0 26719.0 26720.0 26721.0 26722.0 26723.0 26724.0 26725.0 26726.0 26727.0 26728.0 26729.0 26730.0 26731.0 26732.0 26733.0 26734.0 26735.0 26736.0 26737.0 26738.0 26739.0 26740.0 26741.0 26742.0 26743.0 26744.0 26745.0 26746.0 26747.0 26748.0 26749.0 26750.0 26751.0 26752.0 26753.0 26754.0 26755.0 26756.0 26757.0 26758.0 26759.0 26760.0 26761.0 26762.0 26763.0 26764.0 26765.0 26766.0 26767.0 26768.0 26769.0 26770.0 26771.0 26772.0 26773.0 26774.0 26775.0 26776.0 26777.0 26778.0 26779.0 26780.0 26781.0 26782.0 26783.0 26784.0 26785.0 26786.0 26787.0 26788.0 26789.0 26790.0 26791.0 26792.0 26793.0 26794.0 26795.0 26796.0 26797.0 26798.0 26799.0 26800.0 26801.0 26802.0 26803.0 26804.0 26805.0 26806.0 26807.0 26808.0 26809.0 26810.0 26811.0 26812.0 26813.0 26814.0 26815.0 26816.0 26817.0 26818.0 26819.0 26820.0 26821.0 26822.0 26823.0 26824.0 26825.0 26826.0 26827.0 26828.0 26829.0 26830.0 26831.0 26832.0 26833.0 26834.0 26835.0 26836.0 26837.0 26838.0 26839.0 26840.0 26841.0 26842.0 26843.0 26844.0 26845.0 26846.0 26847.0 26848.0 26849.0 26850.0 26851.0 26852.0 26853.0 26854.0 26855.0 26856.0 26857.0 26858.0 26859.0 26860.0 26861.0 26862.0 26863.0 26864.0 26865.0 26866.0 26867.0 26868.0 26869.0 26870.0 26871.0 26872.0 26873.0 26874.0 26875.0 26876.0 26877.0 26878.0 26879.0 26880.0 26881.0 26882.0 26883.0 26884.0 26885.0 26886.0 26887.0 26888.0 26889.0 26890.0 26891.0 26892.0 26893.0 26894.0 26895.0 26896.0 26897.0 26898.0 26899.0 26900.0 26901.0 26902.0 26903.0 26904.0 26905.0 26906.0 26907.0 26908.0 26909.0 26910.0 26911.0 26912.0 26913.0 26914.0 26915.0 26916.0 26917.0 26918.0 26919.0 26920.0 26921.0 26922.0 26923.0 26924.0 26925.0 26926.0 26927.0 26928.0 26929.0 26930.0 26931.0 26932.0 26933.0 26934.0 26935.0 26936.0 26937.0 26938.0 26939.0 26940.0 26941.0 26942.0 26943.0 26944.0 26945.0 26946.0 26947.0 26948.0 26949.0 26950.0 26951.0 26952.0 26953.0 26954.0 26955.0 26956.0 26957.0 26958.0 26959.0 26960.0 26961.0 26962.0 26963.0 26964.0 26965.0 26966.0 26967.0 26968.0 26969.0 26970.0 26971.0 26972.0 26973.0 26974.0 26975.0 26976.0 26977.0 26978.0 26979.0 26980.0 26981.0 26982.0 26983.0 26984.0 26985.0 26986.0 26987.0 26988.0 26989.0 26990.0 26991.0 26992.0 26993.0 26994.0 26995.0 26996.0 26997.0 26998.0 26999.0 27000.0 27001.0 27002.0 27003.0 27004.0 27005.0 27006.0 27007.0 27008.0 27009.0 27010.0 27011.0 27012.0 27013.0 27014.0 27015.0 27016.0 27017.0 27018.0 27019.0 27020.0 27021.0 27022.0 27023.0 27024.0 27025.0 27026.0 27027.0 27028.0 27029.0 27030.0 27031.0 27032.0 27033.0 27034.0 27035.0 27036.0 27037.0 27038.0 27039.0 27040.0 27041.0 27042.0 27043.0 27044.0 27045.0 27046.0 27047.0 27048.0 27049.0 27050.0 27051.0 27052.0 27053.0 27054.0 27055.0 27056.0 27057.0 27058.0 27059.0 27060.0 27061.0 27062.0 27063.0 27064.0 27065.0 27066.0 27067.0 27068.0 27069.0 27070.0 27071.0 27072.0 27073.0 27074.0 27075.0 27076.0 27077.0 27078.0 27079.0 27080.0 27081.0 27082.0 27083.0 27084.0 27085.0 27086.0 27087.0 27088.0 27089.0 27090.0 27091.0 27092.0 27093.0 27094.0 27095.0 27096.0 27097.0 27098.0 27099.0 27100.0 27101.0 27102.0 27103.0 27104.0 27105.0 27106.0 27107.0 27108.0 27109.0 27110.0 27111.0 27112.0 27113.0 27114.0 27115.0 27116.0 27117.0 27118.0 27119.0 27120.0 27121.0 27122.0 27123.0 27124.0 27125.0 27126.0 27127.0 27128.0 27129.0 27130.0 27131.0 27132.0 27133.0 27134.0 27135.0 27136.0 27137.0 27138.0 27139.0 27140.0 27141.0 27142.0 27143.0 27144.0 27145.0 27146.0 27147.0 27148.0 27149.0 27150.0 27151.0 27152.0 27153.0 27154.0 27155.0 27156.0 27157.0 27158.0 27159.0 27160.0 27161.0 27162.0 27163.0 27164.0 27165.0 27166.0 27167.0 27168.0 27169.0 27170.0 27171.0 27172.0 27173.0 27174.0 27175.0 27176.0 27177.0 27178.0 27179.0 27180.0 27181.0 27182.0 27183.0 27184.0 27185.0 27186.0 27187.0 27188.0 27189.0 27190.0 27191.0 27192.0 27193.0 27194.0 27195.0 27196.0 27197.0 27198.0 27199.0 27200.0 27201.0 27202.0 27203.0 27204.0 27205.0 27206.0 27207.0 27208.0 27209.0 27210.0 27211.0 27212.0 27213.0 27214.0 27215.0 27216.0 27217.0 27218.0 27219.0 27220.0 27221.0 27222.0 27223.0 27224.0 27225.0 27226.0 27227.0 27228.0 27229.0 27230.0 27231.0 27232.0 27233.0 27234.0 27235.0 27236.0 27237.0 27238.0 27239.0 27240.0 27241.0 27242.0 27243.0 27244.0 27245.0 27246.0 27247.0 27248.0 27249.0 27250.0 27251.0 27252.0 27253.0 27254.0 27255.0 27256.0 27257.0 27258.0 27259.0 27260.0 27261.0 27262.0 27263.0 27264.0 27265.0 27266.0 27267.0 27268.0 27269.0 27270.0 27271.0 27272.0 27273.0 27274.0 27275.0 27276.0 27277.0 27278.0 27279.0 27280.0 27281.0 27282.0 27283.0 27284.0 27285.0 27286.0 27287.0 27288.0 27289.0 27290.0 27291.0 27292.0 27293.0 27294.0 27295.0 27296.0 27297.0 27298.0 27299.0 27300.0 27301.0 27302.0 27303.0 27304.0 27305.0 27306.0 27307.0 27308.0 27309.0 27310.0 27311.0 27312.0 27313.0 27314.0 27315.0 27316.0 27317.0 27318.0 27319.0 27320.0 27321.0 27322.0 27323.0 27324.0 27325.0 27326.0 27327.0 27328.0 27329.0 27330.0 27331.0 27332.0 27333.0 27334.0 27335.0 27336.0 27337.0 27338.0 27339.0 27340.0 27341.0 27342.0 27343.0 27344.0 27345.0 27346.0 27347.0 27348.0 27349.0 27350.0 27351.0 27352.0 27353.0 27354.0 27355.0 27356.0 27357.0 27358.0 27359.0 27360.0 27361.0 27362.0 27363.0 27364.0 27365.0 27366.0 27367.0 27368.0 27369.0 27370.0 27371.0 27372.0 27373.0 27374.0 27375.0 27376.0 27377.0 27378.0 27379.0 27380.0 27381.0 27382.0 27383.0 27384.0 27385.0 27386.0 27387.0 27388.0 27389.0 27390.0 27391.0 27392.0 27393.0 27394.0 27395.0 27396.0 27397.0 27398.0 27399.0 27400.0 27401.0 27402.0 27403.0 27404.0 27405.0 27406.0 27407.0 27408.0 27409.0 27410.0 27411.0 27412.0 27413.0 27414.0 27415.0 27416.0 27417.0 27418.0 27419.0 27420.0 27421.0 27422.0 27423.0 27424.0 27425.0 27426.0 27427.0 27428.0 27429.0 27430.0 27431.0 27432.0 27433.0 27434.0 27435.0 27436.0 27437.0 27438.0 27439.0 27440.0 27441.0 27442.0 27443.0 27444.0 27445.0 27446.0 27447.0 27448.0 27449.0 27450.0 27451.0 27452.0 27453.0 27454.0 27455.0 27456.0 27457.0 27458.0 27459.0 27460.0 27461.0 27462.0 27463.0 27464.0 27465.0 27466.0 27467.0 27468.0 27469.0 27470.0 27471.0 27472.0 27473.0 27474.0 27475.0 27476.0 27477.0 27478.0 27479.0 27480.0 27481.0 27482.0 27483.0 27484.0 27485.0 27486.0 27487.0 27488.0 27489.0 27490.0 27491.0 27492.0 27493.0 27494.0 27495.0 27496.0 27497.0 27498.0 27499.0 27500.0 27501.0 27502.0 27503.0 27504.0 27505.0 27506.0 27507.0 27508.0 27509.0 27510.0 27511.0 27512.0 27513.0 27514.0 27515.0 27516.0 27517.0 27518.0 27519.0 27520.0 27521.0 27522.0 27523.0 27524.0 27525.0 27526.0 27527.0 27528.0 27529.0 27530.0 27531.0 27532.0 27533.0 27534.0 27535.0 27536.0 27537.0 27538.0 27539.0 27540.0 27541.0 27542.0 27543.0 27544.0 27545.0 27546.0 27547.0 27548.0 27549.0 27550.0 27551.0 27552.0 27553.0 27554.0 27555.0 27556.0 27557.0 27558.0 27559.0 27560.0 27561.0 27562.0 27563.0 27564.0 27565.0 27566.0 27567.0 27568.0 27569.0 27570.0 27571.0 27572.0 27573.0 27574.0 27575.0 27576.0 27577.0 27578.0 27579.0 27580.0 27581.0 27582.0 27583.0 27584.0 27585.0 27586.0 27587.0 27588.0 27589.0 27590.0 27591.0 27592.0 27593.0 27594.0 27595.0 27596.0 27597.0 27598.0 27599.0 27600.0 27601.0 27602.0 27603.0 27604.0 27605.0 27606.0 27607.0 27608.0 27609.0 27610.0 27611.0 27612.0 27613.0 27614.0 27615.0 27616.0 27617.0 27618.0 27619.0 27620.0 27621.0 27622.0 27623.0 27624.0 27625.0 27626.0 27627.0 27628.0 27629.0 27630.0 27631.0 27632.0 27633.0 27634.0 27635.0 27636.0 27637.0 27638.0 27639.0 27640.0 27641.0 27642.0 27643.0 27644.0 27645.0 27646.0 27647.0 27648.0 27649.0 27650.0 27651.0 27652.0 27653.0 27654.0 27655.0 27656.0 27657.0 27658.0 27659.0 27660.0 27661.0 27662.0 27663.0 27664.0 27665.0 27666.0 27667.0 27668.0 27669.0 27670.0 27671.0 27672.0 27673.0 27674.0 27675.0 27676.0 27677.0 27678.0 27679.0 27680.0 27681.0 27682.0 27683.0 27684.0 27685.0 27686.0 27687.0 27688.0 27689.0 27690.0 27691.0 27692.0 27693.0 27694.0 27695.0 27696.0 27697.0 27698.0 27699.0 27700.0 27701.0 27702.0 27703.0 27704.0 27705.0 27706.0 27707.0 27708.0 27709.0 27710.0 27711.0 27712.0 27713.0 27714.0 27715.0 27716.0 27717.0 27718.0 27719.0 27720.0 27721.0 27722.0 27723.0 27724.0 27725.0 27726.0 27727.0 27728.0 27729.0 27730.0 27731.0 27732.0 27733.0 27734.0 27735.0 27736.0 27737.0 27738.0 27739.0 27740.0 27741.0 27742.0 27743.0 27744.0 27745.0 27746.0 27747.0 27748.0 27749.0 27750.0 27751.0 27752.0 27753.0 27754.0 27755.0 27756.0 27757.0 27758.0 27759.0 27760.0 27761.0 27762.0 27763.0 27764.0 27765.0 27766.0 27767.0 27768.0 27769.0 27770.0 27771.0 27772.0 27773.0 27774.0 27775.0 27776.0 27777.0 27778.0 27779.0 27780.0 27781.0 27782.0 27783.0 27784.0 27785.0 27786.0 27787.0 27788.0 27789.0 27790.0 27791.0 27792.0 27793.0 27794.0 27795.0 27796.0 27797.0 27798.0 27799.0 27800.0 27801.0 27802.0 27803.0 27804.0 27805.0 27806.0 27807.0 27808.0 27809.0 27810.0 27811.0 27812.0 27813.0 27814.0 27815.0 27816.0 27817.0 27818.0 27819.0 27820.0 27821.0 27822.0 27823.0 27824.0 27825.0 27826.0 27827.0 27828.0 27829.0 27830.0 27831.0 27832.0 27833.0 27834.0 27835.0 27836.0 27837.0 27838.0 27839.0 27840.0 27841.0 27842.0 27843.0 27844.0 27845.0 27846.0 27847.0 27848.0 27849.0 27850.0 27851.0 27852.0 27853.0 27854.0 27855.0 27856.0 27857.0 27858.0 27859.0 27860.0 27861.0 27862.0 27863.0 27864.0 27865.0 27866.0 27867.0 27868.0 27869.0 27870.0 27871.0 27872.0 27873.0 27874.0 27875.0 27876.0 27877.0 27878.0 27879.0 27880.0 27881.0 27882.0 27883.0 27884.0 27885.0 27886.0 27887.0 27888.0 27889.0 27890.0 27891.0 27892.0 27893.0 27894.0 27895.0 27896.0 27897.0 27898.0 27899.0 27900.0 27901.0 27902.0 27903.0 27904.0 27905.0 27906.0 27907.0 27908.0 27909.0 27910.0 27911.0 27912.0 27913.0 27914.0 27915.0 27916.0 27917.0 27918.0 27919.0 27920.0 27921.0 27922.0 27923.0 27924.0 27925.0 27926.0 27927.0 27928.0 27929.0 27930.0 27931.0 27932.0 27933.0 27934.0 27935.0 27936.0 27937.0 27938.0 27939.0 27940.0 27941.0 27942.0 27943.0 27944.0 27945.0 27946.0 27947.0 27948.0 27949.0 27950.0 27951.0 27952.0 27953.0 27954.0 27955.0 27956.0 27957.0 27958.0 27959.0 27960.0 27961.0 27962.0 27963.0 27964.0 27965.0 27966.0 27967.0 27968.0 27969.0 27970.0 27971.0 27972.0 27973.0 27974.0 27975.0 27976.0 27977.0 27978.0 27979.0 27980.0 27981.0 27982.0 27983.0 27984.0 27985.0 27986.0 27987.0 27988.0 27989.0 27990.0 27991.0 27992.0 27993.0 27994.0 27995.0 27996.0 27997.0 27998.0 27999.0 28000.0 28001.0 28002.0 28003.0 28004.0 28005.0 28006.0 28007.0 28008.0 28009.0 28010.0 28011.0 28012.0 28013.0 28014.0 28015.0 28016.0 28017.0 28018.0 28019.0 28020.0 28021.0 28022.0 28023.0 28024.0 28025.0 28026.0 28027.0 28028.0 28029.0 28030.0 28031.0 28032.0 28033.0 28034.0 28035.0 28036.0 28037.0 28038.0 28039.0 28040.0 28041.0 28042.0 28043.0 28044.0 28045.0 28046.0 28047.0 28048.0 28049.0 28050.0 28051.0 28052.0 28053.0 28054.0 28055.0 28056.0 28057.0 28058.0 28059.0 28060.0 28061.0 28062.0 28063.0 28064.0 28065.0 28066.0 28067.0 28068.0 28069.0 28070.0 28071.0 28072.0 28073.0 28074.0 28075.0 28076.0 28077.0 28078.0 28079.0 28080.0 28081.0 28082.0 28083.0 28084.0 28085.0 28086.0 28087.0 28088.0 28089.0 28090.0 28091.0 28092.0 28093.0 28094.0 28095.0 28096.0 28097.0 28098.0 28099.0 28100.0 28101.0 28102.0 28103.0 28104.0 28105.0 28106.0 28107.0 28108.0 28109.0 28110.0 28111.0 28112.0 28113.0 28114.0 28115.0 28116.0 28117.0 28118.0 28119.0 28120.0 28121.0 28122.0 28123.0 28124.0 28125.0 28126.0 28127.0 28128.0 28129.0 28130.0 28131.0 28132.0 28133.0 28134.0 28135.0 28136.0 28137.0 28138.0 28139.0 28140.0 28141.0 28142.0 28143.0 28144.0 28145.0 28146.0 28147.0 28148.0 28149.0 28150.0 28151.0 28152.0 28153.0 28154.0 28155.0 28156.0 28157.0 28158.0 28159.0 28160.0 28161.0 28162.0 28163.0 28164.0 28165.0 28166.0 28167.0 28168.0 28169.0 28170.0 28171.0 28172.0 28173.0 28174.0 28175.0 28176.0 28177.0 28178.0 28179.0 28180.0 28181.0 28182.0 28183.0 28184.0 28185.0 28186.0 28187.0 28188.0 28189.0 28190.0 28191.0 28192.0 28193.0 28194.0 28195.0 28196.0 28197.0 28198.0 28199.0 28200.0 28201.0 28202.0 28203.0 28204.0 28205.0 28206.0 28207.0 28208.0 28209.0 28210.0 28211.0 28212.0 28213.0 28214.0 28215.0 28216.0 28217.0 28218.0 28219.0 28220.0 28221.0 28222.0 28223.0 28224.0 28225.0 28226.0 28227.0 28228.0 28229.0 28230.0 28231.0 28232.0 28233.0 28234.0 28235.0 28236.0 28237.0 28238.0 28239.0 28240.0 28241.0 28242.0 28243.0 28244.0 28245.0 28246.0 28247.0 28248.0 28249.0 28250.0 28251.0 28252.0 28253.0 28254.0 28255.0 28256.0 28257.0 28258.0 28259.0 28260.0 28261.0 28262.0 28263.0 28264.0 28265.0 28266.0 28267.0 28268.0 28269.0 28270.0 28271.0 28272.0 28273.0 28274.0 28275.0 28276.0 28277.0 28278.0 28279.0 28280.0 28281.0 28282.0 28283.0 28284.0 28285.0 28286.0 28287.0 28288.0 28289.0 28290.0 28291.0 28292.0 28293.0 28294.0 28295.0 28296.0 28297.0 28298.0 28299.0 28300.0 28301.0 28302.0 28303.0 28304.0 28305.0 28306.0 28307.0 28308.0 28309.0 28310.0 28311.0 28312.0 28313.0 28314.0 28315.0 28316.0 28317.0 28318.0 28319.0 28320.0 28321.0 28322.0 28323.0 28324.0 28325.0 28326.0 28327.0 28328.0 28329.0 28330.0 28331.0 28332.0 28333.0 28334.0 28335.0 28336.0 28337.0 28338.0 28339.0 28340.0 28341.0 28342.0 28343.0 28344.0 28345.0 28346.0 28347.0 28348.0 28349.0 28350.0 28351.0 28352.0 28353.0 28354.0 28355.0 28356.0 28357.0 28358.0 28359.0 28360.0 28361.0 28362.0 28363.0 28364.0 28365.0 28366.0 28367.0 28368.0 28369.0 28370.0 28371.0 28372.0 28373.0 28374.0 28375.0 28376.0 28377.0 28378.0 28379.0 28380.0 28381.0 28382.0 28383.0 28384.0 28385.0 28386.0 28387.0 28388.0 28389.0 28390.0 28391.0 28392.0 28393.0 28394.0 28395.0 28396.0 28397.0 28398.0 28399.0 28400.0 28401.0 28402.0 28403.0 28404.0 28405.0 28406.0 28407.0 28408.0 28409.0 28410.0 28411.0 28412.0 28413.0 28414.0 28415.0 28416.0 28417.0 28418.0 28419.0 28420.0 28421.0 28422.0 28423.0 28424.0 28425.0 28426.0 28427.0 28428.0 28429.0 28430.0 28431.0 28432.0 28433.0 28434.0 28435.0 28436.0 28437.0 28438.0 28439.0 28440.0 28441.0 28442.0 28443.0 28444.0 28445.0 28446.0 28447.0 28448.0 28449.0 28450.0 28451.0 28452.0 28453.0 28454.0 28455.0 28456.0 28457.0 28458.0 28459.0 28460.0 28461.0 28462.0 28463.0 28464.0 28465.0 28466.0 28467.0 28468.0 28469.0 28470.0 28471.0 28472.0 28473.0 28474.0 28475.0 28476.0 28477.0 28478.0 28479.0 28480.0 28481.0 28482.0 28483.0 28484.0 28485.0 28486.0 28487.0 28488.0 28489.0 28490.0 28491.0 28492.0 28493.0 28494.0 28495.0 28496.0 28497.0 28498.0 28499.0 28500.0 28501.0 28502.0 28503.0 28504.0 28505.0 28506.0 28507.0 28508.0 28509.0 28510.0 28511.0 28512.0 28513.0 28514.0 28515.0 28516.0 28517.0 28518.0 28519.0 28520.0 28521.0 28522.0 28523.0 28524.0 28525.0 28526.0 28527.0 28528.0 28529.0 28530.0 28531.0 28532.0 28533.0 28534.0 28535.0 28536.0 28537.0 28538.0 28539.0 28540.0 28541.0 28542.0 28543.0 28544.0 28545.0 28546.0 28547.0 28548.0 28549.0 28550.0 28551.0 28552.0 28553.0 28554.0 28555.0 28556.0 28557.0 28558.0 28559.0 28560.0 28561.0 28562.0 28563.0 28564.0 28565.0 28566.0 28567.0 28568.0 28569.0 28570.0 28571.0 28572.0 28573.0 28574.0 28575.0 28576.0 28577.0 28578.0 28579.0 28580.0 28581.0 28582.0 28583.0 28584.0 28585.0 28586.0 28587.0 28588.0 28589.0 28590.0 28591.0 28592.0 28593.0 28594.0 28595.0 28596.0 28597.0 28598.0 28599.0 28600.0 28601.0 28602.0 28603.0 28604.0 28605.0 28606.0 28607.0 28608.0 28609.0 28610.0 28611.0 28612.0 28613.0 28614.0 28615.0 28616.0 28617.0 28618.0 28619.0 28620.0 28621.0 28622.0 28623.0 28624.0 28625.0 28626.0 28627.0 28628.0 28629.0 28630.0 28631.0 28632.0 28633.0 28634.0 28635.0 28636.0 28637.0 28638.0 28639.0 28640.0 28641.0 28642.0 28643.0 28644.0 28645.0 28646.0 28647.0 28648.0 28649.0 28650.0 28651.0 28652.0 28653.0 28654.0 28655.0 28656.0 28657.0 28658.0 28659.0 28660.0 28661.0 28662.0 28663.0 28664.0 28665.0 28666.0 28667.0 28668.0 28669.0 28670.0 28671.0 28672.0 28673.0 28674.0 28675.0 28676.0 28677.0 28678.0 28679.0 28680.0 28681.0 28682.0 28683.0 28684.0 28685.0 28686.0 28687.0 28688.0 28689.0 28690.0 28691.0 28692.0 28693.0 28694.0 28695.0 28696.0 28697.0 28698.0 28699.0 28700.0 28701.0 28702.0 28703.0 28704.0 28705.0 28706.0 28707.0 28708.0 28709.0 28710.0 28711.0 28712.0 28713.0 28714.0 28715.0 28716.0 28717.0 28718.0 28719.0 28720.0 28721.0 28722.0 28723.0 28724.0 28725.0 28726.0 28727.0 28728.0 28729.0 28730.0 28731.0 28732.0 28733.0 28734.0 28735.0 28736.0 28737.0 28738.0 28739.0 28740.0 28741.0 28742.0 28743.0 28744.0 28745.0 28746.0 28747.0 28748.0 28749.0 28750.0 28751.0 28752.0 28753.0 28754.0 28755.0 28756.0 28757.0 28758.0 28759.0 28760.0 28761.0 28762.0 28763.0 28764.0 28765.0 28766.0 28767.0 28768.0 28769.0 28770.0 28771.0 28772.0 28773.0 28774.0 28775.0 28776.0 28777.0 28778.0 28779.0 28780.0 28781.0 28782.0 28783.0 28784.0 28785.0 28786.0 28787.0 28788.0 28789.0 28790.0 28791.0 28792.0 28793.0 28794.0 28795.0 28796.0 28797.0 28798.0 28799.0 28800.0 28801.0 28802.0 28803.0 28804.0 28805.0 28806.0 28807.0 28808.0 28809.0 28810.0 28811.0 28812.0 28813.0 28814.0 28815.0 28816.0 28817.0 28818.0 28819.0 28820.0 28821.0 28822.0 28823.0 28824.0 28825.0 28826.0 28827.0 28828.0 28829.0 28830.0 28831.0 28832.0 28833.0 28834.0 28835.0 28836.0 28837.0 28838.0 28839.0 28840.0 28841.0 28842.0 28843.0 28844.0 28845.0 28846.0 28847.0 28848.0 28849.0 28850.0 28851.0 28852.0 28853.0 28854.0 28855.0 28856.0 28857.0 28858.0 28859.0 28860.0 28861.0 28862.0 28863.0 28864.0 28865.0 28866.0 28867.0 28868.0 28869.0 28870.0 28871.0 28872.0 28873.0 28874.0 28875.0 28876.0 28877.0 28878.0 28879.0 28880.0 28881.0 28882.0 28883.0 28884.0 28885.0 28886.0 28887.0 28888.0 28889.0 28890.0 28891.0 28892.0 28893.0 28894.0 28895.0 28896.0 28897.0 28898.0 28899.0 28900.0 28901.0 28902.0 28903.0 28904.0 28905.0 28906.0 28907.0 28908.0 28909.0 28910.0 28911.0 28912.0 28913.0 28914.0 28915.0 28916.0 28917.0 28918.0 28919.0 28920.0 28921.0 28922.0 28923.0 28924.0 28925.0 28926.0 28927.0 28928.0 28929.0 28930.0 28931.0 28932.0 28933.0 28934.0 28935.0 28936.0 28937.0 28938.0 28939.0 28940.0 28941.0 28942.0 28943.0 28944.0 28945.0 28946.0 28947.0 28948.0 28949.0 28950.0 28951.0 28952.0 28953.0 28954.0 28955.0 28956.0 28957.0 28958.0 28959.0 28960.0 28961.0 28962.0 28963.0 28964.0 28965.0 28966.0 28967.0 28968.0 28969.0 28970.0 28971.0 28972.0 28973.0 28974.0 28975.0 28976.0 28977.0 28978.0 28979.0 28980.0 28981.0 28982.0 28983.0 28984.0 28985.0 28986.0 28987.0 28988.0 28989.0 28990.0 28991.0 28992.0 28993.0 28994.0 28995.0 28996.0 28997.0 28998.0 28999.0 29000.0 29001.0 29002.0 29003.0 29004.0 29005.0 29006.0 29007.0 29008.0 29009.0 29010.0 29011.0 29012.0 29013.0 29014.0 29015.0 29016.0 29017.0 29018.0 29019.0 29020.0 29021.0 29022.0 29023.0 29024.0 29025.0 29026.0 29027.0 29028.0 29029.0 29030.0 29031.0 29032.0 29033.0 29034.0 29035.0 29036.0 29037.0 29038.0 29039.0 29040.0 29041.0 29042.0 29043.0 29044.0 29045.0 29046.0 29047.0 29048.0 29049.0 29050.0 29051.0 29052.0 29053.0 29054.0 29055.0 29056.0 29057.0 29058.0 29059.0 29060.0 29061.0 29062.0 29063.0 29064.0 29065.0 29066.0 29067.0 29068.0 29069.0 29070.0 29071.0 29072.0 29073.0 29074.0 29075.0 29076.0 29077.0 29078.0 29079.0 29080.0 29081.0 29082.0 29083.0 29084.0 29085.0 29086.0 29087.0 29088.0 29089.0 29090.0 29091.0 29092.0 29093.0 29094.0 29095.0 29096.0 29097.0 29098.0 29099.0 29100.0 29101.0 29102.0 29103.0 29104.0 29105.0 29106.0 29107.0 29108.0 29109.0 29110.0 29111.0 29112.0 29113.0 29114.0 29115.0 29116.0 29117.0 29118.0 29119.0 29120.0 29121.0 29122.0 29123.0 29124.0 29125.0 29126.0 29127.0 29128.0 29129.0 29130.0 29131.0 29132.0 29133.0 29134.0 29135.0 29136.0 29137.0 29138.0 29139.0 29140.0 29141.0 29142.0 29143.0 29144.0 29145.0 29146.0 29147.0 29148.0 29149.0 29150.0 29151.0 29152.0 29153.0 29154.0 29155.0 29156.0 29157.0 29158.0 29159.0 29160.0 29161.0 29162.0 29163.0 29164.0 29165.0 29166.0 29167.0 29168.0 29169.0 29170.0 29171.0 29172.0 29173.0 29174.0 29175.0 29176.0 29177.0 29178.0 29179.0 29180.0 29181.0 29182.0 29183.0 29184.0 29185.0 29186.0 29187.0 29188.0 29189.0 29190.0 29191.0 29192.0 29193.0 29194.0 29195.0 29196.0 29197.0 29198.0 29199.0 29200.0 29201.0 29202.0 29203.0 29204.0 29205.0 29206.0 29207.0 29208.0 29209.0 29210.0 29211.0 29212.0 29213.0 29214.0 29215.0 29216.0 29217.0 29218.0 29219.0 29220.0 29221.0 29222.0 29223.0 29224.0 29225.0 29226.0 29227.0 29228.0 29229.0 29230.0 29231.0 29232.0 29233.0 29234.0 29235.0 29236.0 29237.0 29238.0 29239.0 29240.0 29241.0 29242.0 29243.0 29244.0 29245.0 29246.0 29247.0 29248.0 29249.0 29250.0 29251.0 29252.0 29253.0 29254.0 29255.0 29256.0 29257.0 29258.0 29259.0 29260.0 29261.0 29262.0 29263.0 29264.0 29265.0 29266.0 29267.0 29268.0 29269.0 29270.0 29271.0 29272.0 29273.0 29274.0 29275.0 29276.0 29277.0 29278.0 29279.0 29280.0 29281.0 29282.0 29283.0 29284.0 29285.0 29286.0 29287.0 29288.0 29289.0 29290.0 29291.0 29292.0 29293.0 29294.0 29295.0 29296.0 29297.0 29298.0 29299.0 29300.0 29301.0 29302.0 29303.0 29304.0 29305.0 29306.0 29307.0 29308.0 29309.0 29310.0 29311.0 29312.0 29313.0 29314.0 29315.0 29316.0 29317.0 29318.0 29319.0 29320.0 29321.0 29322.0 29323.0 29324.0 29325.0 29326.0 29327.0 29328.0 29329.0 29330.0 29331.0 29332.0 29333.0 29334.0 29335.0 29336.0 29337.0 29338.0 29339.0 29340.0 29341.0 29342.0 29343.0 29344.0 29345.0 29346.0 29347.0 29348.0 29349.0 29350.0 29351.0 29352.0 29353.0 29354.0 29355.0 29356.0 29357.0 29358.0 29359.0 29360.0 29361.0 29362.0 29363.0 29364.0 29365.0 29366.0 29367.0 29368.0 29369.0 29370.0 29371.0 29372.0 29373.0 29374.0 29375.0 29376.0 29377.0 29378.0 29379.0 29380.0 29381.0 29382.0 29383.0 29384.0 29385.0 29386.0 29387.0 29388.0 29389.0 29390.0 29391.0 29392.0 29393.0 29394.0 29395.0 29396.0 29397.0 29398.0 29399.0 29400.0 29401.0 29402.0 29403.0 29404.0 29405.0 29406.0 29407.0 29408.0 29409.0 29410.0 29411.0 29412.0 29413.0 29414.0 29415.0 29416.0 29417.0 29418.0 29419.0 29420.0 29421.0 29422.0 29423.0 29424.0 29425.0 29426.0 29427.0 29428.0 29429.0 29430.0 29431.0 29432.0 29433.0 29434.0 29435.0 29436.0 29437.0 29438.0 29439.0 29440.0 29441.0 29442.0 29443.0 29444.0 29445.0 29446.0 29447.0 29448.0 29449.0 29450.0 29451.0 29452.0 29453.0 29454.0 29455.0 29456.0 29457.0 29458.0 29459.0 29460.0 29461.0 29462.0 29463.0 29464.0 29465.0 29466.0 29467.0 29468.0 29469.0 29470.0 29471.0 29472.0 29473.0 29474.0 29475.0 29476.0 29477.0 29478.0 29479.0 29480.0 29481.0 29482.0 29483.0 29484.0 29485.0 29486.0 29487.0 29488.0 29489.0 29490.0 29491.0 29492.0 29493.0 29494.0 29495.0 29496.0 29497.0 29498.0 29499.0 29500.0 29501.0 29502.0 29503.0 29504.0 29505.0 29506.0 29507.0 29508.0 29509.0 29510.0 29511.0 29512.0 29513.0 29514.0 29515.0 29516.0 29517.0 29518.0 29519.0 29520.0 29521.0 29522.0 29523.0 29524.0 29525.0 29526.0 29527.0 29528.0 29529.0 29530.0 29531.0 29532.0 29533.0 29534.0 29535.0 29536.0 29537.0 29538.0 29539.0 29540.0 29541.0 29542.0 29543.0 29544.0 29545.0 29546.0 29547.0 29548.0 29549.0 29550.0 29551.0 29552.0 29553.0 29554.0 29555.0 29556.0 29557.0 29558.0 29559.0 29560.0 29561.0 29562.0 29563.0 29564.0 29565.0 29566.0 29567.0 29568.0 29569.0 29570.0 29571.0 29572.0 29573.0 29574.0 29575.0 29576.0 29577.0 29578.0 29579.0 29580.0 29581.0 29582.0 29583.0 29584.0 29585.0 29586.0 29587.0 29588.0 29589.0 29590.0 29591.0 29592.0 29593.0 29594.0 29595.0 29596.0 29597.0 29598.0 29599.0 29600.0 29601.0 29602.0 29603.0 29604.0 29605.0 29606.0 29607.0 29608.0 29609.0 29610.0 29611.0 29612.0 29613.0 29614.0 29615.0 29616.0 29617.0 29618.0 29619.0 29620.0 29621.0 29622.0 29623.0 29624.0 29625.0 29626.0 29627.0 29628.0 29629.0 29630.0 29631.0 29632.0 29633.0 29634.0 29635.0 29636.0 29637.0 29638.0 29639.0 29640.0 29641.0 29642.0 29643.0 29644.0 29645.0 29646.0 29647.0 29648.0 29649.0 29650.0 29651.0 29652.0 29653.0 29654.0 29655.0 29656.0 29657.0 29658.0 29659.0 29660.0 29661.0 29662.0 29663.0 29664.0 29665.0 29666.0 29667.0 29668.0 29669.0 29670.0 29671.0 29672.0 29673.0 29674.0 29675.0 29676.0 29677.0 29678.0 29679.0 29680.0 29681.0 29682.0 29683.0 29684.0 29685.0 29686.0 29687.0 29688.0 29689.0 29690.0 29691.0 29692.0 29693.0 29694.0 29695.0 29696.0 29697.0 29698.0 29699.0 29700.0 29701.0 29702.0 29703.0 29704.0 29705.0 29706.0 29707.0 29708.0 29709.0 29710.0 29711.0 29712.0 29713.0 29714.0 29715.0 29716.0 29717.0 29718.0 29719.0 29720.0 29721.0 29722.0 29723.0 29724.0 29725.0 29726.0 29727.0 29728.0 29729.0 29730.0 29731.0 29732.0 29733.0 29734.0 29735.0 29736.0 29737.0 29738.0 29739.0 29740.0 29741.0 29742.0 29743.0 29744.0 29745.0 29746.0 29747.0 29748.0 29749.0 29750.0 29751.0 29752.0 29753.0 29754.0 29755.0 29756.0 29757.0 29758.0 29759.0 29760.0 29761.0 29762.0 29763.0 29764.0 29765.0 29766.0 29767.0 29768.0 29769.0 29770.0 29771.0 29772.0 29773.0 29774.0 29775.0 29776.0 29777.0 29778.0 29779.0 29780.0 29781.0 29782.0 29783.0 29784.0 29785.0 29786.0 29787.0 29788.0 29789.0 29790.0 29791.0 29792.0 29793.0 29794.0 29795.0 29796.0 29797.0 29798.0 29799.0 29800.0 29801.0 29802.0 29803.0 29804.0 29805.0 29806.0 29807.0 29808.0 29809.0 29810.0 29811.0 29812.0 29813.0 29814.0 29815.0 29816.0 29817.0 29818.0 29819.0 29820.0 29821.0 29822.0 29823.0 29824.0 29825.0 29826.0 29827.0 29828.0 29829.0 29830.0 29831.0 29832.0 29833.0 29834.0 29835.0 29836.0 29837.0 29838.0 29839.0 29840.0 29841.0 29842.0 29843.0 29844.0 29845.0 29846.0 29847.0 29848.0 29849.0 29850.0 29851.0 29852.0 29853.0 29854.0 29855.0 29856.0 29857.0 29858.0 29859.0 29860.0 29861.0 29862.0 29863.0 29864.0 29865.0 29866.0 29867.0 29868.0 29869.0 29870.0 29871.0 29872.0 29873.0 29874.0 29875.0 29876.0 29877.0 29878.0 29879.0 29880.0 29881.0 29882.0 29883.0 29884.0 29885.0 29886.0 29887.0 29888.0 29889.0 29890.0 29891.0 29892.0 29893.0 29894.0 29895.0 29896.0 29897.0 29898.0 29899.0 29900.0 29901.0 29902.0 29903.0 29904.0 29905.0 29906.0 29907.0 29908.0 29909.0 29910.0 29911.0 29912.0 29913.0 29914.0 29915.0 29916.0 29917.0 29918.0 29919.0 29920.0 29921.0 29922.0 29923.0 29924.0 29925.0 29926.0 29927.0 29928.0 29929.0 29930.0 29931.0 29932.0 29933.0 29934.0 29935.0 29936.0 29937.0 29938.0 29939.0 29940.0 29941.0 29942.0 29943.0 29944.0 29945.0 29946.0 29947.0 29948.0 29949.0 29950.0 29951.0 29952.0 29953.0 29954.0 29955.0 29956.0 29957.0 29958.0 29959.0 29960.0 29961.0 29962.0 29963.0 29964.0 29965.0 29966.0 29967.0 29968.0 29969.0 29970.0 29971.0 29972.0 29973.0 29974.0 29975.0 29976.0 29977.0 29978.0 29979.0 29980.0 29981.0 29982.0 29983.0 29984.0 29985.0 29986.0 29987.0 29988.0 29989.0 29990.0 29991.0 29992.0 29993.0 29994.0 29995.0 29996.0 29997.0 29998.0 29999.0 30000.0 30001.0 30002.0 30003.0 30004.0 30005.0 30006.0 30007.0 30008.0 30009.0 30010.0 30011.0 30012.0 30013.0 30014.0 30015.0 30016.0 30017.0 30018.0 30019.0 30020.0 30021.0 30022.0 30023.0 30024.0 30025.0 30026.0 30027.0 30028.0 30029.0 30030.0 30031.0 30032.0 30033.0 30034.0 30035.0 30036.0 30037.0 30038.0 30039.0 30040.0 30041.0 30042.0 30043.0 30044.0 30045.0 30046.0 30047.0 30048.0 30049.0 30050.0 30051.0 30052.0 30053.0 30054.0 30055.0 30056.0 30057.0 30058.0 30059.0 30060.0 30061.0 30062.0 30063.0 30064.0 30065.0 30066.0 30067.0 30068.0 30069.0 30070.0 30071.0 30072.0 30073.0 30074.0 30075.0 30076.0 30077.0 30078.0 30079.0 30080.0 30081.0 30082.0 30083.0 30084.0 30085.0 30086.0 30087.0 30088.0 30089.0 30090.0 30091.0 30092.0 30093.0 30094.0 30095.0 30096.0 30097.0 30098.0 30099.0 30100.0 30101.0 30102.0 30103.0 30104.0 30105.0 30106.0 30107.0 30108.0 30109.0 30110.0 30111.0 30112.0 30113.0 30114.0 30115.0 30116.0 30117.0 30118.0 30119.0 30120.0 30121.0 30122.0 30123.0 30124.0 30125.0 30126.0 30127.0 30128.0 30129.0 30130.0 30131.0 30132.0 30133.0 30134.0 30135.0 30136.0 30137.0 30138.0 30139.0 30140.0 30141.0 30142.0 30143.0 30144.0 30145.0 30146.0 30147.0 30148.0 30149.0 30150.0 30151.0 30152.0 30153.0 30154.0 30155.0 30156.0 30157.0 30158.0 30159.0 30160.0 30161.0 30162.0 30163.0 30164.0 30165.0 30166.0 30167.0 30168.0 30169.0 30170.0 30171.0 30172.0 30173.0 30174.0 30175.0 30176.0 30177.0 30178.0 30179.0 30180.0 30181.0 30182.0 30183.0 30184.0 30185.0 30186.0 30187.0 30188.0 30189.0 30190.0 30191.0 30192.0 30193.0 30194.0 30195.0 30196.0 30197.0 30198.0 30199.0 30200.0 30201.0 30202.0 30203.0 30204.0 30205.0 30206.0 30207.0 30208.0 30209.0 30210.0 30211.0 30212.0 30213.0 30214.0 30215.0 30216.0 30217.0 30218.0 30219.0 30220.0 30221.0 30222.0 30223.0 30224.0 30225.0 30226.0 30227.0 30228.0 30229.0 30230.0 30231.0 30232.0 30233.0 30234.0 30235.0 30236.0 30237.0 30238.0 30239.0 30240.0 30241.0 30242.0 30243.0 30244.0 30245.0 30246.0 30247.0 30248.0 30249.0 30250.0 30251.0 30252.0 30253.0 30254.0 30255.0 30256.0 30257.0 30258.0 30259.0 30260.0 30261.0 30262.0 30263.0 30264.0 30265.0 30266.0 30267.0 30268.0 30269.0 30270.0 30271.0 30272.0 30273.0 30274.0 30275.0 30276.0 30277.0 30278.0 30279.0 30280.0 30281.0 30282.0 30283.0 30284.0 30285.0 30286.0 30287.0 30288.0 30289.0 30290.0 30291.0 30292.0 30293.0 30294.0 30295.0 30296.0 30297.0 30298.0 30299.0 30300.0 30301.0 30302.0 30303.0 30304.0 30305.0 30306.0 30307.0 30308.0 30309.0 30310.0 30311.0 30312.0 30313.0 30314.0 30315.0 30316.0 30317.0 30318.0 30319.0 30320.0 30321.0 30322.0 30323.0 30324.0 30325.0 30326.0 30327.0 30328.0 30329.0 30330.0 30331.0 30332.0 30333.0 30334.0 30335.0 30336.0 30337.0 30338.0 30339.0 30340.0 30341.0 30342.0 30343.0 30344.0 30345.0 30346.0 30347.0 30348.0 30349.0 30350.0 30351.0 30352.0 30353.0 30354.0 30355.0 30356.0 30357.0 30358.0 30359.0 30360.0 30361.0 30362.0 30363.0 30364.0 30365.0 30366.0 30367.0 30368.0 30369.0 30370.0 30371.0 30372.0 30373.0 30374.0 30375.0 30376.0 30377.0 30378.0 30379.0 30380.0 30381.0 30382.0 30383.0 30384.0 30385.0 30386.0 30387.0 30388.0 30389.0 30390.0 30391.0 30392.0 30393.0 30394.0 30395.0 30396.0 30397.0 30398.0 30399.0 30400.0 30401.0 30402.0 30403.0 30404.0 30405.0 30406.0 30407.0 30408.0 30409.0 30410.0 30411.0 30412.0 30413.0 30414.0 30415.0 30416.0 30417.0 30418.0 30419.0 30420.0 30421.0 30422.0 30423.0 30424.0 30425.0 30426.0 30427.0 30428.0 30429.0 30430.0 30431.0 30432.0 30433.0 30434.0 30435.0 30436.0 30437.0 30438.0 30439.0 30440.0 30441.0 30442.0 30443.0 30444.0 30445.0 30446.0 30447.0 30448.0 30449.0 30450.0 30451.0 30452.0 30453.0 30454.0 30455.0 30456.0 30457.0 30458.0 30459.0 30460.0 30461.0 30462.0 30463.0 30464.0 30465.0 30466.0 30467.0 30468.0 30469.0 30470.0 30471.0 30472.0 30473.0 30474.0 30475.0 30476.0 30477.0 30478.0 30479.0 30480.0 30481.0 30482.0 30483.0 30484.0 30485.0 30486.0 30487.0 30488.0 30489.0 30490.0 30491.0 30492.0 30493.0 30494.0 30495.0 30496.0 30497.0 30498.0 30499.0 30500.0 30501.0 30502.0 30503.0 30504.0 30505.0 30506.0 30507.0 30508.0 30509.0 30510.0 30511.0 30512.0 30513.0 30514.0 30515.0 30516.0 30517.0 30518.0 30519.0 30520.0 30521.0 30522.0 30523.0 30524.0 30525.0 30526.0 30527.0 30528.0 30529.0 30530.0 30531.0 30532.0 30533.0 30534.0 30535.0 30536.0 30537.0 30538.0 30539.0 30540.0 30541.0 30542.0 30543.0 30544.0 30545.0 30546.0 30547.0 30548.0 30549.0 30550.0 30551.0 30552.0 30553.0 30554.0 30555.0 30556.0 30557.0 30558.0 30559.0 30560.0 30561.0 30562.0 30563.0 30564.0 30565.0 30566.0 30567.0 30568.0 30569.0 30570.0 30571.0 30572.0 30573.0 30574.0 30575.0 30576.0 30577.0 30578.0 30579.0 30580.0 30581.0 30582.0 30583.0 30584.0 30585.0 30586.0 30587.0 30588.0 30589.0 30590.0 30591.0 30592.0 30593.0 30594.0 30595.0 30596.0 30597.0 30598.0 30599.0 30600.0 30601.0 30602.0 30603.0 30604.0 30605.0 30606.0 30607.0 30608.0 30609.0 30610.0 30611.0 30612.0 30613.0 30614.0 30615.0 30616.0 30617.0 30618.0 30619.0 30620.0 30621.0 30622.0 30623.0 30624.0 30625.0 30626.0 30627.0 30628.0 30629.0 30630.0 30631.0 30632.0 30633.0 30634.0 30635.0 30636.0 30637.0 30638.0 30639.0 30640.0 30641.0 30642.0 30643.0 30644.0 30645.0 30646.0 30647.0 30648.0 30649.0 30650.0 30651.0 30652.0 30653.0 30654.0 30655.0 30656.0 30657.0 30658.0 30659.0 30660.0 30661.0 30662.0 30663.0 30664.0 30665.0 30666.0 30667.0 30668.0 30669.0 30670.0 30671.0 30672.0 30673.0 30674.0 30675.0 30676.0 30677.0 30678.0 30679.0 30680.0 30681.0 30682.0 30683.0 30684.0 30685.0 30686.0 30687.0 30688.0 30689.0 30690.0 30691.0 30692.0 30693.0 30694.0 30695.0 30696.0 30697.0 30698.0 30699.0 30700.0 30701.0 30702.0 30703.0 30704.0 30705.0 30706.0 30707.0 30708.0 30709.0 30710.0 30711.0 30712.0 30713.0 30714.0 30715.0 30716.0 30717.0 30718.0 30719.0 30720.0 30721.0 30722.0 30723.0 30724.0 30725.0 30726.0 30727.0 30728.0 30729.0 30730.0 30731.0 30732.0 30733.0 30734.0 30735.0 30736.0 30737.0 30738.0 30739.0 30740.0 30741.0 30742.0 30743.0 30744.0 30745.0 30746.0 30747.0 30748.0 30749.0 30750.0 30751.0 30752.0 30753.0 30754.0 30755.0 30756.0 30757.0 30758.0 30759.0 30760.0 30761.0 30762.0 30763.0 30764.0 30765.0 30766.0 30767.0 30768.0 30769.0 30770.0 30771.0 30772.0 30773.0 30774.0 30775.0 30776.0 30777.0 30778.0 30779.0 30780.0 30781.0 30782.0 30783.0 30784.0 30785.0 30786.0 30787.0 30788.0 30789.0 30790.0 30791.0 30792.0 30793.0 30794.0 30795.0 30796.0 30797.0 30798.0 30799.0 30800.0 30801.0 30802.0 30803.0 30804.0 30805.0 30806.0 30807.0 30808.0 30809.0 30810.0 30811.0 30812.0 30813.0 30814.0 30815.0 30816.0 30817.0 30818.0 30819.0 30820.0 30821.0 30822.0 30823.0 30824.0 30825.0 30826.0 30827.0 30828.0 30829.0 30830.0 30831.0 30832.0 30833.0 30834.0 30835.0 30836.0 30837.0 30838.0 30839.0 30840.0 30841.0 30842.0 30843.0 30844.0 30845.0 30846.0 30847.0 30848.0 30849.0 30850.0 30851.0 30852.0 30853.0 30854.0 30855.0 30856.0 30857.0 30858.0 30859.0 30860.0 30861.0 30862.0 30863.0 30864.0 30865.0 30866.0 30867.0 30868.0 30869.0 30870.0 30871.0 30872.0 30873.0 30874.0 30875.0 30876.0 30877.0 30878.0 30879.0 30880.0 30881.0 30882.0 30883.0 30884.0 30885.0 30886.0 30887.0 30888.0 30889.0 30890.0 30891.0 30892.0 30893.0 30894.0 30895.0 30896.0 30897.0 30898.0 30899.0 30900.0 30901.0 30902.0 30903.0 30904.0 30905.0 30906.0 30907.0 30908.0 30909.0 30910.0 30911.0 30912.0 30913.0 30914.0 30915.0 30916.0 30917.0 30918.0 30919.0 30920.0 30921.0 30922.0 30923.0 30924.0 30925.0 30926.0 30927.0 30928.0 30929.0 30930.0 30931.0 30932.0 30933.0 30934.0 30935.0 30936.0 30937.0 30938.0 30939.0 30940.0 30941.0 30942.0 30943.0 30944.0 30945.0 30946.0 30947.0 30948.0 30949.0 30950.0 30951.0 30952.0 30953.0 30954.0 30955.0 30956.0 30957.0 30958.0 30959.0 30960.0 30961.0 30962.0 30963.0 30964.0 30965.0 30966.0 30967.0 30968.0 30969.0 30970.0 30971.0 30972.0 30973.0 30974.0 30975.0 30976.0 30977.0 30978.0 30979.0 30980.0 30981.0 30982.0 30983.0 30984.0 30985.0 30986.0 30987.0 30988.0 30989.0 30990.0 30991.0 30992.0 30993.0 30994.0 30995.0 30996.0 30997.0 30998.0 30999.0 31000.0 31001.0 31002.0 31003.0 31004.0 31005.0 31006.0 31007.0 31008.0 31009.0 31010.0 31011.0 31012.0 31013.0 31014.0 31015.0 31016.0 31017.0 31018.0 31019.0 31020.0 31021.0 31022.0 31023.0 31024.0 31025.0 31026.0 31027.0 31028.0 31029.0 31030.0 31031.0 31032.0 31033.0 31034.0 31035.0 31036.0 31037.0 31038.0 31039.0 31040.0 31041.0 31042.0 31043.0 31044.0 31045.0 31046.0 31047.0 31048.0 31049.0 31050.0 31051.0 31052.0 31053.0 31054.0 31055.0 31056.0 31057.0 31058.0 31059.0 31060.0 31061.0 31062.0 31063.0 31064.0 31065.0 31066.0 31067.0 31068.0 31069.0 31070.0 31071.0 31072.0 31073.0 31074.0 31075.0 31076.0 31077.0 31078.0 31079.0 31080.0 31081.0 31082.0 31083.0 31084.0 31085.0 31086.0 31087.0 31088.0 31089.0 31090.0 31091.0 31092.0 31093.0 31094.0 31095.0 31096.0 31097.0 31098.0 31099.0 31100.0 31101.0 31102.0 31103.0 31104.0 31105.0 31106.0 31107.0 31108.0 31109.0 31110.0 31111.0 31112.0 31113.0 31114.0 31115.0 31116.0 31117.0 31118.0 31119.0 31120.0 31121.0 31122.0 31123.0 31124.0 31125.0 31126.0 31127.0 31128.0 31129.0 31130.0 31131.0 31132.0 31133.0 31134.0 31135.0 31136.0 31137.0 31138.0 31139.0 31140.0 31141.0 31142.0 31143.0 31144.0 31145.0 31146.0 31147.0 31148.0 31149.0 31150.0 31151.0 31152.0 31153.0 31154.0 31155.0 31156.0 31157.0 31158.0 31159.0 31160.0 31161.0 31162.0 31163.0 31164.0 31165.0 31166.0 31167.0 31168.0 31169.0 31170.0 31171.0 31172.0 31173.0 31174.0 31175.0 31176.0 31177.0 31178.0 31179.0 31180.0 31181.0 31182.0 31183.0 31184.0 31185.0 31186.0 31187.0 31188.0 31189.0 31190.0 31191.0 31192.0 31193.0 31194.0 31195.0 31196.0 31197.0 31198.0 31199.0 31200.0 31201.0 31202.0 31203.0 31204.0 31205.0 31206.0 31207.0 31208.0 31209.0 31210.0 31211.0 31212.0 31213.0 31214.0 31215.0 31216.0 31217.0 31218.0 31219.0 31220.0 31221.0 31222.0 31223.0 31224.0 31225.0 31226.0 31227.0 31228.0 31229.0 31230.0 31231.0 31232.0 31233.0 31234.0 31235.0 31236.0 31237.0 31238.0 31239.0 31240.0 31241.0 31242.0 31243.0 31244.0 31245.0 31246.0 31247.0 31248.0 31249.0 31250.0 31251.0 31252.0 31253.0 31254.0 31255.0 31256.0 31257.0 31258.0 31259.0 31260.0 31261.0 31262.0 31263.0 31264.0 31265.0 31266.0 31267.0 31268.0 31269.0 31270.0 31271.0 31272.0 31273.0 31274.0 31275.0 31276.0 31277.0 31278.0 31279.0 31280.0 31281.0 31282.0 31283.0 31284.0 31285.0 31286.0 31287.0 31288.0 31289.0 31290.0 31291.0 31292.0 31293.0 31294.0 31295.0 31296.0 31297.0 31298.0 31299.0 31300.0 31301.0 31302.0 31303.0 31304.0 31305.0 31306.0 31307.0 31308.0 31309.0 31310.0 31311.0 31312.0 31313.0 31314.0 31315.0 31316.0 31317.0 31318.0 31319.0 31320.0 31321.0 31322.0 31323.0 31324.0 31325.0 31326.0 31327.0 31328.0 31329.0 31330.0 31331.0 31332.0 31333.0 31334.0 31335.0 31336.0 31337.0 31338.0 31339.0 31340.0 31341.0 31342.0 31343.0 31344.0 31345.0 31346.0 31347.0 31348.0 31349.0 31350.0 31351.0 31352.0 31353.0 31354.0 31355.0 31356.0 31357.0 31358.0 31359.0 31360.0 31361.0 31362.0 31363.0 31364.0 31365.0 31366.0 31367.0 31368.0 31369.0 31370.0 31371.0 31372.0 31373.0 31374.0 31375.0 31376.0 31377.0 31378.0 31379.0 31380.0 31381.0 31382.0 31383.0 31384.0 31385.0 31386.0 31387.0 31388.0 31389.0 31390.0 31391.0 31392.0 31393.0 31394.0 31395.0 31396.0 31397.0 31398.0 31399.0 31400.0 31401.0 31402.0 31403.0 31404.0 31405.0 31406.0 31407.0 31408.0 31409.0 31410.0 31411.0 31412.0 31413.0 31414.0 31415.0 31416.0 31417.0 31418.0 31419.0 31420.0 31421.0 31422.0 31423.0 31424.0 31425.0 31426.0 31427.0 31428.0 31429.0 31430.0 31431.0 31432.0 31433.0 31434.0 31435.0 31436.0 31437.0 31438.0 31439.0 31440.0 31441.0 31442.0 31443.0 31444.0 31445.0 31446.0 31447.0 31448.0 31449.0 31450.0 31451.0 31452.0 31453.0 31454.0 31455.0 31456.0 31457.0 31458.0 31459.0 31460.0 31461.0 31462.0 31463.0 31464.0 31465.0 31466.0 31467.0 31468.0 31469.0 31470.0 31471.0 31472.0 31473.0 31474.0 31475.0 31476.0 31477.0 31478.0 31479.0 31480.0 31481.0 31482.0 31483.0 31484.0 31485.0 31486.0 31487.0 31488.0 31489.0 31490.0 31491.0 31492.0 31493.0 31494.0 31495.0 31496.0 31497.0 31498.0 31499.0 31500.0 31501.0 31502.0 31503.0 31504.0 31505.0 31506.0 31507.0 31508.0 31509.0 31510.0 31511.0 31512.0 31513.0 31514.0 31515.0 31516.0 31517.0 31518.0 31519.0 31520.0 31521.0 31522.0 31523.0 31524.0 31525.0 31526.0 31527.0 31528.0 31529.0 31530.0 31531.0 31532.0 31533.0 31534.0 31535.0 31536.0 31537.0 31538.0 31539.0 31540.0 31541.0 31542.0 31543.0 31544.0 31545.0 31546.0 31547.0 31548.0 31549.0 31550.0 31551.0 31552.0 31553.0 31554.0 31555.0 31556.0 31557.0 31558.0 31559.0 31560.0 31561.0 31562.0 31563.0 31564.0 31565.0 31566.0 31567.0 31568.0 31569.0 31570.0 31571.0 31572.0 31573.0 31574.0 31575.0 31576.0 31577.0 31578.0 31579.0 31580.0 31581.0 31582.0 31583.0 31584.0 31585.0 31586.0 31587.0 31588.0 31589.0 31590.0 31591.0 31592.0 31593.0 31594.0 31595.0 31596.0 31597.0 31598.0 31599.0 31600.0 31601.0 31602.0 31603.0 31604.0 31605.0 31606.0 31607.0 31608.0 31609.0 31610.0 31611.0 31612.0 31613.0 31614.0 31615.0 31616.0 31617.0 31618.0 31619.0 31620.0 31621.0 31622.0 31623.0 31624.0 31625.0 31626.0 31627.0 31628.0 31629.0 31630.0 31631.0 31632.0 31633.0 31634.0 31635.0 31636.0 31637.0 31638.0 31639.0 31640.0 31641.0 31642.0 31643.0 31644.0 31645.0 31646.0 31647.0 31648.0 31649.0 31650.0 31651.0 31652.0 31653.0 31654.0 31655.0 31656.0 31657.0 31658.0 31659.0 31660.0 31661.0 31662.0 31663.0 31664.0 31665.0 31666.0 31667.0 31668.0 31669.0 31670.0 31671.0 31672.0 31673.0 31674.0 31675.0 31676.0 31677.0 31678.0 31679.0 31680.0 31681.0 31682.0 31683.0 31684.0 31685.0 31686.0 31687.0 31688.0 31689.0 31690.0 31691.0 31692.0 31693.0 31694.0 31695.0 31696.0 31697.0 31698.0 31699.0 31700.0 31701.0 31702.0 31703.0 31704.0 31705.0 31706.0 31707.0 31708.0 31709.0 31710.0 31711.0 31712.0 31713.0 31714.0 31715.0 31716.0 31717.0 31718.0 31719.0 31720.0 31721.0 31722.0 31723.0 31724.0 31725.0 31726.0 31727.0 31728.0 31729.0 31730.0 31731.0 31732.0 31733.0 31734.0 31735.0 31736.0 31737.0 31738.0 31739.0 31740.0 31741.0 31742.0 31743.0 31744.0 31745.0 31746.0 31747.0 31748.0 31749.0 31750.0 31751.0 31752.0 31753.0 31754.0 31755.0 31756.0 31757.0 31758.0 31759.0 31760.0 31761.0 31762.0 31763.0 31764.0 31765.0 31766.0 31767.0 31768.0 31769.0 31770.0 31771.0 31772.0 31773.0 31774.0 31775.0 31776.0 31777.0 31778.0 31779.0 31780.0 31781.0 31782.0 31783.0 31784.0 31785.0 31786.0 31787.0 31788.0 31789.0 31790.0 31791.0 31792.0 31793.0 31794.0 31795.0 31796.0 31797.0 31798.0 31799.0 31800.0 31801.0 31802.0 31803.0 31804.0 31805.0 31806.0 31807.0 31808.0 31809.0 31810.0 31811.0 31812.0 31813.0 31814.0 31815.0 31816.0 31817.0 31818.0 31819.0 31820.0 31821.0 31822.0 31823.0 31824.0 31825.0 31826.0 31827.0 31828.0 31829.0 31830.0 31831.0 31832.0 31833.0 31834.0 31835.0 31836.0 31837.0 31838.0 31839.0 31840.0 31841.0 31842.0 31843.0 31844.0 31845.0 31846.0 31847.0 31848.0 31849.0 31850.0 31851.0 31852.0 31853.0 31854.0 31855.0 31856.0 31857.0 31858.0 31859.0 31860.0 31861.0 31862.0 31863.0 31864.0 31865.0 31866.0 31867.0 31868.0 31869.0 31870.0 31871.0 31872.0 31873.0 31874.0 31875.0 31876.0 31877.0 31878.0 31879.0 31880.0 31881.0 31882.0 31883.0 31884.0 31885.0 31886.0 31887.0 31888.0 31889.0 31890.0 31891.0 31892.0 31893.0 31894.0 31895.0 31896.0 31897.0 31898.0 31899.0 31900.0 31901.0 31902.0 31903.0 31904.0 31905.0 31906.0 31907.0 31908.0 31909.0 31910.0 31911.0 31912.0 31913.0 31914.0 31915.0 31916.0 31917.0 31918.0 31919.0 31920.0 31921.0 31922.0 31923.0 31924.0 31925.0 31926.0 31927.0 31928.0 31929.0 31930.0 31931.0 31932.0 31933.0 31934.0 31935.0 31936.0 31937.0 31938.0 31939.0 31940.0 31941.0 31942.0 31943.0 31944.0 31945.0 31946.0 31947.0 31948.0 31949.0 31950.0 31951.0 31952.0 31953.0 31954.0 31955.0 31956.0 31957.0 31958.0 31959.0 31960.0 31961.0 31962.0 31963.0 31964.0 31965.0 31966.0 31967.0 31968.0 31969.0 31970.0 31971.0 31972.0 31973.0 31974.0 31975.0 31976.0 31977.0 31978.0 31979.0 31980.0 31981.0 31982.0 31983.0 31984.0 31985.0 31986.0 31987.0 31988.0 31989.0 31990.0 31991.0 31992.0 31993.0 31994.0 31995.0 31996.0 31997.0 31998.0 31999.0 32000.0 32001.0 32002.0 32003.0 32004.0 32005.0 32006.0 32007.0 32008.0 32009.0 32010.0 32011.0 32012.0 32013.0 32014.0 32015.0 32016.0 32017.0 32018.0 32019.0 32020.0 32021.0 32022.0 32023.0 32024.0 32025.0 32026.0 32027.0 32028.0 32029.0 32030.0 32031.0 32032.0 32033.0 32034.0 32035.0 32036.0 32037.0 32038.0 32039.0 32040.0 32041.0 32042.0 32043.0 32044.0 32045.0 32046.0 32047.0 32048.0 32049.0 32050.0 32051.0 32052.0 32053.0 32054.0 32055.0 32056.0 32057.0 32058.0 32059.0 32060.0 32061.0 32062.0 32063.0 32064.0 32065.0 32066.0 32067.0 32068.0 32069.0 32070.0 32071.0 32072.0 32073.0 32074.0 32075.0 32076.0 32077.0 32078.0 32079.0 32080.0 32081.0 32082.0 32083.0 32084.0 32085.0 32086.0 32087.0 32088.0 32089.0 32090.0 32091.0 32092.0 32093.0 32094.0 32095.0 32096.0 32097.0 32098.0 32099.0 32100.0 32101.0 32102.0 32103.0 32104.0 32105.0 32106.0 32107.0 32108.0 32109.0 32110.0 32111.0 32112.0 32113.0 32114.0 32115.0 32116.0 32117.0 32118.0 32119.0 32120.0 32121.0 32122.0 32123.0 32124.0 32125.0 32126.0 32127.0 32128.0 32129.0 32130.0 32131.0 32132.0 32133.0 32134.0 32135.0 32136.0 32137.0 32138.0 32139.0 32140.0 32141.0 32142.0 32143.0 32144.0 32145.0 32146.0 32147.0 32148.0 32149.0 32150.0 32151.0 32152.0 32153.0 32154.0 32155.0 32156.0 32157.0 32158.0 32159.0 32160.0 32161.0 32162.0 32163.0 32164.0 32165.0 32166.0 32167.0 32168.0 32169.0 32170.0 32171.0 32172.0 32173.0 32174.0 32175.0 32176.0 32177.0 32178.0 32179.0 32180.0 32181.0 32182.0 32183.0 32184.0 32185.0 32186.0 32187.0 32188.0 32189.0 32190.0 32191.0 32192.0 32193.0 32194.0 32195.0 32196.0 32197.0 32198.0 32199.0 32200.0 32201.0 32202.0 32203.0 32204.0 32205.0 32206.0 32207.0 32208.0 32209.0 32210.0 32211.0 32212.0 32213.0 32214.0 32215.0 32216.0 32217.0 32218.0 32219.0 32220.0 32221.0 32222.0 32223.0 32224.0 32225.0 32226.0 32227.0 32228.0 32229.0 32230.0 32231.0 32232.0 32233.0 32234.0 32235.0 32236.0 32237.0 32238.0 32239.0 32240.0 32241.0 32242.0 32243.0 32244.0 32245.0 32246.0 32247.0 32248.0 32249.0 32250.0 32251.0 32252.0 32253.0 32254.0 32255.0 32256.0 32257.0 32258.0 32259.0 32260.0 32261.0 32262.0 32263.0 32264.0 32265.0 32266.0 32267.0 32268.0 32269.0 32270.0 32271.0 32272.0 32273.0 32274.0 32275.0 32276.0 32277.0 32278.0 32279.0 32280.0 32281.0 32282.0 32283.0 32284.0 32285.0 32286.0 32287.0 32288.0 32289.0 32290.0 32291.0 32292.0 32293.0 32294.0 32295.0 32296.0 32297.0 32298.0 32299.0 32300.0 32301.0 32302.0 32303.0 32304.0 32305.0 32306.0 32307.0 32308.0 32309.0 32310.0 32311.0 32312.0 32313.0 32314.0 32315.0 32316.0 32317.0 32318.0 32319.0 32320.0 32321.0 32322.0 32323.0 32324.0 32325.0 32326.0 32327.0 32328.0 32329.0 32330.0 32331.0 32332.0 32333.0 32334.0 32335.0 32336.0 32337.0 32338.0 32339.0 32340.0 32341.0 32342.0 32343.0 32344.0 32345.0 32346.0 32347.0 32348.0 32349.0 32350.0 32351.0 32352.0 32353.0 32354.0 32355.0 32356.0 32357.0 32358.0 32359.0 32360.0 32361.0 32362.0 32363.0 32364.0 32365.0 32366.0 32367.0 32368.0 32369.0 32370.0 32371.0 32372.0 32373.0 32374.0 32375.0 32376.0 32377.0 32378.0 32379.0 32380.0 32381.0 32382.0 32383.0 32384.0 32385.0 32386.0 32387.0 32388.0 32389.0 32390.0 32391.0 32392.0 32393.0 32394.0 32395.0 32396.0 32397.0 32398.0 32399.0 32400.0 32401.0 32402.0 32403.0 32404.0 32405.0 32406.0 32407.0 32408.0 32409.0 32410.0 32411.0 32412.0 32413.0 32414.0 32415.0 32416.0 32417.0 32418.0 32419.0 32420.0 32421.0 32422.0 32423.0 32424.0 32425.0 32426.0 32427.0 32428.0 32429.0 32430.0 32431.0 32432.0 32433.0 32434.0 32435.0 32436.0 32437.0 32438.0 32439.0 32440.0 32441.0 32442.0 32443.0 32444.0 32445.0 32446.0 32447.0 32448.0 32449.0 32450.0 32451.0 32452.0 32453.0 32454.0 32455.0 32456.0 32457.0 32458.0 32459.0 32460.0 32461.0 32462.0 32463.0 32464.0 32465.0 32466.0 32467.0 32468.0 32469.0 32470.0 32471.0 32472.0 32473.0 32474.0 32475.0 32476.0 32477.0 32478.0 32479.0 32480.0 32481.0 32482.0 32483.0 32484.0 32485.0 32486.0 32487.0 32488.0 32489.0 32490.0 32491.0 32492.0 32493.0 32494.0 32495.0 32496.0 32497.0 32498.0 32499.0 32500.0 32501.0 32502.0 32503.0 32504.0 32505.0 32506.0 32507.0 32508.0 32509.0 32510.0 32511.0 32512.0 32513.0 32514.0 32515.0 32516.0 32517.0 32518.0 32519.0 32520.0 32521.0 32522.0 32523.0 32524.0 32525.0 32526.0 32527.0 32528.0 32529.0 32530.0 32531.0 32532.0 32533.0 32534.0 32535.0 32536.0 32537.0 32538.0 32539.0 32540.0 32541.0 32542.0 32543.0 32544.0 32545.0 32546.0 32547.0 32548.0 32549.0 32550.0 32551.0 32552.0 32553.0 32554.0 32555.0 32556.0 32557.0 32558.0 32559.0 32560.0 32561.0 32562.0 32563.0 32564.0 32565.0 32566.0 32567.0 32568.0 32569.0 32570.0 32571.0 32572.0 32573.0 32574.0 32575.0 32576.0 32577.0 32578.0 32579.0 32580.0 32581.0 32582.0 32583.0 32584.0 32585.0 32586.0 32587.0 32588.0 32589.0 32590.0 32591.0 32592.0 32593.0 32594.0 32595.0 32596.0 32597.0 32598.0 32599.0 32600.0 32601.0 32602.0 32603.0 32604.0 32605.0 32606.0 32607.0 32608.0 32609.0 32610.0 32611.0 32612.0 32613.0 32614.0 32615.0 32616.0 32617.0 32618.0 32619.0 32620.0 32621.0 32622.0 32623.0 32624.0 32625.0 32626.0 32627.0 32628.0 32629.0 32630.0 32631.0 32632.0 32633.0 32634.0 32635.0 32636.0 32637.0 32638.0 32639.0 32640.0 32641.0 32642.0 32643.0 32644.0 32645.0 32646.0 32647.0 32648.0 32649.0 32650.0 32651.0 32652.0 32653.0 32654.0 32655.0 32656.0 32657.0 32658.0 32659.0 32660.0 32661.0 32662.0 32663.0 32664.0 32665.0 32666.0 32667.0 32668.0 32669.0 32670.0 32671.0 32672.0 32673.0 32674.0 32675.0 32676.0 32677.0 32678.0 32679.0 32680.0 32681.0 32682.0 32683.0 32684.0 32685.0 32686.0 32687.0 32688.0 32689.0 32690.0 32691.0 32692.0 32693.0 32694.0 32695.0 32696.0 32697.0 32698.0 32699.0 32700.0 32701.0 32702.0 32703.0 32704.0 32705.0 32706.0 32707.0 32708.0 32709.0 32710.0 32711.0 32712.0 32713.0 32714.0 32715.0 32716.0 32717.0 32718.0 32719.0 32720.0 32721.0 32722.0 32723.0 32724.0 32725.0 32726.0 32727.0 32728.0 32729.0 32730.0 32731.0 32732.0 32733.0 32734.0 32735.0 32736.0 32737.0 32738.0 32739.0 32740.0 32741.0 32742.0 32743.0 32744.0 32745.0 32746.0 32747.0 32748.0 32749.0 32750.0 32751.0 32752.0 32753.0 32754.0 32755.0 32756.0 32757.0 32758.0 32759.0 32760.0 32761.0 32762.0 32763.0 32764.0 32765.0 32766.0 32767.0 32768.0 32769.0 32770.0 32771.0 32772.0 32773.0 32774.0 32775.0 32776.0 32777.0 32778.0 32779.0 32780.0 32781.0 32782.0 32783.0 32784.0 32785.0 32786.0 32787.0 32788.0 32789.0 32790.0 32791.0 32792.0 32793.0 32794.0 32795.0 32796.0 32797.0 32798.0 32799.0 32800.0 32801.0 32802.0 32803.0 32804.0 32805.0 32806.0 32807.0 32808.0 32809.0 32810.0 32811.0 32812.0 32813.0 32814.0 32815.0 32816.0 32817.0 32818.0 32819.0 32820.0 32821.0 32822.0 32823.0 32824.0 32825.0 32826.0 32827.0 32828.0 32829.0 32830.0 32831.0 32832.0 32833.0 32834.0 32835.0 32836.0 32837.0 32838.0 32839.0 32840.0 32841.0 32842.0 32843.0 32844.0 32845.0 32846.0 32847.0 32848.0 32849.0 32850.0 32851.0 32852.0 32853.0 32854.0 32855.0 32856.0 32857.0 32858.0 32859.0 32860.0 32861.0 32862.0 32863.0 32864.0 32865.0 32866.0 32867.0 32868.0 32869.0 32870.0 32871.0 32872.0 32873.0 32874.0 32875.0 32876.0 32877.0 32878.0 32879.0 32880.0 32881.0 32882.0 32883.0 32884.0 32885.0 32886.0 32887.0 32888.0 32889.0 32890.0 32891.0 32892.0 32893.0 32894.0 32895.0 32896.0 32897.0 32898.0 32899.0 32900.0 32901.0 32902.0 32903.0 32904.0 32905.0 32906.0 32907.0 32908.0 32909.0 32910.0 32911.0 32912.0 32913.0 32914.0 32915.0 32916.0 32917.0 32918.0 32919.0 32920.0 32921.0 32922.0 32923.0 32924.0 32925.0 32926.0 32927.0 32928.0 32929.0 32930.0 32931.0 32932.0 32933.0 32934.0 32935.0 32936.0 32937.0 32938.0 32939.0 32940.0 32941.0 32942.0 32943.0 32944.0 32945.0 32946.0 32947.0 32948.0 32949.0 32950.0 32951.0 32952.0 32953.0 32954.0 32955.0 32956.0 32957.0 32958.0 32959.0 32960.0 32961.0 32962.0 32963.0 32964.0 32965.0 32966.0 32967.0 32968.0 32969.0 32970.0 32971.0 32972.0 32973.0 32974.0 32975.0 32976.0 32977.0 32978.0 32979.0 32980.0 32981.0 32982.0 32983.0 32984.0 32985.0 32986.0 32987.0 32988.0 32989.0 32990.0 32991.0 32992.0 32993.0 32994.0 32995.0 32996.0 32997.0 32998.0 32999.0 33000.0 33001.0 33002.0 33003.0 33004.0 33005.0 33006.0 33007.0 33008.0 33009.0 33010.0 33011.0 33012.0 33013.0 33014.0 33015.0 33016.0 33017.0 33018.0 33019.0 33020.0 33021.0 33022.0 33023.0 33024.0 33025.0 33026.0 33027.0 33028.0 33029.0 33030.0 33031.0 33032.0 33033.0 33034.0 33035.0 33036.0 33037.0 33038.0 33039.0 33040.0 33041.0 33042.0 33043.0 33044.0 33045.0 33046.0 33047.0 33048.0 33049.0 33050.0 33051.0 33052.0 33053.0 33054.0 33055.0 33056.0 33057.0 33058.0 33059.0 33060.0 33061.0 33062.0 33063.0 33064.0 33065.0 33066.0 33067.0 33068.0 33069.0 33070.0 33071.0 33072.0 33073.0 33074.0 33075.0 33076.0 33077.0 33078.0 33079.0 33080.0 33081.0 33082.0 33083.0 33084.0 33085.0 33086.0 33087.0 33088.0 33089.0 33090.0 33091.0 33092.0 33093.0 33094.0 33095.0 33096.0 33097.0 33098.0 33099.0 33100.0 33101.0 33102.0 33103.0 33104.0 33105.0 33106.0 33107.0 33108.0 33109.0 33110.0 33111.0 33112.0 33113.0 33114.0 33115.0 33116.0 33117.0 33118.0 33119.0 33120.0 33121.0 33122.0 33123.0 33124.0 33125.0 33126.0 33127.0 33128.0 33129.0 33130.0 33131.0 33132.0 33133.0 33134.0 33135.0 33136.0 33137.0 33138.0 33139.0 33140.0 33141.0 33142.0 33143.0 33144.0 33145.0 33146.0 33147.0 33148.0 33149.0 33150.0 33151.0 33152.0 33153.0 33154.0 33155.0 33156.0 33157.0 33158.0 33159.0 33160.0 33161.0 33162.0 33163.0 33164.0 33165.0 33166.0 33167.0 33168.0 33169.0 33170.0 33171.0 33172.0 33173.0 33174.0 33175.0 33176.0 33177.0 33178.0 33179.0 33180.0 33181.0 33182.0 33183.0 33184.0 33185.0 33186.0 33187.0 33188.0 33189.0 33190.0 33191.0 33192.0 33193.0 33194.0 33195.0 33196.0 33197.0 33198.0 33199.0 33200.0 33201.0 33202.0 33203.0 33204.0 33205.0 33206.0 33207.0 33208.0 33209.0 33210.0 33211.0 33212.0 33213.0 33214.0 33215.0 33216.0 33217.0 33218.0 33219.0 33220.0 33221.0 33222.0 33223.0 33224.0 33225.0 33226.0 33227.0 33228.0 33229.0 33230.0 33231.0 33232.0 33233.0 33234.0 33235.0 33236.0 33237.0 33238.0 33239.0 33240.0 33241.0 33242.0 33243.0 33244.0 33245.0 33246.0 33247.0 33248.0 33249.0 33250.0 33251.0 33252.0 33253.0 33254.0 33255.0 33256.0 33257.0 33258.0 33259.0 33260.0 33261.0 33262.0 33263.0 33264.0 33265.0 33266.0 33267.0 33268.0 33269.0 33270.0 33271.0 33272.0 33273.0 33274.0 33275.0 33276.0 33277.0 33278.0 33279.0 33280.0 33281.0 33282.0 33283.0 33284.0 33285.0 33286.0 33287.0 33288.0 33289.0 33290.0 33291.0 33292.0 33293.0 33294.0 33295.0 33296.0 33297.0 33298.0 33299.0 33300.0 33301.0 33302.0 33303.0 33304.0 33305.0 33306.0 33307.0 33308.0 33309.0 33310.0 33311.0 33312.0 33313.0 33314.0 33315.0 33316.0 33317.0 33318.0 33319.0 33320.0 33321.0 33322.0 33323.0 33324.0 33325.0 33326.0 33327.0 33328.0 33329.0 33330.0 33331.0 33332.0 33333.0 33334.0 33335.0 33336.0 33337.0 33338.0 33339.0 33340.0 33341.0 33342.0 33343.0 33344.0 33345.0 33346.0 33347.0 33348.0 33349.0 33350.0 33351.0 33352.0 33353.0 33354.0 33355.0 33356.0 33357.0 33358.0 33359.0 33360.0 33361.0 33362.0 33363.0 33364.0 33365.0 33366.0 33367.0 33368.0 33369.0 33370.0 33371.0 33372.0 33373.0 33374.0 33375.0 33376.0 33377.0 33378.0 33379.0 33380.0 33381.0 33382.0 33383.0 33384.0 33385.0 33386.0 33387.0 33388.0 33389.0 33390.0 33391.0 33392.0 33393.0 33394.0 33395.0 33396.0 33397.0 33398.0 33399.0 33400.0 33401.0 33402.0 33403.0 33404.0 33405.0 33406.0 33407.0 33408.0 33409.0 33410.0 33411.0 33412.0 33413.0 33414.0 33415.0 33416.0 33417.0 33418.0 33419.0 33420.0 33421.0 33422.0 33423.0 33424.0 33425.0 33426.0 33427.0 33428.0 33429.0 33430.0 33431.0 33432.0 33433.0 33434.0 33435.0 33436.0 33437.0 33438.0 33439.0 33440.0 33441.0 33442.0 33443.0 33444.0 33445.0 33446.0 33447.0 33448.0 33449.0 33450.0 33451.0 33452.0 33453.0 33454.0 33455.0 33456.0 33457.0 33458.0 33459.0 33460.0 33461.0 33462.0 33463.0 33464.0 33465.0 33466.0 33467.0 33468.0 33469.0 33470.0 33471.0 33472.0 33473.0 33474.0 33475.0 33476.0 33477.0 33478.0 33479.0 33480.0 33481.0 33482.0 33483.0 33484.0 33485.0 33486.0 33487.0 33488.0 33489.0 33490.0 33491.0 33492.0 33493.0 33494.0 33495.0 33496.0 33497.0 33498.0 33499.0 33500.0 33501.0 33502.0 33503.0 33504.0 33505.0 33506.0 33507.0 33508.0 33509.0 33510.0 33511.0 33512.0 33513.0 33514.0 33515.0 33516.0 33517.0 33518.0 33519.0 33520.0 33521.0 33522.0 33523.0 33524.0 33525.0 33526.0 33527.0 33528.0 33529.0 33530.0 33531.0 33532.0 33533.0 33534.0 33535.0 33536.0 33537.0 33538.0 33539.0 33540.0 33541.0 33542.0 33543.0 33544.0 33545.0 33546.0 33547.0 33548.0 33549.0 33550.0 33551.0 33552.0 33553.0 33554.0 33555.0 33556.0 33557.0 33558.0 33559.0 33560.0 33561.0 33562.0 33563.0 33564.0 33565.0 33566.0 33567.0 33568.0 33569.0 33570.0 33571.0 33572.0 33573.0 33574.0 33575.0 33576.0 33577.0 33578.0 33579.0 33580.0 33581.0 33582.0 33583.0 33584.0 33585.0 33586.0 33587.0 33588.0 33589.0 33590.0 33591.0 33592.0 33593.0 33594.0 33595.0 33596.0 33597.0 33598.0 33599.0 33600.0 33601.0 33602.0 33603.0 33604.0 33605.0 33606.0 33607.0 33608.0 33609.0 33610.0 33611.0 33612.0 33613.0 33614.0 33615.0 33616.0 33617.0 33618.0 33619.0 33620.0 33621.0 33622.0 33623.0 33624.0 33625.0 33626.0 33627.0 33628.0 33629.0 33630.0 33631.0 33632.0 33633.0 33634.0 33635.0 33636.0 33637.0 33638.0 33639.0 33640.0 33641.0 33642.0 33643.0 33644.0 33645.0 33646.0 33647.0 33648.0 33649.0 33650.0 33651.0 33652.0 33653.0 33654.0 33655.0 33656.0 33657.0 33658.0 33659.0 33660.0 33661.0 33662.0 33663.0 33664.0 33665.0 33666.0 33667.0 33668.0 33669.0 33670.0 33671.0 33672.0 33673.0 33674.0 33675.0 33676.0 33677.0 33678.0 33679.0 33680.0 33681.0 33682.0 33683.0 33684.0 33685.0 33686.0 33687.0 33688.0 33689.0 33690.0 33691.0 33692.0 33693.0 33694.0 33695.0 33696.0 33697.0 33698.0 33699.0 33700.0 33701.0 33702.0 33703.0 33704.0 33705.0 33706.0 33707.0 33708.0 33709.0 33710.0 33711.0 33712.0 33713.0 33714.0 33715.0 33716.0 33717.0 33718.0 33719.0 33720.0 33721.0 33722.0 33723.0 33724.0 33725.0 33726.0 33727.0 33728.0 33729.0 33730.0 33731.0 33732.0 33733.0 33734.0 33735.0 33736.0 33737.0 33738.0 33739.0 33740.0 33741.0 33742.0 33743.0 33744.0 33745.0 33746.0 33747.0 33748.0 33749.0 33750.0 33751.0 33752.0 33753.0 33754.0 33755.0 33756.0 33757.0 33758.0 33759.0 33760.0 33761.0 33762.0 33763.0 33764.0 33765.0 33766.0 33767.0 33768.0 33769.0 33770.0 33771.0 33772.0 33773.0 33774.0 33775.0 33776.0 33777.0 33778.0 33779.0 33780.0 33781.0 33782.0 33783.0 33784.0 33785.0 33786.0 33787.0 33788.0 33789.0 33790.0 33791.0 33792.0 33793.0 33794.0 33795.0 33796.0 33797.0 33798.0 33799.0 33800.0 33801.0 33802.0 33803.0 33804.0 33805.0 33806.0 33807.0 33808.0 33809.0 33810.0 33811.0 33812.0 33813.0 33814.0 33815.0 33816.0 33817.0 33818.0 33819.0 33820.0 33821.0 33822.0 33823.0 33824.0 33825.0 33826.0 33827.0 33828.0 33829.0 33830.0 33831.0 33832.0 33833.0 33834.0 33835.0 33836.0 33837.0 33838.0 33839.0 33840.0 33841.0 33842.0 33843.0 33844.0 33845.0 33846.0 33847.0 33848.0 33849.0 33850.0 33851.0 33852.0 33853.0 33854.0 33855.0 33856.0 33857.0 33858.0 33859.0 33860.0 33861.0 33862.0 33863.0 33864.0 33865.0 33866.0 33867.0 33868.0 33869.0 33870.0 33871.0 33872.0 33873.0 33874.0 33875.0 33876.0 33877.0 33878.0 33879.0 33880.0 33881.0 33882.0 33883.0 33884.0 33885.0 33886.0 33887.0 33888.0 33889.0 33890.0 33891.0 33892.0 33893.0 33894.0 33895.0 33896.0 33897.0 33898.0 33899.0 33900.0 33901.0 33902.0 33903.0 33904.0 33905.0 33906.0 33907.0 33908.0 33909.0 33910.0 33911.0 33912.0 33913.0 33914.0 33915.0 33916.0 33917.0 33918.0 33919.0 33920.0 33921.0 33922.0 33923.0 33924.0 33925.0 33926.0 33927.0 33928.0 33929.0 33930.0 33931.0 33932.0 33933.0 33934.0 33935.0 33936.0 33937.0 33938.0 33939.0 33940.0 33941.0 33942.0 33943.0 33944.0 33945.0 33946.0 33947.0 33948.0 33949.0 33950.0 33951.0 33952.0 33953.0 33954.0 33955.0 33956.0 33957.0 33958.0 33959.0 33960.0 33961.0 33962.0 33963.0 33964.0 33965.0 33966.0 33967.0 33968.0 33969.0 33970.0 33971.0 33972.0 33973.0 33974.0 33975.0 33976.0 33977.0 33978.0 33979.0 33980.0 33981.0 33982.0 33983.0 33984.0 33985.0 33986.0 33987.0 33988.0 33989.0 33990.0 33991.0 33992.0 33993.0 33994.0 33995.0 33996.0 33997.0 33998.0 33999.0 34000.0 34001.0 34002.0 34003.0 34004.0 34005.0 34006.0 34007.0 34008.0 34009.0 34010.0 34011.0 34012.0 34013.0 34014.0 34015.0 34016.0 34017.0 34018.0 34019.0 34020.0 34021.0 34022.0 34023.0 34024.0 34025.0 34026.0 34027.0 34028.0 34029.0 34030.0 34031.0 34032.0 34033.0 34034.0 34035.0 34036.0 34037.0 34038.0 34039.0 34040.0 34041.0 34042.0 34043.0 34044.0 34045.0 34046.0 34047.0 34048.0 34049.0 34050.0 34051.0 34052.0 34053.0 34054.0 34055.0 34056.0 34057.0 34058.0 34059.0 34060.0 34061.0 34062.0 34063.0 34064.0 34065.0 34066.0 34067.0 34068.0 34069.0 34070.0 34071.0 34072.0 34073.0 34074.0 34075.0 34076.0 34077.0 34078.0 34079.0 34080.0 34081.0 34082.0 34083.0 34084.0 34085.0 34086.0 34087.0 34088.0 34089.0 34090.0 34091.0 34092.0 34093.0 34094.0 34095.0 34096.0 34097.0 34098.0 34099.0 34100.0 34101.0 34102.0 34103.0 34104.0 34105.0 34106.0 34107.0 34108.0 34109.0 34110.0 34111.0 34112.0 34113.0 34114.0 34115.0 34116.0 34117.0 34118.0 34119.0 34120.0 34121.0 34122.0 34123.0 34124.0 34125.0 34126.0 34127.0 34128.0 34129.0 34130.0 34131.0 34132.0 34133.0 34134.0 34135.0 34136.0 34137.0 34138.0 34139.0 34140.0 34141.0 34142.0 34143.0 34144.0 34145.0 34146.0 34147.0 34148.0 34149.0 34150.0 34151.0 34152.0 34153.0 34154.0 34155.0 34156.0 34157.0 34158.0 34159.0 34160.0 34161.0 34162.0 34163.0 34164.0 34165.0 34166.0 34167.0 34168.0 34169.0 34170.0 34171.0 34172.0 34173.0 34174.0 34175.0 34176.0 34177.0 34178.0 34179.0 34180.0 34181.0 34182.0 34183.0 34184.0 34185.0 34186.0 34187.0 34188.0 34189.0 34190.0 34191.0 34192.0 34193.0 34194.0 34195.0 34196.0 34197.0 34198.0 34199.0 34200.0 34201.0 34202.0 34203.0 34204.0 34205.0 34206.0 34207.0 34208.0 34209.0 34210.0 34211.0 34212.0 34213.0 34214.0 34215.0 34216.0 34217.0 34218.0 34219.0 34220.0 34221.0 34222.0 34223.0 34224.0 34225.0 34226.0 34227.0 34228.0 34229.0 34230.0 34231.0 34232.0 34233.0 34234.0 34235.0 34236.0 34237.0 34238.0 34239.0 34240.0 34241.0 34242.0 34243.0 34244.0 34245.0 34246.0 34247.0 34248.0 34249.0 34250.0 34251.0 34252.0 34253.0 34254.0 34255.0 34256.0 34257.0 34258.0 34259.0 34260.0 34261.0 34262.0 34263.0 34264.0 34265.0 34266.0 34267.0 34268.0 34269.0 34270.0 34271.0 34272.0 34273.0 34274.0 34275.0 34276.0 34277.0 34278.0 34279.0 34280.0 34281.0 34282.0 34283.0 34284.0 34285.0 34286.0 34287.0 34288.0 34289.0 34290.0 34291.0 34292.0 34293.0 34294.0 34295.0 34296.0 34297.0 34298.0 34299.0 34300.0 34301.0 34302.0 34303.0 34304.0 34305.0 34306.0 34307.0 34308.0 34309.0 34310.0 34311.0 34312.0 34313.0 34314.0 34315.0 34316.0 34317.0 34318.0 34319.0 34320.0 34321.0 34322.0 34323.0 34324.0 34325.0 34326.0 34327.0 34328.0 34329.0 34330.0 34331.0 34332.0 34333.0 34334.0 34335.0 34336.0 34337.0 34338.0 34339.0 34340.0 34341.0 34342.0 34343.0 34344.0 34345.0 34346.0 34347.0 34348.0 34349.0 34350.0 34351.0 34352.0 34353.0 34354.0 34355.0 34356.0 34357.0 34358.0 34359.0 34360.0 34361.0 34362.0 34363.0 34364.0 34365.0 34366.0 34367.0 34368.0 34369.0 34370.0 34371.0 34372.0 34373.0 34374.0 34375.0 34376.0 34377.0 34378.0 34379.0 34380.0 34381.0 34382.0 34383.0 34384.0 34385.0 34386.0 34387.0 34388.0 34389.0 34390.0 34391.0 34392.0 34393.0 34394.0 34395.0 34396.0 34397.0 34398.0 34399.0 34400.0 34401.0 34402.0 34403.0 34404.0 34405.0 34406.0 34407.0 34408.0 34409.0 34410.0 34411.0 34412.0 34413.0 34414.0 34415.0 34416.0 34417.0 34418.0 34419.0 34420.0 34421.0 34422.0 34423.0 34424.0 34425.0 34426.0 34427.0 34428.0 34429.0 34430.0 34431.0 34432.0 34433.0 34434.0 34435.0 34436.0 34437.0 34438.0 34439.0 34440.0 34441.0 34442.0 34443.0 34444.0 34445.0 34446.0 34447.0 34448.0 34449.0 34450.0 34451.0 34452.0 34453.0 34454.0 34455.0 34456.0 34457.0 34458.0 34459.0 34460.0 34461.0 34462.0 34463.0 34464.0 34465.0 34466.0 34467.0 34468.0 34469.0 34470.0 34471.0 34472.0 34473.0 34474.0 34475.0 34476.0 34477.0 34478.0 34479.0 34480.0 34481.0 34482.0 34483.0 34484.0 34485.0 34486.0 34487.0 34488.0 34489.0 34490.0 34491.0 34492.0 34493.0 34494.0 34495.0 34496.0 34497.0 34498.0 34499.0 34500.0 34501.0 34502.0 34503.0 34504.0 34505.0 34506.0 34507.0 34508.0 34509.0 34510.0 34511.0 34512.0 34513.0 34514.0 34515.0 34516.0 34517.0 34518.0 34519.0 34520.0 34521.0 34522.0 34523.0 34524.0 34525.0 34526.0 34527.0 34528.0 34529.0 34530.0 34531.0 34532.0 34533.0 34534.0 34535.0 34536.0 34537.0 34538.0 34539.0 34540.0 34541.0 34542.0 34543.0 34544.0 34545.0 34546.0 34547.0 34548.0 34549.0 34550.0 34551.0 34552.0 34553.0 34554.0 34555.0 34556.0 34557.0 34558.0 34559.0 34560.0 34561.0 34562.0 34563.0 34564.0 34565.0 34566.0 34567.0 34568.0 34569.0 34570.0 34571.0 34572.0 34573.0 34574.0 34575.0 34576.0 34577.0 34578.0 34579.0 34580.0 34581.0 34582.0 34583.0 34584.0 34585.0 34586.0 34587.0 34588.0 34589.0 34590.0 34591.0 34592.0 34593.0 34594.0 34595.0 34596.0 34597.0 34598.0 34599.0 34600.0 34601.0 34602.0 34603.0 34604.0 34605.0 34606.0 34607.0 34608.0 34609.0 34610.0 34611.0 34612.0 34613.0 34614.0 34615.0 34616.0 34617.0 34618.0 34619.0 34620.0 34621.0 34622.0 34623.0 34624.0 34625.0 34626.0 34627.0 34628.0 34629.0 34630.0 34631.0 34632.0 34633.0 34634.0 34635.0 34636.0 34637.0 34638.0 34639.0 34640.0 34641.0 34642.0 34643.0 34644.0 34645.0 34646.0 34647.0 34648.0 34649.0 34650.0 34651.0 34652.0 34653.0 34654.0 34655.0 34656.0 34657.0 34658.0 34659.0 34660.0 34661.0 34662.0 34663.0 34664.0 34665.0 34666.0 34667.0 34668.0 34669.0 34670.0 34671.0 34672.0 34673.0 34674.0 34675.0 34676.0 34677.0 34678.0 34679.0 34680.0 34681.0 34682.0 34683.0 34684.0 34685.0 34686.0 34687.0 34688.0 34689.0 34690.0 34691.0 34692.0 34693.0 34694.0 34695.0 34696.0 34697.0 34698.0 34699.0 34700.0 34701.0 34702.0 34703.0 34704.0 34705.0 34706.0 34707.0 34708.0 34709.0 34710.0 34711.0 34712.0 34713.0 34714.0 34715.0 34716.0 34717.0 34718.0 34719.0 34720.0 34721.0 34722.0 34723.0 34724.0 34725.0 34726.0 34727.0 34728.0 34729.0 34730.0 34731.0 34732.0 34733.0 34734.0 34735.0 34736.0 34737.0 34738.0 34739.0 34740.0 34741.0 34742.0 34743.0 34744.0 34745.0 34746.0 34747.0 34748.0 34749.0 34750.0 34751.0 34752.0 34753.0 34754.0 34755.0 34756.0 34757.0 34758.0 34759.0 34760.0 34761.0 34762.0 34763.0 34764.0 34765.0 34766.0 34767.0 34768.0 34769.0 34770.0 34771.0 34772.0 34773.0 34774.0 34775.0 34776.0 34777.0 34778.0 34779.0 34780.0 34781.0 34782.0 34783.0 34784.0 34785.0 34786.0 34787.0 34788.0 34789.0 34790.0 34791.0 34792.0 34793.0 34794.0 34795.0 34796.0 34797.0 34798.0 34799.0 34800.0 34801.0 34802.0 34803.0 34804.0 34805.0 34806.0 34807.0 34808.0 34809.0 34810.0 34811.0 34812.0 34813.0 34814.0 34815.0 34816.0 34817.0 34818.0 34819.0 34820.0 34821.0 34822.0 34823.0 34824.0 34825.0 34826.0 34827.0 34828.0 34829.0 34830.0 34831.0 34832.0 34833.0 34834.0 34835.0 34836.0 34837.0 34838.0 34839.0 34840.0 34841.0 34842.0 34843.0 34844.0 34845.0 34846.0 34847.0 34848.0 34849.0 34850.0 34851.0 34852.0 34853.0 34854.0 34855.0 34856.0 34857.0 34858.0 34859.0 34860.0 34861.0 34862.0 34863.0 34864.0 34865.0 34866.0 34867.0 34868.0 34869.0 34870.0 34871.0 34872.0 34873.0 34874.0 34875.0 34876.0 34877.0 34878.0 34879.0 34880.0 34881.0 34882.0 34883.0 34884.0 34885.0 34886.0 34887.0 34888.0 34889.0 34890.0 34891.0 34892.0 34893.0 34894.0 34895.0 34896.0 34897.0 34898.0 34899.0 34900.0 34901.0 34902.0 34903.0 34904.0 34905.0 34906.0 34907.0 34908.0 34909.0 34910.0 34911.0 34912.0 34913.0 34914.0 34915.0 34916.0 34917.0 34918.0 34919.0 34920.0 34921.0 34922.0 34923.0 34924.0 34925.0 34926.0 34927.0 34928.0 34929.0 34930.0 34931.0 34932.0 34933.0 34934.0 34935.0 34936.0 34937.0 34938.0 34939.0 34940.0 34941.0 34942.0 34943.0 34944.0 34945.0 34946.0 34947.0 34948.0 34949.0 34950.0 34951.0 34952.0 34953.0 34954.0 34955.0 34956.0 34957.0 34958.0 34959.0 34960.0 34961.0 34962.0 34963.0 34964.0 34965.0 34966.0 34967.0 34968.0 34969.0 34970.0 34971.0 34972.0 34973.0 34974.0 34975.0 34976.0 34977.0 34978.0 34979.0 34980.0 34981.0 34982.0 34983.0 34984.0 34985.0 34986.0 34987.0 34988.0 34989.0 34990.0 34991.0 34992.0 34993.0 34994.0 34995.0 34996.0 34997.0 34998.0 34999.0 35000.0 35001.0 35002.0 35003.0 35004.0 35005.0 35006.0 35007.0 35008.0 35009.0 35010.0 35011.0 35012.0 35013.0 35014.0 35015.0 35016.0 35017.0 35018.0 35019.0 35020.0 35021.0 35022.0 35023.0 35024.0 35025.0 35026.0 35027.0 35028.0 35029.0 35030.0 35031.0 35032.0 35033.0 35034.0 35035.0 35036.0 35037.0 35038.0 35039.0 35040.0 35041.0 35042.0 35043.0 35044.0 35045.0 35046.0 35047.0 35048.0 35049.0 35050.0 35051.0 35052.0 35053.0 35054.0 35055.0 35056.0 35057.0 35058.0 35059.0 35060.0 35061.0 35062.0 35063.0 35064.0 35065.0 35066.0 35067.0 35068.0 35069.0 35070.0 35071.0 35072.0 35073.0 35074.0 35075.0 35076.0 35077.0 35078.0 35079.0 35080.0 35081.0 35082.0 35083.0 35084.0 35085.0 35086.0 35087.0 35088.0 35089.0 35090.0 35091.0 35092.0 35093.0 35094.0 35095.0 35096.0 35097.0 35098.0 35099.0 35100.0 35101.0 35102.0 35103.0 35104.0 35105.0 35106.0 35107.0 35108.0 35109.0 35110.0 35111.0 35112.0 35113.0 35114.0 35115.0 35116.0 35117.0 35118.0 35119.0 35120.0 35121.0 35122.0 35123.0 35124.0 35125.0 35126.0 35127.0 35128.0 35129.0 35130.0 35131.0 35132.0 35133.0 35134.0 35135.0 35136.0 35137.0 35138.0 35139.0 35140.0 35141.0 35142.0 35143.0 35144.0 35145.0 35146.0 35147.0 35148.0 35149.0 35150.0 35151.0 35152.0 35153.0 35154.0 35155.0 35156.0 35157.0 35158.0 35159.0 35160.0 35161.0 35162.0 35163.0 35164.0 35165.0 35166.0 35167.0 35168.0 35169.0 35170.0 35171.0 35172.0 35173.0 35174.0 35175.0 35176.0 35177.0 35178.0 35179.0 35180.0 35181.0 35182.0 35183.0 35184.0 35185.0 35186.0 35187.0 35188.0 35189.0 35190.0 35191.0 35192.0 35193.0 35194.0 35195.0 35196.0 35197.0 35198.0 35199.0 35200.0 35201.0 35202.0 35203.0 35204.0 35205.0 35206.0 35207.0 35208.0 35209.0 35210.0 35211.0 35212.0 35213.0 35214.0 35215.0 35216.0 35217.0 35218.0 35219.0 35220.0 35221.0 35222.0 35223.0 35224.0 35225.0 35226.0 35227.0 35228.0 35229.0 35230.0 35231.0 35232.0 35233.0 35234.0 35235.0 35236.0 35237.0 35238.0 35239.0 35240.0 35241.0 35242.0 35243.0 35244.0 35245.0 35246.0 35247.0 35248.0 35249.0 35250.0 35251.0 35252.0 35253.0 35254.0 35255.0 35256.0 35257.0 35258.0 35259.0 35260.0 35261.0 35262.0 35263.0 35264.0 35265.0 35266.0 35267.0 35268.0 35269.0 35270.0 35271.0 35272.0 35273.0 35274.0 35275.0 35276.0 35277.0 35278.0 35279.0 35280.0 35281.0 35282.0 35283.0 35284.0 35285.0 35286.0 35287.0 35288.0 35289.0 35290.0 35291.0 35292.0 35293.0 35294.0 35295.0 35296.0 35297.0 35298.0 35299.0 35300.0 35301.0 35302.0 35303.0 35304.0 35305.0 35306.0 35307.0 35308.0 35309.0 35310.0 35311.0 35312.0 35313.0 35314.0 35315.0 35316.0 35317.0 35318.0 35319.0 35320.0 35321.0 35322.0 35323.0 35324.0 35325.0 35326.0 35327.0 35328.0 35329.0 35330.0 35331.0 35332.0 35333.0 35334.0 35335.0 35336.0 35337.0 35338.0 35339.0 35340.0 35341.0 35342.0 35343.0 35344.0 35345.0 35346.0 35347.0 35348.0 35349.0 35350.0 35351.0 35352.0 35353.0 35354.0 35355.0 35356.0 35357.0 35358.0 35359.0 35360.0 35361.0 35362.0 35363.0 35364.0 35365.0 35366.0 35367.0 35368.0 35369.0 35370.0 35371.0 35372.0 35373.0 35374.0 35375.0 35376.0 35377.0 35378.0 35379.0 35380.0 35381.0 35382.0 35383.0 35384.0 35385.0 35386.0 35387.0 35388.0 35389.0 35390.0 35391.0 35392.0 35393.0 35394.0 35395.0 35396.0 35397.0 35398.0 35399.0 35400.0 35401.0 35402.0 35403.0 35404.0 35405.0 35406.0 35407.0 35408.0 35409.0 35410.0 35411.0 35412.0 35413.0 35414.0 35415.0 35416.0 35417.0 35418.0 35419.0 35420.0 35421.0 35422.0 35423.0 35424.0 35425.0 35426.0 35427.0 35428.0 35429.0 35430.0 35431.0 35432.0 35433.0 35434.0 35435.0 35436.0 35437.0 35438.0 35439.0 35440.0 35441.0 35442.0 35443.0 35444.0 35445.0 35446.0 35447.0 35448.0 35449.0 35450.0 35451.0 35452.0 35453.0 35454.0 35455.0 35456.0 35457.0 35458.0 35459.0 35460.0 35461.0 35462.0 35463.0 35464.0 35465.0 35466.0 35467.0 35468.0 35469.0 35470.0 35471.0 35472.0 35473.0 35474.0 35475.0 35476.0 35477.0 35478.0 35479.0 35480.0 35481.0 35482.0 35483.0 35484.0 35485.0 35486.0 35487.0 35488.0 35489.0 35490.0 35491.0 35492.0 35493.0 35494.0 35495.0 35496.0 35497.0 35498.0 35499.0 35500.0 35501.0 35502.0 35503.0 35504.0 35505.0 35506.0 35507.0 35508.0 35509.0 35510.0 35511.0 35512.0 35513.0 35514.0 35515.0 35516.0 35517.0 35518.0 35519.0 35520.0 35521.0 35522.0 35523.0 35524.0 35525.0 35526.0 35527.0 35528.0 35529.0 35530.0 35531.0 35532.0 35533.0 35534.0 35535.0 35536.0 35537.0 35538.0 35539.0 35540.0 35541.0 35542.0 35543.0 35544.0 35545.0 35546.0 35547.0 35548.0 35549.0 35550.0 35551.0 35552.0 35553.0 35554.0 35555.0 35556.0 35557.0 35558.0 35559.0 35560.0 35561.0 35562.0 35563.0 35564.0 35565.0 35566.0 35567.0 35568.0 35569.0 35570.0 35571.0 35572.0 35573.0 35574.0 35575.0 35576.0 35577.0 35578.0 35579.0 35580.0 35581.0 35582.0 35583.0 35584.0 35585.0 35586.0 35587.0 35588.0 35589.0 35590.0 35591.0 35592.0 35593.0 35594.0 35595.0 35596.0 35597.0 35598.0 35599.0 35600.0 35601.0 35602.0 35603.0 35604.0 35605.0 35606.0 35607.0 35608.0 35609.0 35610.0 35611.0 35612.0 35613.0 35614.0 35615.0 35616.0 35617.0 35618.0 35619.0 35620.0 35621.0 35622.0 35623.0 35624.0 35625.0 35626.0 35627.0 35628.0 35629.0 35630.0 35631.0 35632.0 35633.0 35634.0 35635.0 35636.0 35637.0 35638.0 35639.0 35640.0 35641.0 35642.0 35643.0 35644.0 35645.0 35646.0 35647.0 35648.0 35649.0 35650.0 35651.0 35652.0 35653.0 35654.0 35655.0 35656.0 35657.0 35658.0 35659.0 35660.0 35661.0 35662.0 35663.0 35664.0 35665.0 35666.0 35667.0 35668.0 35669.0 35670.0 35671.0 35672.0 35673.0 35674.0 35675.0 35676.0 35677.0 35678.0 35679.0 35680.0 35681.0 35682.0 35683.0 35684.0 35685.0 35686.0 35687.0 35688.0 35689.0 35690.0 35691.0 35692.0 35693.0 35694.0 35695.0 35696.0 35697.0 35698.0 35699.0 35700.0 35701.0 35702.0 35703.0 35704.0 35705.0 35706.0 35707.0 35708.0 35709.0 35710.0 35711.0 35712.0 35713.0 35714.0 35715.0 35716.0 35717.0 35718.0 35719.0 35720.0 35721.0 35722.0 35723.0 35724.0 35725.0 35726.0 35727.0 35728.0 35729.0 35730.0 35731.0 35732.0 35733.0 35734.0 35735.0 35736.0 35737.0 35738.0 35739.0 35740.0 35741.0 35742.0 35743.0 35744.0 35745.0 35746.0 35747.0 35748.0 35749.0 35750.0 35751.0 35752.0 35753.0 35754.0 35755.0 35756.0 35757.0 35758.0 35759.0 35760.0 35761.0 35762.0 35763.0 35764.0 35765.0 35766.0 35767.0 35768.0 35769.0 35770.0 35771.0 35772.0 35773.0 35774.0 35775.0 35776.0 35777.0 35778.0 35779.0 35780.0 35781.0 35782.0 35783.0 35784.0 35785.0 35786.0 35787.0 35788.0 35789.0 35790.0 35791.0 35792.0 35793.0 35794.0 35795.0 35796.0 35797.0 35798.0 35799.0 35800.0 35801.0 35802.0 35803.0 35804.0 35805.0 35806.0 35807.0 35808.0 35809.0 35810.0 35811.0 35812.0 35813.0 35814.0 35815.0 35816.0 35817.0 35818.0 35819.0 35820.0 35821.0 35822.0 35823.0 35824.0 35825.0 35826.0 35827.0 35828.0 35829.0 35830.0 35831.0 35832.0 35833.0 35834.0 35835.0 35836.0 35837.0 35838.0 35839.0 35840.0 35841.0 35842.0 35843.0 35844.0 35845.0 35846.0 35847.0 35848.0 35849.0 35850.0 35851.0 35852.0 35853.0 35854.0 35855.0 35856.0 35857.0 35858.0 35859.0 35860.0 35861.0 35862.0 35863.0 35864.0 35865.0 35866.0 35867.0 35868.0 35869.0 35870.0 35871.0 35872.0 35873.0 35874.0 35875.0 35876.0 35877.0 35878.0 35879.0 35880.0 35881.0 35882.0 35883.0 35884.0 35885.0 35886.0 35887.0 35888.0 35889.0 35890.0 35891.0 35892.0 35893.0 35894.0 35895.0 35896.0 35897.0 35898.0 35899.0 35900.0 35901.0 35902.0 35903.0 35904.0 35905.0 35906.0 35907.0 35908.0 35909.0 35910.0 35911.0 35912.0 35913.0 35914.0 35915.0 35916.0 35917.0 35918.0 35919.0 35920.0 35921.0 35922.0 35923.0 35924.0 35925.0 35926.0 35927.0 35928.0 35929.0 35930.0 35931.0 35932.0 35933.0 35934.0 35935.0 35936.0 35937.0 35938.0 35939.0 35940.0 35941.0 35942.0 35943.0 35944.0 35945.0 35946.0 35947.0 35948.0 35949.0 35950.0 35951.0 35952.0 35953.0 35954.0 35955.0 35956.0 35957.0 35958.0 35959.0 35960.0 35961.0 35962.0 35963.0 35964.0 35965.0 35966.0 35967.0 35968.0 35969.0 35970.0 35971.0 35972.0 35973.0 35974.0 35975.0 35976.0 35977.0 35978.0 35979.0 35980.0 35981.0 35982.0 35983.0 35984.0 35985.0 35986.0 35987.0 35988.0 35989.0 35990.0 35991.0 35992.0 35993.0 35994.0 35995.0 35996.0 35997.0 35998.0 35999.0 36000.0 36001.0 36002.0 36003.0 36004.0 36005.0 36006.0 36007.0 36008.0 36009.0 36010.0 36011.0 36012.0 36013.0 36014.0 36015.0 36016.0 36017.0 36018.0 36019.0 36020.0 36021.0 36022.0 36023.0 36024.0 36025.0 36026.0 36027.0 36028.0 36029.0 36030.0 36031.0 36032.0 36033.0 36034.0 36035.0 36036.0 36037.0 36038.0 36039.0 36040.0 36041.0 36042.0 36043.0 36044.0 36045.0 36046.0 36047.0 36048.0 36049.0 36050.0 36051.0 36052.0 36053.0 36054.0 36055.0 36056.0 36057.0 36058.0 36059.0 36060.0 36061.0 36062.0 36063.0 36064.0 36065.0 36066.0 36067.0 36068.0 36069.0 36070.0 36071.0 36072.0 36073.0 36074.0 36075.0 36076.0 36077.0 36078.0 36079.0 36080.0 36081.0 36082.0 36083.0 36084.0 36085.0 36086.0 36087.0 36088.0 36089.0 36090.0 36091.0 36092.0 36093.0 36094.0 36095.0 36096.0 36097.0 36098.0 36099.0 36100.0 36101.0 36102.0 36103.0 36104.0 36105.0 36106.0 36107.0 36108.0 36109.0 36110.0 36111.0 36112.0 36113.0 36114.0 36115.0 36116.0 36117.0 36118.0 36119.0 36120.0 36121.0 36122.0 36123.0 36124.0 36125.0 36126.0 36127.0 36128.0 36129.0 36130.0 36131.0 36132.0 36133.0 36134.0 36135.0 36136.0 36137.0 36138.0 36139.0 36140.0 36141.0 36142.0 36143.0 36144.0 36145.0 36146.0 36147.0 36148.0 36149.0 36150.0 36151.0 36152.0 36153.0 36154.0 36155.0 36156.0 36157.0 36158.0 36159.0 36160.0 36161.0 36162.0 36163.0 36164.0 36165.0 36166.0 36167.0 36168.0 36169.0 36170.0 36171.0 36172.0 36173.0 36174.0 36175.0 36176.0 36177.0 36178.0 36179.0 36180.0 36181.0 36182.0 36183.0 36184.0 36185.0 36186.0 36187.0 36188.0 36189.0 36190.0 36191.0 36192.0 36193.0 36194.0 36195.0 36196.0 36197.0 36198.0 36199.0 36200.0 36201.0 36202.0 36203.0 36204.0 36205.0 36206.0 36207.0 36208.0 36209.0 36210.0 36211.0 36212.0 36213.0 36214.0 36215.0 36216.0 36217.0 36218.0 36219.0 36220.0 36221.0 36222.0 36223.0 36224.0 36225.0 36226.0 36227.0 36228.0 36229.0 36230.0 36231.0 36232.0 36233.0 36234.0 36235.0 36236.0 36237.0 36238.0 36239.0 36240.0 36241.0 36242.0 36243.0 36244.0 36245.0 36246.0 36247.0 36248.0 36249.0 36250.0 36251.0 36252.0 36253.0 36254.0 36255.0 36256.0 36257.0 36258.0 36259.0 36260.0 36261.0 36262.0 36263.0 36264.0 36265.0 36266.0 36267.0 36268.0 36269.0 36270.0 36271.0 36272.0 36273.0 36274.0 36275.0 36276.0 36277.0 36278.0 36279.0 36280.0 36281.0 36282.0 36283.0 36284.0 36285.0 36286.0 36287.0 36288.0 36289.0 36290.0 36291.0 36292.0 36293.0 36294.0 36295.0 36296.0 36297.0 36298.0 36299.0 36300.0 36301.0 36302.0 36303.0 36304.0 36305.0 36306.0 36307.0 36308.0 36309.0 36310.0 36311.0 36312.0 36313.0 36314.0 36315.0 36316.0 36317.0 36318.0 36319.0 36320.0 36321.0 36322.0 36323.0 36324.0 36325.0 36326.0 36327.0 36328.0 36329.0 36330.0 36331.0 36332.0 36333.0 36334.0 36335.0 36336.0 36337.0 36338.0 36339.0 36340.0 36341.0 36342.0 36343.0 36344.0 36345.0 36346.0 36347.0 36348.0 36349.0 36350.0 36351.0 36352.0 36353.0 36354.0 36355.0 36356.0 36357.0 36358.0 36359.0 36360.0 36361.0 36362.0 36363.0 36364.0 36365.0 36366.0 36367.0 36368.0 36369.0 36370.0 36371.0 36372.0 36373.0 36374.0 36375.0 36376.0 36377.0 36378.0 36379.0 36380.0 36381.0 36382.0 36383.0 36384.0 36385.0 36386.0 36387.0 36388.0 36389.0 36390.0 36391.0 36392.0 36393.0 36394.0 36395.0 36396.0 36397.0 36398.0 36399.0 36400.0 36401.0 36402.0 36403.0 36404.0 36405.0 36406.0 36407.0 36408.0 36409.0 36410.0 36411.0 36412.0 36413.0 36414.0 36415.0 36416.0 36417.0 36418.0 36419.0 36420.0 36421.0 36422.0 36423.0 36424.0 36425.0 36426.0 36427.0 36428.0 36429.0 36430.0 36431.0 36432.0 36433.0 36434.0 36435.0 36436.0 36437.0 36438.0 36439.0 36440.0 36441.0 36442.0 36443.0 36444.0 36445.0 36446.0 36447.0 36448.0 36449.0 36450.0 36451.0 36452.0 36453.0 36454.0 36455.0 36456.0 36457.0 36458.0 36459.0 36460.0 36461.0 36462.0 36463.0 36464.0 36465.0 36466.0 36467.0 36468.0 36469.0 36470.0 36471.0 36472.0 36473.0 36474.0 36475.0 36476.0 36477.0 36478.0 36479.0 36480.0 36481.0 36482.0 36483.0 36484.0 36485.0 36486.0 36487.0 36488.0 36489.0 36490.0 36491.0 36492.0 36493.0 36494.0 36495.0 36496.0 36497.0 36498.0 36499.0 36500.0 36501.0 36502.0 36503.0 36504.0 36505.0 36506.0 36507.0 36508.0 36509.0 36510.0 36511.0 36512.0 36513.0 36514.0 36515.0 36516.0 36517.0 36518.0 36519.0 36520.0 36521.0 36522.0 36523.0 36524.0 36525.0 36526.0 36527.0 36528.0 36529.0 36530.0 36531.0 36532.0 36533.0 36534.0 36535.0 36536.0 36537.0 36538.0 36539.0 36540.0 36541.0 36542.0 36543.0 36544.0 36545.0 36546.0 36547.0 36548.0 36549.0 36550.0 36551.0 36552.0 36553.0 36554.0 36555.0 36556.0 36557.0 36558.0 36559.0 36560.0 36561.0 36562.0 36563.0 36564.0 36565.0 36566.0 36567.0 36568.0 36569.0 36570.0 36571.0 36572.0 36573.0 36574.0 36575.0 36576.0 36577.0 36578.0 36579.0 36580.0 36581.0 36582.0 36583.0 36584.0 36585.0 36586.0 36587.0 36588.0 36589.0 36590.0 36591.0 36592.0 36593.0 36594.0 36595.0 36596.0 36597.0 36598.0 36599.0 36600.0 36601.0 36602.0 36603.0 36604.0 36605.0 36606.0 36607.0 36608.0 36609.0 36610.0 36611.0 36612.0 36613.0 36614.0 36615.0 36616.0 36617.0 36618.0 36619.0 36620.0 36621.0 36622.0 36623.0 36624.0 36625.0 36626.0 36627.0 36628.0 36629.0 36630.0 36631.0 36632.0 36633.0 36634.0 36635.0 36636.0 36637.0 36638.0 36639.0 36640.0 36641.0 36642.0 36643.0 36644.0 36645.0 36646.0 36647.0 36648.0 36649.0 36650.0 36651.0 36652.0 36653.0 36654.0 36655.0 36656.0 36657.0 36658.0 36659.0 36660.0 36661.0 36662.0 36663.0 36664.0 36665.0 36666.0 36667.0 36668.0 36669.0 36670.0 36671.0 36672.0 36673.0 36674.0 36675.0 36676.0 36677.0 36678.0 36679.0 36680.0 36681.0 36682.0 36683.0 36684.0 36685.0 36686.0 36687.0 36688.0 36689.0 36690.0 36691.0 36692.0 36693.0 36694.0 36695.0 36696.0 36697.0 36698.0 36699.0 36700.0 36701.0 36702.0 36703.0 36704.0 36705.0 36706.0 36707.0 36708.0 36709.0 36710.0 36711.0 36712.0 36713.0 36714.0 36715.0 36716.0 36717.0 36718.0 36719.0 36720.0 36721.0 36722.0 36723.0 36724.0 36725.0 36726.0 36727.0 36728.0 36729.0 36730.0 36731.0 36732.0 36733.0 36734.0 36735.0 36736.0 36737.0 36738.0 36739.0 36740.0 36741.0 36742.0 36743.0 36744.0 36745.0 36746.0 36747.0 36748.0 36749.0 36750.0 36751.0 36752.0 36753.0 36754.0 36755.0 36756.0 36757.0 36758.0 36759.0 36760.0 36761.0 36762.0 36763.0 36764.0 36765.0 36766.0 36767.0 36768.0 36769.0 36770.0 36771.0 36772.0 36773.0 36774.0 36775.0 36776.0 36777.0 36778.0 36779.0 36780.0 36781.0 36782.0 36783.0 36784.0 36785.0 36786.0 36787.0 36788.0 36789.0 36790.0 36791.0 36792.0 36793.0 36794.0 36795.0 36796.0 36797.0 36798.0 36799.0 36800.0 36801.0 36802.0 36803.0 36804.0 36805.0 36806.0 36807.0 36808.0 36809.0 36810.0 36811.0 36812.0 36813.0 36814.0 36815.0 36816.0 36817.0 36818.0 36819.0 36820.0 36821.0 36822.0 36823.0 36824.0 36825.0 36826.0 36827.0 36828.0 36829.0 36830.0 36831.0 36832.0 36833.0 36834.0 36835.0 36836.0 36837.0 36838.0 36839.0 36840.0 36841.0 36842.0 36843.0 36844.0 36845.0 36846.0 36847.0 36848.0 36849.0 36850.0 36851.0 36852.0 36853.0 36854.0 36855.0 36856.0 36857.0 36858.0 36859.0 36860.0 36861.0 36862.0 36863.0 36864.0 36865.0 36866.0 36867.0 36868.0 36869.0 36870.0 36871.0 36872.0 36873.0 36874.0 36875.0 36876.0 36877.0 36878.0 36879.0 36880.0 36881.0 36882.0 36883.0 36884.0 36885.0 36886.0 36887.0 36888.0 36889.0 36890.0 36891.0 36892.0 36893.0 36894.0 36895.0 36896.0 36897.0 36898.0 36899.0 36900.0 36901.0 36902.0 36903.0 36904.0 36905.0 36906.0 36907.0 36908.0 36909.0 36910.0 36911.0 36912.0 36913.0 36914.0 36915.0 36916.0 36917.0 36918.0 36919.0 36920.0 36921.0 36922.0 36923.0 36924.0 36925.0 36926.0 36927.0 36928.0 36929.0 36930.0 36931.0 36932.0 36933.0 36934.0 36935.0 36936.0 36937.0 36938.0 36939.0 36940.0 36941.0 36942.0 36943.0 36944.0 36945.0 36946.0 36947.0 36948.0 36949.0 36950.0 36951.0 36952.0 36953.0 36954.0 36955.0 36956.0 36957.0 36958.0 36959.0 36960.0 36961.0 36962.0 36963.0 36964.0 36965.0 36966.0 36967.0 36968.0 36969.0 36970.0 36971.0 36972.0 36973.0 36974.0 36975.0 36976.0 36977.0 36978.0 36979.0 36980.0 36981.0 36982.0 36983.0 36984.0 36985.0 36986.0 36987.0 36988.0 36989.0 36990.0 36991.0 36992.0 36993.0 36994.0 36995.0 36996.0 36997.0 36998.0 36999.0 37000.0 37001.0 37002.0 37003.0 37004.0 37005.0 37006.0 37007.0 37008.0 37009.0 37010.0 37011.0 37012.0 37013.0 37014.0 37015.0 37016.0 37017.0 37018.0 37019.0 37020.0 37021.0 37022.0 37023.0 37024.0 37025.0 37026.0 37027.0 37028.0 37029.0 37030.0 37031.0 37032.0 37033.0 37034.0 37035.0 37036.0 37037.0 37038.0 37039.0 37040.0 37041.0 37042.0 37043.0 37044.0 37045.0 37046.0 37047.0 37048.0 37049.0 37050.0 37051.0 37052.0 37053.0 37054.0 37055.0 37056.0 37057.0 37058.0 37059.0 37060.0 37061.0 37062.0 37063.0 37064.0 37065.0 37066.0 37067.0 37068.0 37069.0 37070.0 37071.0 37072.0 37073.0 37074.0 37075.0 37076.0 37077.0 37078.0 37079.0 37080.0 37081.0 37082.0 37083.0 37084.0 37085.0 37086.0 37087.0 37088.0 37089.0 37090.0 37091.0 37092.0 37093.0 37094.0 37095.0 37096.0 37097.0 37098.0 37099.0 37100.0 37101.0 37102.0 37103.0 37104.0 37105.0 37106.0 37107.0 37108.0 37109.0 37110.0 37111.0 37112.0 37113.0 37114.0 37115.0 37116.0 37117.0 37118.0 37119.0 37120.0 37121.0 37122.0 37123.0 37124.0 37125.0 37126.0 37127.0 37128.0 37129.0 37130.0 37131.0 37132.0 37133.0 37134.0 37135.0 37136.0 37137.0 37138.0 37139.0 37140.0 37141.0 37142.0 37143.0 37144.0 37145.0 37146.0 37147.0 37148.0 37149.0 37150.0 37151.0 37152.0 37153.0 37154.0 37155.0 37156.0 37157.0 37158.0 37159.0 37160.0 37161.0 37162.0 37163.0 37164.0 37165.0 37166.0 37167.0 37168.0 37169.0 37170.0 37171.0 37172.0 37173.0 37174.0 37175.0 37176.0 37177.0 37178.0 37179.0 37180.0 37181.0 37182.0 37183.0 37184.0 37185.0 37186.0 37187.0 37188.0 37189.0 37190.0 37191.0 37192.0 37193.0 37194.0 37195.0 37196.0 37197.0 37198.0 37199.0 37200.0 37201.0 37202.0 37203.0 37204.0 37205.0 37206.0 37207.0 37208.0 37209.0 37210.0 37211.0 37212.0 37213.0 37214.0 37215.0 37216.0 37217.0 37218.0 37219.0 37220.0 37221.0 37222.0 37223.0 37224.0 37225.0 37226.0 37227.0 37228.0 37229.0 37230.0 37231.0 37232.0 37233.0 37234.0 37235.0 37236.0 37237.0 37238.0 37239.0 37240.0 37241.0 37242.0 37243.0 37244.0 37245.0 37246.0 37247.0 37248.0 37249.0 37250.0 37251.0 37252.0 37253.0 37254.0 37255.0 37256.0 37257.0 37258.0 37259.0 37260.0 37261.0 37262.0 37263.0 37264.0 37265.0 37266.0 37267.0 37268.0 37269.0 37270.0 37271.0 37272.0 37273.0 37274.0 37275.0 37276.0 37277.0 37278.0 37279.0 37280.0 37281.0 37282.0 37283.0 37284.0 37285.0 37286.0 37287.0 37288.0 37289.0 37290.0 37291.0 37292.0 37293.0 37294.0 37295.0 37296.0 37297.0 37298.0 37299.0 37300.0 37301.0 37302.0 37303.0 37304.0 37305.0 37306.0 37307.0 37308.0 37309.0 37310.0 37311.0 37312.0 37313.0 37314.0 37315.0 37316.0 37317.0 37318.0 37319.0 37320.0 37321.0 37322.0 37323.0 37324.0 37325.0 37326.0 37327.0 37328.0 37329.0 37330.0 37331.0 37332.0 37333.0 37334.0 37335.0 37336.0 37337.0 37338.0 37339.0 37340.0 37341.0 37342.0 37343.0 37344.0 37345.0 37346.0 37347.0 37348.0 37349.0 37350.0 37351.0 37352.0 37353.0 37354.0 37355.0 37356.0 37357.0 37358.0 37359.0 37360.0 37361.0 37362.0 37363.0 37364.0 37365.0 37366.0 37367.0 37368.0 37369.0 37370.0 37371.0 37372.0 37373.0 37374.0 37375.0 37376.0 37377.0 37378.0 37379.0 37380.0 37381.0 37382.0 37383.0 37384.0 37385.0 37386.0 37387.0 37388.0 37389.0 37390.0 37391.0 37392.0 37393.0 37394.0 37395.0 37396.0 37397.0 37398.0 37399.0 37400.0 37401.0 37402.0 37403.0 37404.0 37405.0 37406.0 37407.0 37408.0 37409.0 37410.0 37411.0 37412.0 37413.0 37414.0 37415.0 37416.0 37417.0 37418.0 37419.0 37420.0 37421.0 37422.0 37423.0 37424.0 37425.0 37426.0 37427.0 37428.0 37429.0 37430.0 37431.0 37432.0 37433.0 37434.0 37435.0 37436.0 37437.0 37438.0 37439.0 37440.0 37441.0 37442.0 37443.0 37444.0 37445.0 37446.0 37447.0 37448.0 37449.0 37450.0 37451.0 37452.0 37453.0 37454.0 37455.0 37456.0 37457.0 37458.0 37459.0 37460.0 37461.0 37462.0 37463.0 37464.0 37465.0 37466.0 37467.0 37468.0 37469.0 37470.0 37471.0 37472.0 37473.0 37474.0 37475.0 37476.0 37477.0 37478.0 37479.0 37480.0 37481.0 37482.0 37483.0 37484.0 37485.0 37486.0 37487.0 37488.0 37489.0 37490.0 37491.0 37492.0 37493.0 37494.0 37495.0 37496.0 37497.0 37498.0 37499.0 37500.0 37501.0 37502.0 37503.0 37504.0 37505.0 37506.0 37507.0 37508.0 37509.0 37510.0 37511.0 37512.0 37513.0 37514.0 37515.0 37516.0 37517.0 37518.0 37519.0 37520.0 37521.0 37522.0 37523.0 37524.0 37525.0 37526.0 37527.0 37528.0 37529.0 37530.0 37531.0 37532.0 37533.0 37534.0 37535.0 37536.0 37537.0 37538.0 37539.0 37540.0 37541.0 37542.0 37543.0 37544.0 37545.0 37546.0 37547.0 37548.0 37549.0 37550.0 37551.0 37552.0 37553.0 37554.0 37555.0 37556.0 37557.0 37558.0 37559.0 37560.0 37561.0 37562.0 37563.0 37564.0 37565.0 37566.0 37567.0 37568.0 37569.0 37570.0 37571.0 37572.0 37573.0 37574.0 37575.0 37576.0 37577.0 37578.0 37579.0 37580.0 37581.0 37582.0 37583.0 37584.0 37585.0 37586.0 37587.0 37588.0 37589.0 37590.0 37591.0 37592.0 37593.0 37594.0 37595.0 37596.0 37597.0 37598.0 37599.0 37600.0 37601.0 37602.0 37603.0 37604.0 37605.0 37606.0 37607.0 37608.0 37609.0 37610.0 37611.0 37612.0 37613.0 37614.0 37615.0 37616.0 37617.0 37618.0 37619.0 37620.0 37621.0 37622.0 37623.0 37624.0 37625.0 37626.0 37627.0 37628.0 37629.0 37630.0 37631.0 37632.0 37633.0 37634.0 37635.0 37636.0 37637.0 37638.0 37639.0 37640.0 37641.0 37642.0 37643.0 37644.0 37645.0 37646.0 37647.0 37648.0 37649.0 37650.0 37651.0 37652.0 37653.0 37654.0 37655.0 37656.0 37657.0 37658.0 37659.0 37660.0 37661.0 37662.0 37663.0 37664.0 37665.0 37666.0 37667.0 37668.0 37669.0 37670.0 37671.0 37672.0 37673.0 37674.0 37675.0 37676.0 37677.0 37678.0 37679.0 37680.0 37681.0 37682.0 37683.0 37684.0 37685.0 37686.0 37687.0 37688.0 37689.0 37690.0 37691.0 37692.0 37693.0 37694.0 37695.0 37696.0 37697.0 37698.0 37699.0 37700.0 37701.0 37702.0 37703.0 37704.0 37705.0 37706.0 37707.0 37708.0 37709.0 37710.0 37711.0 37712.0 37713.0 37714.0 37715.0 37716.0 37717.0 37718.0 37719.0 37720.0 37721.0 37722.0 37723.0 37724.0 37725.0 37726.0 37727.0 37728.0 37729.0 37730.0 37731.0 37732.0 37733.0 37734.0 37735.0 37736.0 37737.0 37738.0 37739.0 37740.0 37741.0 37742.0 37743.0 37744.0 37745.0 37746.0 37747.0 37748.0 37749.0 37750.0 37751.0 37752.0 37753.0 37754.0 37755.0 37756.0 37757.0 37758.0 37759.0 37760.0 37761.0 37762.0 37763.0 37764.0 37765.0 37766.0 37767.0 37768.0 37769.0 37770.0 37771.0 37772.0 37773.0 37774.0 37775.0 37776.0 37777.0 37778.0 37779.0 37780.0 37781.0 37782.0 37783.0 37784.0 37785.0 37786.0 37787.0 37788.0 37789.0 37790.0 37791.0 37792.0 37793.0 37794.0 37795.0 37796.0 37797.0 37798.0 37799.0 37800.0 37801.0 37802.0 37803.0 37804.0 37805.0 37806.0 37807.0 37808.0 37809.0 37810.0 37811.0 37812.0 37813.0 37814.0 37815.0 37816.0 37817.0 37818.0 37819.0 37820.0 37821.0 37822.0 37823.0 37824.0 37825.0 37826.0 37827.0 37828.0 37829.0 37830.0 37831.0 37832.0 37833.0 37834.0 37835.0 37836.0 37837.0 37838.0 37839.0 37840.0 37841.0 37842.0 37843.0 37844.0 37845.0 37846.0 37847.0 37848.0 37849.0 37850.0 37851.0 37852.0 37853.0 37854.0 37855.0 37856.0 37857.0 37858.0 37859.0 37860.0 37861.0 37862.0 37863.0 37864.0 37865.0 37866.0 37867.0 37868.0 37869.0 37870.0 37871.0 37872.0 37873.0 37874.0 37875.0 37876.0 37877.0 37878.0 37879.0 37880.0 37881.0 37882.0 37883.0 37884.0 37885.0 37886.0 37887.0 37888.0 37889.0 37890.0 37891.0 37892.0 37893.0 37894.0 37895.0 37896.0 37897.0 37898.0 37899.0 37900.0 37901.0 37902.0 37903.0 37904.0 37905.0 37906.0 37907.0 37908.0 37909.0 37910.0 37911.0 37912.0 37913.0 37914.0 37915.0 37916.0 37917.0 37918.0 37919.0 37920.0 37921.0 37922.0 37923.0 37924.0 37925.0 37926.0 37927.0 37928.0 37929.0 37930.0 37931.0 37932.0 37933.0 37934.0 37935.0 37936.0 37937.0 37938.0 37939.0 37940.0 37941.0 37942.0 37943.0 37944.0 37945.0 37946.0 37947.0 37948.0 37949.0 37950.0 37951.0 37952.0 37953.0 37954.0 37955.0 37956.0 37957.0 37958.0 37959.0 37960.0 37961.0 37962.0 37963.0 37964.0 37965.0 37966.0 37967.0 37968.0 37969.0 37970.0 37971.0 37972.0 37973.0 37974.0 37975.0 37976.0 37977.0 37978.0 37979.0 37980.0 37981.0 37982.0 37983.0 37984.0 37985.0 37986.0 37987.0 37988.0 37989.0 37990.0 37991.0 37992.0 37993.0 37994.0 37995.0 37996.0 37997.0 37998.0 37999.0 38000.0 38001.0 38002.0 38003.0 38004.0 38005.0 38006.0 38007.0 38008.0 38009.0 38010.0 38011.0 38012.0 38013.0 38014.0 38015.0 38016.0 38017.0 38018.0 38019.0 38020.0 38021.0 38022.0 38023.0 38024.0 38025.0 38026.0 38027.0 38028.0 38029.0 38030.0 38031.0 38032.0 38033.0 38034.0 38035.0 38036.0 38037.0 38038.0 38039.0 38040.0 38041.0 38042.0 38043.0 38044.0 38045.0 38046.0 38047.0 38048.0 38049.0 38050.0 38051.0 38052.0 38053.0 38054.0 38055.0 38056.0 38057.0 38058.0 38059.0 38060.0 38061.0 38062.0 38063.0 38064.0 38065.0 38066.0 38067.0 38068.0 38069.0 38070.0 38071.0 38072.0 38073.0 38074.0 38075.0 38076.0 38077.0 38078.0 38079.0 38080.0 38081.0 38082.0 38083.0 38084.0 38085.0 38086.0 38087.0 38088.0 38089.0 38090.0 38091.0 38092.0 38093.0 38094.0 38095.0 38096.0 38097.0 38098.0 38099.0 38100.0 38101.0 38102.0 38103.0 38104.0 38105.0 38106.0 38107.0 38108.0 38109.0 38110.0 38111.0 38112.0 38113.0 38114.0 38115.0 38116.0 38117.0 38118.0 38119.0 38120.0 38121.0 38122.0 38123.0 38124.0 38125.0 38126.0 38127.0 38128.0 38129.0 38130.0 38131.0 38132.0 38133.0 38134.0 38135.0 38136.0 38137.0 38138.0 38139.0 38140.0 38141.0 38142.0 38143.0 38144.0 38145.0 38146.0 38147.0 38148.0 38149.0 38150.0 38151.0 38152.0 38153.0 38154.0 38155.0 38156.0 38157.0 38158.0 38159.0 38160.0 38161.0 38162.0 38163.0 38164.0 38165.0 38166.0 38167.0 38168.0 38169.0 38170.0 38171.0 38172.0 38173.0 38174.0 38175.0 38176.0 38177.0 38178.0 38179.0 38180.0 38181.0 38182.0 38183.0 38184.0 38185.0 38186.0 38187.0 38188.0 38189.0 38190.0 38191.0 38192.0 38193.0 38194.0 38195.0 38196.0 38197.0 38198.0 38199.0 38200.0 38201.0 38202.0 38203.0 38204.0 38205.0 38206.0 38207.0 38208.0 38209.0 38210.0 38211.0 38212.0 38213.0 38214.0 38215.0 38216.0 38217.0 38218.0 38219.0 38220.0 38221.0 38222.0 38223.0 38224.0 38225.0 38226.0 38227.0 38228.0 38229.0 38230.0 38231.0 38232.0 38233.0 38234.0 38235.0 38236.0 38237.0 38238.0 38239.0 38240.0 38241.0 38242.0 38243.0 38244.0 38245.0 38246.0 38247.0 38248.0 38249.0 38250.0 38251.0 38252.0 38253.0 38254.0 38255.0 38256.0 38257.0 38258.0 38259.0 38260.0 38261.0 38262.0 38263.0 38264.0 38265.0 38266.0 38267.0 38268.0 38269.0 38270.0 38271.0 38272.0 38273.0 38274.0 38275.0 38276.0 38277.0 38278.0 38279.0 38280.0 38281.0 38282.0 38283.0 38284.0 38285.0 38286.0 38287.0 38288.0 38289.0 38290.0 38291.0 38292.0 38293.0 38294.0 38295.0 38296.0 38297.0 38298.0 38299.0 38300.0 38301.0 38302.0 38303.0 38304.0 38305.0 38306.0 38307.0 38308.0 38309.0 38310.0 38311.0 38312.0 38313.0 38314.0 38315.0 38316.0 38317.0 38318.0 38319.0 38320.0 38321.0 38322.0 38323.0 38324.0 38325.0 38326.0 38327.0 38328.0 38329.0 38330.0 38331.0 38332.0 38333.0 38334.0 38335.0 38336.0 38337.0 38338.0 38339.0 38340.0 38341.0 38342.0 38343.0 38344.0 38345.0 38346.0 38347.0 38348.0 38349.0 38350.0 38351.0 38352.0 38353.0 38354.0 38355.0 38356.0 38357.0 38358.0 38359.0 38360.0 38361.0 38362.0 38363.0 38364.0 38365.0 38366.0 38367.0 38368.0 38369.0 38370.0 38371.0 38372.0 38373.0 38374.0 38375.0 38376.0 38377.0 38378.0 38379.0 38380.0 38381.0 38382.0 38383.0 38384.0 38385.0 38386.0 38387.0 38388.0 38389.0 38390.0 38391.0 38392.0 38393.0 38394.0 38395.0 38396.0 38397.0 38398.0 38399.0 38400.0 38401.0 38402.0 38403.0 38404.0 38405.0 38406.0 38407.0 38408.0 38409.0 38410.0 38411.0 38412.0 38413.0 38414.0 38415.0 38416.0 38417.0 38418.0 38419.0 38420.0 38421.0 38422.0 38423.0 38424.0 38425.0 38426.0 38427.0 38428.0 38429.0 38430.0 38431.0 38432.0 38433.0 38434.0 38435.0 38436.0 38437.0 38438.0 38439.0 38440.0 38441.0 38442.0 38443.0 38444.0 38445.0 38446.0 38447.0 38448.0 38449.0 38450.0 38451.0 38452.0 38453.0 38454.0 38455.0 38456.0 38457.0 38458.0 38459.0 38460.0 38461.0 38462.0 38463.0 38464.0 38465.0 38466.0 38467.0 38468.0 38469.0 38470.0 38471.0 38472.0 38473.0 38474.0 38475.0 38476.0 38477.0 38478.0 38479.0 38480.0 38481.0 38482.0 38483.0 38484.0 38485.0 38486.0 38487.0 38488.0 38489.0 38490.0 38491.0 38492.0 38493.0 38494.0 38495.0 38496.0 38497.0 38498.0 38499.0 38500.0 38501.0 38502.0 38503.0 38504.0 38505.0 38506.0 38507.0 38508.0 38509.0 38510.0 38511.0 38512.0 38513.0 38514.0 38515.0 38516.0 38517.0 38518.0 38519.0 38520.0 38521.0 38522.0 38523.0 38524.0 38525.0 38526.0 38527.0 38528.0 38529.0 38530.0 38531.0 38532.0 38533.0 38534.0 38535.0 38536.0 38537.0 38538.0 38539.0 38540.0 38541.0 38542.0 38543.0 38544.0 38545.0 38546.0 38547.0 38548.0 38549.0 38550.0 38551.0 38552.0 38553.0 38554.0 38555.0 38556.0 38557.0 38558.0 38559.0 38560.0 38561.0 38562.0 38563.0 38564.0 38565.0 38566.0 38567.0 38568.0 38569.0 38570.0 38571.0 38572.0 38573.0 38574.0 38575.0 38576.0 38577.0 38578.0 38579.0 38580.0 38581.0 38582.0 38583.0 38584.0 38585.0 38586.0 38587.0 38588.0 38589.0 38590.0 38591.0 38592.0 38593.0 38594.0 38595.0 38596.0 38597.0 38598.0 38599.0 38600.0 38601.0 38602.0 38603.0 38604.0 38605.0 38606.0 38607.0 38608.0 38609.0 38610.0 38611.0 38612.0 38613.0 38614.0 38615.0 38616.0 38617.0 38618.0 38619.0 38620.0 38621.0 38622.0 38623.0 38624.0 38625.0 38626.0 38627.0 38628.0 38629.0 38630.0 38631.0 38632.0 38633.0 38634.0 38635.0 38636.0 38637.0 38638.0 38639.0 38640.0 38641.0 38642.0 38643.0 38644.0 38645.0 38646.0 38647.0 38648.0 38649.0 38650.0 38651.0 38652.0 38653.0 38654.0 38655.0 38656.0 38657.0 38658.0 38659.0 38660.0 38661.0 38662.0 38663.0 38664.0 38665.0 38666.0 38667.0 38668.0 38669.0 38670.0 38671.0 38672.0 38673.0 38674.0 38675.0 38676.0 38677.0 38678.0 38679.0 38680.0 38681.0 38682.0 38683.0 38684.0 38685.0 38686.0 38687.0 38688.0 38689.0 38690.0 38691.0 38692.0 38693.0 38694.0 38695.0 38696.0 38697.0 38698.0 38699.0 38700.0 38701.0 38702.0 38703.0 38704.0 38705.0 38706.0 38707.0 38708.0 38709.0 38710.0 38711.0 38712.0 38713.0 38714.0 38715.0 38716.0 38717.0 38718.0 38719.0 38720.0 38721.0 38722.0 38723.0 38724.0 38725.0 38726.0 38727.0 38728.0 38729.0 38730.0 38731.0 38732.0 38733.0 38734.0 38735.0 38736.0 38737.0 38738.0 38739.0 38740.0 38741.0 38742.0 38743.0 38744.0 38745.0 38746.0 38747.0 38748.0 38749.0 38750.0 38751.0 38752.0 38753.0 38754.0 38755.0 38756.0 38757.0 38758.0 38759.0 38760.0 38761.0 38762.0 38763.0 38764.0 38765.0 38766.0 38767.0 38768.0 38769.0 38770.0 38771.0 38772.0 38773.0 38774.0 38775.0 38776.0 38777.0 38778.0 38779.0 38780.0 38781.0 38782.0 38783.0 38784.0 38785.0 38786.0 38787.0 38788.0 38789.0 38790.0 38791.0 38792.0 38793.0 38794.0 38795.0 38796.0 38797.0 38798.0 38799.0 38800.0 38801.0 38802.0 38803.0 38804.0 38805.0 38806.0 38807.0 38808.0 38809.0 38810.0 38811.0 38812.0 38813.0 38814.0 38815.0 38816.0 38817.0 38818.0 38819.0 38820.0 38821.0 38822.0 38823.0 38824.0 38825.0 38826.0 38827.0 38828.0 38829.0 38830.0 38831.0 38832.0 38833.0 38834.0 38835.0 38836.0 38837.0 38838.0 38839.0 38840.0 38841.0 38842.0 38843.0 38844.0 38845.0 38846.0 38847.0 38848.0 38849.0 38850.0 38851.0 38852.0 38853.0 38854.0 38855.0 38856.0 38857.0 38858.0 38859.0 38860.0 38861.0 38862.0 38863.0 38864.0 38865.0 38866.0 38867.0 38868.0 38869.0 38870.0 38871.0 38872.0 38873.0 38874.0 38875.0 38876.0 38877.0 38878.0 38879.0 38880.0 38881.0 38882.0 38883.0 38884.0 38885.0 38886.0 38887.0 38888.0 38889.0 38890.0 38891.0 38892.0 38893.0 38894.0 38895.0 38896.0 38897.0 38898.0 38899.0 38900.0 38901.0 38902.0 38903.0 38904.0 38905.0 38906.0 38907.0 38908.0 38909.0 38910.0 38911.0 38912.0 38913.0 38914.0 38915.0 38916.0 38917.0 38918.0 38919.0 38920.0 38921.0 38922.0 38923.0 38924.0 38925.0 38926.0 38927.0 38928.0 38929.0 38930.0 38931.0 38932.0 38933.0 38934.0 38935.0 38936.0 38937.0 38938.0 38939.0 38940.0 38941.0 38942.0 38943.0 38944.0 38945.0 38946.0 38947.0 38948.0 38949.0 38950.0 38951.0 38952.0 38953.0 38954.0 38955.0 38956.0 38957.0 38958.0 38959.0 38960.0 38961.0 38962.0 38963.0 38964.0 38965.0 38966.0 38967.0 38968.0 38969.0 38970.0 38971.0 38972.0 38973.0 38974.0 38975.0 38976.0 38977.0 38978.0 38979.0 38980.0 38981.0 38982.0 38983.0 38984.0 38985.0 38986.0 38987.0 38988.0 38989.0 38990.0 38991.0 38992.0 38993.0 38994.0 38995.0 38996.0 38997.0 38998.0 38999.0 39000.0 39001.0 39002.0 39003.0 39004.0 39005.0 39006.0 39007.0 39008.0 39009.0 39010.0 39011.0 39012.0 39013.0 39014.0 39015.0 39016.0 39017.0 39018.0 39019.0 39020.0 39021.0 39022.0 39023.0 39024.0 39025.0 39026.0 39027.0 39028.0 39029.0 39030.0 39031.0 39032.0 39033.0 39034.0 39035.0 39036.0 39037.0 39038.0 39039.0 39040.0 39041.0 39042.0 39043.0 39044.0 39045.0 39046.0 39047.0 39048.0 39049.0 39050.0 39051.0 39052.0 39053.0 39054.0 39055.0 39056.0 39057.0 39058.0 39059.0 39060.0 39061.0 39062.0 39063.0 39064.0 39065.0 39066.0 39067.0 39068.0 39069.0 39070.0 39071.0 39072.0 39073.0 39074.0 39075.0 39076.0 39077.0 39078.0 39079.0 39080.0 39081.0 39082.0 39083.0 39084.0 39085.0 39086.0 39087.0 39088.0 39089.0 39090.0 39091.0 39092.0 39093.0 39094.0 39095.0 39096.0 39097.0 39098.0 39099.0 39100.0 39101.0 39102.0 39103.0 39104.0 39105.0 39106.0 39107.0 39108.0 39109.0 39110.0 39111.0 39112.0 39113.0 39114.0 39115.0 39116.0 39117.0 39118.0 39119.0 39120.0 39121.0 39122.0 39123.0 39124.0 39125.0 39126.0 39127.0 39128.0 39129.0 39130.0 39131.0 39132.0 39133.0 39134.0 39135.0 39136.0 39137.0 39138.0 39139.0 39140.0 39141.0 39142.0 39143.0 39144.0 39145.0 39146.0 39147.0 39148.0 39149.0 39150.0 39151.0 39152.0 39153.0 39154.0 39155.0 39156.0 39157.0 39158.0 39159.0 39160.0 39161.0 39162.0 39163.0 39164.0 39165.0 39166.0 39167.0 39168.0 39169.0 39170.0 39171.0 39172.0 39173.0 39174.0 39175.0 39176.0 39177.0 39178.0 39179.0 39180.0 39181.0 39182.0 39183.0 39184.0 39185.0 39186.0 39187.0 39188.0 39189.0 39190.0 39191.0 39192.0 39193.0 39194.0 39195.0 39196.0 39197.0 39198.0 39199.0 39200.0 39201.0 39202.0 39203.0 39204.0 39205.0 39206.0 39207.0 39208.0 39209.0 39210.0 39211.0 39212.0 39213.0 39214.0 39215.0 39216.0 39217.0 39218.0 39219.0 39220.0 39221.0 39222.0 39223.0 39224.0 39225.0 39226.0 39227.0 39228.0 39229.0 39230.0 39231.0 39232.0 39233.0 39234.0 39235.0 39236.0 39237.0 39238.0 39239.0 39240.0 39241.0 39242.0 39243.0 39244.0 39245.0 39246.0 39247.0 39248.0 39249.0 39250.0 39251.0 39252.0 39253.0 39254.0 39255.0 39256.0 39257.0 39258.0 39259.0 39260.0 39261.0 39262.0 39263.0 39264.0 39265.0 39266.0 39267.0 39268.0 39269.0 39270.0 39271.0 39272.0 39273.0 39274.0 39275.0 39276.0 39277.0 39278.0 39279.0 39280.0 39281.0 39282.0 39283.0 39284.0 39285.0 39286.0 39287.0 39288.0 39289.0 39290.0 39291.0 39292.0 39293.0 39294.0 39295.0 39296.0 39297.0 39298.0 39299.0 39300.0 39301.0 39302.0 39303.0 39304.0 39305.0 39306.0 39307.0 39308.0 39309.0 39310.0 39311.0 39312.0 39313.0 39314.0 39315.0 39316.0 39317.0 39318.0 39319.0 39320.0 39321.0 39322.0 39323.0 39324.0 39325.0 39326.0 39327.0 39328.0 39329.0 39330.0 39331.0 39332.0 39333.0 39334.0 39335.0 39336.0 39337.0 39338.0 39339.0 39340.0 39341.0 39342.0 39343.0 39344.0 39345.0 39346.0 39347.0 39348.0 39349.0 39350.0 39351.0 39352.0 39353.0 39354.0 39355.0 39356.0 39357.0 39358.0 39359.0 39360.0 39361.0 39362.0 39363.0 39364.0 39365.0 39366.0 39367.0 39368.0 39369.0 39370.0 39371.0 39372.0 39373.0 39374.0 39375.0 39376.0 39377.0 39378.0 39379.0 39380.0 39381.0 39382.0 39383.0 39384.0 39385.0 39386.0 39387.0 39388.0 39389.0 39390.0 39391.0 39392.0 39393.0 39394.0 39395.0 39396.0 39397.0 39398.0 39399.0 39400.0 39401.0 39402.0 39403.0 39404.0 39405.0 39406.0 39407.0 39408.0 39409.0 39410.0 39411.0 39412.0 39413.0 39414.0 39415.0 39416.0 39417.0 39418.0 39419.0 39420.0 39421.0 39422.0 39423.0 39424.0 39425.0 39426.0 39427.0 39428.0 39429.0 39430.0 39431.0 39432.0 39433.0 39434.0 39435.0 39436.0 39437.0 39438.0 39439.0 39440.0 39441.0 39442.0 39443.0 39444.0 39445.0 39446.0 39447.0 39448.0 39449.0 39450.0 39451.0 39452.0 39453.0 39454.0 39455.0 39456.0 39457.0 39458.0 39459.0 39460.0 39461.0 39462.0 39463.0 39464.0 39465.0 39466.0 39467.0 39468.0 39469.0 39470.0 39471.0 39472.0 39473.0 39474.0 39475.0 39476.0 39477.0 39478.0 39479.0 39480.0 39481.0 39482.0 39483.0 39484.0 39485.0 39486.0 39487.0 39488.0 39489.0 39490.0 39491.0 39492.0 39493.0 39494.0 39495.0 39496.0 39497.0 39498.0 39499.0 39500.0 39501.0 39502.0 39503.0 39504.0 39505.0 39506.0 39507.0 39508.0 39509.0 39510.0 39511.0 39512.0 39513.0 39514.0 39515.0 39516.0 39517.0 39518.0 39519.0 39520.0 39521.0 39522.0 39523.0 39524.0 39525.0 39526.0 39527.0 39528.0 39529.0 39530.0 39531.0 39532.0 39533.0 39534.0 39535.0 39536.0 39537.0 39538.0 39539.0 39540.0 39541.0 39542.0 39543.0 39544.0 39545.0 39546.0 39547.0 39548.0 39549.0 39550.0 39551.0 39552.0 39553.0 39554.0 39555.0 39556.0 39557.0 39558.0 39559.0 39560.0 39561.0 39562.0 39563.0 39564.0 39565.0 39566.0 39567.0 39568.0 39569.0 39570.0 39571.0 39572.0 39573.0 39574.0 39575.0 39576.0 39577.0 39578.0 39579.0 39580.0 39581.0 39582.0 39583.0 39584.0 39585.0 39586.0 39587.0 39588.0 39589.0 39590.0 39591.0 39592.0 39593.0 39594.0 39595.0 39596.0 39597.0 39598.0 39599.0 39600.0 39601.0 39602.0 39603.0 39604.0 39605.0 39606.0 39607.0 39608.0 39609.0 39610.0 39611.0 39612.0 39613.0 39614.0 39615.0 39616.0 39617.0 39618.0 39619.0 39620.0 39621.0 39622.0 39623.0 39624.0 39625.0 39626.0 39627.0 39628.0 39629.0 39630.0 39631.0 39632.0 39633.0 39634.0 39635.0 39636.0 39637.0 39638.0 39639.0 39640.0 39641.0 39642.0 39643.0 39644.0 39645.0 39646.0 39647.0 39648.0 39649.0 39650.0 39651.0 39652.0 39653.0 39654.0 39655.0 39656.0 39657.0 39658.0 39659.0 39660.0 39661.0 39662.0 39663.0 39664.0 39665.0 39666.0 39667.0 39668.0 39669.0 39670.0 39671.0 39672.0 39673.0 39674.0 39675.0 39676.0 39677.0 39678.0 39679.0 39680.0 39681.0 39682.0 39683.0 39684.0 39685.0 39686.0 39687.0 39688.0 39689.0 39690.0 39691.0 39692.0 39693.0 39694.0 39695.0 39696.0 39697.0 39698.0 39699.0 39700.0 39701.0 39702.0 39703.0 39704.0 39705.0 39706.0 39707.0 39708.0 39709.0 39710.0 39711.0 39712.0 39713.0 39714.0 39715.0 39716.0 39717.0 39718.0 39719.0 39720.0 39721.0 39722.0 39723.0 39724.0 39725.0 39726.0 39727.0 39728.0 39729.0 39730.0 39731.0 39732.0 39733.0 39734.0 39735.0 39736.0 39737.0 39738.0 39739.0 39740.0 39741.0 39742.0 39743.0 39744.0 39745.0 39746.0 39747.0 39748.0 39749.0 39750.0 39751.0 39752.0 39753.0 39754.0 39755.0 39756.0 39757.0 39758.0 39759.0 39760.0 39761.0 39762.0 39763.0 39764.0 39765.0 39766.0 39767.0 39768.0 39769.0 39770.0 39771.0 39772.0 39773.0 39774.0 39775.0 39776.0 39777.0 39778.0 39779.0 39780.0 39781.0 39782.0 39783.0 39784.0 39785.0 39786.0 39787.0 39788.0 39789.0 39790.0 39791.0 39792.0 39793.0 39794.0 39795.0 39796.0 39797.0 39798.0 39799.0 39800.0 39801.0 39802.0 39803.0 39804.0 39805.0 39806.0 39807.0 39808.0 39809.0 39810.0 39811.0 39812.0 39813.0 39814.0 39815.0 39816.0 39817.0 39818.0 39819.0 39820.0 39821.0 39822.0 39823.0 39824.0 39825.0 39826.0 39827.0 39828.0 39829.0 39830.0 39831.0 39832.0 39833.0 39834.0 39835.0 39836.0 39837.0 39838.0 39839.0 39840.0 39841.0 39842.0 39843.0 39844.0 39845.0 39846.0 39847.0 39848.0 39849.0 39850.0 39851.0 39852.0 39853.0 39854.0 39855.0 39856.0 39857.0 39858.0 39859.0 39860.0 39861.0 39862.0 39863.0 39864.0 39865.0 39866.0 39867.0 39868.0 39869.0 39870.0 39871.0 39872.0 39873.0 39874.0 39875.0 39876.0 39877.0 39878.0 39879.0 39880.0 39881.0 39882.0 39883.0 39884.0 39885.0 39886.0 39887.0 39888.0 39889.0 39890.0 39891.0 39892.0 39893.0 39894.0 39895.0 39896.0 39897.0 39898.0 39899.0 39900.0 39901.0 39902.0 39903.0 39904.0 39905.0 39906.0 39907.0 39908.0 39909.0 39910.0 39911.0 39912.0 39913.0 39914.0 39915.0 39916.0 39917.0 39918.0 39919.0 39920.0 39921.0 39922.0 39923.0 39924.0 39925.0 39926.0 39927.0 39928.0 39929.0 39930.0 39931.0 39932.0 39933.0 39934.0 39935.0 39936.0 39937.0 39938.0 39939.0 39940.0 39941.0 39942.0 39943.0 39944.0 39945.0 39946.0 39947.0 39948.0 39949.0 39950.0 39951.0 39952.0 39953.0 39954.0 39955.0 39956.0 39957.0 39958.0 39959.0 39960.0 39961.0 39962.0 39963.0 39964.0 39965.0 39966.0 39967.0 39968.0 39969.0 39970.0 39971.0 39972.0 39973.0 39974.0 39975.0 39976.0 39977.0 39978.0 39979.0 39980.0 39981.0 39982.0 39983.0 39984.0 39985.0 39986.0 39987.0 39988.0 39989.0 39990.0 39991.0 39992.0 39993.0 39994.0 39995.0 39996.0 39997.0 39998.0 39999.0 40000.0 40001.0 40002.0 40003.0 40004.0 40005.0 40006.0 40007.0 40008.0 40009.0 40010.0 40011.0 40012.0 40013.0 40014.0 40015.0 40016.0 40017.0 40018.0 40019.0 40020.0 40021.0 40022.0 40023.0 40024.0 40025.0 40026.0 40027.0 40028.0 40029.0 40030.0 40031.0 40032.0 40033.0 40034.0 40035.0 40036.0 40037.0 40038.0 40039.0 40040.0 40041.0 40042.0 40043.0 40044.0 40045.0 40046.0 40047.0 40048.0 40049.0 40050.0 40051.0 40052.0 40053.0 40054.0 40055.0 40056.0 40057.0 40058.0 40059.0 40060.0 40061.0 40062.0 40063.0 40064.0 40065.0 40066.0 40067.0 40068.0 40069.0 40070.0 40071.0 40072.0 40073.0 40074.0 40075.0 40076.0 40077.0 40078.0 40079.0 40080.0 40081.0 40082.0 40083.0 40084.0 40085.0 40086.0 40087.0 40088.0 40089.0 40090.0 40091.0 40092.0 40093.0 40094.0 40095.0 40096.0 40097.0 40098.0 40099.0 40100.0 40101.0 40102.0 40103.0 40104.0 40105.0 40106.0 40107.0 40108.0 40109.0 40110.0 40111.0 40112.0 40113.0 40114.0 40115.0 40116.0 40117.0 40118.0 40119.0 40120.0 40121.0 40122.0 40123.0 40124.0 40125.0 40126.0 40127.0 40128.0 40129.0 40130.0 40131.0 40132.0 40133.0 40134.0 40135.0 40136.0 40137.0 40138.0 40139.0 40140.0 40141.0 40142.0 40143.0 40144.0 40145.0 40146.0 40147.0 40148.0 40149.0 40150.0 40151.0 40152.0 40153.0 40154.0 40155.0 40156.0 40157.0 40158.0 40159.0 40160.0 40161.0 40162.0 40163.0 40164.0 40165.0 40166.0 40167.0 40168.0 40169.0 40170.0 40171.0 40172.0 40173.0 40174.0 40175.0 40176.0 40177.0 40178.0 40179.0 40180.0 40181.0 40182.0 40183.0 40184.0 40185.0 40186.0 40187.0 40188.0 40189.0 40190.0 40191.0 40192.0 40193.0 40194.0 40195.0 40196.0 40197.0 40198.0 40199.0 40200.0 40201.0 40202.0 40203.0 40204.0 40205.0 40206.0 40207.0 40208.0 40209.0 40210.0 40211.0 40212.0 40213.0 40214.0 40215.0 40216.0 40217.0 40218.0 40219.0 40220.0 40221.0 40222.0 40223.0 40224.0 40225.0 40226.0 40227.0 40228.0 40229.0 40230.0 40231.0 40232.0 40233.0 40234.0 40235.0 40236.0 40237.0 40238.0 40239.0 40240.0 40241.0 40242.0 40243.0 40244.0 40245.0 40246.0 40247.0 40248.0 40249.0 40250.0 40251.0 40252.0 40253.0 40254.0 40255.0 40256.0 40257.0 40258.0 40259.0 40260.0 40261.0 40262.0 40263.0 40264.0 40265.0 40266.0 40267.0 40268.0 40269.0 40270.0 40271.0 40272.0 40273.0 40274.0 40275.0 40276.0 40277.0 40278.0 40279.0 40280.0 40281.0 40282.0 40283.0 40284.0 40285.0 40286.0 40287.0 40288.0 40289.0 40290.0 40291.0 40292.0 40293.0 40294.0 40295.0 40296.0 40297.0 40298.0 40299.0 40300.0 40301.0 40302.0 40303.0 40304.0 40305.0 40306.0 40307.0 40308.0 40309.0 40310.0 40311.0 40312.0 40313.0 40314.0 40315.0 40316.0 40317.0 40318.0 40319.0 40320.0 40321.0 40322.0 40323.0 40324.0 40325.0 40326.0 40327.0 40328.0 40329.0 40330.0 40331.0 40332.0 40333.0 40334.0 40335.0 40336.0 40337.0 40338.0 40339.0 40340.0 40341.0 40342.0 40343.0 40344.0 40345.0 40346.0 40347.0 40348.0 40349.0 40350.0 40351.0 40352.0 40353.0 40354.0 40355.0 40356.0 40357.0 40358.0 40359.0 40360.0 40361.0 40362.0 40363.0 40364.0 40365.0 40366.0 40367.0 40368.0 40369.0 40370.0 40371.0 40372.0 40373.0 40374.0 40375.0 40376.0 40377.0 40378.0 40379.0 40380.0 40381.0 40382.0 40383.0 40384.0 40385.0 40386.0 40387.0 40388.0 40389.0 40390.0 40391.0 40392.0 40393.0 40394.0 40395.0 40396.0 40397.0 40398.0 40399.0 40400.0 40401.0 40402.0 40403.0 40404.0 40405.0 40406.0 40407.0 40408.0 40409.0 40410.0 40411.0 40412.0 40413.0 40414.0 40415.0 40416.0 40417.0 40418.0 40419.0 40420.0 40421.0 40422.0 40423.0 40424.0 40425.0 40426.0 40427.0 40428.0 40429.0 40430.0 40431.0 40432.0 40433.0 40434.0 40435.0 40436.0 40437.0 40438.0 40439.0 40440.0 40441.0 40442.0 40443.0 40444.0 40445.0 40446.0 40447.0 40448.0 40449.0 40450.0 40451.0 40452.0 40453.0 40454.0 40455.0 40456.0 40457.0 40458.0 40459.0 40460.0 40461.0 40462.0 40463.0 40464.0 40465.0 40466.0 40467.0 40468.0 40469.0 40470.0 40471.0 40472.0 40473.0 40474.0 40475.0 40476.0 40477.0 40478.0 40479.0 40480.0 40481.0 40482.0 40483.0 40484.0 40485.0 40486.0 40487.0 40488.0 40489.0 40490.0 40491.0 40492.0 40493.0 40494.0 40495.0 40496.0 40497.0 40498.0 40499.0 40500.0 40501.0 40502.0 40503.0 40504.0 40505.0 40506.0 40507.0 40508.0 40509.0 40510.0 40511.0 40512.0 40513.0 40514.0 40515.0 40516.0 40517.0 40518.0 40519.0 40520.0 40521.0 40522.0 40523.0 40524.0 40525.0 40526.0 40527.0 40528.0 40529.0 40530.0 40531.0 40532.0 40533.0 40534.0 40535.0 40536.0 40537.0 40538.0 40539.0 40540.0 40541.0 40542.0 40543.0 40544.0 40545.0 40546.0 40547.0 40548.0 40549.0 40550.0 40551.0 40552.0 40553.0 40554.0 40555.0 40556.0 40557.0 40558.0 40559.0 40560.0 40561.0 40562.0 40563.0 40564.0 40565.0 40566.0 40567.0 40568.0 40569.0 40570.0 40571.0 40572.0 40573.0 40574.0 40575.0 40576.0 40577.0 40578.0 40579.0 40580.0 40581.0 40582.0 40583.0 40584.0 40585.0 40586.0 40587.0 40588.0 40589.0 40590.0 40591.0 40592.0 40593.0 40594.0 40595.0 40596.0 40597.0 40598.0 40599.0 40600.0 40601.0 40602.0 40603.0 40604.0 40605.0 40606.0 40607.0 40608.0 40609.0 40610.0 40611.0 40612.0 40613.0 40614.0 40615.0 40616.0 40617.0 40618.0 40619.0 40620.0 40621.0 40622.0 40623.0 40624.0 40625.0 40626.0 40627.0 40628.0 40629.0 40630.0 40631.0 40632.0 40633.0 40634.0 40635.0 40636.0 40637.0 40638.0 40639.0 40640.0 40641.0 40642.0 40643.0 40644.0 40645.0 40646.0 40647.0 40648.0 40649.0 40650.0 40651.0 40652.0 40653.0 40654.0 40655.0 40656.0 40657.0 40658.0 40659.0 40660.0 40661.0 40662.0 40663.0 40664.0 40665.0 40666.0 40667.0 40668.0 40669.0 40670.0 40671.0 40672.0 40673.0 40674.0 40675.0 40676.0 40677.0 40678.0 40679.0 40680.0 40681.0 40682.0 40683.0 40684.0 40685.0 40686.0 40687.0 40688.0 40689.0 40690.0 40691.0 40692.0 40693.0 40694.0 40695.0 40696.0 40697.0 40698.0 40699.0 40700.0 40701.0 40702.0 40703.0 40704.0 40705.0 40706.0 40707.0 40708.0 40709.0 40710.0 40711.0 40712.0 40713.0 40714.0 40715.0 40716.0 40717.0 40718.0 40719.0 40720.0 40721.0 40722.0 40723.0 40724.0 40725.0 40726.0 40727.0 40728.0 40729.0 40730.0 40731.0 40732.0 40733.0 40734.0 40735.0 40736.0 40737.0 40738.0 40739.0 40740.0 40741.0 40742.0 40743.0 40744.0 40745.0 40746.0 40747.0 40748.0 40749.0 40750.0 40751.0 40752.0 40753.0 40754.0 40755.0 40756.0 40757.0 40758.0 40759.0 40760.0 40761.0 40762.0 40763.0 40764.0 40765.0 40766.0 40767.0 40768.0 40769.0 40770.0 40771.0 40772.0 40773.0 40774.0 40775.0 40776.0 40777.0 40778.0 40779.0 40780.0 40781.0 40782.0 40783.0 40784.0 40785.0 40786.0 40787.0 40788.0 40789.0 40790.0 40791.0 40792.0 40793.0 40794.0 40795.0 40796.0 40797.0 40798.0 40799.0 40800.0 40801.0 40802.0 40803.0 40804.0 40805.0 40806.0 40807.0 40808.0 40809.0 40810.0 40811.0 40812.0 40813.0 40814.0 40815.0 40816.0 40817.0 40818.0 40819.0 40820.0 40821.0 40822.0 40823.0 40824.0 40825.0 40826.0 40827.0 40828.0 40829.0 40830.0 40831.0 40832.0 40833.0 40834.0 40835.0 40836.0 40837.0 40838.0 40839.0 40840.0 40841.0 40842.0 40843.0 40844.0 40845.0 40846.0 40847.0 40848.0 40849.0 40850.0 40851.0 40852.0 40853.0 40854.0 40855.0 40856.0 40857.0 40858.0 40859.0 40860.0 40861.0 40862.0 40863.0 40864.0 40865.0 40866.0 40867.0 40868.0 40869.0 40870.0 40871.0 40872.0 40873.0 40874.0 40875.0 40876.0 40877.0 40878.0 40879.0 40880.0 40881.0 40882.0 40883.0 40884.0 40885.0 40886.0 40887.0 40888.0 40889.0 40890.0 40891.0 40892.0 40893.0 40894.0 40895.0 40896.0 40897.0 40898.0 40899.0 40900.0 40901.0 40902.0 40903.0 40904.0 40905.0 40906.0 40907.0 40908.0 40909.0 40910.0 40911.0 40912.0 40913.0 40914.0 40915.0 40916.0 40917.0 40918.0 40919.0 40920.0 40921.0 40922.0 40923.0 40924.0 40925.0 40926.0 40927.0 40928.0 40929.0 40930.0 40931.0 40932.0 40933.0 40934.0 40935.0 40936.0 40937.0 40938.0 40939.0 40940.0 40941.0 40942.0 40943.0 40944.0 40945.0 40946.0 40947.0 40948.0 40949.0 40950.0 40951.0 40952.0 40953.0 40954.0 40955.0 40956.0 40957.0 40958.0 40959.0 40960.0 40961.0 40962.0 40963.0 40964.0 40965.0 40966.0 40967.0 40968.0 40969.0 40970.0 40971.0 40972.0 40973.0 40974.0 40975.0 40976.0 40977.0 40978.0 40979.0 40980.0 40981.0 40982.0 40983.0 40984.0 40985.0 40986.0 40987.0 40988.0 40989.0 40990.0 40991.0 40992.0 40993.0 40994.0 40995.0 40996.0 40997.0 40998.0 40999.0 41000.0 41001.0 41002.0 41003.0 41004.0 41005.0 41006.0 41007.0 41008.0 41009.0 41010.0 41011.0 41012.0 41013.0 41014.0 41015.0 41016.0 41017.0 41018.0 41019.0 41020.0 41021.0 41022.0 41023.0 41024.0 41025.0 41026.0 41027.0 41028.0 41029.0 41030.0 41031.0 41032.0 41033.0 41034.0 41035.0 41036.0 41037.0 41038.0 41039.0 41040.0 41041.0 41042.0 41043.0 41044.0 41045.0 41046.0 41047.0 41048.0 41049.0 41050.0 41051.0 41052.0 41053.0 41054.0 41055.0 41056.0 41057.0 41058.0 41059.0 41060.0 41061.0 41062.0 41063.0 41064.0 41065.0 41066.0 41067.0 41068.0 41069.0 41070.0 41071.0 41072.0 41073.0 41074.0 41075.0 41076.0 41077.0 41078.0 41079.0 41080.0 41081.0 41082.0 41083.0 41084.0 41085.0 41086.0 41087.0 41088.0 41089.0 41090.0 41091.0 41092.0 41093.0 41094.0 41095.0 41096.0 41097.0 41098.0 41099.0 41100.0 41101.0 41102.0 41103.0 41104.0 41105.0 41106.0 41107.0 41108.0 41109.0 41110.0 41111.0 41112.0 41113.0 41114.0 41115.0 41116.0 41117.0 41118.0 41119.0 41120.0 41121.0 41122.0 41123.0 41124.0 41125.0 41126.0 41127.0 41128.0 41129.0 41130.0 41131.0 41132.0 41133.0 41134.0 41135.0 41136.0 41137.0 41138.0 41139.0 41140.0 41141.0 41142.0 41143.0 41144.0 41145.0 41146.0 41147.0 41148.0 41149.0 41150.0 41151.0 41152.0 41153.0 41154.0 41155.0 41156.0 41157.0 41158.0 41159.0 41160.0 41161.0 41162.0 41163.0 41164.0 41165.0 41166.0 41167.0 41168.0 41169.0 41170.0 41171.0 41172.0 41173.0 41174.0 41175.0 41176.0 41177.0 41178.0 41179.0 41180.0 41181.0 41182.0 41183.0 41184.0 41185.0 41186.0 41187.0 41188.0 41189.0 41190.0 41191.0 41192.0 41193.0 41194.0 41195.0 41196.0 41197.0 41198.0 41199.0 41200.0 41201.0 41202.0 41203.0 41204.0 41205.0 41206.0 41207.0 41208.0 41209.0 41210.0 41211.0 41212.0 41213.0 41214.0 41215.0 41216.0 41217.0 41218.0 41219.0 41220.0 41221.0 41222.0 41223.0 41224.0 41225.0 41226.0 41227.0 41228.0 41229.0 41230.0 41231.0 41232.0 41233.0 41234.0 41235.0 41236.0 41237.0 41238.0 41239.0 41240.0 41241.0 41242.0 41243.0 41244.0 41245.0 41246.0 41247.0 41248.0 41249.0 41250.0 41251.0 41252.0 41253.0 41254.0 41255.0 41256.0 41257.0 41258.0 41259.0 41260.0 41261.0 41262.0 41263.0 41264.0 41265.0 41266.0 41267.0 41268.0 41269.0 41270.0 41271.0 41272.0 41273.0 41274.0 41275.0 41276.0 41277.0 41278.0 41279.0 41280.0 41281.0 41282.0 41283.0 41284.0 41285.0 41286.0 41287.0 41288.0 41289.0 41290.0 41291.0 41292.0 41293.0 41294.0 41295.0 41296.0 41297.0 41298.0 41299.0 41300.0 41301.0 41302.0 41303.0 41304.0 41305.0 41306.0 41307.0 41308.0 41309.0 41310.0 41311.0 41312.0 41313.0 41314.0 41315.0 41316.0 41317.0 41318.0 41319.0 41320.0 41321.0 41322.0 41323.0 41324.0 41325.0 41326.0 41327.0 41328.0 41329.0 41330.0 41331.0 41332.0 41333.0 41334.0 41335.0 41336.0 41337.0 41338.0 41339.0 41340.0 41341.0 41342.0 41343.0 41344.0 41345.0 41346.0 41347.0 41348.0 41349.0 41350.0 41351.0 41352.0 41353.0 41354.0 41355.0 41356.0 41357.0 41358.0 41359.0 41360.0 41361.0 41362.0 41363.0 41364.0 41365.0 41366.0 41367.0 41368.0 41369.0 41370.0 41371.0 41372.0 41373.0 41374.0 41375.0 41376.0 41377.0 41378.0 41379.0 41380.0 41381.0 41382.0 41383.0 41384.0 41385.0 41386.0 41387.0 41388.0 41389.0 41390.0 41391.0 41392.0 41393.0 41394.0 41395.0 41396.0 41397.0 41398.0 41399.0 41400.0 41401.0 41402.0 41403.0 41404.0 41405.0 41406.0 41407.0 41408.0 41409.0 41410.0 41411.0 41412.0 41413.0 41414.0 41415.0 41416.0 41417.0 41418.0 41419.0 41420.0 41421.0 41422.0 41423.0 41424.0 41425.0 41426.0 41427.0 41428.0 41429.0 41430.0 41431.0 41432.0 41433.0 41434.0 41435.0 41436.0 41437.0 41438.0 41439.0 41440.0 41441.0 41442.0 41443.0 41444.0 41445.0 41446.0 41447.0 41448.0 41449.0 41450.0 41451.0 41452.0 41453.0 41454.0 41455.0 41456.0 41457.0 41458.0 41459.0 41460.0 41461.0 41462.0 41463.0 41464.0 41465.0 41466.0 41467.0 41468.0 41469.0 41470.0 41471.0 41472.0 41473.0 41474.0 41475.0 41476.0 41477.0 41478.0 41479.0 41480.0 41481.0 41482.0 41483.0 41484.0 41485.0 41486.0 41487.0 41488.0 41489.0 41490.0 41491.0 41492.0 41493.0 41494.0 41495.0 41496.0 41497.0 41498.0 41499.0 41500.0 41501.0 41502.0 41503.0 41504.0 41505.0 41506.0 41507.0 41508.0 41509.0 41510.0 41511.0 41512.0 41513.0 41514.0 41515.0 41516.0 41517.0 41518.0 41519.0 41520.0 41521.0 41522.0 41523.0 41524.0 41525.0 41526.0 41527.0 41528.0 41529.0 41530.0 41531.0 41532.0 41533.0 41534.0 41535.0 41536.0 41537.0 41538.0 41539.0 41540.0 41541.0 41542.0 41543.0 41544.0 41545.0 41546.0 41547.0 41548.0 41549.0 41550.0 41551.0 41552.0 41553.0 41554.0 41555.0 41556.0 41557.0 41558.0 41559.0 41560.0 41561.0 41562.0 41563.0 41564.0 41565.0 41566.0 41567.0 41568.0 41569.0 41570.0 41571.0 41572.0 41573.0 41574.0 41575.0 41576.0 41577.0 41578.0 41579.0 41580.0 41581.0 41582.0 41583.0 41584.0 41585.0 41586.0 41587.0 41588.0 41589.0 41590.0 41591.0 41592.0 41593.0 41594.0 41595.0 41596.0 41597.0 41598.0 41599.0 41600.0 41601.0 41602.0 41603.0 41604.0 41605.0 41606.0 41607.0 41608.0 41609.0 41610.0 41611.0 41612.0 41613.0 41614.0 41615.0 41616.0 41617.0 41618.0 41619.0 41620.0 41621.0 41622.0 41623.0 41624.0 41625.0 41626.0 41627.0 41628.0 41629.0 41630.0 41631.0 41632.0 41633.0 41634.0 41635.0 41636.0 41637.0 41638.0 41639.0 41640.0 41641.0 41642.0 41643.0 41644.0 41645.0 41646.0 41647.0 41648.0 41649.0 41650.0 41651.0 41652.0 41653.0 41654.0 41655.0 41656.0 41657.0 41658.0 41659.0 41660.0 41661.0 41662.0 41663.0 41664.0 41665.0 41666.0 41667.0 41668.0 41669.0 41670.0 41671.0 41672.0 41673.0 41674.0 41675.0 41676.0 41677.0 41678.0 41679.0 41680.0 41681.0 41682.0 41683.0 41684.0 41685.0 41686.0 41687.0 41688.0 41689.0 41690.0 41691.0 41692.0 41693.0 41694.0 41695.0 41696.0 41697.0 41698.0 41699.0 41700.0 41701.0 41702.0 41703.0 41704.0 41705.0 41706.0 41707.0 41708.0 41709.0 41710.0 41711.0 41712.0 41713.0 41714.0 41715.0 41716.0 41717.0 41718.0 41719.0 41720.0 41721.0 41722.0 41723.0 41724.0 41725.0 41726.0 41727.0 41728.0 41729.0 41730.0 41731.0 41732.0 41733.0 41734.0 41735.0 41736.0 41737.0 41738.0 41739.0 41740.0 41741.0 41742.0 41743.0 41744.0 41745.0 41746.0 41747.0 41748.0 41749.0 41750.0 41751.0 41752.0 41753.0 41754.0 41755.0 41756.0 41757.0 41758.0 41759.0 41760.0 41761.0 41762.0 41763.0 41764.0 41765.0 41766.0 41767.0 41768.0 41769.0 41770.0 41771.0 41772.0 41773.0 41774.0 41775.0 41776.0 41777.0 41778.0 41779.0 41780.0 41781.0 41782.0 41783.0 41784.0 41785.0 41786.0 41787.0 41788.0 41789.0 41790.0 41791.0 41792.0 41793.0 41794.0 41795.0 41796.0 41797.0 41798.0 41799.0 41800.0 41801.0 41802.0 41803.0 41804.0 41805.0 41806.0 41807.0 41808.0 41809.0 41810.0 41811.0 41812.0 41813.0 41814.0 41815.0 41816.0 41817.0 41818.0 41819.0 41820.0 41821.0 41822.0 41823.0 41824.0 41825.0 41826.0 41827.0 41828.0 41829.0 41830.0 41831.0 41832.0 41833.0 41834.0 41835.0 41836.0 41837.0 41838.0 41839.0 41840.0 41841.0 41842.0 41843.0 41844.0 41845.0 41846.0 41847.0 41848.0 41849.0 41850.0 41851.0 41852.0 41853.0 41854.0 41855.0 41856.0 41857.0 41858.0 41859.0 41860.0 41861.0 41862.0 41863.0 41864.0 41865.0 41866.0 41867.0 41868.0 41869.0 41870.0 41871.0 41872.0 41873.0 41874.0 41875.0 41876.0 41877.0 41878.0 41879.0 41880.0 41881.0 41882.0 41883.0 41884.0 41885.0 41886.0 41887.0 41888.0 41889.0 41890.0 41891.0 41892.0 41893.0 41894.0 41895.0 41896.0 41897.0 41898.0 41899.0 41900.0 41901.0 41902.0 41903.0 41904.0 41905.0 41906.0 41907.0 41908.0 41909.0 41910.0 41911.0 41912.0 41913.0 41914.0 41915.0 41916.0 41917.0 41918.0 41919.0 41920.0 41921.0 41922.0 41923.0 41924.0 41925.0 41926.0 41927.0 41928.0 41929.0 41930.0 41931.0 41932.0 41933.0 41934.0 41935.0 41936.0 41937.0 41938.0 41939.0 41940.0 41941.0 41942.0 41943.0 41944.0 41945.0 41946.0 41947.0 41948.0 41949.0 41950.0 41951.0 41952.0 41953.0 41954.0 41955.0 41956.0 41957.0 41958.0 41959.0 41960.0 41961.0 41962.0 41963.0 41964.0 41965.0 41966.0 41967.0 41968.0 41969.0 41970.0 41971.0 41972.0 41973.0 41974.0 41975.0 41976.0 41977.0 41978.0 41979.0 41980.0 41981.0 41982.0 41983.0 41984.0 41985.0 41986.0 41987.0 41988.0 41989.0 41990.0 41991.0 41992.0 41993.0 41994.0 41995.0 41996.0 41997.0 41998.0 41999.0 42000.0 42001.0 42002.0 42003.0 42004.0 42005.0 42006.0 42007.0 42008.0 42009.0 42010.0 42011.0 42012.0 42013.0 42014.0 42015.0 42016.0 42017.0 42018.0 42019.0 42020.0 42021.0 42022.0 42023.0 42024.0 42025.0 42026.0 42027.0 42028.0 42029.0 42030.0 42031.0 42032.0 42033.0 42034.0 42035.0 42036.0 42037.0 42038.0 42039.0 42040.0 42041.0 42042.0 42043.0 42044.0 42045.0 42046.0 42047.0 42048.0 42049.0 42050.0 42051.0 42052.0 42053.0 42054.0 42055.0 42056.0 42057.0 42058.0 42059.0 42060.0 42061.0 42062.0 42063.0 42064.0 42065.0 42066.0 42067.0 42068.0 42069.0 42070.0 42071.0 42072.0 42073.0 42074.0 42075.0 42076.0 42077.0 42078.0 42079.0 42080.0 42081.0 42082.0 42083.0 42084.0 42085.0 42086.0 42087.0 42088.0 42089.0 42090.0 42091.0 42092.0 42093.0 42094.0 42095.0 42096.0 42097.0 42098.0 42099.0 42100.0 42101.0 42102.0 42103.0 42104.0 42105.0 42106.0 42107.0 42108.0 42109.0 42110.0 42111.0 42112.0 42113.0 42114.0 42115.0 42116.0 42117.0 42118.0 42119.0 42120.0 42121.0 42122.0 42123.0 42124.0 42125.0 42126.0 42127.0 42128.0 42129.0 42130.0 42131.0 42132.0 42133.0 42134.0 42135.0 42136.0 42137.0 42138.0 42139.0 42140.0 42141.0 42142.0 42143.0 42144.0 42145.0 42146.0 42147.0 42148.0 42149.0 42150.0 42151.0 42152.0 42153.0 42154.0 42155.0 42156.0 42157.0 42158.0 42159.0 42160.0 42161.0 42162.0 42163.0 42164.0 42165.0 42166.0 42167.0 42168.0 42169.0 42170.0 42171.0 42172.0 42173.0 42174.0 42175.0 42176.0 42177.0 42178.0 42179.0 42180.0 42181.0 42182.0 42183.0 42184.0 42185.0 42186.0 42187.0 42188.0 42189.0 42190.0 42191.0 42192.0 42193.0 42194.0 42195.0 42196.0 42197.0 42198.0 42199.0 42200.0 42201.0 42202.0 42203.0 42204.0 42205.0 42206.0 42207.0 42208.0 42209.0 42210.0 42211.0 42212.0 42213.0 42214.0 42215.0 42216.0 42217.0 42218.0 42219.0 42220.0 42221.0 42222.0 42223.0 42224.0 42225.0 42226.0 42227.0 42228.0 42229.0 42230.0 42231.0 42232.0 42233.0 42234.0 42235.0 42236.0 42237.0 42238.0 42239.0 42240.0 42241.0 42242.0 42243.0 42244.0 42245.0 42246.0 42247.0 42248.0 42249.0 42250.0 42251.0 42252.0 42253.0 42254.0 42255.0 42256.0 42257.0 42258.0 42259.0 42260.0 42261.0 42262.0 42263.0 42264.0 42265.0 42266.0 42267.0 42268.0 42269.0 42270.0 42271.0 42272.0 42273.0 42274.0 42275.0 42276.0 42277.0 42278.0 42279.0 42280.0 42281.0 42282.0 42283.0 42284.0 42285.0 42286.0 42287.0 42288.0 42289.0 42290.0 42291.0 42292.0 42293.0 42294.0 42295.0 42296.0 42297.0 42298.0 42299.0 42300.0 42301.0 42302.0 42303.0 42304.0 42305.0 42306.0 42307.0 42308.0 42309.0 42310.0 42311.0 42312.0 42313.0 42314.0 42315.0 42316.0 42317.0 42318.0 42319.0 42320.0 42321.0 42322.0 42323.0 42324.0 42325.0 42326.0 42327.0 42328.0 42329.0 42330.0 42331.0 42332.0 42333.0 42334.0 42335.0 42336.0 42337.0 42338.0 42339.0 42340.0 42341.0 42342.0 42343.0 42344.0 42345.0 42346.0 42347.0 42348.0 42349.0 42350.0 42351.0 42352.0 42353.0 42354.0 42355.0 42356.0 42357.0 42358.0 42359.0 42360.0 42361.0 42362.0 42363.0 42364.0 42365.0 42366.0 42367.0 42368.0 42369.0 42370.0 42371.0 42372.0 42373.0 42374.0 42375.0 42376.0 42377.0 42378.0 42379.0 42380.0 42381.0 42382.0 42383.0 42384.0 42385.0 42386.0 42387.0 42388.0 42389.0 42390.0 42391.0 42392.0 42393.0 42394.0 42395.0 42396.0 42397.0 42398.0 42399.0 42400.0 42401.0 42402.0 42403.0 42404.0 42405.0 42406.0 42407.0 42408.0 42409.0 42410.0 42411.0 42412.0 42413.0 42414.0 42415.0 42416.0 42417.0 42418.0 42419.0 42420.0 42421.0 42422.0 42423.0 42424.0 42425.0 42426.0 42427.0 42428.0 42429.0 42430.0 42431.0 42432.0 42433.0 42434.0 42435.0 42436.0 42437.0 42438.0 42439.0 42440.0 42441.0 42442.0 42443.0 42444.0 42445.0 42446.0 42447.0 42448.0 42449.0 42450.0 42451.0 42452.0 42453.0 42454.0 42455.0 42456.0 42457.0 42458.0 42459.0 42460.0 42461.0 42462.0 42463.0 42464.0 42465.0 42466.0 42467.0 42468.0 42469.0 42470.0 42471.0 42472.0 42473.0 42474.0 42475.0 42476.0 42477.0 42478.0 42479.0 42480.0 42481.0 42482.0 42483.0 42484.0 42485.0 42486.0 42487.0 42488.0 42489.0 42490.0 42491.0 42492.0 42493.0 42494.0 42495.0 42496.0 42497.0 42498.0 42499.0 42500.0 42501.0 42502.0 42503.0 42504.0 42505.0 42506.0 42507.0 42508.0 42509.0 42510.0 42511.0 42512.0 42513.0 42514.0 42515.0 42516.0 42517.0 42518.0 42519.0 42520.0 42521.0 42522.0 42523.0 42524.0 42525.0 42526.0 42527.0 42528.0 42529.0 42530.0 42531.0 42532.0 42533.0 42534.0 42535.0 42536.0 42537.0 42538.0 42539.0 42540.0 42541.0 42542.0 42543.0 42544.0 42545.0 42546.0 42547.0 42548.0 42549.0 42550.0 42551.0 42552.0 42553.0 42554.0 42555.0 42556.0 42557.0 42558.0 42559.0 42560.0 42561.0 42562.0 42563.0 42564.0 42565.0 42566.0 42567.0 42568.0 42569.0 42570.0 42571.0 42572.0 42573.0 42574.0 42575.0 42576.0 42577.0 42578.0 42579.0 42580.0 42581.0 42582.0 42583.0 42584.0 42585.0 42586.0 42587.0 42588.0 42589.0 42590.0 42591.0 42592.0 42593.0 42594.0 42595.0 42596.0 42597.0 42598.0 42599.0 42600.0 42601.0 42602.0 42603.0 42604.0 42605.0 42606.0 42607.0 42608.0 42609.0 42610.0 42611.0 42612.0 42613.0 42614.0 42615.0 42616.0 42617.0 42618.0 42619.0 42620.0 42621.0 42622.0 42623.0 42624.0 42625.0 42626.0 42627.0 42628.0 42629.0 42630.0 42631.0 42632.0 42633.0 42634.0 42635.0 42636.0 42637.0 42638.0 42639.0 42640.0 42641.0 42642.0 42643.0 42644.0 42645.0 42646.0 42647.0 42648.0 42649.0 42650.0 42651.0 42652.0 42653.0 42654.0 42655.0 42656.0 42657.0 42658.0 42659.0 42660.0 42661.0 42662.0 42663.0 42664.0 42665.0 42666.0 42667.0 42668.0 42669.0 42670.0 42671.0 42672.0 42673.0 42674.0 42675.0 42676.0 42677.0 42678.0 42679.0 42680.0 42681.0 42682.0 42683.0 42684.0 42685.0 42686.0 42687.0 42688.0 42689.0 42690.0 42691.0 42692.0 42693.0 42694.0 42695.0 42696.0 42697.0 42698.0 42699.0 42700.0 42701.0 42702.0 42703.0 42704.0 42705.0 42706.0 42707.0 42708.0 42709.0 42710.0 42711.0 42712.0 42713.0 42714.0 42715.0 42716.0 42717.0 42718.0 42719.0 42720.0 42721.0 42722.0 42723.0 42724.0 42725.0 42726.0 42727.0 42728.0 42729.0 42730.0 42731.0 42732.0 42733.0 42734.0 42735.0 42736.0 42737.0 42738.0 42739.0 42740.0 42741.0 42742.0 42743.0 42744.0 42745.0 42746.0 42747.0 42748.0 42749.0 42750.0 42751.0 42752.0 42753.0 42754.0 42755.0 42756.0 42757.0 42758.0 42759.0 42760.0 42761.0 42762.0 42763.0 42764.0 42765.0 42766.0 42767.0 42768.0 42769.0 42770.0 42771.0 42772.0 42773.0 42774.0 42775.0 42776.0 42777.0 42778.0 42779.0 42780.0 42781.0 42782.0 42783.0 42784.0 42785.0 42786.0 42787.0 42788.0 42789.0 42790.0 42791.0 42792.0 42793.0 42794.0 42795.0 42796.0 42797.0 42798.0 42799.0 42800.0 42801.0 42802.0 42803.0 42804.0 42805.0 42806.0 42807.0 42808.0 42809.0 42810.0 42811.0 42812.0 42813.0 42814.0 42815.0 42816.0 42817.0 42818.0 42819.0 42820.0 42821.0 42822.0 42823.0 42824.0 42825.0 42826.0 42827.0 42828.0 42829.0 42830.0 42831.0 42832.0 42833.0 42834.0 42835.0 42836.0 42837.0 42838.0 42839.0 42840.0 42841.0 42842.0 42843.0 42844.0 42845.0 42846.0 42847.0 42848.0 42849.0 42850.0 42851.0 42852.0 42853.0 42854.0 42855.0 42856.0 42857.0 42858.0 42859.0 42860.0 42861.0 42862.0 42863.0 42864.0 42865.0 42866.0 42867.0 42868.0 42869.0 42870.0 42871.0 42872.0 42873.0 42874.0 42875.0 42876.0 42877.0 42878.0 42879.0 42880.0 42881.0 42882.0 42883.0 42884.0 42885.0 42886.0 42887.0 42888.0 42889.0 42890.0 42891.0 42892.0 42893.0 42894.0 42895.0 42896.0 42897.0 42898.0 42899.0 42900.0 42901.0 42902.0 42903.0 42904.0 42905.0 42906.0 42907.0 42908.0 42909.0 42910.0 42911.0 42912.0 42913.0 42914.0 42915.0 42916.0 42917.0 42918.0 42919.0 42920.0 42921.0 42922.0 42923.0 42924.0 42925.0 42926.0 42927.0 42928.0 42929.0 42930.0 42931.0 42932.0 42933.0 42934.0 42935.0 42936.0 42937.0 42938.0 42939.0 42940.0 42941.0 42942.0 42943.0 42944.0 42945.0 42946.0 42947.0 42948.0 42949.0 42950.0 42951.0 42952.0 42953.0 42954.0 42955.0 42956.0 42957.0 42958.0 42959.0 42960.0 42961.0 42962.0 42963.0 42964.0 42965.0 42966.0 42967.0 42968.0 42969.0 42970.0 42971.0 42972.0 42973.0 42974.0 42975.0 42976.0 42977.0 42978.0 42979.0 42980.0 42981.0 42982.0 42983.0 42984.0 42985.0 42986.0 42987.0 42988.0 42989.0 42990.0 42991.0 42992.0 42993.0 42994.0 42995.0 42996.0 42997.0 42998.0 42999.0 43000.0 43001.0 43002.0 43003.0 43004.0 43005.0 43006.0 43007.0 43008.0 43009.0 43010.0 43011.0 43012.0 43013.0 43014.0 43015.0 43016.0 43017.0 43018.0 43019.0 43020.0 43021.0 43022.0 43023.0 43024.0 43025.0 43026.0 43027.0 43028.0 43029.0 43030.0 43031.0 43032.0 43033.0 43034.0 43035.0 43036.0 43037.0 43038.0 43039.0 43040.0 43041.0 43042.0 43043.0 43044.0 43045.0 43046.0 43047.0 43048.0 43049.0 43050.0 43051.0 43052.0 43053.0 43054.0 43055.0 43056.0 43057.0 43058.0 43059.0 43060.0 43061.0 43062.0 43063.0 43064.0 43065.0 43066.0 43067.0 43068.0 43069.0 43070.0 43071.0 43072.0 43073.0 43074.0 43075.0 43076.0 43077.0 43078.0 43079.0 43080.0 43081.0 43082.0 43083.0 43084.0 43085.0 43086.0 43087.0 43088.0 43089.0 43090.0 43091.0 43092.0 43093.0 43094.0 43095.0 43096.0 43097.0 43098.0 43099.0 43100.0 43101.0 43102.0 43103.0 43104.0 43105.0 43106.0 43107.0 43108.0 43109.0 43110.0 43111.0 43112.0 43113.0 43114.0 43115.0 43116.0 43117.0 43118.0 43119.0 43120.0 43121.0 43122.0 43123.0 43124.0 43125.0 43126.0 43127.0 43128.0 43129.0 43130.0 43131.0 43132.0 43133.0 43134.0 43135.0 43136.0 43137.0 43138.0 43139.0 43140.0 43141.0 43142.0 43143.0 43144.0 43145.0 43146.0 43147.0 43148.0 43149.0 43150.0 43151.0 43152.0 43153.0 43154.0 43155.0 43156.0 43157.0 43158.0 43159.0 43160.0 43161.0 43162.0 43163.0 43164.0 43165.0 43166.0 43167.0 43168.0 43169.0 43170.0 43171.0 43172.0 43173.0 43174.0 43175.0 43176.0 43177.0 43178.0 43179.0 43180.0 43181.0 43182.0 43183.0 43184.0 43185.0 43186.0 43187.0 43188.0 43189.0 43190.0 43191.0 43192.0 43193.0 43194.0 43195.0 43196.0 43197.0 43198.0 43199.0 43200.0 43201.0 43202.0 43203.0 43204.0 43205.0 43206.0 43207.0 43208.0 43209.0 43210.0 43211.0 43212.0 43213.0 43214.0 43215.0 43216.0 43217.0 43218.0 43219.0 43220.0 43221.0 43222.0 43223.0 43224.0 43225.0 43226.0 43227.0 43228.0 43229.0 43230.0 43231.0 43232.0 43233.0 43234.0 43235.0 43236.0 43237.0 43238.0 43239.0 43240.0 43241.0 43242.0 43243.0 43244.0 43245.0 43246.0 43247.0 43248.0 43249.0 43250.0 43251.0 43252.0 43253.0 43254.0 43255.0 43256.0 43257.0 43258.0 43259.0 43260.0 43261.0 43262.0 43263.0 43264.0 43265.0 43266.0 43267.0 43268.0 43269.0 43270.0 43271.0 43272.0 43273.0 43274.0 43275.0 43276.0 43277.0 43278.0 43279.0 43280.0 43281.0 43282.0 43283.0 43284.0 43285.0 43286.0 43287.0 43288.0 43289.0 43290.0 43291.0 43292.0 43293.0 43294.0 43295.0 43296.0 43297.0 43298.0 43299.0 43300.0 43301.0 43302.0 43303.0 43304.0 43305.0 43306.0 43307.0 43308.0 43309.0 43310.0 43311.0 43312.0 43313.0 43314.0 43315.0 43316.0 43317.0 43318.0 43319.0 43320.0 43321.0 43322.0 43323.0 43324.0 43325.0 43326.0 43327.0 43328.0 43329.0 43330.0 43331.0 43332.0 43333.0 43334.0 43335.0 43336.0 43337.0 43338.0 43339.0 43340.0 43341.0 43342.0 43343.0 43344.0 43345.0 43346.0 43347.0 43348.0 43349.0 43350.0 43351.0 43352.0 43353.0 43354.0 43355.0 43356.0 43357.0 43358.0 43359.0 43360.0 43361.0 43362.0 43363.0 43364.0 43365.0 43366.0 43367.0 43368.0 43369.0 43370.0 43371.0 43372.0 43373.0 43374.0 43375.0 43376.0 43377.0 43378.0 43379.0 43380.0 43381.0 43382.0 43383.0 43384.0 43385.0 43386.0 43387.0 43388.0 43389.0 43390.0 43391.0 43392.0 43393.0 43394.0 43395.0 43396.0 43397.0 43398.0 43399.0 43400.0 43401.0 43402.0 43403.0 43404.0 43405.0 43406.0 43407.0 43408.0 43409.0 43410.0 43411.0 43412.0 43413.0 43414.0 43415.0 43416.0 43417.0 43418.0 43419.0 43420.0 43421.0 43422.0 43423.0 43424.0 43425.0 43426.0 43427.0 43428.0 43429.0 43430.0 43431.0 43432.0 43433.0 43434.0 43435.0 43436.0 43437.0 43438.0 43439.0 43440.0 43441.0 43442.0 43443.0 43444.0 43445.0 43446.0 43447.0 43448.0 43449.0 43450.0 43451.0 43452.0 43453.0 43454.0 43455.0 43456.0 43457.0 43458.0 43459.0 43460.0 43461.0 43462.0 43463.0 43464.0 43465.0 43466.0 43467.0 43468.0 43469.0 43470.0 43471.0 43472.0 43473.0 43474.0 43475.0 43476.0 43477.0 43478.0 43479.0 43480.0 43481.0 43482.0 43483.0 43484.0 43485.0 43486.0 43487.0 43488.0 43489.0 43490.0 43491.0 43492.0 43493.0 43494.0 43495.0 43496.0 43497.0 43498.0 43499.0 43500.0 43501.0 43502.0 43503.0 43504.0 43505.0 43506.0 43507.0 43508.0 43509.0 43510.0 43511.0 43512.0 43513.0 43514.0 43515.0 43516.0 43517.0 43518.0 43519.0 43520.0 43521.0 43522.0 43523.0 43524.0 43525.0 43526.0 43527.0 43528.0 43529.0 43530.0 43531.0 43532.0 43533.0 43534.0 43535.0 43536.0 43537.0 43538.0 43539.0 43540.0 43541.0 43542.0 43543.0 43544.0 43545.0 43546.0 43547.0 43548.0 43549.0 43550.0 43551.0 43552.0 43553.0 43554.0 43555.0 43556.0 43557.0 43558.0 43559.0 43560.0 43561.0 43562.0 43563.0 43564.0 43565.0 43566.0 43567.0 43568.0 43569.0 43570.0 43571.0 43572.0 43573.0 43574.0 43575.0 43576.0 43577.0 43578.0 43579.0 43580.0 43581.0 43582.0 43583.0 43584.0 43585.0 43586.0 43587.0 43588.0 43589.0 43590.0 43591.0 43592.0 43593.0 43594.0 43595.0 43596.0 43597.0 43598.0 43599.0 43600.0 43601.0 43602.0 43603.0 43604.0 43605.0 43606.0 43607.0 43608.0 43609.0 43610.0 43611.0 43612.0 43613.0 43614.0 43615.0 43616.0 43617.0 43618.0 43619.0 43620.0 43621.0 43622.0 43623.0 43624.0 43625.0 43626.0 43627.0 43628.0 43629.0 43630.0 43631.0 43632.0 43633.0 43634.0 43635.0 43636.0 43637.0 43638.0 43639.0 43640.0 43641.0 43642.0 43643.0 43644.0 43645.0 43646.0 43647.0 43648.0 43649.0 43650.0 43651.0 43652.0 43653.0 43654.0 43655.0 43656.0 43657.0 43658.0 43659.0 43660.0 43661.0 43662.0 43663.0 43664.0 43665.0 43666.0 43667.0 43668.0 43669.0 43670.0 43671.0 43672.0 43673.0 43674.0 43675.0 43676.0 43677.0 43678.0 43679.0 43680.0 43681.0 43682.0 43683.0 43684.0 43685.0 43686.0 43687.0 43688.0 43689.0 43690.0 43691.0 43692.0 43693.0 43694.0 43695.0 43696.0 43697.0 43698.0 43699.0 43700.0 43701.0 43702.0 43703.0 43704.0 43705.0 43706.0 43707.0 43708.0 43709.0 43710.0 43711.0 43712.0 43713.0 43714.0 43715.0 43716.0 43717.0 43718.0 43719.0 43720.0 43721.0 43722.0 43723.0 43724.0 43725.0 43726.0 43727.0 43728.0 43729.0 43730.0 43731.0 43732.0 43733.0 43734.0 43735.0 43736.0 43737.0 43738.0 43739.0 43740.0 43741.0 43742.0 43743.0 43744.0 43745.0 43746.0 43747.0 43748.0 43749.0 43750.0 43751.0 43752.0 43753.0 43754.0 43755.0 43756.0 43757.0 43758.0 43759.0 43760.0 43761.0 43762.0 43763.0 43764.0 43765.0 43766.0 43767.0 43768.0 43769.0 43770.0 43771.0 43772.0 43773.0 43774.0 43775.0 43776.0 43777.0 43778.0 43779.0 43780.0 43781.0 43782.0 43783.0 43784.0 43785.0 43786.0 43787.0 43788.0 43789.0 43790.0 43791.0 43792.0 43793.0 43794.0 43795.0 43796.0 43797.0 43798.0 43799.0 43800.0 43801.0 43802.0 43803.0 43804.0 43805.0 43806.0 43807.0 43808.0 43809.0 43810.0 43811.0 43812.0 43813.0 43814.0 43815.0 43816.0 43817.0 43818.0 43819.0 43820.0 43821.0 43822.0 43823.0 43824.0 43825.0 43826.0 43827.0 43828.0 43829.0 43830.0 43831.0 43832.0 43833.0 43834.0 43835.0 43836.0 43837.0 43838.0 43839.0 43840.0 43841.0 43842.0 43843.0 43844.0 43845.0 43846.0 43847.0 43848.0 43849.0 43850.0 43851.0 43852.0 43853.0 43854.0 43855.0 43856.0 43857.0 43858.0 43859.0 43860.0 43861.0 43862.0 43863.0 43864.0 43865.0 43866.0 43867.0 43868.0 43869.0 43870.0 43871.0 43872.0 43873.0 43874.0 43875.0 43876.0 43877.0 43878.0 43879.0 43880.0 43881.0 43882.0 43883.0 43884.0 43885.0 43886.0 43887.0 43888.0 43889.0 43890.0 43891.0 43892.0 43893.0 43894.0 43895.0 43896.0 43897.0 43898.0 43899.0 43900.0 43901.0 43902.0 43903.0 43904.0 43905.0 43906.0 43907.0 43908.0 43909.0 43910.0 43911.0 43912.0 43913.0 43914.0 43915.0 43916.0 43917.0 43918.0 43919.0 43920.0 43921.0 43922.0 43923.0 43924.0 43925.0 43926.0 43927.0 43928.0 43929.0 43930.0 43931.0 43932.0 43933.0 43934.0 43935.0 43936.0 43937.0 43938.0 43939.0 43940.0 43941.0 43942.0 43943.0 43944.0 43945.0 43946.0 43947.0 43948.0 43949.0 43950.0 43951.0 43952.0 43953.0 43954.0 43955.0 43956.0 43957.0 43958.0 43959.0 43960.0 43961.0 43962.0 43963.0 43964.0 43965.0 43966.0 43967.0 43968.0 43969.0 43970.0 43971.0 43972.0 43973.0 43974.0 43975.0 43976.0 43977.0 43978.0 43979.0 43980.0 43981.0 43982.0 43983.0 43984.0 43985.0 43986.0 43987.0 43988.0 43989.0 43990.0 43991.0 43992.0 43993.0 43994.0 43995.0 43996.0 43997.0 43998.0 43999.0 44000.0 44001.0 44002.0 44003.0 44004.0 44005.0 44006.0 44007.0 44008.0 44009.0 44010.0 44011.0 44012.0 44013.0 44014.0 44015.0 44016.0 44017.0 44018.0 44019.0 44020.0 44021.0 44022.0 44023.0 44024.0 44025.0 44026.0 44027.0 44028.0 44029.0 44030.0 44031.0 44032.0 44033.0 44034.0 44035.0 44036.0 44037.0 44038.0 44039.0 44040.0 44041.0 44042.0 44043.0 44044.0 44045.0 44046.0 44047.0 44048.0 44049.0 44050.0 44051.0 44052.0 44053.0 44054.0 44055.0 44056.0 44057.0 44058.0 44059.0 44060.0 44061.0 44062.0 44063.0 44064.0 44065.0 44066.0 44067.0 44068.0 44069.0 44070.0 44071.0 44072.0 44073.0 44074.0 44075.0 44076.0 44077.0 44078.0 44079.0 44080.0 44081.0 44082.0 44083.0 44084.0 44085.0 44086.0 44087.0 44088.0 44089.0 44090.0 44091.0 44092.0 44093.0 44094.0 44095.0 44096.0 44097.0 44098.0 44099.0 44100.0 44101.0 44102.0 44103.0 44104.0 44105.0 44106.0 44107.0 44108.0 44109.0 44110.0 44111.0 44112.0 44113.0 44114.0 44115.0 44116.0 44117.0 44118.0 44119.0 44120.0 44121.0 44122.0 44123.0 44124.0 44125.0 44126.0 44127.0 44128.0 44129.0 44130.0 44131.0 44132.0 44133.0 44134.0 44135.0 44136.0 44137.0 44138.0 44139.0 44140.0 44141.0 44142.0 44143.0 44144.0 44145.0 44146.0 44147.0 44148.0 44149.0 44150.0 44151.0 44152.0 44153.0 44154.0 44155.0 44156.0 44157.0 44158.0 44159.0 44160.0 44161.0 44162.0 44163.0 44164.0 44165.0 44166.0 44167.0 44168.0 44169.0 44170.0 44171.0 44172.0 44173.0 44174.0 44175.0 44176.0 44177.0 44178.0 44179.0 44180.0 44181.0 44182.0 44183.0 44184.0 44185.0 44186.0 44187.0 44188.0 44189.0 44190.0 44191.0 44192.0 44193.0 44194.0 44195.0 44196.0 44197.0 44198.0 44199.0 44200.0 44201.0 44202.0 44203.0 44204.0 44205.0 44206.0 44207.0 44208.0 44209.0 44210.0 44211.0 44212.0 44213.0 44214.0 44215.0 44216.0 44217.0 44218.0 44219.0 44220.0 44221.0 44222.0 44223.0 44224.0 44225.0 44226.0 44227.0 44228.0 44229.0 44230.0 44231.0 44232.0 44233.0 44234.0 44235.0 44236.0 44237.0 44238.0 44239.0 44240.0 44241.0 44242.0 44243.0 44244.0 44245.0 44246.0 44247.0 44248.0 44249.0 44250.0 44251.0 44252.0 44253.0 44254.0 44255.0 44256.0 44257.0 44258.0 44259.0 44260.0 44261.0 44262.0 44263.0 44264.0 44265.0 44266.0 44267.0 44268.0 44269.0 44270.0 44271.0 44272.0 44273.0 44274.0 44275.0 44276.0 44277.0 44278.0 44279.0 44280.0 44281.0 44282.0 44283.0 44284.0 44285.0 44286.0 44287.0 44288.0 44289.0 44290.0 44291.0 44292.0 44293.0 44294.0 44295.0 44296.0 44297.0 44298.0 44299.0 44300.0 44301.0 44302.0 44303.0 44304.0 44305.0 44306.0 44307.0 44308.0 44309.0 44310.0 44311.0 44312.0 44313.0 44314.0 44315.0 44316.0 44317.0 44318.0 44319.0 44320.0 44321.0 44322.0 44323.0 44324.0 44325.0 44326.0 44327.0 44328.0 44329.0 44330.0 44331.0 44332.0 44333.0 44334.0 44335.0 44336.0 44337.0 44338.0 44339.0 44340.0 44341.0 44342.0 44343.0 44344.0 44345.0 44346.0 44347.0 44348.0 44349.0 44350.0 44351.0 44352.0 44353.0 44354.0 44355.0 44356.0 44357.0 44358.0 44359.0 44360.0 44361.0 44362.0 44363.0 44364.0 44365.0 44366.0 44367.0 44368.0 44369.0 44370.0 44371.0 44372.0 44373.0 44374.0 44375.0 44376.0 44377.0 44378.0 44379.0 44380.0 44381.0 44382.0 44383.0 44384.0 44385.0 44386.0 44387.0 44388.0 44389.0 44390.0 44391.0 44392.0 44393.0 44394.0 44395.0 44396.0 44397.0 44398.0 44399.0 44400.0 44401.0 44402.0 44403.0 44404.0 44405.0 44406.0 44407.0 44408.0 44409.0 44410.0 44411.0 44412.0 44413.0 44414.0 44415.0 44416.0 44417.0 44418.0 44419.0 44420.0 44421.0 44422.0 44423.0 44424.0 44425.0 44426.0 44427.0 44428.0 44429.0 44430.0 44431.0 44432.0 44433.0 44434.0 44435.0 44436.0 44437.0 44438.0 44439.0 44440.0 44441.0 44442.0 44443.0 44444.0 44445.0 44446.0 44447.0 44448.0 44449.0 44450.0 44451.0 44452.0 44453.0 44454.0 44455.0 44456.0 44457.0 44458.0 44459.0 44460.0 44461.0 44462.0 44463.0 44464.0 44465.0 44466.0 44467.0 44468.0 44469.0 44470.0 44471.0 44472.0 44473.0 44474.0 44475.0 44476.0 44477.0 44478.0 44479.0 44480.0 44481.0 44482.0 44483.0 44484.0 44485.0 44486.0 44487.0 44488.0 44489.0 44490.0 44491.0 44492.0 44493.0 44494.0 44495.0 44496.0 44497.0 44498.0 44499.0 44500.0 44501.0 44502.0 44503.0 44504.0 44505.0 44506.0 44507.0 44508.0 44509.0 44510.0 44511.0 44512.0 44513.0 44514.0 44515.0 44516.0 44517.0 44518.0 44519.0 44520.0 44521.0 44522.0 44523.0 44524.0 44525.0 44526.0 44527.0 44528.0 44529.0 44530.0 44531.0 44532.0 44533.0 44534.0 44535.0 44536.0 44537.0 44538.0 44539.0 44540.0 44541.0 44542.0 44543.0 44544.0 44545.0 44546.0 44547.0 44548.0 44549.0 44550.0 44551.0 44552.0 44553.0 44554.0 44555.0 44556.0 44557.0 44558.0 44559.0 44560.0 44561.0 44562.0 44563.0 44564.0 44565.0 44566.0 44567.0 44568.0 44569.0 44570.0 44571.0 44572.0 44573.0 44574.0 44575.0 44576.0 44577.0 44578.0 44579.0 44580.0 44581.0 44582.0 44583.0 44584.0 44585.0 44586.0 44587.0 44588.0 44589.0 44590.0 44591.0 44592.0 44593.0 44594.0 44595.0 44596.0 44597.0 44598.0 44599.0 44600.0 44601.0 44602.0 44603.0 44604.0 44605.0 44606.0 44607.0 44608.0 44609.0 44610.0 44611.0 44612.0 44613.0 44614.0 44615.0 44616.0 44617.0 44618.0 44619.0 44620.0 44621.0 44622.0 44623.0 44624.0 44625.0 44626.0 44627.0 44628.0 44629.0 44630.0 44631.0 44632.0 44633.0 44634.0 44635.0 44636.0 44637.0 44638.0 44639.0 44640.0 44641.0 44642.0 44643.0 44644.0 44645.0 44646.0 44647.0 44648.0 44649.0 44650.0 44651.0 44652.0 44653.0 44654.0 44655.0 44656.0 44657.0 44658.0 44659.0 44660.0 44661.0 44662.0 44663.0 44664.0 44665.0 44666.0 44667.0 44668.0 44669.0 44670.0 44671.0 44672.0 44673.0 44674.0 44675.0 44676.0 44677.0 44678.0 44679.0 44680.0 44681.0 44682.0 44683.0 44684.0 44685.0 44686.0 44687.0 44688.0 44689.0 44690.0 44691.0 44692.0 44693.0 44694.0 44695.0 44696.0 44697.0 44698.0 44699.0 44700.0 44701.0 44702.0 44703.0 44704.0 44705.0 44706.0 44707.0 44708.0 44709.0 44710.0 44711.0 44712.0 44713.0 44714.0 44715.0 44716.0 44717.0 44718.0 44719.0 44720.0 44721.0 44722.0 44723.0 44724.0 44725.0 44726.0 44727.0 44728.0 44729.0 44730.0 44731.0 44732.0 44733.0 44734.0 44735.0 44736.0 44737.0 44738.0 44739.0 44740.0 44741.0 44742.0 44743.0 44744.0 44745.0 44746.0 44747.0 44748.0 44749.0 44750.0 44751.0 44752.0 44753.0 44754.0 44755.0 44756.0 44757.0 44758.0 44759.0 44760.0 44761.0 44762.0 44763.0 44764.0 44765.0 44766.0 44767.0 44768.0 44769.0 44770.0 44771.0 44772.0 44773.0 44774.0 44775.0 44776.0 44777.0 44778.0 44779.0 44780.0 44781.0 44782.0 44783.0 44784.0 44785.0 44786.0 44787.0 44788.0 44789.0 44790.0 44791.0 44792.0 44793.0 44794.0 44795.0 44796.0 44797.0 44798.0 44799.0 44800.0 44801.0 44802.0 44803.0 44804.0 44805.0 44806.0 44807.0 44808.0 44809.0 44810.0 44811.0 44812.0 44813.0 44814.0 44815.0 44816.0 44817.0 44818.0 44819.0 44820.0 44821.0 44822.0 44823.0 44824.0 44825.0 44826.0 44827.0 44828.0 44829.0 44830.0 44831.0 44832.0 44833.0 44834.0 44835.0 44836.0 44837.0 44838.0 44839.0 44840.0 44841.0 44842.0 44843.0 44844.0 44845.0 44846.0 44847.0 44848.0 44849.0 44850.0 44851.0 44852.0 44853.0 44854.0 44855.0 44856.0 44857.0 44858.0 44859.0 44860.0 44861.0 44862.0 44863.0 44864.0 44865.0 44866.0 44867.0 44868.0 44869.0 44870.0 44871.0 44872.0 44873.0 44874.0 44875.0 44876.0 44877.0 44878.0 44879.0 44880.0 44881.0 44882.0 44883.0 44884.0 44885.0 44886.0 44887.0 44888.0 44889.0 44890.0 44891.0 44892.0 44893.0 44894.0 44895.0 44896.0 44897.0 44898.0 44899.0 44900.0 44901.0 44902.0 44903.0 44904.0 44905.0 44906.0 44907.0 44908.0 44909.0 44910.0 44911.0 44912.0 44913.0 44914.0 44915.0 44916.0 44917.0 44918.0 44919.0 44920.0 44921.0 44922.0 44923.0 44924.0 44925.0 44926.0 44927.0 44928.0 44929.0 44930.0 44931.0 44932.0 44933.0 44934.0 44935.0 44936.0 44937.0 44938.0 44939.0 44940.0 44941.0 44942.0 44943.0 44944.0 44945.0 44946.0 44947.0 44948.0 44949.0 44950.0 44951.0 44952.0 44953.0 44954.0 44955.0 44956.0 44957.0 44958.0 44959.0 44960.0 44961.0 44962.0 44963.0 44964.0 44965.0 44966.0 44967.0 44968.0 44969.0 44970.0 44971.0 44972.0 44973.0 44974.0 44975.0 44976.0 44977.0 44978.0 44979.0 44980.0 44981.0 44982.0 44983.0 44984.0 44985.0 44986.0 44987.0 44988.0 44989.0 44990.0 44991.0 44992.0 44993.0 44994.0 44995.0 44996.0 44997.0 44998.0 44999.0 45000.0 45001.0 45002.0 45003.0 45004.0 45005.0 45006.0 45007.0 45008.0 45009.0 45010.0 45011.0 45012.0 45013.0 45014.0 45015.0 45016.0 45017.0 45018.0 45019.0 45020.0 45021.0 45022.0 45023.0 45024.0 45025.0 45026.0 45027.0 45028.0 45029.0 45030.0 45031.0 45032.0 45033.0 45034.0 45035.0 45036.0 45037.0 45038.0 45039.0 45040.0 45041.0 45042.0 45043.0 45044.0 45045.0 45046.0 45047.0 45048.0 45049.0 45050.0 45051.0 45052.0 45053.0 45054.0 45055.0 45056.0 45057.0 45058.0 45059.0 45060.0 45061.0 45062.0 45063.0 45064.0 45065.0 45066.0 45067.0 45068.0 45069.0 45070.0 45071.0 45072.0 45073.0 45074.0 45075.0 45076.0 45077.0 45078.0 45079.0 45080.0 45081.0 45082.0 45083.0 45084.0 45085.0 45086.0 45087.0 45088.0 45089.0 45090.0 45091.0 45092.0 45093.0 45094.0 45095.0 45096.0 45097.0 45098.0 45099.0 45100.0 45101.0 45102.0 45103.0 45104.0 45105.0 45106.0 45107.0 45108.0 45109.0 45110.0 45111.0 45112.0 45113.0 45114.0 45115.0 45116.0 45117.0 45118.0 45119.0 45120.0 45121.0 45122.0 45123.0 45124.0 45125.0 45126.0 45127.0 45128.0 45129.0 45130.0 45131.0 45132.0 45133.0 45134.0 45135.0 45136.0 45137.0 45138.0 45139.0 45140.0 45141.0 45142.0 45143.0 45144.0 45145.0 45146.0 45147.0 45148.0 45149.0 45150.0 45151.0 45152.0 45153.0 45154.0 45155.0 45156.0 45157.0 45158.0 45159.0 45160.0 45161.0 45162.0 45163.0 45164.0 45165.0 45166.0 45167.0 45168.0 45169.0 45170.0 45171.0 45172.0 45173.0 45174.0 45175.0 45176.0 45177.0 45178.0 45179.0 45180.0 45181.0 45182.0 45183.0 45184.0 45185.0 45186.0 45187.0 45188.0 45189.0 45190.0 45191.0 45192.0 45193.0 45194.0 45195.0 45196.0 45197.0 45198.0 45199.0 45200.0 45201.0 45202.0 45203.0 45204.0 45205.0 45206.0 45207.0 45208.0 45209.0 45210.0 45211.0 45212.0 45213.0 45214.0 45215.0 45216.0 45217.0 45218.0 45219.0 45220.0 45221.0 45222.0 45223.0 45224.0 45225.0 45226.0 45227.0 45228.0 45229.0 45230.0 45231.0 45232.0 45233.0 45234.0 45235.0 45236.0 45237.0 45238.0 45239.0 45240.0 45241.0 45242.0 45243.0 45244.0 45245.0 45246.0 45247.0 45248.0 45249.0 45250.0 45251.0 45252.0 45253.0 45254.0 45255.0 45256.0 45257.0 45258.0 45259.0 45260.0 45261.0 45262.0 45263.0 45264.0 45265.0 45266.0 45267.0 45268.0 45269.0 45270.0 45271.0 45272.0 45273.0 45274.0 45275.0 45276.0 45277.0 45278.0 45279.0 45280.0 45281.0 45282.0 45283.0 45284.0 45285.0 45286.0 45287.0 45288.0 45289.0 45290.0 45291.0 45292.0 45293.0 45294.0 45295.0 45296.0 45297.0 45298.0 45299.0 45300.0 45301.0 45302.0 45303.0 45304.0 45305.0 45306.0 45307.0 45308.0 45309.0 45310.0 45311.0 45312.0 45313.0 45314.0 45315.0 45316.0 45317.0 45318.0 45319.0 45320.0 45321.0 45322.0 45323.0 45324.0 45325.0 45326.0 45327.0 45328.0 45329.0 45330.0 45331.0 45332.0 45333.0 45334.0 45335.0 45336.0 45337.0 45338.0 45339.0 45340.0 45341.0 45342.0 45343.0 45344.0 45345.0 45346.0 45347.0 45348.0 45349.0 45350.0 45351.0 45352.0 45353.0 45354.0 45355.0 45356.0 45357.0 45358.0 45359.0 45360.0 45361.0 45362.0 45363.0 45364.0 45365.0 45366.0 45367.0 45368.0 45369.0 45370.0 45371.0 45372.0 45373.0 45374.0 45375.0 45376.0 45377.0 45378.0 45379.0 45380.0 45381.0 45382.0 45383.0 45384.0 45385.0 45386.0 45387.0 45388.0 45389.0 45390.0 45391.0 45392.0 45393.0 45394.0 45395.0 45396.0 45397.0 45398.0 45399.0 45400.0 45401.0 45402.0 45403.0 45404.0 45405.0 45406.0 45407.0 45408.0 45409.0 45410.0 45411.0 45412.0 45413.0 45414.0 45415.0 45416.0 45417.0 45418.0 45419.0 45420.0 45421.0 45422.0 45423.0 45424.0 45425.0 45426.0 45427.0 45428.0 45429.0 45430.0 45431.0 45432.0 45433.0 45434.0 45435.0 45436.0 45437.0 45438.0 45439.0 45440.0 45441.0 45442.0 45443.0 45444.0 45445.0 45446.0 45447.0 45448.0 45449.0 45450.0 45451.0 45452.0 45453.0 45454.0 45455.0 45456.0 45457.0 45458.0 45459.0 45460.0 45461.0 45462.0 45463.0 45464.0 45465.0 45466.0 45467.0 45468.0 45469.0 45470.0 45471.0 45472.0 45473.0 45474.0 45475.0 45476.0 45477.0 45478.0 45479.0 45480.0 45481.0 45482.0 45483.0 45484.0 45485.0 45486.0 45487.0 45488.0 45489.0 45490.0 45491.0 45492.0 45493.0 45494.0 45495.0 45496.0 45497.0 45498.0 45499.0 45500.0 45501.0 45502.0 45503.0 45504.0 45505.0 45506.0 45507.0 45508.0 45509.0 45510.0 45511.0 45512.0 45513.0 45514.0 45515.0 45516.0 45517.0 45518.0 45519.0 45520.0 45521.0 45522.0 45523.0 45524.0 45525.0 45526.0 45527.0 45528.0 45529.0 45530.0 45531.0 45532.0 45533.0 45534.0 45535.0 45536.0 45537.0 45538.0 45539.0 45540.0 45541.0 45542.0 45543.0 45544.0 45545.0 45546.0 45547.0 45548.0 45549.0 45550.0 45551.0 45552.0 45553.0 45554.0 45555.0 45556.0 45557.0 45558.0 45559.0 45560.0 45561.0 45562.0 45563.0 45564.0 45565.0 45566.0 45567.0 45568.0 45569.0 45570.0 45571.0 45572.0 45573.0 45574.0 45575.0 45576.0 45577.0 45578.0 45579.0 45580.0 45581.0 45582.0 45583.0 45584.0 45585.0 45586.0 45587.0 45588.0 45589.0 45590.0 45591.0 45592.0 45593.0 45594.0 45595.0 45596.0 45597.0 45598.0 45599.0 45600.0 45601.0 45602.0 45603.0 45604.0 45605.0 45606.0 45607.0 45608.0 45609.0 45610.0 45611.0 45612.0 45613.0 45614.0 45615.0 45616.0 45617.0 45618.0 45619.0 45620.0 45621.0 45622.0 45623.0 45624.0 45625.0 45626.0 45627.0 45628.0 45629.0 45630.0 45631.0 45632.0 45633.0 45634.0 45635.0 45636.0 45637.0 45638.0 45639.0 45640.0 45641.0 45642.0 45643.0 45644.0 45645.0 45646.0 45647.0 45648.0 45649.0 45650.0 45651.0 45652.0 45653.0 45654.0 45655.0 45656.0 45657.0 45658.0 45659.0 45660.0 45661.0 45662.0 45663.0 45664.0 45665.0 45666.0 45667.0 45668.0 45669.0 45670.0 45671.0 45672.0 45673.0 45674.0 45675.0 45676.0 45677.0 45678.0 45679.0 45680.0 45681.0 45682.0 45683.0 45684.0 45685.0 45686.0 45687.0 45688.0 45689.0 45690.0 45691.0 45692.0 45693.0 45694.0 45695.0 45696.0 45697.0 45698.0 45699.0 45700.0 45701.0 45702.0 45703.0 45704.0 45705.0 45706.0 45707.0 45708.0 45709.0 45710.0 45711.0 45712.0 45713.0 45714.0 45715.0 45716.0 45717.0 45718.0 45719.0 45720.0 45721.0 45722.0 45723.0 45724.0 45725.0 45726.0 45727.0 45728.0 45729.0 45730.0 45731.0 45732.0 45733.0 45734.0 45735.0 45736.0 45737.0 45738.0 45739.0 45740.0 45741.0 45742.0 45743.0 45744.0 45745.0 45746.0 45747.0 45748.0 45749.0 45750.0 45751.0 45752.0 45753.0 45754.0 45755.0 45756.0 45757.0 45758.0 45759.0 45760.0 45761.0 45762.0 45763.0 45764.0 45765.0 45766.0 45767.0 45768.0 45769.0 45770.0 45771.0 45772.0 45773.0 45774.0 45775.0 45776.0 45777.0 45778.0 45779.0 45780.0 45781.0 45782.0 45783.0 45784.0 45785.0 45786.0 45787.0 45788.0 45789.0 45790.0 45791.0 45792.0 45793.0 45794.0 45795.0 45796.0 45797.0 45798.0 45799.0 45800.0 45801.0 45802.0 45803.0 45804.0 45805.0 45806.0 45807.0 45808.0 45809.0 45810.0 45811.0 45812.0 45813.0 45814.0 45815.0 45816.0 45817.0 45818.0 45819.0 45820.0 45821.0 45822.0 45823.0 45824.0 45825.0 45826.0 45827.0 45828.0 45829.0 45830.0 45831.0 45832.0 45833.0 45834.0 45835.0 45836.0 45837.0 45838.0 45839.0 45840.0 45841.0 45842.0 45843.0 45844.0 45845.0 45846.0 45847.0 45848.0 45849.0 45850.0 45851.0 45852.0 45853.0 45854.0 45855.0 45856.0 45857.0 45858.0 45859.0 45860.0 45861.0 45862.0 45863.0 45864.0 45865.0 45866.0 45867.0 45868.0 45869.0 45870.0 45871.0 45872.0 45873.0 45874.0 45875.0 45876.0 45877.0 45878.0 45879.0 45880.0 45881.0 45882.0 45883.0 45884.0 45885.0 45886.0 45887.0 45888.0 45889.0 45890.0 45891.0 45892.0 45893.0 45894.0 45895.0 45896.0 45897.0 45898.0 45899.0 45900.0 45901.0 45902.0 45903.0 45904.0 45905.0 45906.0 45907.0 45908.0 45909.0 45910.0 45911.0 45912.0 45913.0 45914.0 45915.0 45916.0 45917.0 45918.0 45919.0 45920.0 45921.0 45922.0 45923.0 45924.0 45925.0 45926.0 45927.0 45928.0 45929.0 45930.0 45931.0 45932.0 45933.0 45934.0 45935.0 45936.0 45937.0 45938.0 45939.0 45940.0 45941.0 45942.0 45943.0 45944.0 45945.0 45946.0 45947.0 45948.0 45949.0 45950.0 45951.0 45952.0 45953.0 45954.0 45955.0 45956.0 45957.0 45958.0 45959.0 45960.0 45961.0 45962.0 45963.0 45964.0 45965.0 45966.0 45967.0 45968.0 45969.0 45970.0 45971.0 45972.0 45973.0 45974.0 45975.0 45976.0 45977.0 45978.0 45979.0 45980.0 45981.0 45982.0 45983.0 45984.0 45985.0 45986.0 45987.0 45988.0 45989.0 45990.0 45991.0 45992.0 45993.0 45994.0 45995.0 45996.0 45997.0 45998.0 45999.0 46000.0 46001.0 46002.0 46003.0 46004.0 46005.0 46006.0 46007.0 46008.0 46009.0 46010.0 46011.0 46012.0 46013.0 46014.0 46015.0 46016.0 46017.0 46018.0 46019.0 46020.0 46021.0 46022.0 46023.0 46024.0 46025.0 46026.0 46027.0 46028.0 46029.0 46030.0 46031.0 46032.0 46033.0 46034.0 46035.0 46036.0 46037.0 46038.0 46039.0 46040.0 46041.0 46042.0 46043.0 46044.0 46045.0 46046.0 46047.0 46048.0 46049.0 46050.0 46051.0 46052.0 46053.0 46054.0 46055.0 46056.0 46057.0 46058.0 46059.0 46060.0 46061.0 46062.0 46063.0 46064.0 46065.0 46066.0 46067.0 46068.0 46069.0 46070.0 46071.0 46072.0 46073.0 46074.0 46075.0 46076.0 46077.0 46078.0 46079.0 46080.0 46081.0 46082.0 46083.0 46084.0 46085.0 46086.0 46087.0 46088.0 46089.0 46090.0 46091.0 46092.0 46093.0 46094.0 46095.0 46096.0 46097.0 46098.0 46099.0 46100.0 46101.0 46102.0 46103.0 46104.0 46105.0 46106.0 46107.0 46108.0 46109.0 46110.0 46111.0 46112.0 46113.0 46114.0 46115.0 46116.0 46117.0 46118.0 46119.0 46120.0 46121.0 46122.0 46123.0 46124.0 46125.0 46126.0 46127.0 46128.0 46129.0 46130.0 46131.0 46132.0 46133.0 46134.0 46135.0 46136.0 46137.0 46138.0 46139.0 46140.0 46141.0 46142.0 46143.0 46144.0 46145.0 46146.0 46147.0 46148.0 46149.0 46150.0 46151.0 46152.0 46153.0 46154.0 46155.0 46156.0 46157.0 46158.0 46159.0 46160.0 46161.0 46162.0 46163.0 46164.0 46165.0 46166.0 46167.0 46168.0 46169.0 46170.0 46171.0 46172.0 46173.0 46174.0 46175.0 46176.0 46177.0 46178.0 46179.0 46180.0 46181.0 46182.0 46183.0 46184.0 46185.0 46186.0 46187.0 46188.0 46189.0 46190.0 46191.0 46192.0 46193.0 46194.0 46195.0 46196.0 46197.0 46198.0 46199.0 46200.0 46201.0 46202.0 46203.0 46204.0 46205.0 46206.0 46207.0 46208.0 46209.0 46210.0 46211.0 46212.0 46213.0 46214.0 46215.0 46216.0 46217.0 46218.0 46219.0 46220.0 46221.0 46222.0 46223.0 46224.0 46225.0 46226.0 46227.0 46228.0 46229.0 46230.0 46231.0 46232.0 46233.0 46234.0 46235.0 46236.0 46237.0 46238.0 46239.0 46240.0 46241.0 46242.0 46243.0 46244.0 46245.0 46246.0 46247.0 46248.0 46249.0 46250.0 46251.0 46252.0 46253.0 46254.0 46255.0 46256.0 46257.0 46258.0 46259.0 46260.0 46261.0 46262.0 46263.0 46264.0 46265.0 46266.0 46267.0 46268.0 46269.0 46270.0 46271.0 46272.0 46273.0 46274.0 46275.0 46276.0 46277.0 46278.0 46279.0 46280.0 46281.0 46282.0 46283.0 46284.0 46285.0 46286.0 46287.0 46288.0 46289.0 46290.0 46291.0 46292.0 46293.0 46294.0 46295.0 46296.0 46297.0 46298.0 46299.0 46300.0 46301.0 46302.0 46303.0 46304.0 46305.0 46306.0 46307.0 46308.0 46309.0 46310.0 46311.0 46312.0 46313.0 46314.0 46315.0 46316.0 46317.0 46318.0 46319.0 46320.0 46321.0 46322.0 46323.0 46324.0 46325.0 46326.0 46327.0 46328.0 46329.0 46330.0 46331.0 46332.0 46333.0 46334.0 46335.0 46336.0 46337.0 46338.0 46339.0 46340.0 46341.0 46342.0 46343.0 46344.0 46345.0 46346.0 46347.0 46348.0 46349.0 46350.0 46351.0 46352.0 46353.0 46354.0 46355.0 46356.0 46357.0 46358.0 46359.0 46360.0 46361.0 46362.0 46363.0 46364.0 46365.0 46366.0 46367.0 46368.0 46369.0 46370.0 46371.0 46372.0 46373.0 46374.0 46375.0 46376.0 46377.0 46378.0 46379.0 46380.0 46381.0 46382.0 46383.0 46384.0 46385.0 46386.0 46387.0 46388.0 46389.0 46390.0 46391.0 46392.0 46393.0 46394.0 46395.0 46396.0 46397.0 46398.0 46399.0 46400.0 46401.0 46402.0 46403.0 46404.0 46405.0 46406.0 46407.0 46408.0 46409.0 46410.0 46411.0 46412.0 46413.0 46414.0 46415.0 46416.0 46417.0 46418.0 46419.0 46420.0 46421.0 46422.0 46423.0 46424.0 46425.0 46426.0 46427.0 46428.0 46429.0 46430.0 46431.0 46432.0 46433.0 46434.0 46435.0 46436.0 46437.0 46438.0 46439.0 46440.0 46441.0 46442.0 46443.0 46444.0 46445.0 46446.0 46447.0 46448.0 46449.0 46450.0 46451.0 46452.0 46453.0 46454.0 46455.0 46456.0 46457.0 46458.0 46459.0 46460.0 46461.0 46462.0 46463.0 46464.0 46465.0 46466.0 46467.0 46468.0 46469.0 46470.0 46471.0 46472.0 46473.0 46474.0 46475.0 46476.0 46477.0 46478.0 46479.0 46480.0 46481.0 46482.0 46483.0 46484.0 46485.0 46486.0 46487.0 46488.0 46489.0 46490.0 46491.0 46492.0 46493.0 46494.0 46495.0 46496.0 46497.0 46498.0 46499.0 46500.0 46501.0 46502.0 46503.0 46504.0 46505.0 46506.0 46507.0 46508.0 46509.0 46510.0 46511.0 46512.0 46513.0 46514.0 46515.0 46516.0 46517.0 46518.0 46519.0 46520.0 46521.0 46522.0 46523.0 46524.0 46525.0 46526.0 46527.0 46528.0 46529.0 46530.0 46531.0 46532.0 46533.0 46534.0 46535.0 46536.0 46537.0 46538.0 46539.0 46540.0 46541.0 46542.0 46543.0 46544.0 46545.0 46546.0 46547.0 46548.0 46549.0 46550.0 46551.0 46552.0 46553.0 46554.0 46555.0 46556.0 46557.0 46558.0 46559.0 46560.0 46561.0 46562.0 46563.0 46564.0 46565.0 46566.0 46567.0 46568.0 46569.0 46570.0 46571.0 46572.0 46573.0 46574.0 46575.0 46576.0 46577.0 46578.0 46579.0 46580.0 46581.0 46582.0 46583.0 46584.0 46585.0 46586.0 46587.0 46588.0 46589.0 46590.0 46591.0 46592.0 46593.0 46594.0 46595.0 46596.0 46597.0 46598.0 46599.0 46600.0 46601.0 46602.0 46603.0 46604.0 46605.0 46606.0 46607.0 46608.0 46609.0 46610.0 46611.0 46612.0 46613.0 46614.0 46615.0 46616.0 46617.0 46618.0 46619.0 46620.0 46621.0 46622.0 46623.0 46624.0 46625.0 46626.0 46627.0 46628.0 46629.0 46630.0 46631.0 46632.0 46633.0 46634.0 46635.0 46636.0 46637.0 46638.0 46639.0 46640.0 46641.0 46642.0 46643.0 46644.0 46645.0 46646.0 46647.0 46648.0 46649.0 46650.0 46651.0 46652.0 46653.0 46654.0 46655.0 46656.0 46657.0 46658.0 46659.0 46660.0 46661.0 46662.0 46663.0 46664.0 46665.0 46666.0 46667.0 46668.0 46669.0 46670.0 46671.0 46672.0 46673.0 46674.0 46675.0 46676.0 46677.0 46678.0 46679.0 46680.0 46681.0 46682.0 46683.0 46684.0 46685.0 46686.0 46687.0 46688.0 46689.0 46690.0 46691.0 46692.0 46693.0 46694.0 46695.0 46696.0 46697.0 46698.0 46699.0 46700.0 46701.0 46702.0 46703.0 46704.0 46705.0 46706.0 46707.0 46708.0 46709.0 46710.0 46711.0 46712.0 46713.0 46714.0 46715.0 46716.0 46717.0 46718.0 46719.0 46720.0 46721.0 46722.0 46723.0 46724.0 46725.0 46726.0 46727.0 46728.0 46729.0 46730.0 46731.0 46732.0 46733.0 46734.0 46735.0 46736.0 46737.0 46738.0 46739.0 46740.0 46741.0 46742.0 46743.0 46744.0 46745.0 46746.0 46747.0 46748.0 46749.0 46750.0 46751.0 46752.0 46753.0 46754.0 46755.0 46756.0 46757.0 46758.0 46759.0 46760.0 46761.0 46762.0 46763.0 46764.0 46765.0 46766.0 46767.0 46768.0 46769.0 46770.0 46771.0 46772.0 46773.0 46774.0 46775.0 46776.0 46777.0 46778.0 46779.0 46780.0 46781.0 46782.0 46783.0 46784.0 46785.0 46786.0 46787.0 46788.0 46789.0 46790.0 46791.0 46792.0 46793.0 46794.0 46795.0 46796.0 46797.0 46798.0 46799.0 46800.0 46801.0 46802.0 46803.0 46804.0 46805.0 46806.0 46807.0 46808.0 46809.0 46810.0 46811.0 46812.0 46813.0 46814.0 46815.0 46816.0 46817.0 46818.0 46819.0 46820.0 46821.0 46822.0 46823.0 46824.0 46825.0 46826.0 46827.0 46828.0 46829.0 46830.0 46831.0 46832.0 46833.0 46834.0 46835.0 46836.0 46837.0 46838.0 46839.0 46840.0 46841.0 46842.0 46843.0 46844.0 46845.0 46846.0 46847.0 46848.0 46849.0 46850.0 46851.0 46852.0 46853.0 46854.0 46855.0 46856.0 46857.0 46858.0 46859.0 46860.0 46861.0 46862.0 46863.0 46864.0 46865.0 46866.0 46867.0 46868.0 46869.0 46870.0 46871.0 46872.0 46873.0 46874.0 46875.0 46876.0 46877.0 46878.0 46879.0 46880.0 46881.0 46882.0 46883.0 46884.0 46885.0 46886.0 46887.0 46888.0 46889.0 46890.0 46891.0 46892.0 46893.0 46894.0 46895.0 46896.0 46897.0 46898.0 46899.0 46900.0 46901.0 46902.0 46903.0 46904.0 46905.0 46906.0 46907.0 46908.0 46909.0 46910.0 46911.0 46912.0 46913.0 46914.0 46915.0 46916.0 46917.0 46918.0 46919.0 46920.0 46921.0 46922.0 46923.0 46924.0 46925.0 46926.0 46927.0 46928.0 46929.0 46930.0 46931.0 46932.0 46933.0 46934.0 46935.0 46936.0 46937.0 46938.0 46939.0 46940.0 46941.0 46942.0 46943.0 46944.0 46945.0 46946.0 46947.0 46948.0 46949.0 46950.0 46951.0 46952.0 46953.0 46954.0 46955.0 46956.0 46957.0 46958.0 46959.0 46960.0 46961.0 46962.0 46963.0 46964.0 46965.0 46966.0 46967.0 46968.0 46969.0 46970.0 46971.0 46972.0 46973.0 46974.0 46975.0 46976.0 46977.0 46978.0 46979.0 46980.0 46981.0 46982.0 46983.0 46984.0 46985.0 46986.0 46987.0 46988.0 46989.0 46990.0 46991.0 46992.0 46993.0 46994.0 46995.0 46996.0 46997.0 46998.0 46999.0 47000.0 47001.0 47002.0 47003.0 47004.0 47005.0 47006.0 47007.0 47008.0 47009.0 47010.0 47011.0 47012.0 47013.0 47014.0 47015.0 47016.0 47017.0 47018.0 47019.0 47020.0 47021.0 47022.0 47023.0 47024.0 47025.0 47026.0 47027.0 47028.0 47029.0 47030.0 47031.0 47032.0 47033.0 47034.0 47035.0 47036.0 47037.0 47038.0 47039.0 47040.0 47041.0 47042.0 47043.0 47044.0 47045.0 47046.0 47047.0 47048.0 47049.0 47050.0 47051.0 47052.0 47053.0 47054.0 47055.0 47056.0 47057.0 47058.0 47059.0 47060.0 47061.0 47062.0 47063.0 47064.0 47065.0 47066.0 47067.0 47068.0 47069.0 47070.0 47071.0 47072.0 47073.0 47074.0 47075.0 47076.0 47077.0 47078.0 47079.0 47080.0 47081.0 47082.0 47083.0 47084.0 47085.0 47086.0 47087.0 47088.0 47089.0 47090.0 47091.0 47092.0 47093.0 47094.0 47095.0 47096.0 47097.0 47098.0 47099.0 47100.0 47101.0 47102.0 47103.0 47104.0 47105.0 47106.0 47107.0 47108.0 47109.0 47110.0 47111.0 47112.0 47113.0 47114.0 47115.0 47116.0 47117.0 47118.0 47119.0 47120.0 47121.0 47122.0 47123.0 47124.0 47125.0 47126.0 47127.0 47128.0 47129.0 47130.0 47131.0 47132.0 47133.0 47134.0 47135.0 47136.0 47137.0 47138.0 47139.0 47140.0 47141.0 47142.0 47143.0 47144.0 47145.0 47146.0 47147.0 47148.0 47149.0 47150.0 47151.0 47152.0 47153.0 47154.0 47155.0 47156.0 47157.0 47158.0 47159.0 47160.0 47161.0 47162.0 47163.0 47164.0 47165.0 47166.0 47167.0 47168.0 47169.0 47170.0 47171.0 47172.0 47173.0 47174.0 47175.0 47176.0 47177.0 47178.0 47179.0 47180.0 47181.0 47182.0 47183.0 47184.0 47185.0 47186.0 47187.0 47188.0 47189.0 47190.0 47191.0 47192.0 47193.0 47194.0 47195.0 47196.0 47197.0 47198.0 47199.0 47200.0 47201.0 47202.0 47203.0 47204.0 47205.0 47206.0 47207.0 47208.0 47209.0 47210.0 47211.0 47212.0 47213.0 47214.0 47215.0 47216.0 47217.0 47218.0 47219.0 47220.0 47221.0 47222.0 47223.0 47224.0 47225.0 47226.0 47227.0 47228.0 47229.0 47230.0 47231.0 47232.0 47233.0 47234.0 47235.0 47236.0 47237.0 47238.0 47239.0 47240.0 47241.0 47242.0 47243.0 47244.0 47245.0 47246.0 47247.0 47248.0 47249.0 47250.0 47251.0 47252.0 47253.0 47254.0 47255.0 47256.0 47257.0 47258.0 47259.0 47260.0 47261.0 47262.0 47263.0 47264.0 47265.0 47266.0 47267.0 47268.0 47269.0 47270.0 47271.0 47272.0 47273.0 47274.0 47275.0 47276.0 47277.0 47278.0 47279.0 47280.0 47281.0 47282.0 47283.0 47284.0 47285.0 47286.0 47287.0 47288.0 47289.0 47290.0 47291.0 47292.0 47293.0 47294.0 47295.0 47296.0 47297.0 47298.0 47299.0 47300.0 47301.0 47302.0 47303.0 47304.0 47305.0 47306.0 47307.0 47308.0 47309.0 47310.0 47311.0 47312.0 47313.0 47314.0 47315.0 47316.0 47317.0 47318.0 47319.0 47320.0 47321.0 47322.0 47323.0 47324.0 47325.0 47326.0 47327.0 47328.0 47329.0 47330.0 47331.0 47332.0 47333.0 47334.0 47335.0 47336.0 47337.0 47338.0 47339.0 47340.0 47341.0 47342.0 47343.0 47344.0 47345.0 47346.0 47347.0 47348.0 47349.0 47350.0 47351.0 47352.0 47353.0 47354.0 47355.0 47356.0 47357.0 47358.0 47359.0 47360.0 47361.0 47362.0 47363.0 47364.0 47365.0 47366.0 47367.0 47368.0 47369.0 47370.0 47371.0 47372.0 47373.0 47374.0 47375.0 47376.0 47377.0 47378.0 47379.0 47380.0 47381.0 47382.0 47383.0 47384.0 47385.0 47386.0 47387.0 47388.0 47389.0 47390.0 47391.0 47392.0 47393.0 47394.0 47395.0 47396.0 47397.0 47398.0 47399.0 47400.0 47401.0 47402.0 47403.0 47404.0 47405.0 47406.0 47407.0 47408.0 47409.0 47410.0 47411.0 47412.0 47413.0 47414.0 47415.0 47416.0 47417.0 47418.0 47419.0 47420.0 47421.0 47422.0 47423.0 47424.0 47425.0 47426.0 47427.0 47428.0 47429.0 47430.0 47431.0 47432.0 47433.0 47434.0 47435.0 47436.0 47437.0 47438.0 47439.0 47440.0 47441.0 47442.0 47443.0 47444.0 47445.0 47446.0 47447.0 47448.0 47449.0 47450.0 47451.0 47452.0 47453.0 47454.0 47455.0 47456.0 47457.0 47458.0 47459.0 47460.0 47461.0 47462.0 47463.0 47464.0 47465.0 47466.0 47467.0 47468.0 47469.0 47470.0 47471.0 47472.0 47473.0 47474.0 47475.0 47476.0 47477.0 47478.0 47479.0 47480.0 47481.0 47482.0 47483.0 47484.0 47485.0 47486.0 47487.0 47488.0 47489.0 47490.0 47491.0 47492.0 47493.0 47494.0 47495.0 47496.0 47497.0 47498.0 47499.0 47500.0 47501.0 47502.0 47503.0 47504.0 47505.0 47506.0 47507.0 47508.0 47509.0 47510.0 47511.0 47512.0 47513.0 47514.0 47515.0 47516.0 47517.0 47518.0 47519.0 47520.0 47521.0 47522.0 47523.0 47524.0 47525.0 47526.0 47527.0 47528.0 47529.0 47530.0 47531.0 47532.0 47533.0 47534.0 47535.0 47536.0 47537.0 47538.0 47539.0 47540.0 47541.0 47542.0 47543.0 47544.0 47545.0 47546.0 47547.0 47548.0 47549.0 47550.0 47551.0 47552.0 47553.0 47554.0 47555.0 47556.0 47557.0 47558.0 47559.0 47560.0 47561.0 47562.0 47563.0 47564.0 47565.0 47566.0 47567.0 47568.0 47569.0 47570.0 47571.0 47572.0 47573.0 47574.0 47575.0 47576.0 47577.0 47578.0 47579.0 47580.0 47581.0 47582.0 47583.0 47584.0 47585.0 47586.0 47587.0 47588.0 47589.0 47590.0 47591.0 47592.0 47593.0 47594.0 47595.0 47596.0 47597.0 47598.0 47599.0 47600.0 47601.0 47602.0 47603.0 47604.0 47605.0 47606.0 47607.0 47608.0 47609.0 47610.0 47611.0 47612.0 47613.0 47614.0 47615.0 47616.0 47617.0 47618.0 47619.0 47620.0 47621.0 47622.0 47623.0 47624.0 47625.0 47626.0 47627.0 47628.0 47629.0 47630.0 47631.0 47632.0 47633.0 47634.0 47635.0 47636.0 47637.0 47638.0 47639.0 47640.0 47641.0 47642.0 47643.0 47644.0 47645.0 47646.0 47647.0 47648.0 47649.0 47650.0 47651.0 47652.0 47653.0 47654.0 47655.0 47656.0 47657.0 47658.0 47659.0 47660.0 47661.0 47662.0 47663.0 47664.0 47665.0 47666.0 47667.0 47668.0 47669.0 47670.0 47671.0 47672.0 47673.0 47674.0 47675.0 47676.0 47677.0 47678.0 47679.0 47680.0 47681.0 47682.0 47683.0 47684.0 47685.0 47686.0 47687.0 47688.0 47689.0 47690.0 47691.0 47692.0 47693.0 47694.0 47695.0 47696.0 47697.0 47698.0 47699.0 47700.0 47701.0 47702.0 47703.0 47704.0 47705.0 47706.0 47707.0 47708.0 47709.0 47710.0 47711.0 47712.0 47713.0 47714.0 47715.0 47716.0 47717.0 47718.0 47719.0 47720.0 47721.0 47722.0 47723.0 47724.0 47725.0 47726.0 47727.0 47728.0 47729.0 47730.0 47731.0 47732.0 47733.0 47734.0 47735.0 47736.0 47737.0 47738.0 47739.0 47740.0 47741.0 47742.0 47743.0 47744.0 47745.0 47746.0 47747.0 47748.0 47749.0 47750.0 47751.0 47752.0 47753.0 47754.0 47755.0 47756.0 47757.0 47758.0 47759.0 47760.0 47761.0 47762.0 47763.0 47764.0 47765.0 47766.0 47767.0 47768.0 47769.0 47770.0 47771.0 47772.0 47773.0 47774.0 47775.0 47776.0 47777.0 47778.0 47779.0 47780.0 47781.0 47782.0 47783.0 47784.0 47785.0 47786.0 47787.0 47788.0 47789.0 47790.0 47791.0 47792.0 47793.0 47794.0 47795.0 47796.0 47797.0 47798.0 47799.0 47800.0 47801.0 47802.0 47803.0 47804.0 47805.0 47806.0 47807.0 47808.0 47809.0 47810.0 47811.0 47812.0 47813.0 47814.0 47815.0 47816.0 47817.0 47818.0 47819.0 47820.0 47821.0 47822.0 47823.0 47824.0 47825.0 47826.0 47827.0 47828.0 47829.0 47830.0 47831.0 47832.0 47833.0 47834.0 47835.0 47836.0 47837.0 47838.0 47839.0 47840.0 47841.0 47842.0 47843.0 47844.0 47845.0 47846.0 47847.0 47848.0 47849.0 47850.0 47851.0 47852.0 47853.0 47854.0 47855.0 47856.0 47857.0 47858.0 47859.0 47860.0 47861.0 47862.0 47863.0 47864.0 47865.0 47866.0 47867.0 47868.0 47869.0 47870.0 47871.0 47872.0 47873.0 47874.0 47875.0 47876.0 47877.0 47878.0 47879.0 47880.0 47881.0 47882.0 47883.0 47884.0 47885.0 47886.0 47887.0 47888.0 47889.0 47890.0 47891.0 47892.0 47893.0 47894.0 47895.0 47896.0 47897.0 47898.0 47899.0 47900.0 47901.0 47902.0 47903.0 47904.0 47905.0 47906.0 47907.0 47908.0 47909.0 47910.0 47911.0 47912.0 47913.0 47914.0 47915.0 47916.0 47917.0 47918.0 47919.0 47920.0 47921.0 47922.0 47923.0 47924.0 47925.0 47926.0 47927.0 47928.0 47929.0 47930.0 47931.0 47932.0 47933.0 47934.0 47935.0 47936.0 47937.0 47938.0 47939.0 47940.0 47941.0 47942.0 47943.0 47944.0 47945.0 47946.0 47947.0 47948.0 47949.0 47950.0 47951.0 47952.0 47953.0 47954.0 47955.0 47956.0 47957.0 47958.0 47959.0 47960.0 47961.0 47962.0 47963.0 47964.0 47965.0 47966.0 47967.0 47968.0 47969.0 47970.0 47971.0 47972.0 47973.0 47974.0 47975.0 47976.0 47977.0 47978.0 47979.0 47980.0 47981.0 47982.0 47983.0 47984.0 47985.0 47986.0 47987.0 47988.0 47989.0 47990.0 47991.0 47992.0 47993.0 47994.0 47995.0 47996.0 47997.0 47998.0 47999.0 48000.0 48001.0 48002.0 48003.0 48004.0 48005.0 48006.0 48007.0 48008.0 48009.0 48010.0 48011.0 48012.0 48013.0 48014.0 48015.0 48016.0 48017.0 48018.0 48019.0 48020.0 48021.0 48022.0 48023.0 48024.0 48025.0 48026.0 48027.0 48028.0 48029.0 48030.0 48031.0 48032.0 48033.0 48034.0 48035.0 48036.0 48037.0 48038.0 48039.0 48040.0 48041.0 48042.0 48043.0 48044.0 48045.0 48046.0 48047.0 48048.0 48049.0 48050.0 48051.0 48052.0 48053.0 48054.0 48055.0 48056.0 48057.0 48058.0 48059.0 48060.0 48061.0 48062.0 48063.0 48064.0 48065.0 48066.0 48067.0 48068.0 48069.0 48070.0 48071.0 48072.0 48073.0 48074.0 48075.0 48076.0 48077.0 48078.0 48079.0 48080.0 48081.0 48082.0 48083.0 48084.0 48085.0 48086.0 48087.0 48088.0 48089.0 48090.0 48091.0 48092.0 48093.0 48094.0 48095.0 48096.0 48097.0 48098.0 48099.0 48100.0 48101.0 48102.0 48103.0 48104.0 48105.0 48106.0 48107.0 48108.0 48109.0 48110.0 48111.0 48112.0 48113.0 48114.0 48115.0 48116.0 48117.0 48118.0 48119.0 48120.0 48121.0 48122.0 48123.0 48124.0 48125.0 48126.0 48127.0 48128.0 48129.0 48130.0 48131.0 48132.0 48133.0 48134.0 48135.0 48136.0 48137.0 48138.0 48139.0 48140.0 48141.0 48142.0 48143.0 48144.0 48145.0 48146.0 48147.0 48148.0 48149.0 48150.0 48151.0 48152.0 48153.0 48154.0 48155.0 48156.0 48157.0 48158.0 48159.0 48160.0 48161.0 48162.0 48163.0 48164.0 48165.0 48166.0 48167.0 48168.0 48169.0 48170.0 48171.0 48172.0 48173.0 48174.0 48175.0 48176.0 48177.0 48178.0 48179.0 48180.0 48181.0 48182.0 48183.0 48184.0 48185.0 48186.0 48187.0 48188.0 48189.0 48190.0 48191.0 48192.0 48193.0 48194.0 48195.0 48196.0 48197.0 48198.0 48199.0 48200.0 48201.0 48202.0 48203.0 48204.0 48205.0 48206.0 48207.0 48208.0 48209.0 48210.0 48211.0 48212.0 48213.0 48214.0 48215.0 48216.0 48217.0 48218.0 48219.0 48220.0 48221.0 48222.0 48223.0 48224.0 48225.0 48226.0 48227.0 48228.0 48229.0 48230.0 48231.0 48232.0 48233.0 48234.0 48235.0 48236.0 48237.0 48238.0 48239.0 48240.0 48241.0 48242.0 48243.0 48244.0 48245.0 48246.0 48247.0 48248.0 48249.0 48250.0 48251.0 48252.0 48253.0 48254.0 48255.0 48256.0 48257.0 48258.0 48259.0 48260.0 48261.0 48262.0 48263.0 48264.0 48265.0 48266.0 48267.0 48268.0 48269.0 48270.0 48271.0 48272.0 48273.0 48274.0 48275.0 48276.0 48277.0 48278.0 48279.0 48280.0 48281.0 48282.0 48283.0 48284.0 48285.0 48286.0 48287.0 48288.0 48289.0 48290.0 48291.0 48292.0 48293.0 48294.0 48295.0 48296.0 48297.0 48298.0 48299.0 48300.0 48301.0 48302.0 48303.0 48304.0 48305.0 48306.0 48307.0 48308.0 48309.0 48310.0 48311.0 48312.0 48313.0 48314.0 48315.0 48316.0 48317.0 48318.0 48319.0 48320.0 48321.0 48322.0 48323.0 48324.0 48325.0 48326.0 48327.0 48328.0 48329.0 48330.0 48331.0 48332.0 48333.0 48334.0 48335.0 48336.0 48337.0 48338.0 48339.0 48340.0 48341.0 48342.0 48343.0 48344.0 48345.0 48346.0 48347.0 48348.0 48349.0 48350.0 48351.0 48352.0 48353.0 48354.0 48355.0 48356.0 48357.0 48358.0 48359.0 48360.0 48361.0 48362.0 48363.0 48364.0 48365.0 48366.0 48367.0 48368.0 48369.0 48370.0 48371.0 48372.0 48373.0 48374.0 48375.0 48376.0 48377.0 48378.0 48379.0 48380.0 48381.0 48382.0 48383.0 48384.0 48385.0 48386.0 48387.0 48388.0 48389.0 48390.0 48391.0 48392.0 48393.0 48394.0 48395.0 48396.0 48397.0 48398.0 48399.0 48400.0 48401.0 48402.0 48403.0 48404.0 48405.0 48406.0 48407.0 48408.0 48409.0 48410.0 48411.0 48412.0 48413.0 48414.0 48415.0 48416.0 48417.0 48418.0 48419.0 48420.0 48421.0 48422.0 48423.0 48424.0 48425.0 48426.0 48427.0 48428.0 48429.0 48430.0 48431.0 48432.0 48433.0 48434.0 48435.0 48436.0 48437.0 48438.0 48439.0 48440.0 48441.0 48442.0 48443.0 48444.0 48445.0 48446.0 48447.0 48448.0 48449.0 48450.0 48451.0 48452.0 48453.0 48454.0 48455.0 48456.0 48457.0 48458.0 48459.0 48460.0 48461.0 48462.0 48463.0 48464.0 48465.0 48466.0 48467.0 48468.0 48469.0 48470.0 48471.0 48472.0 48473.0 48474.0 48475.0 48476.0 48477.0 48478.0 48479.0 48480.0 48481.0 48482.0 48483.0 48484.0 48485.0 48486.0 48487.0 48488.0 48489.0 48490.0 48491.0 48492.0 48493.0 48494.0 48495.0 48496.0 48497.0 48498.0 48499.0 48500.0 48501.0 48502.0 48503.0 48504.0 48505.0 48506.0 48507.0 48508.0 48509.0 48510.0 48511.0 48512.0 48513.0 48514.0 48515.0 48516.0 48517.0 48518.0 48519.0 48520.0 48521.0 48522.0 48523.0 48524.0 48525.0 48526.0 48527.0 48528.0 48529.0 48530.0 48531.0 48532.0 48533.0 48534.0 48535.0 48536.0 48537.0 48538.0 48539.0 48540.0 48541.0 48542.0 48543.0 48544.0 48545.0 48546.0 48547.0 48548.0 48549.0 48550.0 48551.0 48552.0 48553.0 48554.0 48555.0 48556.0 48557.0 48558.0 48559.0 48560.0 48561.0 48562.0 48563.0 48564.0 48565.0 48566.0 48567.0 48568.0 48569.0 48570.0 48571.0 48572.0 48573.0 48574.0 48575.0 48576.0 48577.0 48578.0 48579.0 48580.0 48581.0 48582.0 48583.0 48584.0 48585.0 48586.0 48587.0 48588.0 48589.0 48590.0 48591.0 48592.0 48593.0 48594.0 48595.0 48596.0 48597.0 48598.0 48599.0 48600.0 48601.0 48602.0 48603.0 48604.0 48605.0 48606.0 48607.0 48608.0 48609.0 48610.0 48611.0 48612.0 48613.0 48614.0 48615.0 48616.0 48617.0 48618.0 48619.0 48620.0 48621.0 48622.0 48623.0 48624.0 48625.0 48626.0 48627.0 48628.0 48629.0 48630.0 48631.0 48632.0 48633.0 48634.0 48635.0 48636.0 48637.0 48638.0 48639.0 48640.0 48641.0 48642.0 48643.0 48644.0 48645.0 48646.0 48647.0 48648.0 48649.0 48650.0 48651.0 48652.0 48653.0 48654.0 48655.0 48656.0 48657.0 48658.0 48659.0 48660.0 48661.0 48662.0 48663.0 48664.0 48665.0 48666.0 48667.0 48668.0 48669.0 48670.0 48671.0 48672.0 48673.0 48674.0 48675.0 48676.0 48677.0 48678.0 48679.0 48680.0 48681.0 48682.0 48683.0 48684.0 48685.0 48686.0 48687.0 48688.0 48689.0 48690.0 48691.0 48692.0 48693.0 48694.0 48695.0 48696.0 48697.0 48698.0 48699.0 48700.0 48701.0 48702.0 48703.0 48704.0 48705.0 48706.0 48707.0 48708.0 48709.0 48710.0 48711.0 48712.0 48713.0 48714.0 48715.0 48716.0 48717.0 48718.0 48719.0 48720.0 48721.0 48722.0 48723.0 48724.0 48725.0 48726.0 48727.0 48728.0 48729.0 48730.0 48731.0 48732.0 48733.0 48734.0 48735.0 48736.0 48737.0 48738.0 48739.0 48740.0 48741.0 48742.0 48743.0 48744.0 48745.0 48746.0 48747.0 48748.0 48749.0 48750.0 48751.0 48752.0 48753.0 48754.0 48755.0 48756.0 48757.0 48758.0 48759.0 48760.0 48761.0 48762.0 48763.0 48764.0 48765.0 48766.0 48767.0 48768.0 48769.0 48770.0 48771.0 48772.0 48773.0 48774.0 48775.0 48776.0 48777.0 48778.0 48779.0 48780.0 48781.0 48782.0 48783.0 48784.0 48785.0 48786.0 48787.0 48788.0 48789.0 48790.0 48791.0 48792.0 48793.0 48794.0 48795.0 48796.0 48797.0 48798.0 48799.0 48800.0 48801.0 48802.0 48803.0 48804.0 48805.0 48806.0 48807.0 48808.0 48809.0 48810.0 48811.0 48812.0 48813.0 48814.0 48815.0 48816.0 48817.0 48818.0 48819.0 48820.0 48821.0 48822.0 48823.0 48824.0 48825.0 48826.0 48827.0 48828.0 48829.0 48830.0 48831.0 48832.0 48833.0 48834.0 48835.0 48836.0 48837.0 48838.0 48839.0 48840.0 48841.0 48842.0 48843.0 48844.0 48845.0 48846.0 48847.0 48848.0 48849.0 48850.0 48851.0 48852.0 48853.0 48854.0 48855.0 48856.0 48857.0 48858.0 48859.0 48860.0 48861.0 48862.0 48863.0 48864.0 48865.0 48866.0 48867.0 48868.0 48869.0 48870.0 48871.0 48872.0 48873.0 48874.0 48875.0 48876.0 48877.0 48878.0 48879.0 48880.0 48881.0 48882.0 48883.0 48884.0 48885.0 48886.0 48887.0 48888.0 48889.0 48890.0 48891.0 48892.0 48893.0 48894.0 48895.0 48896.0 48897.0 48898.0 48899.0 48900.0 48901.0 48902.0 48903.0 48904.0 48905.0 48906.0 48907.0 48908.0 48909.0 48910.0 48911.0 48912.0 48913.0 48914.0 48915.0 48916.0 48917.0 48918.0 48919.0 48920.0 48921.0 48922.0 48923.0 48924.0 48925.0 48926.0 48927.0 48928.0 48929.0 48930.0 48931.0 48932.0 48933.0 48934.0 48935.0 48936.0 48937.0 48938.0 48939.0 48940.0 48941.0 48942.0 48943.0 48944.0 48945.0 48946.0 48947.0 48948.0 48949.0 48950.0 48951.0 48952.0 48953.0 48954.0 48955.0 48956.0 48957.0 48958.0 48959.0 48960.0 48961.0 48962.0 48963.0 48964.0 48965.0 48966.0 48967.0 48968.0 48969.0 48970.0 48971.0 48972.0 48973.0 48974.0 48975.0 48976.0 48977.0 48978.0 48979.0 48980.0 48981.0 48982.0 48983.0 48984.0 48985.0 48986.0 48987.0 48988.0 48989.0 48990.0 48991.0 48992.0 48993.0 48994.0 48995.0 48996.0 48997.0 48998.0 48999.0 49000.0 49001.0 49002.0 49003.0 49004.0 49005.0 49006.0 49007.0 49008.0 49009.0 49010.0 49011.0 49012.0 49013.0 49014.0 49015.0 49016.0 49017.0 49018.0 49019.0 49020.0 49021.0 49022.0 49023.0 49024.0 49025.0 49026.0 49027.0 49028.0 49029.0 49030.0 49031.0 49032.0 49033.0 49034.0 49035.0 49036.0 49037.0 49038.0 49039.0 49040.0 49041.0 49042.0 49043.0 49044.0 49045.0 49046.0 49047.0 49048.0 49049.0 49050.0 49051.0 49052.0 49053.0 49054.0 49055.0 49056.0 49057.0 49058.0 49059.0 49060.0 49061.0 49062.0 49063.0 49064.0 49065.0 49066.0 49067.0 49068.0 49069.0 49070.0 49071.0 49072.0 49073.0 49074.0 49075.0 49076.0 49077.0 49078.0 49079.0 49080.0 49081.0 49082.0 49083.0 49084.0 49085.0 49086.0 49087.0 49088.0 49089.0 49090.0 49091.0 49092.0 49093.0 49094.0 49095.0 49096.0 49097.0 49098.0 49099.0 49100.0 49101.0 49102.0 49103.0 49104.0 49105.0 49106.0 49107.0 49108.0 49109.0 49110.0 49111.0 49112.0 49113.0 49114.0 49115.0 49116.0 49117.0 49118.0 49119.0 49120.0 49121.0 49122.0 49123.0 49124.0 49125.0 49126.0 49127.0 49128.0 49129.0 49130.0 49131.0 49132.0 49133.0 49134.0 49135.0 49136.0 49137.0 49138.0 49139.0 49140.0 49141.0 49142.0 49143.0 49144.0 49145.0 49146.0 49147.0 49148.0 49149.0 49150.0 49151.0 49152.0 49153.0 49154.0 49155.0 49156.0 49157.0 49158.0 49159.0 49160.0 49161.0 49162.0 49163.0 49164.0 49165.0 49166.0 49167.0 49168.0 49169.0 49170.0 49171.0 49172.0 49173.0 49174.0 49175.0 49176.0 49177.0 49178.0 49179.0 49180.0 49181.0 49182.0 49183.0 49184.0 49185.0 49186.0 49187.0 49188.0 49189.0 49190.0 49191.0 49192.0 49193.0 49194.0 49195.0 49196.0 49197.0 49198.0 49199.0 49200.0 49201.0 49202.0 49203.0 49204.0 49205.0 49206.0 49207.0 49208.0 49209.0 49210.0 49211.0 49212.0 49213.0 49214.0 49215.0 49216.0 49217.0 49218.0 49219.0 49220.0 49221.0 49222.0 49223.0 49224.0 49225.0 49226.0 49227.0 49228.0 49229.0 49230.0 49231.0 49232.0 49233.0 49234.0 49235.0 49236.0 49237.0 49238.0 49239.0 49240.0 49241.0 49242.0 49243.0 49244.0 49245.0 49246.0 49247.0 49248.0 49249.0 49250.0 49251.0 49252.0 49253.0 49254.0 49255.0 49256.0 49257.0 49258.0 49259.0 49260.0 49261.0 49262.0 49263.0 49264.0 49265.0 49266.0 49267.0 49268.0 49269.0 49270.0 49271.0 49272.0 49273.0 49274.0 49275.0 49276.0 49277.0 49278.0 49279.0 49280.0 49281.0 49282.0 49283.0 49284.0 49285.0 49286.0 49287.0 49288.0 49289.0 49290.0 49291.0 49292.0 49293.0 49294.0 49295.0 49296.0 49297.0 49298.0 49299.0 49300.0 49301.0 49302.0 49303.0 49304.0 49305.0 49306.0 49307.0 49308.0 49309.0 49310.0 49311.0 49312.0 49313.0 49314.0 49315.0 49316.0 49317.0 49318.0 49319.0 49320.0 49321.0 49322.0 49323.0 49324.0 49325.0 49326.0 49327.0 49328.0 49329.0 49330.0 49331.0 49332.0 49333.0 49334.0 49335.0 49336.0 49337.0 49338.0 49339.0 49340.0 49341.0 49342.0 49343.0 49344.0 49345.0 49346.0 49347.0 49348.0 49349.0 49350.0 49351.0 49352.0 49353.0 49354.0 49355.0 49356.0 49357.0 49358.0 49359.0 49360.0 49361.0 49362.0 49363.0 49364.0 49365.0 49366.0 49367.0 49368.0 49369.0 49370.0 49371.0 49372.0 49373.0 49374.0 49375.0 49376.0 49377.0 49378.0 49379.0 49380.0 49381.0 49382.0 49383.0 49384.0 49385.0 49386.0 49387.0 49388.0 49389.0 49390.0 49391.0 49392.0 49393.0 49394.0 49395.0 49396.0 49397.0 49398.0 49399.0 49400.0 49401.0 49402.0 49403.0 49404.0 49405.0 49406.0 49407.0 49408.0 49409.0 49410.0 49411.0 49412.0 49413.0 49414.0 49415.0 49416.0 49417.0 49418.0 49419.0 49420.0 49421.0 49422.0 49423.0 49424.0 49425.0 49426.0 49427.0 49428.0 49429.0 49430.0 49431.0 49432.0 49433.0 49434.0 49435.0 49436.0 49437.0 49438.0 49439.0 49440.0 49441.0 49442.0 49443.0 49444.0 49445.0 49446.0 49447.0 49448.0 49449.0 49450.0 49451.0 49452.0 49453.0 49454.0 49455.0 49456.0 49457.0 49458.0 49459.0 49460.0 49461.0 49462.0 49463.0 49464.0 49465.0 49466.0 49467.0 49468.0 49469.0 49470.0 49471.0 49472.0 49473.0 49474.0 49475.0 49476.0 49477.0 49478.0 49479.0 49480.0 49481.0 49482.0 49483.0 49484.0 49485.0 49486.0 49487.0 49488.0 49489.0 49490.0 49491.0 49492.0 49493.0 49494.0 49495.0 49496.0 49497.0 49498.0 49499.0 49500.0 49501.0 49502.0 49503.0 49504.0 49505.0 49506.0 49507.0 49508.0 49509.0 49510.0 49511.0 49512.0 49513.0 49514.0 49515.0 49516.0 49517.0 49518.0 49519.0 49520.0 49521.0 49522.0 49523.0 49524.0 49525.0 49526.0 49527.0 49528.0 49529.0 49530.0 49531.0 49532.0 49533.0 49534.0 49535.0 49536.0 49537.0 49538.0 49539.0 49540.0 49541.0 49542.0 49543.0 49544.0 49545.0 49546.0 49547.0 49548.0 49549.0 49550.0 49551.0 49552.0 49553.0 49554.0 49555.0 49556.0 49557.0 49558.0 49559.0 49560.0 49561.0 49562.0 49563.0 49564.0 49565.0 49566.0 49567.0 49568.0 49569.0 49570.0 49571.0 49572.0 49573.0 49574.0 49575.0 49576.0 49577.0 49578.0 49579.0 49580.0 49581.0 49582.0 49583.0 49584.0 49585.0 49586.0 49587.0 49588.0 49589.0 49590.0 49591.0 49592.0 49593.0 49594.0 49595.0 49596.0 49597.0 49598.0 49599.0 49600.0 49601.0 49602.0 49603.0 49604.0 49605.0 49606.0 49607.0 49608.0 49609.0 49610.0 49611.0 49612.0 49613.0 49614.0 49615.0 49616.0 49617.0 49618.0 49619.0 49620.0 49621.0 49622.0 49623.0 49624.0 49625.0 49626.0 49627.0 49628.0 49629.0 49630.0 49631.0 49632.0 49633.0 49634.0 49635.0 49636.0 49637.0 49638.0 49639.0 49640.0 49641.0 49642.0 49643.0 49644.0 49645.0 49646.0 49647.0 49648.0 49649.0 49650.0 49651.0 49652.0 49653.0 49654.0 49655.0 49656.0 49657.0 49658.0 49659.0 49660.0 49661.0 49662.0 49663.0 49664.0 49665.0 49666.0 49667.0 49668.0 49669.0 49670.0 49671.0 49672.0 49673.0 49674.0 49675.0 49676.0 49677.0 49678.0 49679.0 49680.0 49681.0 49682.0 49683.0 49684.0 49685.0 49686.0 49687.0 49688.0 49689.0 49690.0 49691.0 49692.0 49693.0 49694.0 49695.0 49696.0 49697.0 49698.0 49699.0 49700.0 49701.0 49702.0 49703.0 49704.0 49705.0 49706.0 49707.0 49708.0 49709.0 49710.0 49711.0 49712.0 49713.0 49714.0 49715.0 49716.0 49717.0 49718.0 49719.0 49720.0 49721.0 49722.0 49723.0 49724.0 49725.0 49726.0 49727.0 49728.0 49729.0 49730.0 49731.0 49732.0 49733.0 49734.0 49735.0 49736.0 49737.0 49738.0 49739.0 49740.0 49741.0 49742.0 49743.0 49744.0 49745.0 49746.0 49747.0 49748.0 49749.0 49750.0 49751.0 49752.0 49753.0 49754.0 49755.0 49756.0 49757.0 49758.0 49759.0 49760.0 49761.0 49762.0 49763.0 49764.0 49765.0 49766.0 49767.0 49768.0 49769.0 49770.0 49771.0 49772.0 49773.0 49774.0 49775.0 49776.0 49777.0 49778.0 49779.0 49780.0 49781.0 49782.0 49783.0 49784.0 49785.0 49786.0 49787.0 49788.0 49789.0 49790.0 49791.0 49792.0 49793.0 49794.0 49795.0 49796.0 49797.0 49798.0 49799.0 49800.0 49801.0 49802.0 49803.0 49804.0 49805.0 49806.0 49807.0 49808.0 49809.0 49810.0 49811.0 49812.0 49813.0 49814.0 49815.0 49816.0 49817.0 49818.0 49819.0 49820.0 49821.0 49822.0 49823.0 49824.0 49825.0 49826.0 49827.0 49828.0 49829.0 49830.0 49831.0 49832.0 49833.0 49834.0 49835.0 49836.0 49837.0 49838.0 49839.0 49840.0 49841.0 49842.0 49843.0 49844.0 49845.0 49846.0 49847.0 49848.0 49849.0 49850.0 49851.0 49852.0 49853.0 49854.0 49855.0 49856.0 49857.0 49858.0 49859.0 49860.0 49861.0 49862.0 49863.0 49864.0 49865.0 49866.0 49867.0 49868.0 49869.0 49870.0 49871.0 49872.0 49873.0 49874.0 49875.0 49876.0 49877.0 49878.0 49879.0 49880.0 49881.0 49882.0 49883.0 49884.0 49885.0 49886.0 49887.0 49888.0 49889.0 49890.0 49891.0 49892.0 49893.0 49894.0 49895.0 49896.0 49897.0 49898.0 49899.0 49900.0 49901.0 49902.0 49903.0 49904.0 49905.0 49906.0 49907.0 49908.0 49909.0 49910.0 49911.0 49912.0 49913.0 49914.0 49915.0 49916.0 49917.0 49918.0 49919.0 49920.0 49921.0 49922.0 49923.0 49924.0 49925.0 49926.0 49927.0 49928.0 49929.0 49930.0 49931.0 49932.0 49933.0 49934.0 49935.0 49936.0 49937.0 49938.0 49939.0 49940.0 49941.0 49942.0 49943.0 49944.0 49945.0 49946.0 49947.0 49948.0 49949.0 49950.0 49951.0 49952.0 49953.0 49954.0 49955.0 49956.0 49957.0 49958.0 49959.0 49960.0 49961.0 49962.0 49963.0 49964.0 49965.0 49966.0 49967.0 49968.0 49969.0 49970.0 49971.0 49972.0 49973.0 49974.0 49975.0 49976.0 49977.0 49978.0 49979.0 49980.0 49981.0 49982.0 49983.0 49984.0 49985.0 49986.0 49987.0 49988.0 49989.0 49990.0 49991.0 49992.0 49993.0 49994.0 49995.0 49996.0 49997.0 49998.0 49999.0 50000.0 50001.0 50002.0 50003.0 50004.0 50005.0 50006.0 50007.0 50008.0 50009.0 50010.0 50011.0 50012.0 50013.0 50014.0 50015.0 50016.0 50017.0 50018.0 50019.0 50020.0 50021.0 50022.0 50023.0 50024.0 50025.0 50026.0 50027.0 50028.0 50029.0 50030.0 50031.0 50032.0 50033.0 50034.0 50035.0 50036.0 50037.0 50038.0 50039.0 50040.0 50041.0 50042.0 50043.0 50044.0 50045.0 50046.0 50047.0 50048.0 50049.0 50050.0 50051.0 50052.0 50053.0 50054.0 50055.0 50056.0 50057.0 50058.0 50059.0 50060.0 50061.0 50062.0 50063.0 50064.0 50065.0 50066.0 50067.0 50068.0 50069.0 50070.0 50071.0 50072.0 50073.0 50074.0 50075.0 50076.0 50077.0 50078.0 50079.0 50080.0 50081.0 50082.0 50083.0 50084.0 50085.0 50086.0 50087.0 50088.0 50089.0 50090.0 50091.0 50092.0 50093.0 50094.0 50095.0 50096.0 50097.0 50098.0 50099.0 50100.0 50101.0 50102.0 50103.0 50104.0 50105.0 50106.0 50107.0 50108.0 50109.0 50110.0 50111.0 50112.0 50113.0 50114.0 50115.0 50116.0 50117.0 50118.0 50119.0 50120.0 50121.0 50122.0 50123.0 50124.0 50125.0 50126.0 50127.0 50128.0 50129.0 50130.0 50131.0 50132.0 50133.0 50134.0 50135.0 50136.0 50137.0 50138.0 50139.0 50140.0 50141.0 50142.0 50143.0 50144.0 50145.0 50146.0 50147.0 50148.0 50149.0 50150.0 50151.0 50152.0 50153.0 50154.0 50155.0 50156.0 50157.0 50158.0 50159.0 50160.0 50161.0 50162.0 50163.0 50164.0 50165.0 50166.0 50167.0 50168.0 50169.0 50170.0 50171.0 50172.0 50173.0 50174.0 50175.0 50176.0 50177.0 50178.0 50179.0 50180.0 50181.0 50182.0 50183.0 50184.0 50185.0 50186.0 50187.0 50188.0 50189.0 50190.0 50191.0 50192.0 50193.0 50194.0 50195.0 50196.0 50197.0 50198.0 50199.0 50200.0 50201.0 50202.0 50203.0 50204.0 50205.0 50206.0 50207.0 50208.0 50209.0 50210.0 50211.0 50212.0 50213.0 50214.0 50215.0 50216.0 50217.0 50218.0 50219.0 50220.0 50221.0 50222.0 50223.0 50224.0 50225.0 50226.0 50227.0 50228.0 50229.0 50230.0 50231.0 50232.0 50233.0 50234.0 50235.0 50236.0 50237.0 50238.0 50239.0 50240.0 50241.0 50242.0 50243.0 50244.0 50245.0 50246.0 50247.0 50248.0 50249.0 50250.0 50251.0 50252.0 50253.0 50254.0 50255.0 50256.0 50257.0 50258.0 50259.0 50260.0 50261.0 50262.0 50263.0 50264.0 50265.0 50266.0 50267.0 50268.0 50269.0 50270.0 50271.0 50272.0 50273.0 50274.0 50275.0 50276.0 50277.0 50278.0 50279.0 50280.0 50281.0 50282.0 50283.0 50284.0 50285.0 50286.0 50287.0 50288.0 50289.0 50290.0 50291.0 50292.0 50293.0 50294.0 50295.0 50296.0 50297.0 50298.0 50299.0 50300.0 50301.0 50302.0 50303.0 50304.0 50305.0 50306.0 50307.0 50308.0 50309.0 50310.0 50311.0 50312.0 50313.0 50314.0 50315.0 50316.0 50317.0 50318.0 50319.0 50320.0 50321.0 50322.0 50323.0 50324.0 50325.0 50326.0 50327.0 50328.0 50329.0 50330.0 50331.0 50332.0 50333.0 50334.0 50335.0 50336.0 50337.0 50338.0 50339.0 50340.0 50341.0 50342.0 50343.0 50344.0 50345.0 50346.0 50347.0 50348.0 50349.0 50350.0 50351.0 50352.0 50353.0 50354.0 50355.0 50356.0 50357.0 50358.0 50359.0 50360.0 50361.0 50362.0 50363.0 50364.0 50365.0 50366.0 50367.0 50368.0 50369.0 50370.0 50371.0 50372.0 50373.0 50374.0 50375.0 50376.0 50377.0 50378.0 50379.0 50380.0 50381.0 50382.0 50383.0 50384.0 50385.0 50386.0 50387.0 50388.0 50389.0 50390.0 50391.0 50392.0 50393.0 50394.0 50395.0 50396.0 50397.0 50398.0 50399.0 50400.0 50401.0 50402.0 50403.0 50404.0 50405.0 50406.0 50407.0 50408.0 50409.0 50410.0 50411.0 50412.0 50413.0 50414.0 50415.0 50416.0 50417.0 50418.0 50419.0 50420.0 50421.0 50422.0 50423.0 50424.0 50425.0 50426.0 50427.0 50428.0 50429.0 50430.0 50431.0 50432.0 50433.0 50434.0 50435.0 50436.0 50437.0 50438.0 50439.0 50440.0 50441.0 50442.0 50443.0 50444.0 50445.0 50446.0 50447.0 50448.0 50449.0 50450.0 50451.0 50452.0 50453.0 50454.0 50455.0 50456.0 50457.0 50458.0 50459.0 50460.0 50461.0 50462.0 50463.0 50464.0 50465.0 50466.0 50467.0 50468.0 50469.0 50470.0 50471.0 50472.0 50473.0 50474.0 50475.0 50476.0 50477.0 50478.0 50479.0 50480.0 50481.0 50482.0 50483.0 50484.0 50485.0 50486.0 50487.0 50488.0 50489.0 50490.0 50491.0 50492.0 50493.0 50494.0 50495.0 50496.0 50497.0 50498.0 50499.0 50500.0 50501.0 50502.0 50503.0 50504.0 50505.0 50506.0 50507.0 50508.0 50509.0 50510.0 50511.0 50512.0 50513.0 50514.0 50515.0 50516.0 50517.0 50518.0 50519.0 50520.0 50521.0 50522.0 50523.0 50524.0 50525.0 50526.0 50527.0 50528.0 50529.0 50530.0 50531.0 50532.0 50533.0 50534.0 50535.0 50536.0 50537.0 50538.0 50539.0 50540.0 50541.0 50542.0 50543.0 50544.0 50545.0 50546.0 50547.0 50548.0 50549.0 50550.0 50551.0 50552.0 50553.0 50554.0 50555.0 50556.0 50557.0 50558.0 50559.0 50560.0 50561.0 50562.0 50563.0 50564.0 50565.0 50566.0 50567.0 50568.0 50569.0 50570.0 50571.0 50572.0 50573.0 50574.0 50575.0 50576.0 50577.0 50578.0 50579.0 50580.0 50581.0 50582.0 50583.0 50584.0 50585.0 50586.0 50587.0 50588.0 50589.0 50590.0 50591.0 50592.0 50593.0 50594.0 50595.0 50596.0 50597.0 50598.0 50599.0 50600.0 50601.0 50602.0 50603.0 50604.0 50605.0 50606.0 50607.0 50608.0 50609.0 50610.0 50611.0 50612.0 50613.0 50614.0 50615.0 50616.0 50617.0 50618.0 50619.0 50620.0 50621.0 50622.0 50623.0 50624.0 50625.0 50626.0 50627.0 50628.0 50629.0 50630.0 50631.0 50632.0 50633.0 50634.0 50635.0 50636.0 50637.0 50638.0 50639.0 50640.0 50641.0 50642.0 50643.0 50644.0 50645.0 50646.0 50647.0 50648.0 50649.0 50650.0 50651.0 50652.0 50653.0 50654.0 50655.0 50656.0 50657.0 50658.0 50659.0 50660.0 50661.0 50662.0 50663.0 50664.0 50665.0 50666.0 50667.0 50668.0 50669.0 50670.0 50671.0 50672.0 50673.0 50674.0 50675.0 50676.0 50677.0 50678.0 50679.0 50680.0 50681.0 50682.0 50683.0 50684.0 50685.0 50686.0 50687.0 50688.0 50689.0 50690.0 50691.0 50692.0 50693.0 50694.0 50695.0 50696.0 50697.0 50698.0 50699.0 50700.0 50701.0 50702.0 50703.0 50704.0 50705.0 50706.0 50707.0 50708.0 50709.0 50710.0 50711.0 50712.0 50713.0 50714.0 50715.0 50716.0 50717.0 50718.0 50719.0 50720.0 50721.0 50722.0 50723.0 50724.0 50725.0 50726.0 50727.0 50728.0 50729.0 50730.0 50731.0 50732.0 50733.0 50734.0 50735.0 50736.0 50737.0 50738.0 50739.0 50740.0 50741.0 50742.0 50743.0 50744.0 50745.0 50746.0 50747.0 50748.0 50749.0 50750.0 50751.0 50752.0 50753.0 50754.0 50755.0 50756.0 50757.0 50758.0 50759.0 50760.0 50761.0 50762.0 50763.0 50764.0 50765.0 50766.0 50767.0 50768.0 50769.0 50770.0 50771.0 50772.0 50773.0 50774.0 50775.0 50776.0 50777.0 50778.0 50779.0 50780.0 50781.0 50782.0 50783.0 50784.0 50785.0 50786.0 50787.0 50788.0 50789.0 50790.0 50791.0 50792.0 50793.0 50794.0 50795.0 50796.0 50797.0 50798.0 50799.0 50800.0 50801.0 50802.0 50803.0 50804.0 50805.0 50806.0 50807.0 50808.0 50809.0 50810.0 50811.0 50812.0 50813.0 50814.0 50815.0 50816.0 50817.0 50818.0 50819.0 50820.0 50821.0 50822.0 50823.0 50824.0 50825.0 50826.0 50827.0 50828.0 50829.0 50830.0 50831.0 50832.0 50833.0 50834.0 50835.0 50836.0 50837.0 50838.0 50839.0 50840.0 50841.0 50842.0 50843.0 50844.0 50845.0 50846.0 50847.0 50848.0 50849.0 50850.0 50851.0 50852.0 50853.0 50854.0 50855.0 50856.0 50857.0 50858.0 50859.0 50860.0 50861.0 50862.0 50863.0 50864.0 50865.0 50866.0 50867.0 50868.0 50869.0 50870.0 50871.0 50872.0 50873.0 50874.0 50875.0 50876.0 50877.0 50878.0 50879.0 50880.0 50881.0 50882.0 50883.0 50884.0 50885.0 50886.0 50887.0 50888.0 50889.0 50890.0 50891.0 50892.0 50893.0 50894.0 50895.0 50896.0 50897.0 50898.0 50899.0 50900.0 50901.0 50902.0 50903.0 50904.0 50905.0 50906.0 50907.0 50908.0 50909.0 50910.0 50911.0 50912.0 50913.0 50914.0 50915.0 50916.0 50917.0 50918.0 50919.0 50920.0 50921.0 50922.0 50923.0 50924.0 50925.0 50926.0 50927.0 50928.0 50929.0 50930.0 50931.0 50932.0 50933.0 50934.0 50935.0 50936.0 50937.0 50938.0 50939.0 50940.0 50941.0 50942.0 50943.0 50944.0 50945.0 50946.0 50947.0 50948.0 50949.0 50950.0 50951.0 50952.0 50953.0 50954.0 50955.0 50956.0 50957.0 50958.0 50959.0 50960.0 50961.0 50962.0 50963.0 50964.0 50965.0 50966.0 50967.0 50968.0 50969.0 50970.0 50971.0 50972.0 50973.0 50974.0 50975.0 50976.0 50977.0 50978.0 50979.0 50980.0 50981.0 50982.0 50983.0 50984.0 50985.0 50986.0 50987.0 50988.0 50989.0 50990.0 50991.0 50992.0 50993.0 50994.0 50995.0 50996.0 50997.0 50998.0 50999.0 51000.0 51001.0 51002.0 51003.0 51004.0 51005.0 51006.0 51007.0 51008.0 51009.0 51010.0 51011.0 51012.0 51013.0 51014.0 51015.0 51016.0 51017.0 51018.0 51019.0 51020.0 51021.0 51022.0 51023.0 51024.0 51025.0 51026.0 51027.0 51028.0 51029.0 51030.0 51031.0 51032.0 51033.0 51034.0 51035.0 51036.0 51037.0 51038.0 51039.0 51040.0 51041.0 51042.0 51043.0 51044.0 51045.0 51046.0 51047.0 51048.0 51049.0 51050.0 51051.0 51052.0 51053.0 51054.0 51055.0 51056.0 51057.0 51058.0 51059.0 51060.0 51061.0 51062.0 51063.0 51064.0 51065.0 51066.0 51067.0 51068.0 51069.0 51070.0 51071.0 51072.0 51073.0 51074.0 51075.0 51076.0 51077.0 51078.0 51079.0 51080.0 51081.0 51082.0 51083.0 51084.0 51085.0 51086.0 51087.0 51088.0 51089.0 51090.0 51091.0 51092.0 51093.0 51094.0 51095.0 51096.0 51097.0 51098.0 51099.0 51100.0 51101.0 51102.0 51103.0 51104.0 51105.0 51106.0 51107.0 51108.0 51109.0 51110.0 51111.0 51112.0 51113.0 51114.0 51115.0 51116.0 51117.0 51118.0 51119.0 51120.0 51121.0 51122.0 51123.0 51124.0 51125.0 51126.0 51127.0 51128.0 51129.0 51130.0 51131.0 51132.0 51133.0 51134.0 51135.0 51136.0 51137.0 51138.0 51139.0 51140.0 51141.0 51142.0 51143.0 51144.0 51145.0 51146.0 51147.0 51148.0 51149.0 51150.0 51151.0 51152.0 51153.0 51154.0 51155.0 51156.0 51157.0 51158.0 51159.0 51160.0 51161.0 51162.0 51163.0 51164.0 51165.0 51166.0 51167.0 51168.0 51169.0 51170.0 51171.0 51172.0 51173.0 51174.0 51175.0 51176.0 51177.0 51178.0 51179.0 51180.0 51181.0 51182.0 51183.0 51184.0 51185.0 51186.0 51187.0 51188.0 51189.0 51190.0 51191.0 51192.0 51193.0 51194.0 51195.0 51196.0 51197.0 51198.0 51199.0 51200.0 51201.0 51202.0 51203.0 51204.0 51205.0 51206.0 51207.0 51208.0 51209.0 51210.0 51211.0 51212.0 51213.0 51214.0 51215.0 51216.0 51217.0 51218.0 51219.0 51220.0 51221.0 51222.0 51223.0 51224.0 51225.0 51226.0 51227.0 51228.0 51229.0 51230.0 51231.0 51232.0 51233.0 51234.0 51235.0 51236.0 51237.0 51238.0 51239.0 51240.0 51241.0 51242.0 51243.0 51244.0 51245.0 51246.0 51247.0 51248.0 51249.0 51250.0 51251.0 51252.0 51253.0 51254.0 51255.0 51256.0 51257.0 51258.0 51259.0 51260.0 51261.0 51262.0 51263.0 51264.0 51265.0 51266.0 51267.0 51268.0 51269.0 51270.0 51271.0 51272.0 51273.0 51274.0 51275.0 51276.0 51277.0 51278.0 51279.0 51280.0 51281.0 51282.0 51283.0 51284.0 51285.0 51286.0 51287.0 51288.0 51289.0 51290.0 51291.0 51292.0 51293.0 51294.0 51295.0 51296.0 51297.0 51298.0 51299.0 51300.0 51301.0 51302.0 51303.0 51304.0 51305.0 51306.0 51307.0 51308.0 51309.0 51310.0 51311.0 51312.0 51313.0 51314.0 51315.0 51316.0 51317.0 51318.0 51319.0 51320.0 51321.0 51322.0 51323.0 51324.0 51325.0 51326.0 51327.0 51328.0 51329.0 51330.0 51331.0 51332.0 51333.0 51334.0 51335.0 51336.0 51337.0 51338.0 51339.0 51340.0 51341.0 51342.0 51343.0 51344.0 51345.0 51346.0 51347.0 51348.0 51349.0 51350.0 51351.0 51352.0 51353.0 51354.0 51355.0 51356.0 51357.0 51358.0 51359.0 51360.0 51361.0 51362.0 51363.0 51364.0 51365.0 51366.0 51367.0 51368.0 51369.0 51370.0 51371.0 51372.0 51373.0 51374.0 51375.0 51376.0 51377.0 51378.0 51379.0 51380.0 51381.0 51382.0 51383.0 51384.0 51385.0 51386.0 51387.0 51388.0 51389.0 51390.0 51391.0 51392.0 51393.0 51394.0 51395.0 51396.0 51397.0 51398.0 51399.0 51400.0 51401.0 51402.0 51403.0 51404.0 51405.0 51406.0 51407.0 51408.0 51409.0 51410.0 51411.0 51412.0 51413.0 51414.0 51415.0 51416.0 51417.0 51418.0 51419.0 51420.0 51421.0 51422.0 51423.0 51424.0 51425.0 51426.0 51427.0 51428.0 51429.0 51430.0 51431.0 51432.0 51433.0 51434.0 51435.0 51436.0 51437.0 51438.0 51439.0 51440.0 51441.0 51442.0 51443.0 51444.0 51445.0 51446.0 51447.0 51448.0 51449.0 51450.0 51451.0 51452.0 51453.0 51454.0 51455.0 51456.0 51457.0 51458.0 51459.0 51460.0 51461.0 51462.0 51463.0 51464.0 51465.0 51466.0 51467.0 51468.0 51469.0 51470.0 51471.0 51472.0 51473.0 51474.0 51475.0 51476.0 51477.0 51478.0 51479.0 51480.0 51481.0 51482.0 51483.0 51484.0 51485.0 51486.0 51487.0 51488.0 51489.0 51490.0 51491.0 51492.0 51493.0 51494.0 51495.0 51496.0 51497.0 51498.0 51499.0 51500.0 51501.0 51502.0 51503.0 51504.0 51505.0 51506.0 51507.0 51508.0 51509.0 51510.0 51511.0 51512.0 51513.0 51514.0 51515.0 51516.0 51517.0 51518.0 51519.0 51520.0 51521.0 51522.0 51523.0 51524.0 51525.0 51526.0 51527.0 51528.0 51529.0 51530.0 51531.0 51532.0 51533.0 51534.0 51535.0 51536.0 51537.0 51538.0 51539.0 51540.0 51541.0 51542.0 51543.0 51544.0 51545.0 51546.0 51547.0 51548.0 51549.0 51550.0 51551.0 51552.0 51553.0 51554.0 51555.0 51556.0 51557.0 51558.0 51559.0 51560.0 51561.0 51562.0 51563.0 51564.0 51565.0 51566.0 51567.0 51568.0 51569.0 51570.0 51571.0 51572.0 51573.0 51574.0 51575.0 51576.0 51577.0 51578.0 51579.0 51580.0 51581.0 51582.0 51583.0 51584.0 51585.0 51586.0 51587.0 51588.0 51589.0 51590.0 51591.0 51592.0 51593.0 51594.0 51595.0 51596.0 51597.0 51598.0 51599.0 51600.0 51601.0 51602.0 51603.0 51604.0 51605.0 51606.0 51607.0 51608.0 51609.0 51610.0 51611.0 51612.0 51613.0 51614.0 51615.0 51616.0 51617.0 51618.0 51619.0 51620.0 51621.0 51622.0 51623.0 51624.0 51625.0 51626.0 51627.0 51628.0 51629.0 51630.0 51631.0 51632.0 51633.0 51634.0 51635.0 51636.0 51637.0 51638.0 51639.0 51640.0 51641.0 51642.0 51643.0 51644.0 51645.0 51646.0 51647.0 51648.0 51649.0 51650.0 51651.0 51652.0 51653.0 51654.0 51655.0 51656.0 51657.0 51658.0 51659.0 51660.0 51661.0 51662.0 51663.0 51664.0 51665.0 51666.0 51667.0 51668.0 51669.0 51670.0 51671.0 51672.0 51673.0 51674.0 51675.0 51676.0 51677.0 51678.0 51679.0 51680.0 51681.0 51682.0 51683.0 51684.0 51685.0 51686.0 51687.0 51688.0 51689.0 51690.0 51691.0 51692.0 51693.0 51694.0 51695.0 51696.0 51697.0 51698.0 51699.0 51700.0 51701.0 51702.0 51703.0 51704.0 51705.0 51706.0 51707.0 51708.0 51709.0 51710.0 51711.0 51712.0 51713.0 51714.0 51715.0 51716.0 51717.0 51718.0 51719.0 51720.0 51721.0 51722.0 51723.0 51724.0 51725.0 51726.0 51727.0 51728.0 51729.0 51730.0 51731.0 51732.0 51733.0 51734.0 51735.0 51736.0 51737.0 51738.0 51739.0 51740.0 51741.0 51742.0 51743.0 51744.0 51745.0 51746.0 51747.0 51748.0 51749.0 51750.0 51751.0 51752.0 51753.0 51754.0 51755.0 51756.0 51757.0 51758.0 51759.0 51760.0 51761.0 51762.0 51763.0 51764.0 51765.0 51766.0 51767.0 51768.0 51769.0 51770.0 51771.0 51772.0 51773.0 51774.0 51775.0 51776.0 51777.0 51778.0 51779.0 51780.0 51781.0 51782.0 51783.0 51784.0 51785.0 51786.0 51787.0 51788.0 51789.0 51790.0 51791.0 51792.0 51793.0 51794.0 51795.0 51796.0 51797.0 51798.0 51799.0 51800.0 51801.0 51802.0 51803.0 51804.0 51805.0 51806.0 51807.0 51808.0 51809.0 51810.0 51811.0 51812.0 51813.0 51814.0 51815.0 51816.0 51817.0 51818.0 51819.0 51820.0 51821.0 51822.0 51823.0 51824.0 51825.0 51826.0 51827.0 51828.0 51829.0 51830.0 51831.0 51832.0 51833.0 51834.0 51835.0 51836.0 51837.0 51838.0 51839.0 51840.0 51841.0 51842.0 51843.0 51844.0 51845.0 51846.0 51847.0 51848.0 51849.0 51850.0 51851.0 51852.0 51853.0 51854.0 51855.0 51856.0 51857.0 51858.0 51859.0 51860.0 51861.0 51862.0 51863.0 51864.0 51865.0 51866.0 51867.0 51868.0 51869.0 51870.0 51871.0 51872.0 51873.0 51874.0 51875.0 51876.0 51877.0 51878.0 51879.0 51880.0 51881.0 51882.0 51883.0 51884.0 51885.0 51886.0 51887.0 51888.0 51889.0 51890.0 51891.0 51892.0 51893.0 51894.0 51895.0 51896.0 51897.0 51898.0 51899.0 51900.0 51901.0 51902.0 51903.0 51904.0 51905.0 51906.0 51907.0 51908.0 51909.0 51910.0 51911.0 51912.0 51913.0 51914.0 51915.0 51916.0 51917.0 51918.0 51919.0 51920.0 51921.0 51922.0 51923.0 51924.0 51925.0 51926.0 51927.0 51928.0 51929.0 51930.0 51931.0 51932.0 51933.0 51934.0 51935.0 51936.0 51937.0 51938.0 51939.0 51940.0 51941.0 51942.0 51943.0 51944.0 51945.0 51946.0 51947.0 51948.0 51949.0 51950.0 51951.0 51952.0 51953.0 51954.0 51955.0 51956.0 51957.0 51958.0 51959.0 51960.0 51961.0 51962.0 51963.0 51964.0 51965.0 51966.0 51967.0 51968.0 51969.0 51970.0 51971.0 51972.0 51973.0 51974.0 51975.0 51976.0 51977.0 51978.0 51979.0 51980.0 51981.0 51982.0 51983.0 51984.0 51985.0 51986.0 51987.0 51988.0 51989.0 51990.0 51991.0 51992.0 51993.0 51994.0 51995.0 51996.0 51997.0 51998.0 51999.0 52000.0 52001.0 52002.0 52003.0 52004.0 52005.0 52006.0 52007.0 52008.0 52009.0 52010.0 52011.0 52012.0 52013.0 52014.0 52015.0 52016.0 52017.0 52018.0 52019.0 52020.0 52021.0 52022.0 52023.0 52024.0 52025.0 52026.0 52027.0 52028.0 52029.0 52030.0 52031.0 52032.0 52033.0 52034.0 52035.0 52036.0 52037.0 52038.0 52039.0 52040.0 52041.0 52042.0 52043.0 52044.0 52045.0 52046.0 52047.0 52048.0 52049.0 52050.0 52051.0 52052.0 52053.0 52054.0 52055.0 52056.0 52057.0 52058.0 52059.0 52060.0 52061.0 52062.0 52063.0 52064.0 52065.0 52066.0 52067.0 52068.0 52069.0 52070.0 52071.0 52072.0 52073.0 52074.0 52075.0 52076.0 52077.0 52078.0 52079.0 52080.0 52081.0 52082.0 52083.0 52084.0 52085.0 52086.0 52087.0 52088.0 52089.0 52090.0 52091.0 52092.0 52093.0 52094.0 52095.0 52096.0 52097.0 52098.0 52099.0 52100.0 52101.0 52102.0 52103.0 52104.0 52105.0 52106.0 52107.0 52108.0 52109.0 52110.0 52111.0 52112.0 52113.0 52114.0 52115.0 52116.0 52117.0 52118.0 52119.0 52120.0 52121.0 52122.0 52123.0 52124.0 52125.0 52126.0 52127.0 52128.0 52129.0 52130.0 52131.0 52132.0 52133.0 52134.0 52135.0 52136.0 52137.0 52138.0 52139.0 52140.0 52141.0 52142.0 52143.0 52144.0 52145.0 52146.0 52147.0 52148.0 52149.0 52150.0 52151.0 52152.0 52153.0 52154.0 52155.0 52156.0 52157.0 52158.0 52159.0 52160.0 52161.0 52162.0 52163.0 52164.0 52165.0 52166.0 52167.0 52168.0 52169.0 52170.0 52171.0 52172.0 52173.0 52174.0 52175.0 52176.0 52177.0 52178.0 52179.0 52180.0 52181.0 52182.0 52183.0 52184.0 52185.0 52186.0 52187.0 52188.0 52189.0 52190.0 52191.0 52192.0 52193.0 52194.0 52195.0 52196.0 52197.0 52198.0 52199.0 52200.0 52201.0 52202.0 52203.0 52204.0 52205.0 52206.0 52207.0 52208.0 52209.0 52210.0 52211.0 52212.0 52213.0 52214.0 52215.0 52216.0 52217.0 52218.0 52219.0 52220.0 52221.0 52222.0 52223.0 52224.0 52225.0 52226.0 52227.0 52228.0 52229.0 52230.0 52231.0 52232.0 52233.0 52234.0 52235.0 52236.0 52237.0 52238.0 52239.0 52240.0 52241.0 52242.0 52243.0 52244.0 52245.0 52246.0 52247.0 52248.0 52249.0 52250.0 52251.0 52252.0 52253.0 52254.0 52255.0 52256.0 52257.0 52258.0 52259.0 52260.0 52261.0 52262.0 52263.0 52264.0 52265.0 52266.0 52267.0 52268.0 52269.0 52270.0 52271.0 52272.0 52273.0 52274.0 52275.0 52276.0 52277.0 52278.0 52279.0 52280.0 52281.0 52282.0 52283.0 52284.0 52285.0 52286.0 52287.0 52288.0 52289.0 52290.0 52291.0 52292.0 52293.0 52294.0 52295.0 52296.0 52297.0 52298.0 52299.0 52300.0 52301.0 52302.0 52303.0 52304.0 52305.0 52306.0 52307.0 52308.0 52309.0 52310.0 52311.0 52312.0 52313.0 52314.0 52315.0 52316.0 52317.0 52318.0 52319.0 52320.0 52321.0 52322.0 52323.0 52324.0 52325.0 52326.0 52327.0 52328.0 52329.0 52330.0 52331.0 52332.0 52333.0 52334.0 52335.0 52336.0 52337.0 52338.0 52339.0 52340.0 52341.0 52342.0 52343.0 52344.0 52345.0 52346.0 52347.0 52348.0 52349.0 52350.0 52351.0 52352.0 52353.0 52354.0 52355.0 52356.0 52357.0 52358.0 52359.0 52360.0 52361.0 52362.0 52363.0 52364.0 52365.0 52366.0 52367.0 52368.0 52369.0 52370.0 52371.0 52372.0 52373.0 52374.0 52375.0 52376.0 52377.0 52378.0 52379.0 52380.0 52381.0 52382.0 52383.0 52384.0 52385.0 52386.0 52387.0 52388.0 52389.0 52390.0 52391.0 52392.0 52393.0 52394.0 52395.0 52396.0 52397.0 52398.0 52399.0 52400.0 52401.0 52402.0 52403.0 52404.0 52405.0 52406.0 52407.0 52408.0 52409.0 52410.0 52411.0 52412.0 52413.0 52414.0 52415.0 52416.0 52417.0 52418.0 52419.0 52420.0 52421.0 52422.0 52423.0 52424.0 52425.0 52426.0 52427.0 52428.0 52429.0 52430.0 52431.0 52432.0 52433.0 52434.0 52435.0 52436.0 52437.0 52438.0 52439.0 52440.0 52441.0 52442.0 52443.0 52444.0 52445.0 52446.0 52447.0 52448.0 52449.0 52450.0 52451.0 52452.0 52453.0 52454.0 52455.0 52456.0 52457.0 52458.0 52459.0 52460.0 52461.0 52462.0 52463.0 52464.0 52465.0 52466.0 52467.0 52468.0 52469.0 52470.0 52471.0 52472.0 52473.0 52474.0 52475.0 52476.0 52477.0 52478.0 52479.0 52480.0 52481.0 52482.0 52483.0 52484.0 52485.0 52486.0 52487.0 52488.0 52489.0 52490.0 52491.0 52492.0 52493.0 52494.0 52495.0 52496.0 52497.0 52498.0 52499.0 52500.0 52501.0 52502.0 52503.0 52504.0 52505.0 52506.0 52507.0 52508.0 52509.0 52510.0 52511.0 52512.0 52513.0 52514.0 52515.0 52516.0 52517.0 52518.0 52519.0 52520.0 52521.0 52522.0 52523.0 52524.0 52525.0 52526.0 52527.0 52528.0 52529.0 52530.0 52531.0 52532.0 52533.0 52534.0 52535.0 52536.0 52537.0 52538.0 52539.0 52540.0 52541.0 52542.0 52543.0 52544.0 52545.0 52546.0 52547.0 52548.0 52549.0 52550.0 52551.0 52552.0 52553.0 52554.0 52555.0 52556.0 52557.0 52558.0 52559.0 52560.0 52561.0 52562.0 52563.0 52564.0 52565.0 52566.0 52567.0 52568.0 52569.0 52570.0 52571.0 52572.0 52573.0 52574.0 52575.0 52576.0 52577.0 52578.0 52579.0 52580.0 52581.0 52582.0 52583.0 52584.0 52585.0 52586.0 52587.0 52588.0 52589.0 52590.0 52591.0 52592.0 52593.0 52594.0 52595.0 52596.0 52597.0 52598.0 52599.0 52600.0 52601.0 52602.0 52603.0 52604.0 52605.0 52606.0 52607.0 52608.0 52609.0 52610.0 52611.0 52612.0 52613.0 52614.0 52615.0 52616.0 52617.0 52618.0 52619.0 52620.0 52621.0 52622.0 52623.0 52624.0 52625.0 52626.0 52627.0 52628.0 52629.0 52630.0 52631.0 52632.0 52633.0 52634.0 52635.0 52636.0 52637.0 52638.0 52639.0 52640.0 52641.0 52642.0 52643.0 52644.0 52645.0 52646.0 52647.0 52648.0 52649.0 52650.0 52651.0 52652.0 52653.0 52654.0 52655.0 52656.0 52657.0 52658.0 52659.0 52660.0 52661.0 52662.0 52663.0 52664.0 52665.0 52666.0 52667.0 52668.0 52669.0 52670.0 52671.0 52672.0 52673.0 52674.0 52675.0 52676.0 52677.0 52678.0 52679.0 52680.0 52681.0 52682.0 52683.0 52684.0 52685.0 52686.0 52687.0 52688.0 52689.0 52690.0 52691.0 52692.0 52693.0 52694.0 52695.0 52696.0 52697.0 52698.0 52699.0 52700.0 52701.0 52702.0 52703.0 52704.0 52705.0 52706.0 52707.0 52708.0 52709.0 52710.0 52711.0 52712.0 52713.0 52714.0 52715.0 52716.0 52717.0 52718.0 52719.0 52720.0 52721.0 52722.0 52723.0 52724.0 52725.0 52726.0 52727.0 52728.0 52729.0 52730.0 52731.0 52732.0 52733.0 52734.0 52735.0 52736.0 52737.0 52738.0 52739.0 52740.0 52741.0 52742.0 52743.0 52744.0 52745.0 52746.0 52747.0 52748.0 52749.0 52750.0 52751.0 52752.0 52753.0 52754.0 52755.0 52756.0 52757.0 52758.0 52759.0 52760.0 52761.0 52762.0 52763.0 52764.0 52765.0 52766.0 52767.0 52768.0 52769.0 52770.0 52771.0 52772.0 52773.0 52774.0 52775.0 52776.0 52777.0 52778.0 52779.0 52780.0 52781.0 52782.0 52783.0 52784.0 52785.0 52786.0 52787.0 52788.0 52789.0 52790.0 52791.0 52792.0 52793.0 52794.0 52795.0 52796.0 52797.0 52798.0 52799.0 52800.0 52801.0 52802.0 52803.0 52804.0 52805.0 52806.0 52807.0 52808.0 52809.0 52810.0 52811.0 52812.0 52813.0 52814.0 52815.0 52816.0 52817.0 52818.0 52819.0 52820.0 52821.0 52822.0 52823.0 52824.0 52825.0 52826.0 52827.0 52828.0 52829.0 52830.0 52831.0 52832.0 52833.0 52834.0 52835.0 52836.0 52837.0 52838.0 52839.0 52840.0 52841.0 52842.0 52843.0 52844.0 52845.0 52846.0 52847.0 52848.0 52849.0 52850.0 52851.0 52852.0 52853.0 52854.0 52855.0 52856.0 52857.0 52858.0 52859.0 52860.0 52861.0 52862.0 52863.0 52864.0 52865.0 52866.0 52867.0 52868.0 52869.0 52870.0 52871.0 52872.0 52873.0 52874.0 52875.0 52876.0 52877.0 52878.0 52879.0 52880.0 52881.0 52882.0 52883.0 52884.0 52885.0 52886.0 52887.0 52888.0 52889.0 52890.0 52891.0 52892.0 52893.0 52894.0 52895.0 52896.0 52897.0 52898.0 52899.0 52900.0 52901.0 52902.0 52903.0 52904.0 52905.0 52906.0 52907.0 52908.0 52909.0 52910.0 52911.0 52912.0 52913.0 52914.0 52915.0 52916.0 52917.0 52918.0 52919.0 52920.0 52921.0 52922.0 52923.0 52924.0 52925.0 52926.0 52927.0 52928.0 52929.0 52930.0 52931.0 52932.0 52933.0 52934.0 52935.0 52936.0 52937.0 52938.0 52939.0 52940.0 52941.0 52942.0 52943.0 52944.0 52945.0 52946.0 52947.0 52948.0 52949.0 52950.0 52951.0 52952.0 52953.0 52954.0 52955.0 52956.0 52957.0 52958.0 52959.0 52960.0 52961.0 52962.0 52963.0 52964.0 52965.0 52966.0 52967.0 52968.0 52969.0 52970.0 52971.0 52972.0 52973.0 52974.0 52975.0 52976.0 52977.0 52978.0 52979.0 52980.0 52981.0 52982.0 52983.0 52984.0 52985.0 52986.0 52987.0 52988.0 52989.0 52990.0 52991.0 52992.0 52993.0 52994.0 52995.0 52996.0 52997.0 52998.0 52999.0 53000.0 53001.0 53002.0 53003.0 53004.0 53005.0 53006.0 53007.0 53008.0 53009.0 53010.0 53011.0 53012.0 53013.0 53014.0 53015.0 53016.0 53017.0 53018.0 53019.0 53020.0 53021.0 53022.0 53023.0 53024.0 53025.0 53026.0 53027.0 53028.0 53029.0 53030.0 53031.0 53032.0 53033.0 53034.0 53035.0 53036.0 53037.0 53038.0 53039.0 53040.0 53041.0 53042.0 53043.0 53044.0 53045.0 53046.0 53047.0 53048.0 53049.0 53050.0 53051.0 53052.0 53053.0 53054.0 53055.0 53056.0 53057.0 53058.0 53059.0 53060.0 53061.0 53062.0 53063.0 53064.0 53065.0 53066.0 53067.0 53068.0 53069.0 53070.0 53071.0 53072.0 53073.0 53074.0 53075.0 53076.0 53077.0 53078.0 53079.0 53080.0 53081.0 53082.0 53083.0 53084.0 53085.0 53086.0 53087.0 53088.0 53089.0 53090.0 53091.0 53092.0 53093.0 53094.0 53095.0 53096.0 53097.0 53098.0 53099.0 53100.0 53101.0 53102.0 53103.0 53104.0 53105.0 53106.0 53107.0 53108.0 53109.0 53110.0 53111.0 53112.0 53113.0 53114.0 53115.0 53116.0 53117.0 53118.0 53119.0 53120.0 53121.0 53122.0 53123.0 53124.0 53125.0 53126.0 53127.0 53128.0 53129.0 53130.0 53131.0 53132.0 53133.0 53134.0 53135.0 53136.0 53137.0 53138.0 53139.0 53140.0 53141.0 53142.0 53143.0 53144.0 53145.0 53146.0 53147.0 53148.0 53149.0 53150.0 53151.0 53152.0 53153.0 53154.0 53155.0 53156.0 53157.0 53158.0 53159.0 53160.0 53161.0 53162.0 53163.0 53164.0 53165.0 53166.0 53167.0 53168.0 53169.0 53170.0 53171.0 53172.0 53173.0 53174.0 53175.0 53176.0 53177.0 53178.0 53179.0 53180.0 53181.0 53182.0 53183.0 53184.0 53185.0 53186.0 53187.0 53188.0 53189.0 53190.0 53191.0 53192.0 53193.0 53194.0 53195.0 53196.0 53197.0 53198.0 53199.0 53200.0 53201.0 53202.0 53203.0 53204.0 53205.0 53206.0 53207.0 53208.0 53209.0 53210.0 53211.0 53212.0 53213.0 53214.0 53215.0 53216.0 53217.0 53218.0 53219.0 53220.0 53221.0 53222.0 53223.0 53224.0 53225.0 53226.0 53227.0 53228.0 53229.0 53230.0 53231.0 53232.0 53233.0 53234.0 53235.0 53236.0 53237.0 53238.0 53239.0 53240.0 53241.0 53242.0 53243.0 53244.0 53245.0 53246.0 53247.0 53248.0 53249.0 53250.0 53251.0 53252.0 53253.0 53254.0 53255.0 53256.0 53257.0 53258.0 53259.0 53260.0 53261.0 53262.0 53263.0 53264.0 53265.0 53266.0 53267.0 53268.0 53269.0 53270.0 53271.0 53272.0 53273.0 53274.0 53275.0 53276.0 53277.0 53278.0 53279.0 53280.0 53281.0 53282.0 53283.0 53284.0 53285.0 53286.0 53287.0 53288.0 53289.0 53290.0 53291.0 53292.0 53293.0 53294.0 53295.0 53296.0 53297.0 53298.0 53299.0 53300.0 53301.0 53302.0 53303.0 53304.0 53305.0 53306.0 53307.0 53308.0 53309.0 53310.0 53311.0 53312.0 53313.0 53314.0 53315.0 53316.0 53317.0 53318.0 53319.0 53320.0 53321.0 53322.0 53323.0 53324.0 53325.0 53326.0 53327.0 53328.0 53329.0 53330.0 53331.0 53332.0 53333.0 53334.0 53335.0 53336.0 53337.0 53338.0 53339.0 53340.0 53341.0 53342.0 53343.0 53344.0 53345.0 53346.0 53347.0 53348.0 53349.0 53350.0 53351.0 53352.0 53353.0 53354.0 53355.0 53356.0 53357.0 53358.0 53359.0 53360.0 53361.0 53362.0 53363.0 53364.0 53365.0 53366.0 53367.0 53368.0 53369.0 53370.0 53371.0 53372.0 53373.0 53374.0 53375.0 53376.0 53377.0 53378.0 53379.0 53380.0 53381.0 53382.0 53383.0 53384.0 53385.0 53386.0 53387.0 53388.0 53389.0 53390.0 53391.0 53392.0 53393.0 53394.0 53395.0 53396.0 53397.0 53398.0 53399.0 53400.0 53401.0 53402.0 53403.0 53404.0 53405.0 53406.0 53407.0 53408.0 53409.0 53410.0 53411.0 53412.0 53413.0 53414.0 53415.0 53416.0 53417.0 53418.0 53419.0 53420.0 53421.0 53422.0 53423.0 53424.0 53425.0 53426.0 53427.0 53428.0 53429.0 53430.0 53431.0 53432.0 53433.0 53434.0 53435.0 53436.0 53437.0 53438.0 53439.0 53440.0 53441.0 53442.0 53443.0 53444.0 53445.0 53446.0 53447.0 53448.0 53449.0 53450.0 53451.0 53452.0 53453.0 53454.0 53455.0 53456.0 53457.0 53458.0 53459.0 53460.0 53461.0 53462.0 53463.0 53464.0 53465.0 53466.0 53467.0 53468.0 53469.0 53470.0 53471.0 53472.0 53473.0 53474.0 53475.0 53476.0 53477.0 53478.0 53479.0 53480.0 53481.0 53482.0 53483.0 53484.0 53485.0 53486.0 53487.0 53488.0 53489.0 53490.0 53491.0 53492.0 53493.0 53494.0 53495.0 53496.0 53497.0 53498.0 53499.0 53500.0 53501.0 53502.0 53503.0 53504.0 53505.0 53506.0 53507.0 53508.0 53509.0 53510.0 53511.0 53512.0 53513.0 53514.0 53515.0 53516.0 53517.0 53518.0 53519.0 53520.0 53521.0 53522.0 53523.0 53524.0 53525.0 53526.0 53527.0 53528.0 53529.0 53530.0 53531.0 53532.0 53533.0 53534.0 53535.0 53536.0 53537.0 53538.0 53539.0 53540.0 53541.0 53542.0 53543.0 53544.0 53545.0 53546.0 53547.0 53548.0 53549.0 53550.0 53551.0 53552.0 53553.0 53554.0 53555.0 53556.0 53557.0 53558.0 53559.0 53560.0 53561.0 53562.0 53563.0 53564.0 53565.0 53566.0 53567.0 53568.0 53569.0 53570.0 53571.0 53572.0 53573.0 53574.0 53575.0 53576.0 53577.0 53578.0 53579.0 53580.0 53581.0 53582.0 53583.0 53584.0 53585.0 53586.0 53587.0 53588.0 53589.0 53590.0 53591.0 53592.0 53593.0 53594.0 53595.0 53596.0 53597.0 53598.0 53599.0 53600.0 53601.0 53602.0 53603.0 53604.0 53605.0 53606.0 53607.0 53608.0 53609.0 53610.0 53611.0 53612.0 53613.0 53614.0 53615.0 53616.0 53617.0 53618.0 53619.0 53620.0 53621.0 53622.0 53623.0 53624.0 53625.0 53626.0 53627.0 53628.0 53629.0 53630.0 53631.0 53632.0 53633.0 53634.0 53635.0 53636.0 53637.0 53638.0 53639.0 53640.0 53641.0 53642.0 53643.0 53644.0 53645.0 53646.0 53647.0 53648.0 53649.0 53650.0 53651.0 53652.0 53653.0 53654.0 53655.0 53656.0 53657.0 53658.0 53659.0 53660.0 53661.0 53662.0 53663.0 53664.0 53665.0 53666.0 53667.0 53668.0 53669.0 53670.0 53671.0 53672.0 53673.0 53674.0 53675.0 53676.0 53677.0 53678.0 53679.0 53680.0 53681.0 53682.0 53683.0 53684.0 53685.0 53686.0 53687.0 53688.0 53689.0 53690.0 53691.0 53692.0 53693.0 53694.0 53695.0 53696.0 53697.0 53698.0 53699.0 53700.0 53701.0 53702.0 53703.0 53704.0 53705.0 53706.0 53707.0 53708.0 53709.0 53710.0 53711.0 53712.0 53713.0 53714.0 53715.0 53716.0 53717.0 53718.0 53719.0 53720.0 53721.0 53722.0 53723.0 53724.0 53725.0 53726.0 53727.0 53728.0 53729.0 53730.0 53731.0 53732.0 53733.0 53734.0 53735.0 53736.0 53737.0 53738.0 53739.0 53740.0 53741.0 53742.0 53743.0 53744.0 53745.0 53746.0 53747.0 53748.0 53749.0 53750.0 53751.0 53752.0 53753.0 53754.0 53755.0 53756.0 53757.0 53758.0 53759.0 53760.0 53761.0 53762.0 53763.0 53764.0 53765.0 53766.0 53767.0 53768.0 53769.0 53770.0 53771.0 53772.0 53773.0 53774.0 53775.0 53776.0 53777.0 53778.0 53779.0 53780.0 53781.0 53782.0 53783.0 53784.0 53785.0 53786.0 53787.0 53788.0 53789.0 53790.0 53791.0 53792.0 53793.0 53794.0 53795.0 53796.0 53797.0 53798.0 53799.0 53800.0 53801.0 53802.0 53803.0 53804.0 53805.0 53806.0 53807.0 53808.0 53809.0 53810.0 53811.0 53812.0 53813.0 53814.0 53815.0 53816.0 53817.0 53818.0 53819.0 53820.0 53821.0 53822.0 53823.0 53824.0 53825.0 53826.0 53827.0 53828.0 53829.0 53830.0 53831.0 53832.0 53833.0 53834.0 53835.0 53836.0 53837.0 53838.0 53839.0 53840.0 53841.0 53842.0 53843.0 53844.0 53845.0 53846.0 53847.0 53848.0 53849.0 53850.0 53851.0 53852.0 53853.0 53854.0 53855.0 53856.0 53857.0 53858.0 53859.0 53860.0 53861.0 53862.0 53863.0 53864.0 53865.0 53866.0 53867.0 53868.0 53869.0 53870.0 53871.0 53872.0 53873.0 53874.0 53875.0 53876.0 53877.0 53878.0 53879.0 53880.0 53881.0 53882.0 53883.0 53884.0 53885.0 53886.0 53887.0 53888.0 53889.0 53890.0 53891.0 53892.0 53893.0 53894.0 53895.0 53896.0 53897.0 53898.0 53899.0 53900.0 53901.0 53902.0 53903.0 53904.0 53905.0 53906.0 53907.0 53908.0 53909.0 53910.0 53911.0 53912.0 53913.0 53914.0 53915.0 53916.0 53917.0 53918.0 53919.0 53920.0 53921.0 53922.0 53923.0 53924.0 53925.0 53926.0 53927.0 53928.0 53929.0 53930.0 53931.0 53932.0 53933.0 53934.0 53935.0 53936.0 53937.0 53938.0 53939.0 53940.0 53941.0 53942.0 53943.0 53944.0 53945.0 53946.0 53947.0 53948.0 53949.0 53950.0 53951.0 53952.0 53953.0 53954.0 53955.0 53956.0 53957.0 53958.0 53959.0 53960.0 53961.0 53962.0 53963.0 53964.0 53965.0 53966.0 53967.0 53968.0 53969.0 53970.0 53971.0 53972.0 53973.0 53974.0 53975.0 53976.0 53977.0 53978.0 53979.0 53980.0 53981.0 53982.0 53983.0 53984.0 53985.0 53986.0 53987.0 53988.0 53989.0 53990.0 53991.0 53992.0 53993.0 53994.0 53995.0 53996.0 53997.0 53998.0 53999.0 54000.0 54001.0 54002.0 54003.0 54004.0 54005.0 54006.0 54007.0 54008.0 54009.0 54010.0 54011.0 54012.0 54013.0 54014.0 54015.0 54016.0 54017.0 54018.0 54019.0 54020.0 54021.0 54022.0 54023.0 54024.0 54025.0 54026.0 54027.0 54028.0 54029.0 54030.0 54031.0 54032.0 54033.0 54034.0 54035.0 54036.0 54037.0 54038.0 54039.0 54040.0 54041.0 54042.0 54043.0 54044.0 54045.0 54046.0 54047.0 54048.0 54049.0 54050.0 54051.0 54052.0 54053.0 54054.0 54055.0 54056.0 54057.0 54058.0 54059.0 54060.0 54061.0 54062.0 54063.0 54064.0 54065.0 54066.0 54067.0 54068.0 54069.0 54070.0 54071.0 54072.0 54073.0 54074.0 54075.0 54076.0 54077.0 54078.0 54079.0 54080.0 54081.0 54082.0 54083.0 54084.0 54085.0 54086.0 54087.0 54088.0 54089.0 54090.0 54091.0 54092.0 54093.0 54094.0 54095.0 54096.0 54097.0 54098.0 54099.0 54100.0 54101.0 54102.0 54103.0 54104.0 54105.0 54106.0 54107.0 54108.0 54109.0 54110.0 54111.0 54112.0 54113.0 54114.0 54115.0 54116.0 54117.0 54118.0 54119.0 54120.0 54121.0 54122.0 54123.0 54124.0 54125.0 54126.0 54127.0 54128.0 54129.0 54130.0 54131.0 54132.0 54133.0 54134.0 54135.0 54136.0 54137.0 54138.0 54139.0 54140.0 54141.0 54142.0 54143.0 54144.0 54145.0 54146.0 54147.0 54148.0 54149.0 54150.0 54151.0 54152.0 54153.0 54154.0 54155.0 54156.0 54157.0 54158.0 54159.0 54160.0 54161.0 54162.0 54163.0 54164.0 54165.0 54166.0 54167.0 54168.0 54169.0 54170.0 54171.0 54172.0 54173.0 54174.0 54175.0 54176.0 54177.0 54178.0 54179.0 54180.0 54181.0 54182.0 54183.0 54184.0 54185.0 54186.0 54187.0 54188.0 54189.0 54190.0 54191.0 54192.0 54193.0 54194.0 54195.0 54196.0 54197.0 54198.0 54199.0 54200.0 54201.0 54202.0 54203.0 54204.0 54205.0 54206.0 54207.0 54208.0 54209.0 54210.0 54211.0 54212.0 54213.0 54214.0 54215.0 54216.0 54217.0 54218.0 54219.0 54220.0 54221.0 54222.0 54223.0 54224.0 54225.0 54226.0 54227.0 54228.0 54229.0 54230.0 54231.0 54232.0 54233.0 54234.0 54235.0 54236.0 54237.0 54238.0 54239.0 54240.0 54241.0 54242.0 54243.0 54244.0 54245.0 54246.0 54247.0 54248.0 54249.0 54250.0 54251.0 54252.0 54253.0 54254.0 54255.0 54256.0 54257.0 54258.0 54259.0 54260.0 54261.0 54262.0 54263.0 54264.0 54265.0 54266.0 54267.0 54268.0 54269.0 54270.0 54271.0 54272.0 54273.0 54274.0 54275.0 54276.0 54277.0 54278.0 54279.0 54280.0 54281.0 54282.0 54283.0 54284.0 54285.0 54286.0 54287.0 54288.0 54289.0 54290.0 54291.0 54292.0 54293.0 54294.0 54295.0 54296.0 54297.0 54298.0 54299.0 54300.0 54301.0 54302.0 54303.0 54304.0 54305.0 54306.0 54307.0 54308.0 54309.0 54310.0 54311.0 54312.0 54313.0 54314.0 54315.0 54316.0 54317.0 54318.0 54319.0 54320.0 54321.0 54322.0 54323.0 54324.0 54325.0 54326.0 54327.0 54328.0 54329.0 54330.0 54331.0 54332.0 54333.0 54334.0 54335.0 54336.0 54337.0 54338.0 54339.0 54340.0 54341.0 54342.0 54343.0 54344.0 54345.0 54346.0 54347.0 54348.0 54349.0 54350.0 54351.0 54352.0 54353.0 54354.0 54355.0 54356.0 54357.0 54358.0 54359.0 54360.0 54361.0 54362.0 54363.0 54364.0 54365.0 54366.0 54367.0 54368.0 54369.0 54370.0 54371.0 54372.0 54373.0 54374.0 54375.0 54376.0 54377.0 54378.0 54379.0 54380.0 54381.0 54382.0 54383.0 54384.0 54385.0 54386.0 54387.0 54388.0 54389.0 54390.0 54391.0 54392.0 54393.0 54394.0 54395.0 54396.0 54397.0 54398.0 54399.0 54400.0 54401.0 54402.0 54403.0 54404.0 54405.0 54406.0 54407.0 54408.0 54409.0 54410.0 54411.0 54412.0 54413.0 54414.0 54415.0 54416.0 54417.0 54418.0 54419.0 54420.0 54421.0 54422.0 54423.0 54424.0 54425.0 54426.0 54427.0 54428.0 54429.0 54430.0 54431.0 54432.0 54433.0 54434.0 54435.0 54436.0 54437.0 54438.0 54439.0 54440.0 54441.0 54442.0 54443.0 54444.0 54445.0 54446.0 54447.0 54448.0 54449.0 54450.0 54451.0 54452.0 54453.0 54454.0 54455.0 54456.0 54457.0 54458.0 54459.0 54460.0 54461.0 54462.0 54463.0 54464.0 54465.0 54466.0 54467.0 54468.0 54469.0 54470.0 54471.0 54472.0 54473.0 54474.0 54475.0 54476.0 54477.0 54478.0 54479.0 54480.0 54481.0 54482.0 54483.0 54484.0 54485.0 54486.0 54487.0 54488.0 54489.0 54490.0 54491.0 54492.0 54493.0 54494.0 54495.0 54496.0 54497.0 54498.0 54499.0 54500.0 54501.0 54502.0 54503.0 54504.0 54505.0 54506.0 54507.0 54508.0 54509.0 54510.0 54511.0 54512.0 54513.0 54514.0 54515.0 54516.0 54517.0 54518.0 54519.0 54520.0 54521.0 54522.0 54523.0 54524.0 54525.0 54526.0 54527.0 54528.0 54529.0 54530.0 54531.0 54532.0 54533.0 54534.0 54535.0 54536.0 54537.0 54538.0 54539.0 54540.0 54541.0 54542.0 54543.0 54544.0 54545.0 54546.0 54547.0 54548.0 54549.0 54550.0 54551.0 54552.0 54553.0 54554.0 54555.0 54556.0 54557.0 54558.0 54559.0 54560.0 54561.0 54562.0 54563.0 54564.0 54565.0 54566.0 54567.0 54568.0 54569.0 54570.0 54571.0 54572.0 54573.0 54574.0 54575.0 54576.0 54577.0 54578.0 54579.0 54580.0 54581.0 54582.0 54583.0 54584.0 54585.0 54586.0 54587.0 54588.0 54589.0 54590.0 54591.0 54592.0 54593.0 54594.0 54595.0 54596.0 54597.0 54598.0 54599.0 54600.0 54601.0 54602.0 54603.0 54604.0 54605.0 54606.0 54607.0 54608.0 54609.0 54610.0 54611.0 54612.0 54613.0 54614.0 54615.0 54616.0 54617.0 54618.0 54619.0 54620.0 54621.0 54622.0 54623.0 54624.0 54625.0 54626.0 54627.0 54628.0 54629.0 54630.0 54631.0 54632.0 54633.0 54634.0 54635.0 54636.0 54637.0 54638.0 54639.0 54640.0 54641.0 54642.0 54643.0 54644.0 54645.0 54646.0 54647.0 54648.0 54649.0 54650.0 54651.0 54652.0 54653.0 54654.0 54655.0 54656.0 54657.0 54658.0 54659.0 54660.0 54661.0 54662.0 54663.0 54664.0 54665.0 54666.0 54667.0 54668.0 54669.0 54670.0 54671.0 54672.0 54673.0 54674.0 54675.0 54676.0 54677.0 54678.0 54679.0 54680.0 54681.0 54682.0 54683.0 54684.0 54685.0 54686.0 54687.0 54688.0 54689.0 54690.0 54691.0 54692.0 54693.0 54694.0 54695.0 54696.0 54697.0 54698.0 54699.0 54700.0 54701.0 54702.0 54703.0 54704.0 54705.0 54706.0 54707.0 54708.0 54709.0 54710.0 54711.0 54712.0 54713.0 54714.0 54715.0 54716.0 54717.0 54718.0 54719.0 54720.0 54721.0 54722.0 54723.0 54724.0 54725.0 54726.0 54727.0 54728.0 54729.0 54730.0 54731.0 54732.0 54733.0 54734.0 54735.0 54736.0 54737.0 54738.0 54739.0 54740.0 54741.0 54742.0 54743.0 54744.0 54745.0 54746.0 54747.0 54748.0 54749.0 54750.0 54751.0 54752.0 54753.0 54754.0 54755.0 54756.0 54757.0 54758.0 54759.0 54760.0 54761.0 54762.0 54763.0 54764.0 54765.0 54766.0 54767.0 54768.0 54769.0 54770.0 54771.0 54772.0 54773.0 54774.0 54775.0 54776.0 54777.0 54778.0 54779.0 54780.0 54781.0 54782.0 54783.0 54784.0 54785.0 54786.0 54787.0 54788.0 54789.0 54790.0 54791.0 54792.0 54793.0 54794.0 54795.0 54796.0 54797.0 54798.0 54799.0 54800.0 54801.0 54802.0 54803.0 54804.0 54805.0 54806.0 54807.0 54808.0 54809.0 54810.0 54811.0 54812.0 54813.0 54814.0 54815.0 54816.0 54817.0 54818.0 54819.0 54820.0 54821.0 54822.0 54823.0 54824.0 54825.0 54826.0 54827.0 54828.0 54829.0 54830.0 54831.0 54832.0 54833.0 54834.0 54835.0 54836.0 54837.0 54838.0 54839.0 54840.0 54841.0 54842.0 54843.0 54844.0 54845.0 54846.0 54847.0 54848.0 54849.0 54850.0 54851.0 54852.0 54853.0 54854.0 54855.0 54856.0 54857.0 54858.0 54859.0 54860.0 54861.0 54862.0 54863.0 54864.0 54865.0 54866.0 54867.0 54868.0 54869.0 54870.0 54871.0 54872.0 54873.0 54874.0 54875.0 54876.0 54877.0 54878.0 54879.0 54880.0 54881.0 54882.0 54883.0 54884.0 54885.0 54886.0 54887.0 54888.0 54889.0 54890.0 54891.0 54892.0 54893.0 54894.0 54895.0 54896.0 54897.0 54898.0 54899.0 54900.0 54901.0 54902.0 54903.0 54904.0 54905.0 54906.0 54907.0 54908.0 54909.0 54910.0 54911.0 54912.0 54913.0 54914.0 54915.0 54916.0 54917.0 54918.0 54919.0 54920.0 54921.0 54922.0 54923.0 54924.0 54925.0 54926.0 54927.0 54928.0 54929.0 54930.0 54931.0 54932.0 54933.0 54934.0 54935.0 54936.0 54937.0 54938.0 54939.0 54940.0 54941.0 54942.0 54943.0 54944.0 54945.0 54946.0 54947.0 54948.0 54949.0 54950.0 54951.0 54952.0 54953.0 54954.0 54955.0 54956.0 54957.0 54958.0 54959.0 54960.0 54961.0 54962.0 54963.0 54964.0 54965.0 54966.0 54967.0 54968.0 54969.0 54970.0 54971.0 54972.0 54973.0 54974.0 54975.0 54976.0 54977.0 54978.0 54979.0 54980.0 54981.0 54982.0 54983.0 54984.0 54985.0 54986.0 54987.0 54988.0 54989.0 54990.0 54991.0 54992.0 54993.0 54994.0 54995.0 54996.0 54997.0 54998.0 54999.0 55000.0 55001.0 55002.0 55003.0 55004.0 55005.0 55006.0 55007.0 55008.0 55009.0 55010.0 55011.0 55012.0 55013.0 55014.0 55015.0 55016.0 55017.0 55018.0 55019.0 55020.0 55021.0 55022.0 55023.0 55024.0 55025.0 55026.0 55027.0 55028.0 55029.0 55030.0 55031.0 55032.0 55033.0 55034.0 55035.0 55036.0 55037.0 55038.0 55039.0 55040.0 55041.0 55042.0 55043.0 55044.0 55045.0 55046.0 55047.0 55048.0 55049.0 55050.0 55051.0 55052.0 55053.0 55054.0 55055.0 55056.0 55057.0 55058.0 55059.0 55060.0 55061.0 55062.0 55063.0 55064.0 55065.0 55066.0 55067.0 55068.0 55069.0 55070.0 55071.0 55072.0 55073.0 55074.0 55075.0 55076.0 55077.0 55078.0 55079.0 55080.0 55081.0 55082.0 55083.0 55084.0 55085.0 55086.0 55087.0 55088.0 55089.0 55090.0 55091.0 55092.0 55093.0 55094.0 55095.0 55096.0 55097.0 55098.0 55099.0 55100.0 55101.0 55102.0 55103.0 55104.0 55105.0 55106.0 55107.0 55108.0 55109.0 55110.0 55111.0 55112.0 55113.0 55114.0 55115.0 55116.0 55117.0 55118.0 55119.0 55120.0 55121.0 55122.0 55123.0 55124.0 55125.0 55126.0 55127.0 55128.0 55129.0 55130.0 55131.0 55132.0 55133.0 55134.0 55135.0 55136.0 55137.0 55138.0 55139.0 55140.0 55141.0 55142.0 55143.0 55144.0 55145.0 55146.0 55147.0 55148.0 55149.0 55150.0 55151.0 55152.0 55153.0 55154.0 55155.0 55156.0 55157.0 55158.0 55159.0 55160.0 55161.0 55162.0 55163.0 55164.0 55165.0 55166.0 55167.0 55168.0 55169.0 55170.0 55171.0 55172.0 55173.0 55174.0 55175.0 55176.0 55177.0 55178.0 55179.0 55180.0 55181.0 55182.0 55183.0 55184.0 55185.0 55186.0 55187.0 55188.0 55189.0 55190.0 55191.0 55192.0 55193.0 55194.0 55195.0 55196.0 55197.0 55198.0 55199.0 55200.0 55201.0 55202.0 55203.0 55204.0 55205.0 55206.0 55207.0 55208.0 55209.0 55210.0 55211.0 55212.0 55213.0 55214.0 55215.0 55216.0 55217.0 55218.0 55219.0 55220.0 55221.0 55222.0 55223.0 55224.0 55225.0 55226.0 55227.0 55228.0 55229.0 55230.0 55231.0 55232.0 55233.0 55234.0 55235.0 55236.0 55237.0 55238.0 55239.0 55240.0 55241.0 55242.0 55243.0 55244.0 55245.0 55246.0 55247.0 55248.0 55249.0 55250.0 55251.0 55252.0 55253.0 55254.0 55255.0 55256.0 55257.0 55258.0 55259.0 55260.0 55261.0 55262.0 55263.0 55264.0 55265.0 55266.0 55267.0 55268.0 55269.0 55270.0 55271.0 55272.0 55273.0 55274.0 55275.0 55276.0 55277.0 55278.0 55279.0 55280.0 55281.0 55282.0 55283.0 55284.0 55285.0 55286.0 55287.0 55288.0 55289.0 55290.0 55291.0 55292.0 55293.0 55294.0 55295.0 55296.0 55297.0 55298.0 55299.0 55300.0 55301.0 55302.0 55303.0 55304.0 55305.0 55306.0 55307.0 55308.0 55309.0 55310.0 55311.0 55312.0 55313.0 55314.0 55315.0 55316.0 55317.0 55318.0 55319.0 55320.0 55321.0 55322.0 55323.0 55324.0 55325.0 55326.0 55327.0 55328.0 55329.0 55330.0 55331.0 55332.0 55333.0 55334.0 55335.0 55336.0 55337.0 55338.0 55339.0 55340.0 55341.0 55342.0 55343.0 55344.0 55345.0 55346.0 55347.0 55348.0 55349.0 55350.0 55351.0 55352.0 55353.0 55354.0 55355.0 55356.0 55357.0 55358.0 55359.0 55360.0 55361.0 55362.0 55363.0 55364.0 55365.0 55366.0 55367.0 55368.0 55369.0 55370.0 55371.0 55372.0 55373.0 55374.0 55375.0 55376.0 55377.0 55378.0 55379.0 55380.0 55381.0 55382.0 55383.0 55384.0 55385.0 55386.0 55387.0 55388.0 55389.0 55390.0 55391.0 55392.0 55393.0 55394.0 55395.0 55396.0 55397.0 55398.0 55399.0 55400.0 55401.0 55402.0 55403.0 55404.0 55405.0 55406.0 55407.0 55408.0 55409.0 55410.0 55411.0 55412.0 55413.0 55414.0 55415.0 55416.0 55417.0 55418.0 55419.0 55420.0 55421.0 55422.0 55423.0 55424.0 55425.0 55426.0 55427.0 55428.0 55429.0 55430.0 55431.0 55432.0 55433.0 55434.0 55435.0 55436.0 55437.0 55438.0 55439.0 55440.0 55441.0 55442.0 55443.0 55444.0 55445.0 55446.0 55447.0 55448.0 55449.0 55450.0 55451.0 55452.0 55453.0 55454.0 55455.0 55456.0 55457.0 55458.0 55459.0 55460.0 55461.0 55462.0 55463.0 55464.0 55465.0 55466.0 55467.0 55468.0 55469.0 55470.0 55471.0 55472.0 55473.0 55474.0 55475.0 55476.0 55477.0 55478.0 55479.0 55480.0 55481.0 55482.0 55483.0 55484.0 55485.0 55486.0 55487.0 55488.0 55489.0 55490.0 55491.0 55492.0 55493.0 55494.0 55495.0 55496.0 55497.0 55498.0 55499.0 55500.0 55501.0 55502.0 55503.0 55504.0 55505.0 55506.0 55507.0 55508.0 55509.0 55510.0 55511.0 55512.0 55513.0 55514.0 55515.0 55516.0 55517.0 55518.0 55519.0 55520.0 55521.0 55522.0 55523.0 55524.0 55525.0 55526.0 55527.0 55528.0 55529.0 55530.0 55531.0 55532.0 55533.0 55534.0 55535.0 55536.0 55537.0 55538.0 55539.0 55540.0 55541.0 55542.0 55543.0 55544.0 55545.0 55546.0 55547.0 55548.0 55549.0 55550.0 55551.0 55552.0 55553.0 55554.0 55555.0 55556.0 55557.0 55558.0 55559.0 55560.0 55561.0 55562.0 55563.0 55564.0 55565.0 55566.0 55567.0 55568.0 55569.0 55570.0 55571.0 55572.0 55573.0 55574.0 55575.0 55576.0 55577.0 55578.0 55579.0 55580.0 55581.0 55582.0 55583.0 55584.0 55585.0 55586.0 55587.0 55588.0 55589.0 55590.0 55591.0 55592.0 55593.0 55594.0 55595.0 55596.0 55597.0 55598.0 55599.0 55600.0 55601.0 55602.0 55603.0 55604.0 55605.0 55606.0 55607.0 55608.0 55609.0 55610.0 55611.0 55612.0 55613.0 55614.0 55615.0 55616.0 55617.0 55618.0 55619.0 55620.0 55621.0 55622.0 55623.0 55624.0 55625.0 55626.0 55627.0 55628.0 55629.0 55630.0 55631.0 55632.0 55633.0 55634.0 55635.0 55636.0 55637.0 55638.0 55639.0 55640.0 55641.0 55642.0 55643.0 55644.0 55645.0 55646.0 55647.0 55648.0 55649.0 55650.0 55651.0 55652.0 55653.0 55654.0 55655.0 55656.0 55657.0 55658.0 55659.0 55660.0 55661.0 55662.0 55663.0 55664.0 55665.0 55666.0 55667.0 55668.0 55669.0 55670.0 55671.0 55672.0 55673.0 55674.0 55675.0 55676.0 55677.0 55678.0 55679.0 55680.0 55681.0 55682.0 55683.0 55684.0 55685.0 55686.0 55687.0 55688.0 55689.0 55690.0 55691.0 55692.0 55693.0 55694.0 55695.0 55696.0 55697.0 55698.0 55699.0 55700.0 55701.0 55702.0 55703.0 55704.0 55705.0 55706.0 55707.0 55708.0 55709.0 55710.0 55711.0 55712.0 55713.0 55714.0 55715.0 55716.0 55717.0 55718.0 55719.0 55720.0 55721.0 55722.0 55723.0 55724.0 55725.0 55726.0 55727.0 55728.0 55729.0 55730.0 55731.0 55732.0 55733.0 55734.0 55735.0 55736.0 55737.0 55738.0 55739.0 55740.0 55741.0 55742.0 55743.0 55744.0 55745.0 55746.0 55747.0 55748.0 55749.0 55750.0 55751.0 55752.0 55753.0 55754.0 55755.0 55756.0 55757.0 55758.0 55759.0 55760.0 55761.0 55762.0 55763.0 55764.0 55765.0 55766.0 55767.0 55768.0 55769.0 55770.0 55771.0 55772.0 55773.0 55774.0 55775.0 55776.0 55777.0 55778.0 55779.0 55780.0 55781.0 55782.0 55783.0 55784.0 55785.0 55786.0 55787.0 55788.0 55789.0 55790.0 55791.0 55792.0 55793.0 55794.0 55795.0 55796.0 55797.0 55798.0 55799.0 55800.0 55801.0 55802.0 55803.0 55804.0 55805.0 55806.0 55807.0 55808.0 55809.0 55810.0 55811.0 55812.0 55813.0 55814.0 55815.0 55816.0 55817.0 55818.0 55819.0 55820.0 55821.0 55822.0 55823.0 55824.0 55825.0 55826.0 55827.0 55828.0 55829.0 55830.0 55831.0 55832.0 55833.0 55834.0 55835.0 55836.0 55837.0 55838.0 55839.0 55840.0 55841.0 55842.0 55843.0 55844.0 55845.0 55846.0 55847.0 55848.0 55849.0 55850.0 55851.0 55852.0 55853.0 55854.0 55855.0 55856.0 55857.0 55858.0 55859.0 55860.0 55861.0 55862.0 55863.0 55864.0 55865.0 55866.0 55867.0 55868.0 55869.0 55870.0 55871.0 55872.0 55873.0 55874.0 55875.0 55876.0 55877.0 55878.0 55879.0 55880.0 55881.0 55882.0 55883.0 55884.0 55885.0 55886.0 55887.0 55888.0 55889.0 55890.0 55891.0 55892.0 55893.0 55894.0 55895.0 55896.0 55897.0 55898.0 55899.0 55900.0 55901.0 55902.0 55903.0 55904.0 55905.0 55906.0 55907.0 55908.0 55909.0 55910.0 55911.0 55912.0 55913.0 55914.0 55915.0 55916.0 55917.0 55918.0 55919.0 55920.0 55921.0 55922.0 55923.0 55924.0 55925.0 55926.0 55927.0 55928.0 55929.0 55930.0 55931.0 55932.0 55933.0 55934.0 55935.0 55936.0 55937.0 55938.0 55939.0 55940.0 55941.0 55942.0 55943.0 55944.0 55945.0 55946.0 55947.0 55948.0 55949.0 55950.0 55951.0 55952.0 55953.0 55954.0 55955.0 55956.0 55957.0 55958.0 55959.0 55960.0 55961.0 55962.0 55963.0 55964.0 55965.0 55966.0 55967.0 55968.0 55969.0 55970.0 55971.0 55972.0 55973.0 55974.0 55975.0 55976.0 55977.0 55978.0 55979.0 55980.0 55981.0 55982.0 55983.0 55984.0 55985.0 55986.0 55987.0 55988.0 55989.0 55990.0 55991.0 55992.0 55993.0 55994.0 55995.0 55996.0 55997.0 55998.0 55999.0 56000.0 56001.0 56002.0 56003.0 56004.0 56005.0 56006.0 56007.0 56008.0 56009.0 56010.0 56011.0 56012.0 56013.0 56014.0 56015.0 56016.0 56017.0 56018.0 56019.0 56020.0 56021.0 56022.0 56023.0 56024.0 56025.0 56026.0 56027.0 56028.0 56029.0 56030.0 56031.0 56032.0 56033.0 56034.0 56035.0 56036.0 56037.0 56038.0 56039.0 56040.0 56041.0 56042.0 56043.0 56044.0 56045.0 56046.0 56047.0 56048.0 56049.0 56050.0 56051.0 56052.0 56053.0 56054.0 56055.0 56056.0 56057.0 56058.0 56059.0 56060.0 56061.0 56062.0 56063.0 56064.0 56065.0 56066.0 56067.0 56068.0 56069.0 56070.0 56071.0 56072.0 56073.0 56074.0 56075.0 56076.0 56077.0 56078.0 56079.0 56080.0 56081.0 56082.0 56083.0 56084.0 56085.0 56086.0 56087.0 56088.0 56089.0 56090.0 56091.0 56092.0 56093.0 56094.0 56095.0 56096.0 56097.0 56098.0 56099.0 56100.0 56101.0 56102.0 56103.0 56104.0 56105.0 56106.0 56107.0 56108.0 56109.0 56110.0 56111.0 56112.0 56113.0 56114.0 56115.0 56116.0 56117.0 56118.0 56119.0 56120.0 56121.0 56122.0 56123.0 56124.0 56125.0 56126.0 56127.0 56128.0 56129.0 56130.0 56131.0 56132.0 56133.0 56134.0 56135.0 56136.0 56137.0 56138.0 56139.0 56140.0 56141.0 56142.0 56143.0 56144.0 56145.0 56146.0 56147.0 56148.0 56149.0 56150.0 56151.0 56152.0 56153.0 56154.0 56155.0 56156.0 56157.0 56158.0 56159.0 56160.0 56161.0 56162.0 56163.0 56164.0 56165.0 56166.0 56167.0 56168.0 56169.0 56170.0 56171.0 56172.0 56173.0 56174.0 56175.0 56176.0 56177.0 56178.0 56179.0 56180.0 56181.0 56182.0 56183.0 56184.0 56185.0 56186.0 56187.0 56188.0 56189.0 56190.0 56191.0 56192.0 56193.0 56194.0 56195.0 56196.0 56197.0 56198.0 56199.0 56200.0 56201.0 56202.0 56203.0 56204.0 56205.0 56206.0 56207.0 56208.0 56209.0 56210.0 56211.0 56212.0 56213.0 56214.0 56215.0 56216.0 56217.0 56218.0 56219.0 56220.0 56221.0 56222.0 56223.0 56224.0 56225.0 56226.0 56227.0 56228.0 56229.0 56230.0 56231.0 56232.0 56233.0 56234.0 56235.0 56236.0 56237.0 56238.0 56239.0 56240.0 56241.0 56242.0 56243.0 56244.0 56245.0 56246.0 56247.0 56248.0 56249.0 56250.0 56251.0 56252.0 56253.0 56254.0 56255.0 56256.0 56257.0 56258.0 56259.0 56260.0 56261.0 56262.0 56263.0 56264.0 56265.0 56266.0 56267.0 56268.0 56269.0 56270.0 56271.0 56272.0 56273.0 56274.0 56275.0 56276.0 56277.0 56278.0 56279.0 56280.0 56281.0 56282.0 56283.0 56284.0 56285.0 56286.0 56287.0 56288.0 56289.0 56290.0 56291.0 56292.0 56293.0 56294.0 56295.0 56296.0 56297.0 56298.0 56299.0 56300.0 56301.0 56302.0 56303.0 56304.0 56305.0 56306.0 56307.0 56308.0 56309.0 56310.0 56311.0 56312.0 56313.0 56314.0 56315.0 56316.0 56317.0 56318.0 56319.0 56320.0 56321.0 56322.0 56323.0 56324.0 56325.0 56326.0 56327.0 56328.0 56329.0 56330.0 56331.0 56332.0 56333.0 56334.0 56335.0 56336.0 56337.0 56338.0 56339.0 56340.0 56341.0 56342.0 56343.0 56344.0 56345.0 56346.0 56347.0 56348.0 56349.0 56350.0 56351.0 56352.0 56353.0 56354.0 56355.0 56356.0 56357.0 56358.0 56359.0 56360.0 56361.0 56362.0 56363.0 56364.0 56365.0 56366.0 56367.0 56368.0 56369.0 56370.0 56371.0 56372.0 56373.0 56374.0 56375.0 56376.0 56377.0 56378.0 56379.0 56380.0 56381.0 56382.0 56383.0 56384.0 56385.0 56386.0 56387.0 56388.0 56389.0 56390.0 56391.0 56392.0 56393.0 56394.0 56395.0 56396.0 56397.0 56398.0 56399.0 56400.0 56401.0 56402.0 56403.0 56404.0 56405.0 56406.0 56407.0 56408.0 56409.0 56410.0 56411.0 56412.0 56413.0 56414.0 56415.0 56416.0 56417.0 56418.0 56419.0 56420.0 56421.0 56422.0 56423.0 56424.0 56425.0 56426.0 56427.0 56428.0 56429.0 56430.0 56431.0 56432.0 56433.0 56434.0 56435.0 56436.0 56437.0 56438.0 56439.0 56440.0 56441.0 56442.0 56443.0 56444.0 56445.0 56446.0 56447.0 56448.0 56449.0 56450.0 56451.0 56452.0 56453.0 56454.0 56455.0 56456.0 56457.0 56458.0 56459.0 56460.0 56461.0 56462.0 56463.0 56464.0 56465.0 56466.0 56467.0 56468.0 56469.0 56470.0 56471.0 56472.0 56473.0 56474.0 56475.0 56476.0 56477.0 56478.0 56479.0 56480.0 56481.0 56482.0 56483.0 56484.0 56485.0 56486.0 56487.0 56488.0 56489.0 56490.0 56491.0 56492.0 56493.0 56494.0 56495.0 56496.0 56497.0 56498.0 56499.0 56500.0 56501.0 56502.0 56503.0 56504.0 56505.0 56506.0 56507.0 56508.0 56509.0 56510.0 56511.0 56512.0 56513.0 56514.0 56515.0 56516.0 56517.0 56518.0 56519.0 56520.0 56521.0 56522.0 56523.0 56524.0 56525.0 56526.0 56527.0 56528.0 56529.0 56530.0 56531.0 56532.0 56533.0 56534.0 56535.0 56536.0 56537.0 56538.0 56539.0 56540.0 56541.0 56542.0 56543.0 56544.0 56545.0 56546.0 56547.0 56548.0 56549.0 56550.0 56551.0 56552.0 56553.0 56554.0 56555.0 56556.0 56557.0 56558.0 56559.0 56560.0 56561.0 56562.0 56563.0 56564.0 56565.0 56566.0 56567.0 56568.0 56569.0 56570.0 56571.0 56572.0 56573.0 56574.0 56575.0 56576.0 56577.0 56578.0 56579.0 56580.0 56581.0 56582.0 56583.0 56584.0 56585.0 56586.0 56587.0 56588.0 56589.0 56590.0 56591.0 56592.0 56593.0 56594.0 56595.0 56596.0 56597.0 56598.0 56599.0 56600.0 56601.0 56602.0 56603.0 56604.0 56605.0 56606.0 56607.0 56608.0 56609.0 56610.0 56611.0 56612.0 56613.0 56614.0 56615.0 56616.0 56617.0 56618.0 56619.0 56620.0 56621.0 56622.0 56623.0 56624.0 56625.0 56626.0 56627.0 56628.0 56629.0 56630.0 56631.0 56632.0 56633.0 56634.0 56635.0 56636.0 56637.0 56638.0 56639.0 56640.0 56641.0 56642.0 56643.0 56644.0 56645.0 56646.0 56647.0 56648.0 56649.0 56650.0 56651.0 56652.0 56653.0 56654.0 56655.0 56656.0 56657.0 56658.0 56659.0 56660.0 56661.0 56662.0 56663.0 56664.0 56665.0 56666.0 56667.0 56668.0 56669.0 56670.0 56671.0 56672.0 56673.0 56674.0 56675.0 56676.0 56677.0 56678.0 56679.0 56680.0 56681.0 56682.0 56683.0 56684.0 56685.0 56686.0 56687.0 56688.0 56689.0 56690.0 56691.0 56692.0 56693.0 56694.0 56695.0 56696.0 56697.0 56698.0 56699.0 56700.0 56701.0 56702.0 56703.0 56704.0 56705.0 56706.0 56707.0 56708.0 56709.0 56710.0 56711.0 56712.0 56713.0 56714.0 56715.0 56716.0 56717.0 56718.0 56719.0 56720.0 56721.0 56722.0 56723.0 56724.0 56725.0 56726.0 56727.0 56728.0 56729.0 56730.0 56731.0 56732.0 56733.0 56734.0 56735.0 56736.0 56737.0 56738.0 56739.0 56740.0 56741.0 56742.0 56743.0 56744.0 56745.0 56746.0 56747.0 56748.0 56749.0 56750.0 56751.0 56752.0 56753.0 56754.0 56755.0 56756.0 56757.0 56758.0 56759.0 56760.0 56761.0 56762.0 56763.0 56764.0 56765.0 56766.0 56767.0 56768.0 56769.0 56770.0 56771.0 56772.0 56773.0 56774.0 56775.0 56776.0 56777.0 56778.0 56779.0 56780.0 56781.0 56782.0 56783.0 56784.0 56785.0 56786.0 56787.0 56788.0 56789.0 56790.0 56791.0 56792.0 56793.0 56794.0 56795.0 56796.0 56797.0 56798.0 56799.0 56800.0 56801.0 56802.0 56803.0 56804.0 56805.0 56806.0 56807.0 56808.0 56809.0 56810.0 56811.0 56812.0 56813.0 56814.0 56815.0 56816.0 56817.0 56818.0 56819.0 56820.0 56821.0 56822.0 56823.0 56824.0 56825.0 56826.0 56827.0 56828.0 56829.0 56830.0 56831.0 56832.0 56833.0 56834.0 56835.0 56836.0 56837.0 56838.0 56839.0 56840.0 56841.0 56842.0 56843.0 56844.0 56845.0 56846.0 56847.0 56848.0 56849.0 56850.0 56851.0 56852.0 56853.0 56854.0 56855.0 56856.0 56857.0 56858.0 56859.0 56860.0 56861.0 56862.0 56863.0 56864.0 56865.0 56866.0 56867.0 56868.0 56869.0 56870.0 56871.0 56872.0 56873.0 56874.0 56875.0 56876.0 56877.0 56878.0 56879.0 56880.0 56881.0 56882.0 56883.0 56884.0 56885.0 56886.0 56887.0 56888.0 56889.0 56890.0 56891.0 56892.0 56893.0 56894.0 56895.0 56896.0 56897.0 56898.0 56899.0 56900.0 56901.0 56902.0 56903.0 56904.0 56905.0 56906.0 56907.0 56908.0 56909.0 56910.0 56911.0 56912.0 56913.0 56914.0 56915.0 56916.0 56917.0 56918.0 56919.0 56920.0 56921.0 56922.0 56923.0 56924.0 56925.0 56926.0 56927.0 56928.0 56929.0 56930.0 56931.0 56932.0 56933.0 56934.0 56935.0 56936.0 56937.0 56938.0 56939.0 56940.0 56941.0 56942.0 56943.0 56944.0 56945.0 56946.0 56947.0 56948.0 56949.0 56950.0 56951.0 56952.0 56953.0 56954.0 56955.0 56956.0 56957.0 56958.0 56959.0 56960.0 56961.0 56962.0 56963.0 56964.0 56965.0 56966.0 56967.0 56968.0 56969.0 56970.0 56971.0 56972.0 56973.0 56974.0 56975.0 56976.0 56977.0 56978.0 56979.0 56980.0 56981.0 56982.0 56983.0 56984.0 56985.0 56986.0 56987.0 56988.0 56989.0 56990.0 56991.0 56992.0 56993.0 56994.0 56995.0 56996.0 56997.0 56998.0 56999.0 57000.0 57001.0 57002.0 57003.0 57004.0 57005.0 57006.0 57007.0 57008.0 57009.0 57010.0 57011.0 57012.0 57013.0 57014.0 57015.0 57016.0 57017.0 57018.0 57019.0 57020.0 57021.0 57022.0 57023.0 57024.0 57025.0 57026.0 57027.0 57028.0 57029.0 57030.0 57031.0 57032.0 57033.0 57034.0 57035.0 57036.0 57037.0 57038.0 57039.0 57040.0 57041.0 57042.0 57043.0 57044.0 57045.0 57046.0 57047.0 57048.0 57049.0 57050.0 57051.0 57052.0 57053.0 57054.0 57055.0 57056.0 57057.0 57058.0 57059.0 57060.0 57061.0 57062.0 57063.0 57064.0 57065.0 57066.0 57067.0 57068.0 57069.0 57070.0 57071.0 57072.0 57073.0 57074.0 57075.0 57076.0 57077.0 57078.0 57079.0 57080.0 57081.0 57082.0 57083.0 57084.0 57085.0 57086.0 57087.0 57088.0 57089.0 57090.0 57091.0 57092.0 57093.0 57094.0 57095.0 57096.0 57097.0 57098.0 57099.0 57100.0 57101.0 57102.0 57103.0 57104.0 57105.0 57106.0 57107.0 57108.0 57109.0 57110.0 57111.0 57112.0 57113.0 57114.0 57115.0 57116.0 57117.0 57118.0 57119.0 57120.0 57121.0 57122.0 57123.0 57124.0 57125.0 57126.0 57127.0 57128.0 57129.0 57130.0 57131.0 57132.0 57133.0 57134.0 57135.0 57136.0 57137.0 57138.0 57139.0 57140.0 57141.0 57142.0 57143.0 57144.0 57145.0 57146.0 57147.0 57148.0 57149.0 57150.0 57151.0 57152.0 57153.0 57154.0 57155.0 57156.0 57157.0 57158.0 57159.0 57160.0 57161.0 57162.0 57163.0 57164.0 57165.0 57166.0 57167.0 57168.0 57169.0 57170.0 57171.0 57172.0 57173.0 57174.0 57175.0 57176.0 57177.0 57178.0 57179.0 57180.0 57181.0 57182.0 57183.0 57184.0 57185.0 57186.0 57187.0 57188.0 57189.0 57190.0 57191.0 57192.0 57193.0 57194.0 57195.0 57196.0 57197.0 57198.0 57199.0 57200.0 57201.0 57202.0 57203.0 57204.0 57205.0 57206.0 57207.0 57208.0 57209.0 57210.0 57211.0 57212.0 57213.0 57214.0 57215.0 57216.0 57217.0 57218.0 57219.0 57220.0 57221.0 57222.0 57223.0 57224.0 57225.0 57226.0 57227.0 57228.0 57229.0 57230.0 57231.0 57232.0 57233.0 57234.0 57235.0 57236.0 57237.0 57238.0 57239.0 57240.0 57241.0 57242.0 57243.0 57244.0 57245.0 57246.0 57247.0 57248.0 57249.0 57250.0 57251.0 57252.0 57253.0 57254.0 57255.0 57256.0 57257.0 57258.0 57259.0 57260.0 57261.0 57262.0 57263.0 57264.0 57265.0 57266.0 57267.0 57268.0 57269.0 57270.0 57271.0 57272.0 57273.0 57274.0 57275.0 57276.0 57277.0 57278.0 57279.0 57280.0 57281.0 57282.0 57283.0 57284.0 57285.0 57286.0 57287.0 57288.0 57289.0 57290.0 57291.0 57292.0 57293.0 57294.0 57295.0 57296.0 57297.0 57298.0 57299.0 57300.0 57301.0 57302.0 57303.0 57304.0 57305.0 57306.0 57307.0 57308.0 57309.0 57310.0 57311.0 57312.0 57313.0 57314.0 57315.0 57316.0 57317.0 57318.0 57319.0 57320.0 57321.0 57322.0 57323.0 57324.0 57325.0 57326.0 57327.0 57328.0 57329.0 57330.0 57331.0 57332.0 57333.0 57334.0 57335.0 57336.0 57337.0 57338.0 57339.0 57340.0 57341.0 57342.0 57343.0 57344.0 57345.0 57346.0 57347.0 57348.0 57349.0 57350.0 57351.0 57352.0 57353.0 57354.0 57355.0 57356.0 57357.0 57358.0 57359.0 57360.0 57361.0 57362.0 57363.0 57364.0 57365.0 57366.0 57367.0 57368.0 57369.0 57370.0 57371.0 57372.0 57373.0 57374.0 57375.0 57376.0 57377.0 57378.0 57379.0 57380.0 57381.0 57382.0 57383.0 57384.0 57385.0 57386.0 57387.0 57388.0 57389.0 57390.0 57391.0 57392.0 57393.0 57394.0 57395.0 57396.0 57397.0 57398.0 57399.0 57400.0 57401.0 57402.0 57403.0 57404.0 57405.0 57406.0 57407.0 57408.0 57409.0 57410.0 57411.0 57412.0 57413.0 57414.0 57415.0 57416.0 57417.0 57418.0 57419.0 57420.0 57421.0 57422.0 57423.0 57424.0 57425.0 57426.0 57427.0 57428.0 57429.0 57430.0 57431.0 57432.0 57433.0 57434.0 57435.0 57436.0 57437.0 57438.0 57439.0 57440.0 57441.0 57442.0 57443.0 57444.0 57445.0 57446.0 57447.0 57448.0 57449.0 57450.0 57451.0 57452.0 57453.0 57454.0 57455.0 57456.0 57457.0 57458.0 57459.0 57460.0 57461.0 57462.0 57463.0 57464.0 57465.0 57466.0 57467.0 57468.0 57469.0 57470.0 57471.0 57472.0 57473.0 57474.0 57475.0 57476.0 57477.0 57478.0 57479.0 57480.0 57481.0 57482.0 57483.0 57484.0 57485.0 57486.0 57487.0 57488.0 57489.0 57490.0 57491.0 57492.0 57493.0 57494.0 57495.0 57496.0 57497.0 57498.0 57499.0 57500.0 57501.0 57502.0 57503.0 57504.0 57505.0 57506.0 57507.0 57508.0 57509.0 57510.0 57511.0 57512.0 57513.0 57514.0 57515.0 57516.0 57517.0 57518.0 57519.0 57520.0 57521.0 57522.0 57523.0 57524.0 57525.0 57526.0 57527.0 57528.0 57529.0 57530.0 57531.0 57532.0 57533.0 57534.0 57535.0 57536.0 57537.0 57538.0 57539.0 57540.0 57541.0 57542.0 57543.0 57544.0 57545.0 57546.0 57547.0 57548.0 57549.0 57550.0 57551.0 57552.0 57553.0 57554.0 57555.0 57556.0 57557.0 57558.0 57559.0 57560.0 57561.0 57562.0 57563.0 57564.0 57565.0 57566.0 57567.0 57568.0 57569.0 57570.0 57571.0 57572.0 57573.0 57574.0 57575.0 57576.0 57577.0 57578.0 57579.0 57580.0 57581.0 57582.0 57583.0 57584.0 57585.0 57586.0 57587.0 57588.0 57589.0 57590.0 57591.0 57592.0 57593.0 57594.0 57595.0 57596.0 57597.0 57598.0 57599.0 57600.0 57601.0 57602.0 57603.0 57604.0 57605.0 57606.0 57607.0 57608.0 57609.0 57610.0 57611.0 57612.0 57613.0 57614.0 57615.0 57616.0 57617.0 57618.0 57619.0 57620.0 57621.0 57622.0 57623.0 57624.0 57625.0 57626.0 57627.0 57628.0 57629.0 57630.0 57631.0 57632.0 57633.0 57634.0 57635.0 57636.0 57637.0 57638.0 57639.0 57640.0 57641.0 57642.0 57643.0 57644.0 57645.0 57646.0 57647.0 57648.0 57649.0 57650.0 57651.0 57652.0 57653.0 57654.0 57655.0 57656.0 57657.0 57658.0 57659.0 57660.0 57661.0 57662.0 57663.0 57664.0 57665.0 57666.0 57667.0 57668.0 57669.0 57670.0 57671.0 57672.0 57673.0 57674.0 57675.0 57676.0 57677.0 57678.0 57679.0 57680.0 57681.0 57682.0 57683.0 57684.0 57685.0 57686.0 57687.0 57688.0 57689.0 57690.0 57691.0 57692.0 57693.0 57694.0 57695.0 57696.0 57697.0 57698.0 57699.0 57700.0 57701.0 57702.0 57703.0 57704.0 57705.0 57706.0 57707.0 57708.0 57709.0 57710.0 57711.0 57712.0 57713.0 57714.0 57715.0 57716.0 57717.0 57718.0 57719.0 57720.0 57721.0 57722.0 57723.0 57724.0 57725.0 57726.0 57727.0 57728.0 57729.0 57730.0 57731.0 57732.0 57733.0 57734.0 57735.0 57736.0 57737.0 57738.0 57739.0 57740.0 57741.0 57742.0 57743.0 57744.0 57745.0 57746.0 57747.0 57748.0 57749.0 57750.0 57751.0 57752.0 57753.0 57754.0 57755.0 57756.0 57757.0 57758.0 57759.0 57760.0 57761.0 57762.0 57763.0 57764.0 57765.0 57766.0 57767.0 57768.0 57769.0 57770.0 57771.0 57772.0 57773.0 57774.0 57775.0 57776.0 57777.0 57778.0 57779.0 57780.0 57781.0 57782.0 57783.0 57784.0 57785.0 57786.0 57787.0 57788.0 57789.0 57790.0 57791.0 57792.0 57793.0 57794.0 57795.0 57796.0 57797.0 57798.0 57799.0 57800.0 57801.0 57802.0 57803.0 57804.0 57805.0 57806.0 57807.0 57808.0 57809.0 57810.0 57811.0 57812.0 57813.0 57814.0 57815.0 57816.0 57817.0 57818.0 57819.0 57820.0 57821.0 57822.0 57823.0 57824.0 57825.0 57826.0 57827.0 57828.0 57829.0 57830.0 57831.0 57832.0 57833.0 57834.0 57835.0 57836.0 57837.0 57838.0 57839.0 57840.0 57841.0 57842.0 57843.0 57844.0 57845.0 57846.0 57847.0 57848.0 57849.0 57850.0 57851.0 57852.0 57853.0 57854.0 57855.0 57856.0 57857.0 57858.0 57859.0 57860.0 57861.0 57862.0 57863.0 57864.0 57865.0 57866.0 57867.0 57868.0 57869.0 57870.0 57871.0 57872.0 57873.0 57874.0 57875.0 57876.0 57877.0 57878.0 57879.0 57880.0 57881.0 57882.0 57883.0 57884.0 57885.0 57886.0 57887.0 57888.0 57889.0 57890.0 57891.0 57892.0 57893.0 57894.0 57895.0 57896.0 57897.0 57898.0 57899.0 57900.0 57901.0 57902.0 57903.0 57904.0 57905.0 57906.0 57907.0 57908.0 57909.0 57910.0 57911.0 57912.0 57913.0 57914.0 57915.0 57916.0 57917.0 57918.0 57919.0 57920.0 57921.0 57922.0 57923.0 57924.0 57925.0 57926.0 57927.0 57928.0 57929.0 57930.0 57931.0 57932.0 57933.0 57934.0 57935.0 57936.0 57937.0 57938.0 57939.0 57940.0 57941.0 57942.0 57943.0 57944.0 57945.0 57946.0 57947.0 57948.0 57949.0 57950.0 57951.0 57952.0 57953.0 57954.0 57955.0 57956.0 57957.0 57958.0 57959.0 57960.0 57961.0 57962.0 57963.0 57964.0 57965.0 57966.0 57967.0 57968.0 57969.0 57970.0 57971.0 57972.0 57973.0 57974.0 57975.0 57976.0 57977.0 57978.0 57979.0 57980.0 57981.0 57982.0 57983.0 57984.0 57985.0 57986.0 57987.0 57988.0 57989.0 57990.0 57991.0 57992.0 57993.0 57994.0 57995.0 57996.0 57997.0 57998.0 57999.0 58000.0 58001.0 58002.0 58003.0 58004.0 58005.0 58006.0 58007.0 58008.0 58009.0 58010.0 58011.0 58012.0 58013.0 58014.0 58015.0 58016.0 58017.0 58018.0 58019.0 58020.0 58021.0 58022.0 58023.0 58024.0 58025.0 58026.0 58027.0 58028.0 58029.0 58030.0 58031.0 58032.0 58033.0 58034.0 58035.0 58036.0 58037.0 58038.0 58039.0 58040.0 58041.0 58042.0 58043.0 58044.0 58045.0 58046.0 58047.0 58048.0 58049.0 58050.0 58051.0 58052.0 58053.0 58054.0 58055.0 58056.0 58057.0 58058.0 58059.0 58060.0 58061.0 58062.0 58063.0 58064.0 58065.0 58066.0 58067.0 58068.0 58069.0 58070.0 58071.0 58072.0 58073.0 58074.0 58075.0 58076.0 58077.0 58078.0 58079.0 58080.0 58081.0 58082.0 58083.0 58084.0 58085.0 58086.0 58087.0 58088.0 58089.0 58090.0 58091.0 58092.0 58093.0 58094.0 58095.0 58096.0 58097.0 58098.0 58099.0 58100.0 58101.0 58102.0 58103.0 58104.0 58105.0 58106.0 58107.0 58108.0 58109.0 58110.0 58111.0 58112.0 58113.0 58114.0 58115.0 58116.0 58117.0 58118.0 58119.0 58120.0 58121.0 58122.0 58123.0 58124.0 58125.0 58126.0 58127.0 58128.0 58129.0 58130.0 58131.0 58132.0 58133.0 58134.0 58135.0 58136.0 58137.0 58138.0 58139.0 58140.0 58141.0 58142.0 58143.0 58144.0 58145.0 58146.0 58147.0 58148.0 58149.0 58150.0 58151.0 58152.0 58153.0 58154.0 58155.0 58156.0 58157.0 58158.0 58159.0 58160.0 58161.0 58162.0 58163.0 58164.0 58165.0 58166.0 58167.0 58168.0 58169.0 58170.0 58171.0 58172.0 58173.0 58174.0 58175.0 58176.0 58177.0 58178.0 58179.0 58180.0 58181.0 58182.0 58183.0 58184.0 58185.0 58186.0 58187.0 58188.0 58189.0 58190.0 58191.0 58192.0 58193.0 58194.0 58195.0 58196.0 58197.0 58198.0 58199.0 58200.0 58201.0 58202.0 58203.0 58204.0 58205.0 58206.0 58207.0 58208.0 58209.0 58210.0 58211.0 58212.0 58213.0 58214.0 58215.0 58216.0 58217.0 58218.0 58219.0 58220.0 58221.0 58222.0 58223.0 58224.0 58225.0 58226.0 58227.0 58228.0 58229.0 58230.0 58231.0 58232.0 58233.0 58234.0 58235.0 58236.0 58237.0 58238.0 58239.0 58240.0 58241.0 58242.0 58243.0 58244.0 58245.0 58246.0 58247.0 58248.0 58249.0 58250.0 58251.0 58252.0 58253.0 58254.0 58255.0 58256.0 58257.0 58258.0 58259.0 58260.0 58261.0 58262.0 58263.0 58264.0 58265.0 58266.0 58267.0 58268.0 58269.0 58270.0 58271.0 58272.0 58273.0 58274.0 58275.0 58276.0 58277.0 58278.0 58279.0 58280.0 58281.0 58282.0 58283.0 58284.0 58285.0 58286.0 58287.0 58288.0 58289.0 58290.0 58291.0 58292.0 58293.0 58294.0 58295.0 58296.0 58297.0 58298.0 58299.0 58300.0 58301.0 58302.0 58303.0 58304.0 58305.0 58306.0 58307.0 58308.0 58309.0 58310.0 58311.0 58312.0 58313.0 58314.0 58315.0 58316.0 58317.0 58318.0 58319.0 58320.0 58321.0 58322.0 58323.0 58324.0 58325.0 58326.0 58327.0 58328.0 58329.0 58330.0 58331.0 58332.0 58333.0 58334.0 58335.0 58336.0 58337.0 58338.0 58339.0 58340.0 58341.0 58342.0 58343.0 58344.0 58345.0 58346.0 58347.0 58348.0 58349.0 58350.0 58351.0 58352.0 58353.0 58354.0 58355.0 58356.0 58357.0 58358.0 58359.0 58360.0 58361.0 58362.0 58363.0 58364.0 58365.0 58366.0 58367.0 58368.0 58369.0 58370.0 58371.0 58372.0 58373.0 58374.0 58375.0 58376.0 58377.0 58378.0 58379.0 58380.0 58381.0 58382.0 58383.0 58384.0 58385.0 58386.0 58387.0 58388.0 58389.0 58390.0 58391.0 58392.0 58393.0 58394.0 58395.0 58396.0 58397.0 58398.0 58399.0 58400.0 58401.0 58402.0 58403.0 58404.0 58405.0 58406.0 58407.0 58408.0 58409.0 58410.0 58411.0 58412.0 58413.0 58414.0 58415.0 58416.0 58417.0 58418.0 58419.0 58420.0 58421.0 58422.0 58423.0 58424.0 58425.0 58426.0 58427.0 58428.0 58429.0 58430.0 58431.0 58432.0 58433.0 58434.0 58435.0 58436.0 58437.0 58438.0 58439.0 58440.0 58441.0 58442.0 58443.0 58444.0 58445.0 58446.0 58447.0 58448.0 58449.0 58450.0 58451.0 58452.0 58453.0 58454.0 58455.0 58456.0 58457.0 58458.0 58459.0 58460.0 58461.0 58462.0 58463.0 58464.0 58465.0 58466.0 58467.0 58468.0 58469.0 58470.0 58471.0 58472.0 58473.0 58474.0 58475.0 58476.0 58477.0 58478.0 58479.0 58480.0 58481.0 58482.0 58483.0 58484.0 58485.0 58486.0 58487.0 58488.0 58489.0 58490.0 58491.0 58492.0 58493.0 58494.0 58495.0 58496.0 58497.0 58498.0 58499.0 58500.0 58501.0 58502.0 58503.0 58504.0 58505.0 58506.0 58507.0 58508.0 58509.0 58510.0 58511.0 58512.0 58513.0 58514.0 58515.0 58516.0 58517.0 58518.0 58519.0 58520.0 58521.0 58522.0 58523.0 58524.0 58525.0 58526.0 58527.0 58528.0 58529.0 58530.0 58531.0 58532.0 58533.0 58534.0 58535.0 58536.0 58537.0 58538.0 58539.0 58540.0 58541.0 58542.0 58543.0 58544.0 58545.0 58546.0 58547.0 58548.0 58549.0 58550.0 58551.0 58552.0 58553.0 58554.0 58555.0 58556.0 58557.0 58558.0 58559.0 58560.0 58561.0 58562.0 58563.0 58564.0 58565.0 58566.0 58567.0 58568.0 58569.0 58570.0 58571.0 58572.0 58573.0 58574.0 58575.0 58576.0 58577.0 58578.0 58579.0 58580.0 58581.0 58582.0 58583.0 58584.0 58585.0 58586.0 58587.0 58588.0 58589.0 58590.0 58591.0 58592.0 58593.0 58594.0 58595.0 58596.0 58597.0 58598.0 58599.0 58600.0 58601.0 58602.0 58603.0 58604.0 58605.0 58606.0 58607.0 58608.0 58609.0 58610.0 58611.0 58612.0 58613.0 58614.0 58615.0 58616.0 58617.0 58618.0 58619.0 58620.0 58621.0 58622.0 58623.0 58624.0 58625.0 58626.0 58627.0 58628.0 58629.0 58630.0 58631.0 58632.0 58633.0 58634.0 58635.0 58636.0 58637.0 58638.0 58639.0 58640.0 58641.0 58642.0 58643.0 58644.0 58645.0 58646.0 58647.0 58648.0 58649.0 58650.0 58651.0 58652.0 58653.0 58654.0 58655.0 58656.0 58657.0 58658.0 58659.0 58660.0 58661.0 58662.0 58663.0 58664.0 58665.0 58666.0 58667.0 58668.0 58669.0 58670.0 58671.0 58672.0 58673.0 58674.0 58675.0 58676.0 58677.0 58678.0 58679.0 58680.0 58681.0 58682.0 58683.0 58684.0 58685.0 58686.0 58687.0 58688.0 58689.0 58690.0 58691.0 58692.0 58693.0 58694.0 58695.0 58696.0 58697.0 58698.0 58699.0 58700.0 58701.0 58702.0 58703.0 58704.0 58705.0 58706.0 58707.0 58708.0 58709.0 58710.0 58711.0 58712.0 58713.0 58714.0 58715.0 58716.0 58717.0 58718.0 58719.0 58720.0 58721.0 58722.0 58723.0 58724.0 58725.0 58726.0 58727.0 58728.0 58729.0 58730.0 58731.0 58732.0 58733.0 58734.0 58735.0 58736.0 58737.0 58738.0 58739.0 58740.0 58741.0 58742.0 58743.0 58744.0 58745.0 58746.0 58747.0 58748.0 58749.0 58750.0 58751.0 58752.0 58753.0 58754.0 58755.0 58756.0 58757.0 58758.0 58759.0 58760.0 58761.0 58762.0 58763.0 58764.0 58765.0 58766.0 58767.0 58768.0 58769.0 58770.0 58771.0 58772.0 58773.0 58774.0 58775.0 58776.0 58777.0 58778.0 58779.0 58780.0 58781.0 58782.0 58783.0 58784.0 58785.0 58786.0 58787.0 58788.0 58789.0 58790.0 58791.0 58792.0 58793.0 58794.0 58795.0 58796.0 58797.0 58798.0 58799.0 58800.0 58801.0 58802.0 58803.0 58804.0 58805.0 58806.0 58807.0 58808.0 58809.0 58810.0 58811.0 58812.0 58813.0 58814.0 58815.0 58816.0 58817.0 58818.0 58819.0 58820.0 58821.0 58822.0 58823.0 58824.0 58825.0 58826.0 58827.0 58828.0 58829.0 58830.0 58831.0 58832.0 58833.0 58834.0 58835.0 58836.0 58837.0 58838.0 58839.0 58840.0 58841.0 58842.0 58843.0 58844.0 58845.0 58846.0 58847.0 58848.0 58849.0 58850.0 58851.0 58852.0 58853.0 58854.0 58855.0 58856.0 58857.0 58858.0 58859.0 58860.0 58861.0 58862.0 58863.0 58864.0 58865.0 58866.0 58867.0 58868.0 58869.0 58870.0 58871.0 58872.0 58873.0 58874.0 58875.0 58876.0 58877.0 58878.0 58879.0 58880.0 58881.0 58882.0 58883.0 58884.0 58885.0 58886.0 58887.0 58888.0 58889.0 58890.0 58891.0 58892.0 58893.0 58894.0 58895.0 58896.0 58897.0 58898.0 58899.0 58900.0 58901.0 58902.0 58903.0 58904.0 58905.0 58906.0 58907.0 58908.0 58909.0 58910.0 58911.0 58912.0 58913.0 58914.0 58915.0 58916.0 58917.0 58918.0 58919.0 58920.0 58921.0 58922.0 58923.0 58924.0 58925.0 58926.0 58927.0 58928.0 58929.0 58930.0 58931.0 58932.0 58933.0 58934.0 58935.0 58936.0 58937.0 58938.0 58939.0 58940.0 58941.0 58942.0 58943.0 58944.0 58945.0 58946.0 58947.0 58948.0 58949.0 58950.0 58951.0 58952.0 58953.0 58954.0 58955.0 58956.0 58957.0 58958.0 58959.0 58960.0 58961.0 58962.0 58963.0 58964.0 58965.0 58966.0 58967.0 58968.0 58969.0 58970.0 58971.0 58972.0 58973.0 58974.0 58975.0 58976.0 58977.0 58978.0 58979.0 58980.0 58981.0 58982.0 58983.0 58984.0 58985.0 58986.0 58987.0 58988.0 58989.0 58990.0 58991.0 58992.0 58993.0 58994.0 58995.0 58996.0 58997.0 58998.0 58999.0 59000.0 59001.0 59002.0 59003.0 59004.0 59005.0 59006.0 59007.0 59008.0 59009.0 59010.0 59011.0 59012.0 59013.0 59014.0 59015.0 59016.0 59017.0 59018.0 59019.0 59020.0 59021.0 59022.0 59023.0 59024.0 59025.0 59026.0 59027.0 59028.0 59029.0 59030.0 59031.0 59032.0 59033.0 59034.0 59035.0 59036.0 59037.0 59038.0 59039.0 59040.0 59041.0 59042.0 59043.0 59044.0 59045.0 59046.0 59047.0 59048.0 59049.0 59050.0 59051.0 59052.0 59053.0 59054.0 59055.0 59056.0 59057.0 59058.0 59059.0 59060.0 59061.0 59062.0 59063.0 59064.0 59065.0 59066.0 59067.0 59068.0 59069.0 59070.0 59071.0 59072.0 59073.0 59074.0 59075.0 59076.0 59077.0 59078.0 59079.0 59080.0 59081.0 59082.0 59083.0 59084.0 59085.0 59086.0 59087.0 59088.0 59089.0 59090.0 59091.0 59092.0 59093.0 59094.0 59095.0 59096.0 59097.0 59098.0 59099.0 59100.0 59101.0 59102.0 59103.0 59104.0 59105.0 59106.0 59107.0 59108.0 59109.0 59110.0 59111.0 59112.0 59113.0 59114.0 59115.0 59116.0 59117.0 59118.0 59119.0 59120.0 59121.0 59122.0 59123.0 59124.0 59125.0 59126.0 59127.0 59128.0 59129.0 59130.0 59131.0 59132.0 59133.0 59134.0 59135.0 59136.0 59137.0 59138.0 59139.0 59140.0 59141.0 59142.0 59143.0 59144.0 59145.0 59146.0 59147.0 59148.0 59149.0 59150.0 59151.0 59152.0 59153.0 59154.0 59155.0 59156.0 59157.0 59158.0 59159.0 59160.0 59161.0 59162.0 59163.0 59164.0 59165.0 59166.0 59167.0 59168.0 59169.0 59170.0 59171.0 59172.0 59173.0 59174.0 59175.0 59176.0 59177.0 59178.0 59179.0 59180.0 59181.0 59182.0 59183.0 59184.0 59185.0 59186.0 59187.0 59188.0 59189.0 59190.0 59191.0 59192.0 59193.0 59194.0 59195.0 59196.0 59197.0 59198.0 59199.0 59200.0 59201.0 59202.0 59203.0 59204.0 59205.0 59206.0 59207.0 59208.0 59209.0 59210.0 59211.0 59212.0 59213.0 59214.0 59215.0 59216.0 59217.0 59218.0 59219.0 59220.0 59221.0 59222.0 59223.0 59224.0 59225.0 59226.0 59227.0 59228.0 59229.0 59230.0 59231.0 59232.0 59233.0 59234.0 59235.0 59236.0 59237.0 59238.0 59239.0 59240.0 59241.0 59242.0 59243.0 59244.0 59245.0 59246.0 59247.0 59248.0 59249.0 59250.0 59251.0 59252.0 59253.0 59254.0 59255.0 59256.0 59257.0 59258.0 59259.0 59260.0 59261.0 59262.0 59263.0 59264.0 59265.0 59266.0 59267.0 59268.0 59269.0 59270.0 59271.0 59272.0 59273.0 59274.0 59275.0 59276.0 59277.0 59278.0 59279.0 59280.0 59281.0 59282.0 59283.0 59284.0 59285.0 59286.0 59287.0 59288.0 59289.0 59290.0 59291.0 59292.0 59293.0 59294.0 59295.0 59296.0 59297.0 59298.0 59299.0 59300.0 59301.0 59302.0 59303.0 59304.0 59305.0 59306.0 59307.0 59308.0 59309.0 59310.0 59311.0 59312.0 59313.0 59314.0 59315.0 59316.0 59317.0 59318.0 59319.0 59320.0 59321.0 59322.0 59323.0 59324.0 59325.0 59326.0 59327.0 59328.0 59329.0 59330.0 59331.0 59332.0 59333.0 59334.0 59335.0 59336.0 59337.0 59338.0 59339.0 59340.0 59341.0 59342.0 59343.0 59344.0 59345.0 59346.0 59347.0 59348.0 59349.0 59350.0 59351.0 59352.0 59353.0 59354.0 59355.0 59356.0 59357.0 59358.0 59359.0 59360.0 59361.0 59362.0 59363.0 59364.0 59365.0 59366.0 59367.0 59368.0 59369.0 59370.0 59371.0 59372.0 59373.0 59374.0 59375.0 59376.0 59377.0 59378.0 59379.0 59380.0 59381.0 59382.0 59383.0 59384.0 59385.0 59386.0 59387.0 59388.0 59389.0 59390.0 59391.0 59392.0 59393.0 59394.0 59395.0 59396.0 59397.0 59398.0 59399.0 59400.0 59401.0 59402.0 59403.0 59404.0 59405.0 59406.0 59407.0 59408.0 59409.0 59410.0 59411.0 59412.0 59413.0 59414.0 59415.0 59416.0 59417.0 59418.0 59419.0 59420.0 59421.0 59422.0 59423.0 59424.0 59425.0 59426.0 59427.0 59428.0 59429.0 59430.0 59431.0 59432.0 59433.0 59434.0 59435.0 59436.0 59437.0 59438.0 59439.0 59440.0 59441.0 59442.0 59443.0 59444.0 59445.0 59446.0 59447.0 59448.0 59449.0 59450.0 59451.0 59452.0 59453.0 59454.0 59455.0 59456.0 59457.0 59458.0 59459.0 59460.0 59461.0 59462.0 59463.0 59464.0 59465.0 59466.0 59467.0 59468.0 59469.0 59470.0 59471.0 59472.0 59473.0 59474.0 59475.0 59476.0 59477.0 59478.0 59479.0 59480.0 59481.0 59482.0 59483.0 59484.0 59485.0 59486.0 59487.0 59488.0 59489.0 59490.0 59491.0 59492.0 59493.0 59494.0 59495.0 59496.0 59497.0 59498.0 59499.0 59500.0 59501.0 59502.0 59503.0 59504.0 59505.0 59506.0 59507.0 59508.0 59509.0 59510.0 59511.0 59512.0 59513.0 59514.0 59515.0 59516.0 59517.0 59518.0 59519.0 59520.0 59521.0 59522.0 59523.0 59524.0 59525.0 59526.0 59527.0 59528.0 59529.0 59530.0 59531.0 59532.0 59533.0 59534.0 59535.0 59536.0 59537.0 59538.0 59539.0 59540.0 59541.0 59542.0 59543.0 59544.0 59545.0 59546.0 59547.0 59548.0 59549.0 59550.0 59551.0 59552.0 59553.0 59554.0 59555.0 59556.0 59557.0 59558.0 59559.0 59560.0 59561.0 59562.0 59563.0 59564.0 59565.0 59566.0 59567.0 59568.0 59569.0 59570.0 59571.0 59572.0 59573.0 59574.0 59575.0 59576.0 59577.0 59578.0 59579.0 59580.0 59581.0 59582.0 59583.0 59584.0 59585.0 59586.0 59587.0 59588.0 59589.0 59590.0 59591.0 59592.0 59593.0 59594.0 59595.0 59596.0 59597.0 59598.0 59599.0 59600.0 59601.0 59602.0 59603.0 59604.0 59605.0 59606.0 59607.0 59608.0 59609.0 59610.0 59611.0 59612.0 59613.0 59614.0 59615.0 59616.0 59617.0 59618.0 59619.0 59620.0 59621.0 59622.0 59623.0 59624.0 59625.0 59626.0 59627.0 59628.0 59629.0 59630.0 59631.0 59632.0 59633.0 59634.0 59635.0 59636.0 59637.0 59638.0 59639.0 59640.0 59641.0 59642.0 59643.0 59644.0 59645.0 59646.0 59647.0 59648.0 59649.0 59650.0 59651.0 59652.0 59653.0 59654.0 59655.0 59656.0 59657.0 59658.0 59659.0 59660.0 59661.0 59662.0 59663.0 59664.0 59665.0 59666.0 59667.0 59668.0 59669.0 59670.0 59671.0 59672.0 59673.0 59674.0 59675.0 59676.0 59677.0 59678.0 59679.0 59680.0 59681.0 59682.0 59683.0 59684.0 59685.0 59686.0 59687.0 59688.0 59689.0 59690.0 59691.0 59692.0 59693.0 59694.0 59695.0 59696.0 59697.0 59698.0 59699.0 59700.0 59701.0 59702.0 59703.0 59704.0 59705.0 59706.0 59707.0 59708.0 59709.0 59710.0 59711.0 59712.0 59713.0 59714.0 59715.0 59716.0 59717.0 59718.0 59719.0 59720.0 59721.0 59722.0 59723.0 59724.0 59725.0 59726.0 59727.0 59728.0 59729.0 59730.0 59731.0 59732.0 59733.0 59734.0 59735.0 59736.0 59737.0 59738.0 59739.0 59740.0 59741.0 59742.0 59743.0 59744.0 59745.0 59746.0 59747.0 59748.0 59749.0 59750.0 59751.0 59752.0 59753.0 59754.0 59755.0 59756.0 59757.0 59758.0 59759.0 59760.0 59761.0 59762.0 59763.0 59764.0 59765.0 59766.0 59767.0 59768.0 59769.0 59770.0 59771.0 59772.0 59773.0 59774.0 59775.0 59776.0 59777.0 59778.0 59779.0 59780.0 59781.0 59782.0 59783.0 59784.0 59785.0 59786.0 59787.0 59788.0 59789.0 59790.0 59791.0 59792.0 59793.0 59794.0 59795.0 59796.0 59797.0 59798.0 59799.0 59800.0 59801.0 59802.0 59803.0 59804.0 59805.0 59806.0 59807.0 59808.0 59809.0 59810.0 59811.0 59812.0 59813.0 59814.0 59815.0 59816.0 59817.0 59818.0 59819.0 59820.0 59821.0 59822.0 59823.0 59824.0 59825.0 59826.0 59827.0 59828.0 59829.0 59830.0 59831.0 59832.0 59833.0 59834.0 59835.0 59836.0 59837.0 59838.0 59839.0 59840.0 59841.0 59842.0 59843.0 59844.0 59845.0 59846.0 59847.0 59848.0 59849.0 59850.0 59851.0 59852.0 59853.0 59854.0 59855.0 59856.0 59857.0 59858.0 59859.0 59860.0 59861.0 59862.0 59863.0 59864.0 59865.0 59866.0 59867.0 59868.0 59869.0 59870.0 59871.0 59872.0 59873.0 59874.0 59875.0 59876.0 59877.0 59878.0 59879.0 59880.0 59881.0 59882.0 59883.0 59884.0 59885.0 59886.0 59887.0 59888.0 59889.0 59890.0 59891.0 59892.0 59893.0 59894.0 59895.0 59896.0 59897.0 59898.0 59899.0 59900.0 59901.0 59902.0 59903.0 59904.0 59905.0 59906.0 59907.0 59908.0 59909.0 59910.0 59911.0 59912.0 59913.0 59914.0 59915.0 59916.0 59917.0 59918.0 59919.0 59920.0 59921.0 59922.0 59923.0 59924.0 59925.0 59926.0 59927.0 59928.0 59929.0 59930.0 59931.0 59932.0 59933.0 59934.0 59935.0 59936.0 59937.0 59938.0 59939.0 59940.0 59941.0 59942.0 59943.0 59944.0 59945.0 59946.0 59947.0 59948.0 59949.0 59950.0 59951.0 59952.0 59953.0 59954.0 59955.0 59956.0 59957.0 59958.0 59959.0 59960.0 59961.0 59962.0 59963.0 59964.0 59965.0 59966.0 59967.0 59968.0 59969.0 59970.0 59971.0 59972.0 59973.0 59974.0 59975.0 59976.0 59977.0 59978.0 59979.0 59980.0 59981.0 59982.0 59983.0 59984.0 59985.0 59986.0 59987.0 59988.0 59989.0 59990.0 59991.0 59992.0 59993.0 59994.0 59995.0 59996.0 59997.0 59998.0 59999.0 60000.0 60001.0 60002.0 60003.0 60004.0 60005.0 60006.0 60007.0 60008.0 60009.0 60010.0 60011.0 60012.0 60013.0 60014.0 60015.0 60016.0 60017.0 60018.0 60019.0 60020.0 60021.0 60022.0 60023.0 60024.0 60025.0 60026.0 60027.0 60028.0 60029.0 60030.0 60031.0 60032.0 60033.0 60034.0 60035.0 60036.0 60037.0 60038.0 60039.0 60040.0 60041.0 60042.0 60043.0 60044.0 60045.0 60046.0 60047.0 60048.0 60049.0 60050.0 60051.0 60052.0 60053.0 60054.0 60055.0 60056.0 60057.0 60058.0 60059.0 60060.0 60061.0 60062.0 60063.0 60064.0 60065.0 60066.0 60067.0 60068.0 60069.0 60070.0 60071.0 60072.0 60073.0 60074.0 60075.0 60076.0 60077.0 60078.0 60079.0 60080.0 60081.0 60082.0 60083.0 60084.0 60085.0 60086.0 60087.0 60088.0 60089.0 60090.0 60091.0 60092.0 60093.0 60094.0 60095.0 60096.0 60097.0 60098.0 60099.0 60100.0 60101.0 60102.0 60103.0 60104.0 60105.0 60106.0 60107.0 60108.0 60109.0 60110.0 60111.0 60112.0 60113.0 60114.0 60115.0 60116.0 60117.0 60118.0 60119.0 60120.0 60121.0 60122.0 60123.0 60124.0 60125.0 60126.0 60127.0 60128.0 60129.0 60130.0 60131.0 60132.0 60133.0 60134.0 60135.0 60136.0 60137.0 60138.0 60139.0 60140.0 60141.0 60142.0 60143.0 60144.0 60145.0 60146.0 60147.0 60148.0 60149.0 60150.0 60151.0 60152.0 60153.0 60154.0 60155.0 60156.0 60157.0 60158.0 60159.0 60160.0 60161.0 60162.0 60163.0 60164.0 60165.0 60166.0 60167.0 60168.0 60169.0 60170.0 60171.0 60172.0 60173.0 60174.0 60175.0 60176.0 60177.0 60178.0 60179.0 60180.0 60181.0 60182.0 60183.0 60184.0 60185.0 60186.0 60187.0 60188.0 60189.0 60190.0 60191.0 60192.0 60193.0 60194.0 60195.0 60196.0 60197.0 60198.0 60199.0 60200.0 60201.0 60202.0 60203.0 60204.0 60205.0 60206.0 60207.0 60208.0 60209.0 60210.0 60211.0 60212.0 60213.0 60214.0 60215.0 60216.0 60217.0 60218.0 60219.0 60220.0 60221.0 60222.0 60223.0 60224.0 60225.0 60226.0 60227.0 60228.0 60229.0 60230.0 60231.0 60232.0 60233.0 60234.0 60235.0 60236.0 60237.0 60238.0 60239.0 60240.0 60241.0 60242.0 60243.0 60244.0 60245.0 60246.0 60247.0 60248.0 60249.0 60250.0 60251.0 60252.0 60253.0 60254.0 60255.0 60256.0 60257.0 60258.0 60259.0 60260.0 60261.0 60262.0 60263.0 60264.0 60265.0 60266.0 60267.0 60268.0 60269.0 60270.0 60271.0 60272.0 60273.0 60274.0 60275.0 60276.0 60277.0 60278.0 60279.0 60280.0 60281.0 60282.0 60283.0 60284.0 60285.0 60286.0 60287.0 60288.0 60289.0 60290.0 60291.0 60292.0 60293.0 60294.0 60295.0 60296.0 60297.0 60298.0 60299.0 60300.0 60301.0 60302.0 60303.0 60304.0 60305.0 60306.0 60307.0 60308.0 60309.0 60310.0 60311.0 60312.0 60313.0 60314.0 60315.0 60316.0 60317.0 60318.0 60319.0 60320.0 60321.0 60322.0 60323.0 60324.0 60325.0 60326.0 60327.0 60328.0 60329.0 60330.0 60331.0 60332.0 60333.0 60334.0 60335.0 60336.0 60337.0 60338.0 60339.0 60340.0 60341.0 60342.0 60343.0 60344.0 60345.0 60346.0 60347.0 60348.0 60349.0 60350.0 60351.0 60352.0 60353.0 60354.0 60355.0 60356.0 60357.0 60358.0 60359.0 60360.0 60361.0 60362.0 60363.0 60364.0 60365.0 60366.0 60367.0 60368.0 60369.0 60370.0 60371.0 60372.0 60373.0 60374.0 60375.0 60376.0 60377.0 60378.0 60379.0 60380.0 60381.0 60382.0 60383.0 60384.0 60385.0 60386.0 60387.0 60388.0 60389.0 60390.0 60391.0 60392.0 60393.0 60394.0 60395.0 60396.0 60397.0 60398.0 60399.0 60400.0 60401.0 60402.0 60403.0 60404.0 60405.0 60406.0 60407.0 60408.0 60409.0 60410.0 60411.0 60412.0 60413.0 60414.0 60415.0 60416.0 60417.0 60418.0 60419.0 60420.0 60421.0 60422.0 60423.0 60424.0 60425.0 60426.0 60427.0 60428.0 60429.0 60430.0 60431.0 60432.0 60433.0 60434.0 60435.0 60436.0 60437.0 60438.0 60439.0 60440.0 60441.0 60442.0 60443.0 60444.0 60445.0 60446.0 60447.0 60448.0 60449.0 60450.0 60451.0 60452.0 60453.0 60454.0 60455.0 60456.0 60457.0 60458.0 60459.0 60460.0 60461.0 60462.0 60463.0 60464.0 60465.0 60466.0 60467.0 60468.0 60469.0 60470.0 60471.0 60472.0 60473.0 60474.0 60475.0 60476.0 60477.0 60478.0 60479.0 60480.0 60481.0 60482.0 60483.0 60484.0 60485.0 60486.0 60487.0 60488.0 60489.0 60490.0 60491.0 60492.0 60493.0 60494.0 60495.0 60496.0 60497.0 60498.0 60499.0 60500.0 60501.0 60502.0 60503.0 60504.0 60505.0 60506.0 60507.0 60508.0 60509.0 60510.0 60511.0 60512.0 60513.0 60514.0 60515.0 60516.0 60517.0 60518.0 60519.0 60520.0 60521.0 60522.0 60523.0 60524.0 60525.0 60526.0 60527.0 60528.0 60529.0 60530.0 60531.0 60532.0 60533.0 60534.0 60535.0 60536.0 60537.0 60538.0 60539.0 60540.0 60541.0 60542.0 60543.0 60544.0 60545.0 60546.0 60547.0 60548.0 60549.0 60550.0 60551.0 60552.0 60553.0 60554.0 60555.0 60556.0 60557.0 60558.0 60559.0 60560.0 60561.0 60562.0 60563.0 60564.0 60565.0 60566.0 60567.0 60568.0 60569.0 60570.0 60571.0 60572.0 60573.0 60574.0 60575.0 60576.0 60577.0 60578.0 60579.0 60580.0 60581.0 60582.0 60583.0 60584.0 60585.0 60586.0 60587.0 60588.0 60589.0 60590.0 60591.0 60592.0 60593.0 60594.0 60595.0 60596.0 60597.0 60598.0 60599.0 60600.0 60601.0 60602.0 60603.0 60604.0 60605.0 60606.0 60607.0 60608.0 60609.0 60610.0 60611.0 60612.0 60613.0 60614.0 60615.0 60616.0 60617.0 60618.0 60619.0 60620.0 60621.0 60622.0 60623.0 60624.0 60625.0 60626.0 60627.0 60628.0 60629.0 60630.0 60631.0 60632.0 60633.0 60634.0 60635.0 60636.0 60637.0 60638.0 60639.0 60640.0 60641.0 60642.0 60643.0 60644.0 60645.0 60646.0 60647.0 60648.0 60649.0 60650.0 60651.0 60652.0 60653.0 60654.0 60655.0 60656.0 60657.0 60658.0 60659.0 60660.0 60661.0 60662.0 60663.0 60664.0 60665.0 60666.0 60667.0 60668.0 60669.0 60670.0 60671.0 60672.0 60673.0 60674.0 60675.0 60676.0 60677.0 60678.0 60679.0 60680.0 60681.0 60682.0 60683.0 60684.0 60685.0 60686.0 60687.0 60688.0 60689.0 60690.0 60691.0 60692.0 60693.0 60694.0 60695.0 60696.0 60697.0 60698.0 60699.0 60700.0 60701.0 60702.0 60703.0 60704.0 60705.0 60706.0 60707.0 60708.0 60709.0 60710.0 60711.0 60712.0 60713.0 60714.0 60715.0 60716.0 60717.0 60718.0 60719.0 60720.0 60721.0 60722.0 60723.0 60724.0 60725.0 60726.0 60727.0 60728.0 60729.0 60730.0 60731.0 60732.0 60733.0 60734.0 60735.0 60736.0 60737.0 60738.0 60739.0 60740.0 60741.0 60742.0 60743.0 60744.0 60745.0 60746.0 60747.0 60748.0 60749.0 60750.0 60751.0 60752.0 60753.0 60754.0 60755.0 60756.0 60757.0 60758.0 60759.0 60760.0 60761.0 60762.0 60763.0 60764.0 60765.0 60766.0 60767.0 60768.0 60769.0 60770.0 60771.0 60772.0 60773.0 60774.0 60775.0 60776.0 60777.0 60778.0 60779.0 60780.0 60781.0 60782.0 60783.0 60784.0 60785.0 60786.0 60787.0 60788.0 60789.0 60790.0 60791.0 60792.0 60793.0 60794.0 60795.0 60796.0 60797.0 60798.0 60799.0 60800.0 60801.0 60802.0 60803.0 60804.0 60805.0 60806.0 60807.0 60808.0 60809.0 60810.0 60811.0 60812.0 60813.0 60814.0 60815.0 60816.0 60817.0 60818.0 60819.0 60820.0 60821.0 60822.0 60823.0 60824.0 60825.0 60826.0 60827.0 60828.0 60829.0 60830.0 60831.0 60832.0 60833.0 60834.0 60835.0 60836.0 60837.0 60838.0 60839.0 60840.0 60841.0 60842.0 60843.0 60844.0 60845.0 60846.0 60847.0 60848.0 60849.0 60850.0 60851.0 60852.0 60853.0 60854.0 60855.0 60856.0 60857.0 60858.0 60859.0 60860.0 60861.0 60862.0 60863.0 60864.0 60865.0 60866.0 60867.0 60868.0 60869.0 60870.0 60871.0 60872.0 60873.0 60874.0 60875.0 60876.0 60877.0 60878.0 60879.0 60880.0 60881.0 60882.0 60883.0 60884.0 60885.0 60886.0 60887.0 60888.0 60889.0 60890.0 60891.0 60892.0 60893.0 60894.0 60895.0 60896.0 60897.0 60898.0 60899.0 60900.0 60901.0 60902.0 60903.0 60904.0 60905.0 60906.0 60907.0 60908.0 60909.0 60910.0 60911.0 60912.0 60913.0 60914.0 60915.0 60916.0 60917.0 60918.0 60919.0 60920.0 60921.0 60922.0 60923.0 60924.0 60925.0 60926.0 60927.0 60928.0 60929.0 60930.0 60931.0 60932.0 60933.0 60934.0 60935.0 60936.0 60937.0 60938.0 60939.0 60940.0 60941.0 60942.0 60943.0 60944.0 60945.0 60946.0 60947.0 60948.0 60949.0 60950.0 60951.0 60952.0 60953.0 60954.0 60955.0 60956.0 60957.0 60958.0 60959.0 60960.0 60961.0 60962.0 60963.0 60964.0 60965.0 60966.0 60967.0 60968.0 60969.0 60970.0 60971.0 60972.0 60973.0 60974.0 60975.0 60976.0 60977.0 60978.0 60979.0 60980.0 60981.0 60982.0 60983.0 60984.0 60985.0 60986.0 60987.0 60988.0 60989.0 60990.0 60991.0 60992.0 60993.0 60994.0 60995.0 60996.0 60997.0 60998.0 60999.0 61000.0 61001.0 61002.0 61003.0 61004.0 61005.0 61006.0 61007.0 61008.0 61009.0 61010.0 61011.0 61012.0 61013.0 61014.0 61015.0 61016.0 61017.0 61018.0 61019.0 61020.0 61021.0 61022.0 61023.0 61024.0 61025.0 61026.0 61027.0 61028.0 61029.0 61030.0 61031.0 61032.0 61033.0 61034.0 61035.0 61036.0 61037.0 61038.0 61039.0 61040.0 61041.0 61042.0 61043.0 61044.0 61045.0 61046.0 61047.0 61048.0 61049.0 61050.0 61051.0 61052.0 61053.0 61054.0 61055.0 61056.0 61057.0 61058.0 61059.0 61060.0 61061.0 61062.0 61063.0 61064.0 61065.0 61066.0 61067.0 61068.0 61069.0 61070.0 61071.0 61072.0 61073.0 61074.0 61075.0 61076.0 61077.0 61078.0 61079.0 61080.0 61081.0 61082.0 61083.0 61084.0 61085.0 61086.0 61087.0 61088.0 61089.0 61090.0 61091.0 61092.0 61093.0 61094.0 61095.0 61096.0 61097.0 61098.0 61099.0 61100.0 61101.0 61102.0 61103.0 61104.0 61105.0 61106.0 61107.0 61108.0 61109.0 61110.0 61111.0 61112.0 61113.0 61114.0 61115.0 61116.0 61117.0 61118.0 61119.0 61120.0 61121.0 61122.0 61123.0 61124.0 61125.0 61126.0 61127.0 61128.0 61129.0 61130.0 61131.0 61132.0 61133.0 61134.0 61135.0 61136.0 61137.0 61138.0 61139.0 61140.0 61141.0 61142.0 61143.0 61144.0 61145.0 61146.0 61147.0 61148.0 61149.0 61150.0 61151.0 61152.0 61153.0 61154.0 61155.0 61156.0 61157.0 61158.0 61159.0 61160.0 61161.0 61162.0 61163.0 61164.0 61165.0 61166.0 61167.0 61168.0 61169.0 61170.0 61171.0 61172.0 61173.0 61174.0 61175.0 61176.0 61177.0 61178.0 61179.0 61180.0 61181.0 61182.0 61183.0 61184.0 61185.0 61186.0 61187.0 61188.0 61189.0 61190.0 61191.0 61192.0 61193.0 61194.0 61195.0 61196.0 61197.0 61198.0 61199.0 61200.0 61201.0 61202.0 61203.0 61204.0 61205.0 61206.0 61207.0 61208.0 61209.0 61210.0 61211.0 61212.0 61213.0 61214.0 61215.0 61216.0 61217.0 61218.0 61219.0 61220.0 61221.0 61222.0 61223.0 61224.0 61225.0 61226.0 61227.0 61228.0 61229.0 61230.0 61231.0 61232.0 61233.0 61234.0 61235.0 61236.0 61237.0 61238.0 61239.0 61240.0 61241.0 61242.0 61243.0 61244.0 61245.0 61246.0 61247.0 61248.0 61249.0 61250.0 61251.0 61252.0 61253.0 61254.0 61255.0 61256.0 61257.0 61258.0 61259.0 61260.0 61261.0 61262.0 61263.0 61264.0 61265.0 61266.0 61267.0 61268.0 61269.0 61270.0 61271.0 61272.0 61273.0 61274.0 61275.0 61276.0 61277.0 61278.0 61279.0 61280.0 61281.0 61282.0 61283.0 61284.0 61285.0 61286.0 61287.0 61288.0 61289.0 61290.0 61291.0 61292.0 61293.0 61294.0 61295.0 61296.0 61297.0 61298.0 61299.0 61300.0 61301.0 61302.0 61303.0 61304.0 61305.0 61306.0 61307.0 61308.0 61309.0 61310.0 61311.0 61312.0 61313.0 61314.0 61315.0 61316.0 61317.0 61318.0 61319.0 61320.0 61321.0 61322.0 61323.0 61324.0 61325.0 61326.0 61327.0 61328.0 61329.0 61330.0 61331.0 61332.0 61333.0 61334.0 61335.0 61336.0 61337.0 61338.0 61339.0 61340.0 61341.0 61342.0 61343.0 61344.0 61345.0 61346.0 61347.0 61348.0 61349.0 61350.0 61351.0 61352.0 61353.0 61354.0 61355.0 61356.0 61357.0 61358.0 61359.0 61360.0 61361.0 61362.0 61363.0 61364.0 61365.0 61366.0 61367.0 61368.0 61369.0 61370.0 61371.0 61372.0 61373.0 61374.0 61375.0 61376.0 61377.0 61378.0 61379.0 61380.0 61381.0 61382.0 61383.0 61384.0 61385.0 61386.0 61387.0 61388.0 61389.0 61390.0 61391.0 61392.0 61393.0 61394.0 61395.0 61396.0 61397.0 61398.0 61399.0 61400.0 61401.0 61402.0 61403.0 61404.0 61405.0 61406.0 61407.0 61408.0 61409.0 61410.0 61411.0 61412.0 61413.0 61414.0 61415.0 61416.0 61417.0 61418.0 61419.0 61420.0 61421.0 61422.0 61423.0 61424.0 61425.0 61426.0 61427.0 61428.0 61429.0 61430.0 61431.0 61432.0 61433.0 61434.0 61435.0 61436.0 61437.0 61438.0 61439.0 61440.0 61441.0 61442.0 61443.0 61444.0 61445.0 61446.0 61447.0 61448.0 61449.0 61450.0 61451.0 61452.0 61453.0 61454.0 61455.0 61456.0 61457.0 61458.0 61459.0 61460.0 61461.0 61462.0 61463.0 61464.0 61465.0 61466.0 61467.0 61468.0 61469.0 61470.0 61471.0 61472.0 61473.0 61474.0 61475.0 61476.0 61477.0 61478.0 61479.0 61480.0 61481.0 61482.0 61483.0 61484.0 61485.0 61486.0 61487.0 61488.0 61489.0 61490.0 61491.0 61492.0 61493.0 61494.0 61495.0 61496.0 61497.0 61498.0 61499.0 61500.0 61501.0 61502.0 61503.0 61504.0 61505.0 61506.0 61507.0 61508.0 61509.0 61510.0 61511.0 61512.0 61513.0 61514.0 61515.0 61516.0 61517.0 61518.0 61519.0 61520.0 61521.0 61522.0 61523.0 61524.0 61525.0 61526.0 61527.0 61528.0 61529.0 61530.0 61531.0 61532.0 61533.0 61534.0 61535.0 61536.0 61537.0 61538.0 61539.0 61540.0 61541.0 61542.0 61543.0 61544.0 61545.0 61546.0 61547.0 61548.0 61549.0 61550.0 61551.0 61552.0 61553.0 61554.0 61555.0 61556.0 61557.0 61558.0 61559.0 61560.0 61561.0 61562.0 61563.0 61564.0 61565.0 61566.0 61567.0 61568.0 61569.0 61570.0 61571.0 61572.0 61573.0 61574.0 61575.0 61576.0 61577.0 61578.0 61579.0 61580.0 61581.0 61582.0 61583.0 61584.0 61585.0 61586.0 61587.0 61588.0 61589.0 61590.0 61591.0 61592.0 61593.0 61594.0 61595.0 61596.0 61597.0 61598.0 61599.0 61600.0 61601.0 61602.0 61603.0 61604.0 61605.0 61606.0 61607.0 61608.0 61609.0 61610.0 61611.0 61612.0 61613.0 61614.0 61615.0 61616.0 61617.0 61618.0 61619.0 61620.0 61621.0 61622.0 61623.0 61624.0 61625.0 61626.0 61627.0 61628.0 61629.0 61630.0 61631.0 61632.0 61633.0 61634.0 61635.0 61636.0 61637.0 61638.0 61639.0 61640.0 61641.0 61642.0 61643.0 61644.0 61645.0 61646.0 61647.0 61648.0 61649.0 61650.0 61651.0 61652.0 61653.0 61654.0 61655.0 61656.0 61657.0 61658.0 61659.0 61660.0 61661.0 61662.0 61663.0 61664.0 61665.0 61666.0 61667.0 61668.0 61669.0 61670.0 61671.0 61672.0 61673.0 61674.0 61675.0 61676.0 61677.0 61678.0 61679.0 61680.0 61681.0 61682.0 61683.0 61684.0 61685.0 61686.0 61687.0 61688.0 61689.0 61690.0 61691.0 61692.0 61693.0 61694.0 61695.0 61696.0 61697.0 61698.0 61699.0 61700.0 61701.0 61702.0 61703.0 61704.0 61705.0 61706.0 61707.0 61708.0 61709.0 61710.0 61711.0 61712.0 61713.0 61714.0 61715.0 61716.0 61717.0 61718.0 61719.0 61720.0 61721.0 61722.0 61723.0 61724.0 61725.0 61726.0 61727.0 61728.0 61729.0 61730.0 61731.0 61732.0 61733.0 61734.0 61735.0 61736.0 61737.0 61738.0 61739.0 61740.0 61741.0 61742.0 61743.0 61744.0 61745.0 61746.0 61747.0 61748.0 61749.0 61750.0 61751.0 61752.0 61753.0 61754.0 61755.0 61756.0 61757.0 61758.0 61759.0 61760.0 61761.0 61762.0 61763.0 61764.0 61765.0 61766.0 61767.0 61768.0 61769.0 61770.0 61771.0 61772.0 61773.0 61774.0 61775.0 61776.0 61777.0 61778.0 61779.0 61780.0 61781.0 61782.0 61783.0 61784.0 61785.0 61786.0 61787.0 61788.0 61789.0 61790.0 61791.0 61792.0 61793.0 61794.0 61795.0 61796.0 61797.0 61798.0 61799.0 61800.0 61801.0 61802.0 61803.0 61804.0 61805.0 61806.0 61807.0 61808.0 61809.0 61810.0 61811.0 61812.0 61813.0 61814.0 61815.0 61816.0 61817.0 61818.0 61819.0 61820.0 61821.0 61822.0 61823.0 61824.0 61825.0 61826.0 61827.0 61828.0 61829.0 61830.0 61831.0 61832.0 61833.0 61834.0 61835.0 61836.0 61837.0 61838.0 61839.0 61840.0 61841.0 61842.0 61843.0 61844.0 61845.0 61846.0 61847.0 61848.0 61849.0 61850.0 61851.0 61852.0 61853.0 61854.0 61855.0 61856.0 61857.0 61858.0 61859.0 61860.0 61861.0 61862.0 61863.0 61864.0 61865.0 61866.0 61867.0 61868.0 61869.0 61870.0 61871.0 61872.0 61873.0 61874.0 61875.0 61876.0 61877.0 61878.0 61879.0 61880.0 61881.0 61882.0 61883.0 61884.0 61885.0 61886.0 61887.0 61888.0 61889.0 61890.0 61891.0 61892.0 61893.0 61894.0 61895.0 61896.0 61897.0 61898.0 61899.0 61900.0 61901.0 61902.0 61903.0 61904.0 61905.0 61906.0 61907.0 61908.0 61909.0 61910.0 61911.0 61912.0 61913.0 61914.0 61915.0 61916.0 61917.0 61918.0 61919.0 61920.0 61921.0 61922.0 61923.0 61924.0 61925.0 61926.0 61927.0 61928.0 61929.0 61930.0 61931.0 61932.0 61933.0 61934.0 61935.0 61936.0 61937.0 61938.0 61939.0 61940.0 61941.0 61942.0 61943.0 61944.0 61945.0 61946.0 61947.0 61948.0 61949.0 61950.0 61951.0 61952.0 61953.0 61954.0 61955.0 61956.0 61957.0 61958.0 61959.0 61960.0 61961.0 61962.0 61963.0 61964.0 61965.0 61966.0 61967.0 61968.0 61969.0 61970.0 61971.0 61972.0 61973.0 61974.0 61975.0 61976.0 61977.0 61978.0 61979.0 61980.0 61981.0 61982.0 61983.0 61984.0 61985.0 61986.0 61987.0 61988.0 61989.0 61990.0 61991.0 61992.0 61993.0 61994.0 61995.0 61996.0 61997.0 61998.0 61999.0 62000.0 62001.0 62002.0 62003.0 62004.0 62005.0 62006.0 62007.0 62008.0 62009.0 62010.0 62011.0 62012.0 62013.0 62014.0 62015.0 62016.0 62017.0 62018.0 62019.0 62020.0 62021.0 62022.0 62023.0 62024.0 62025.0 62026.0 62027.0 62028.0 62029.0 62030.0 62031.0 62032.0 62033.0 62034.0 62035.0 62036.0 62037.0 62038.0 62039.0 62040.0 62041.0 62042.0 62043.0 62044.0 62045.0 62046.0 62047.0 62048.0 62049.0 62050.0 62051.0 62052.0 62053.0 62054.0 62055.0 62056.0 62057.0 62058.0 62059.0 62060.0 62061.0 62062.0 62063.0 62064.0 62065.0 62066.0 62067.0 62068.0 62069.0 62070.0 62071.0 62072.0 62073.0 62074.0 62075.0 62076.0 62077.0 62078.0 62079.0 62080.0 62081.0 62082.0 62083.0 62084.0 62085.0 62086.0 62087.0 62088.0 62089.0 62090.0 62091.0 62092.0 62093.0 62094.0 62095.0 62096.0 62097.0 62098.0 62099.0 62100.0 62101.0 62102.0 62103.0 62104.0 62105.0 62106.0 62107.0 62108.0 62109.0 62110.0 62111.0 62112.0 62113.0 62114.0 62115.0 62116.0 62117.0 62118.0 62119.0 62120.0 62121.0 62122.0 62123.0 62124.0 62125.0 62126.0 62127.0 62128.0 62129.0 62130.0 62131.0 62132.0 62133.0 62134.0 62135.0 62136.0 62137.0 62138.0 62139.0 62140.0 62141.0 62142.0 62143.0 62144.0 62145.0 62146.0 62147.0 62148.0 62149.0 62150.0 62151.0 62152.0 62153.0 62154.0 62155.0 62156.0 62157.0 62158.0 62159.0 62160.0 62161.0 62162.0 62163.0 62164.0 62165.0 62166.0 62167.0 62168.0 62169.0 62170.0 62171.0 62172.0 62173.0 62174.0 62175.0 62176.0 62177.0 62178.0 62179.0 62180.0 62181.0 62182.0 62183.0 62184.0 62185.0 62186.0 62187.0 62188.0 62189.0 62190.0 62191.0 62192.0 62193.0 62194.0 62195.0 62196.0 62197.0 62198.0 62199.0 62200.0 62201.0 62202.0 62203.0 62204.0 62205.0 62206.0 62207.0 62208.0 62209.0 62210.0 62211.0 62212.0 62213.0 62214.0 62215.0 62216.0 62217.0 62218.0 62219.0 62220.0 62221.0 62222.0 62223.0 62224.0 62225.0 62226.0 62227.0 62228.0 62229.0 62230.0 62231.0 62232.0 62233.0 62234.0 62235.0 62236.0 62237.0 62238.0 62239.0 62240.0 62241.0 62242.0 62243.0 62244.0 62245.0 62246.0 62247.0 62248.0 62249.0 62250.0 62251.0 62252.0 62253.0 62254.0 62255.0 62256.0 62257.0 62258.0 62259.0 62260.0 62261.0 62262.0 62263.0 62264.0 62265.0 62266.0 62267.0 62268.0 62269.0 62270.0 62271.0 62272.0 62273.0 62274.0 62275.0 62276.0 62277.0 62278.0 62279.0 62280.0 62281.0 62282.0 62283.0 62284.0 62285.0 62286.0 62287.0 62288.0 62289.0 62290.0 62291.0 62292.0 62293.0 62294.0 62295.0 62296.0 62297.0 62298.0 62299.0 62300.0 62301.0 62302.0 62303.0 62304.0 62305.0 62306.0 62307.0 62308.0 62309.0 62310.0 62311.0 62312.0 62313.0 62314.0 62315.0 62316.0 62317.0 62318.0 62319.0 62320.0 62321.0 62322.0 62323.0 62324.0 62325.0 62326.0 62327.0 62328.0 62329.0 62330.0 62331.0 62332.0 62333.0 62334.0 62335.0 62336.0 62337.0 62338.0 62339.0 62340.0 62341.0 62342.0 62343.0 62344.0 62345.0 62346.0 62347.0 62348.0 62349.0 62350.0 62351.0 62352.0 62353.0 62354.0 62355.0 62356.0 62357.0 62358.0 62359.0 62360.0 62361.0 62362.0 62363.0 62364.0 62365.0 62366.0 62367.0 62368.0 62369.0 62370.0 62371.0 62372.0 62373.0 62374.0 62375.0 62376.0 62377.0 62378.0 62379.0 62380.0 62381.0 62382.0 62383.0 62384.0 62385.0 62386.0 62387.0 62388.0 62389.0 62390.0 62391.0 62392.0 62393.0 62394.0 62395.0 62396.0 62397.0 62398.0 62399.0 62400.0 62401.0 62402.0 62403.0 62404.0 62405.0 62406.0 62407.0 62408.0 62409.0 62410.0 62411.0 62412.0 62413.0 62414.0 62415.0 62416.0 62417.0 62418.0 62419.0 62420.0 62421.0 62422.0 62423.0 62424.0 62425.0 62426.0 62427.0 62428.0 62429.0 62430.0 62431.0 62432.0 62433.0 62434.0 62435.0 62436.0 62437.0 62438.0 62439.0 62440.0 62441.0 62442.0 62443.0 62444.0 62445.0 62446.0 62447.0 62448.0 62449.0 62450.0 62451.0 62452.0 62453.0 62454.0 62455.0 62456.0 62457.0 62458.0 62459.0 62460.0 62461.0 62462.0 62463.0 62464.0 62465.0 62466.0 62467.0 62468.0 62469.0 62470.0 62471.0 62472.0 62473.0 62474.0 62475.0 62476.0 62477.0 62478.0 62479.0 62480.0 62481.0 62482.0 62483.0 62484.0 62485.0 62486.0 62487.0 62488.0 62489.0 62490.0 62491.0 62492.0 62493.0 62494.0 62495.0 62496.0 62497.0 62498.0 62499.0 62500.0 62501.0 62502.0 62503.0 62504.0 62505.0 62506.0 62507.0 62508.0 62509.0 62510.0 62511.0 62512.0 62513.0 62514.0 62515.0 62516.0 62517.0 62518.0 62519.0 62520.0 62521.0 62522.0 62523.0 62524.0 62525.0 62526.0 62527.0 62528.0 62529.0 62530.0 62531.0 62532.0 62533.0 62534.0 62535.0 62536.0 62537.0 62538.0 62539.0 62540.0 62541.0 62542.0 62543.0 62544.0 62545.0 62546.0 62547.0 62548.0 62549.0 62550.0 62551.0 62552.0 62553.0 62554.0 62555.0 62556.0 62557.0 62558.0 62559.0 62560.0 62561.0 62562.0 62563.0 62564.0 62565.0 62566.0 62567.0 62568.0 62569.0 62570.0 62571.0 62572.0 62573.0 62574.0 62575.0 62576.0 62577.0 62578.0 62579.0 62580.0 62581.0 62582.0 62583.0 62584.0 62585.0 62586.0 62587.0 62588.0 62589.0 62590.0 62591.0 62592.0 62593.0 62594.0 62595.0 62596.0 62597.0 62598.0 62599.0 62600.0 62601.0 62602.0 62603.0 62604.0 62605.0 62606.0 62607.0 62608.0 62609.0 62610.0 62611.0 62612.0 62613.0 62614.0 62615.0 62616.0 62617.0 62618.0 62619.0 62620.0 62621.0 62622.0 62623.0 62624.0 62625.0 62626.0 62627.0 62628.0 62629.0 62630.0 62631.0 62632.0 62633.0 62634.0 62635.0 62636.0 62637.0 62638.0 62639.0 62640.0 62641.0 62642.0 62643.0 62644.0 62645.0 62646.0 62647.0 62648.0 62649.0 62650.0 62651.0 62652.0 62653.0 62654.0 62655.0 62656.0 62657.0 62658.0 62659.0 62660.0 62661.0 62662.0 62663.0 62664.0 62665.0 62666.0 62667.0 62668.0 62669.0 62670.0 62671.0 62672.0 62673.0 62674.0 62675.0 62676.0 62677.0 62678.0 62679.0 62680.0 62681.0 62682.0 62683.0 62684.0 62685.0 62686.0 62687.0 62688.0 62689.0 62690.0 62691.0 62692.0 62693.0 62694.0 62695.0 62696.0 62697.0 62698.0 62699.0 62700.0 62701.0 62702.0 62703.0 62704.0 62705.0 62706.0 62707.0 62708.0 62709.0 62710.0 62711.0 62712.0 62713.0 62714.0 62715.0 62716.0 62717.0 62718.0 62719.0 62720.0 62721.0 62722.0 62723.0 62724.0 62725.0 62726.0 62727.0 62728.0 62729.0 62730.0 62731.0 62732.0 62733.0 62734.0 62735.0 62736.0 62737.0 62738.0 62739.0 62740.0 62741.0 62742.0 62743.0 62744.0 62745.0 62746.0 62747.0 62748.0 62749.0 62750.0 62751.0 62752.0 62753.0 62754.0 62755.0 62756.0 62757.0 62758.0 62759.0 62760.0 62761.0 62762.0 62763.0 62764.0 62765.0 62766.0 62767.0 62768.0 62769.0 62770.0 62771.0 62772.0 62773.0 62774.0 62775.0 62776.0 62777.0 62778.0 62779.0 62780.0 62781.0 62782.0 62783.0 62784.0 62785.0 62786.0 62787.0 62788.0 62789.0 62790.0 62791.0 62792.0 62793.0 62794.0 62795.0 62796.0 62797.0 62798.0 62799.0 62800.0 62801.0 62802.0 62803.0 62804.0 62805.0 62806.0 62807.0 62808.0 62809.0 62810.0 62811.0 62812.0 62813.0 62814.0 62815.0 62816.0 62817.0 62818.0 62819.0 62820.0 62821.0 62822.0 62823.0 62824.0 62825.0 62826.0 62827.0 62828.0 62829.0 62830.0 62831.0 62832.0 62833.0 62834.0 62835.0 62836.0 62837.0 62838.0 62839.0 62840.0 62841.0 62842.0 62843.0 62844.0 62845.0 62846.0 62847.0 62848.0 62849.0 62850.0 62851.0 62852.0 62853.0 62854.0 62855.0 62856.0 62857.0 62858.0 62859.0 62860.0 62861.0 62862.0 62863.0 62864.0 62865.0 62866.0 62867.0 62868.0 62869.0 62870.0 62871.0 62872.0 62873.0 62874.0 62875.0 62876.0 62877.0 62878.0 62879.0 62880.0 62881.0 62882.0 62883.0 62884.0 62885.0 62886.0 62887.0 62888.0 62889.0 62890.0 62891.0 62892.0 62893.0 62894.0 62895.0 62896.0 62897.0 62898.0 62899.0 62900.0 62901.0 62902.0 62903.0 62904.0 62905.0 62906.0 62907.0 62908.0 62909.0 62910.0 62911.0 62912.0 62913.0 62914.0 62915.0 62916.0 62917.0 62918.0 62919.0 62920.0 62921.0 62922.0 62923.0 62924.0 62925.0 62926.0 62927.0 62928.0 62929.0 62930.0 62931.0 62932.0 62933.0 62934.0 62935.0 62936.0 62937.0 62938.0 62939.0 62940.0 62941.0 62942.0 62943.0 62944.0 62945.0 62946.0 62947.0 62948.0 62949.0 62950.0 62951.0 62952.0 62953.0 62954.0 62955.0 62956.0 62957.0 62958.0 62959.0 62960.0 62961.0 62962.0 62963.0 62964.0 62965.0 62966.0 62967.0 62968.0 62969.0 62970.0 62971.0 62972.0 62973.0 62974.0 62975.0 62976.0 62977.0 62978.0 62979.0 62980.0 62981.0 62982.0 62983.0 62984.0 62985.0 62986.0 62987.0 62988.0 62989.0 62990.0 62991.0 62992.0 62993.0 62994.0 62995.0 62996.0 62997.0 62998.0 62999.0 63000.0 63001.0 63002.0 63003.0 63004.0 63005.0 63006.0 63007.0 63008.0 63009.0 63010.0 63011.0 63012.0 63013.0 63014.0 63015.0 63016.0 63017.0 63018.0 63019.0 63020.0 63021.0 63022.0 63023.0 63024.0 63025.0 63026.0 63027.0 63028.0 63029.0 63030.0 63031.0 63032.0 63033.0 63034.0 63035.0 63036.0 63037.0 63038.0 63039.0 63040.0 63041.0 63042.0 63043.0 63044.0 63045.0 63046.0 63047.0 63048.0 63049.0 63050.0 63051.0 63052.0 63053.0 63054.0 63055.0 63056.0 63057.0 63058.0 63059.0 63060.0 63061.0 63062.0 63063.0 63064.0 63065.0 63066.0 63067.0 63068.0 63069.0 63070.0 63071.0 63072.0 63073.0 63074.0 63075.0 63076.0 63077.0 63078.0 63079.0 63080.0 63081.0 63082.0 63083.0 63084.0 63085.0 63086.0 63087.0 63088.0 63089.0 63090.0 63091.0 63092.0 63093.0 63094.0 63095.0 63096.0 63097.0 63098.0 63099.0 63100.0 63101.0 63102.0 63103.0 63104.0 63105.0 63106.0 63107.0 63108.0 63109.0 63110.0 63111.0 63112.0 63113.0 63114.0 63115.0 63116.0 63117.0 63118.0 63119.0 63120.0 63121.0 63122.0 63123.0 63124.0 63125.0 63126.0 63127.0 63128.0 63129.0 63130.0 63131.0 63132.0 63133.0 63134.0 63135.0 63136.0 63137.0 63138.0 63139.0 63140.0 63141.0 63142.0 63143.0 63144.0 63145.0 63146.0 63147.0 63148.0 63149.0 63150.0 63151.0 63152.0 63153.0 63154.0 63155.0 63156.0 63157.0 63158.0 63159.0 63160.0 63161.0 63162.0 63163.0 63164.0 63165.0 63166.0 63167.0 63168.0 63169.0 63170.0 63171.0 63172.0 63173.0 63174.0 63175.0 63176.0 63177.0 63178.0 63179.0 63180.0 63181.0 63182.0 63183.0 63184.0 63185.0 63186.0 63187.0 63188.0 63189.0 63190.0 63191.0 63192.0 63193.0 63194.0 63195.0 63196.0 63197.0 63198.0 63199.0 63200.0 63201.0 63202.0 63203.0 63204.0 63205.0 63206.0 63207.0 63208.0 63209.0 63210.0 63211.0 63212.0 63213.0 63214.0 63215.0 63216.0 63217.0 63218.0 63219.0 63220.0 63221.0 63222.0 63223.0 63224.0 63225.0 63226.0 63227.0 63228.0 63229.0 63230.0 63231.0 63232.0 63233.0 63234.0 63235.0 63236.0 63237.0 63238.0 63239.0 63240.0 63241.0 63242.0 63243.0 63244.0 63245.0 63246.0 63247.0 63248.0 63249.0 63250.0 63251.0 63252.0 63253.0 63254.0 63255.0 63256.0 63257.0 63258.0 63259.0 63260.0 63261.0 63262.0 63263.0 63264.0 63265.0 63266.0 63267.0 63268.0 63269.0 63270.0 63271.0 63272.0 63273.0 63274.0 63275.0 63276.0 63277.0 63278.0 63279.0 63280.0 63281.0 63282.0 63283.0 63284.0 63285.0 63286.0 63287.0 63288.0 63289.0 63290.0 63291.0 63292.0 63293.0 63294.0 63295.0 63296.0 63297.0 63298.0 63299.0 63300.0 63301.0 63302.0 63303.0 63304.0 63305.0 63306.0 63307.0 63308.0 63309.0 63310.0 63311.0 63312.0 63313.0 63314.0 63315.0 63316.0 63317.0 63318.0 63319.0 63320.0 63321.0 63322.0 63323.0 63324.0 63325.0 63326.0 63327.0 63328.0 63329.0 63330.0 63331.0 63332.0 63333.0 63334.0 63335.0 63336.0 63337.0 63338.0 63339.0 63340.0 63341.0 63342.0 63343.0 63344.0 63345.0 63346.0 63347.0 63348.0 63349.0 63350.0 63351.0 63352.0 63353.0 63354.0 63355.0 63356.0 63357.0 63358.0 63359.0 63360.0 63361.0 63362.0 63363.0 63364.0 63365.0 63366.0 63367.0 63368.0 63369.0 63370.0 63371.0 63372.0 63373.0 63374.0 63375.0 63376.0 63377.0 63378.0 63379.0 63380.0 63381.0 63382.0 63383.0 63384.0 63385.0 63386.0 63387.0 63388.0 63389.0 63390.0 63391.0 63392.0 63393.0 63394.0 63395.0 63396.0 63397.0 63398.0 63399.0 63400.0 63401.0 63402.0 63403.0 63404.0 63405.0 63406.0 63407.0 63408.0 63409.0 63410.0 63411.0 63412.0 63413.0 63414.0 63415.0 63416.0 63417.0 63418.0 63419.0 63420.0 63421.0 63422.0 63423.0 63424.0 63425.0 63426.0 63427.0 63428.0 63429.0 63430.0 63431.0 63432.0 63433.0 63434.0 63435.0 63436.0 63437.0 63438.0 63439.0 63440.0 63441.0 63442.0 63443.0 63444.0 63445.0 63446.0 63447.0 63448.0 63449.0 63450.0 63451.0 63452.0 63453.0 63454.0 63455.0 63456.0 63457.0 63458.0 63459.0 63460.0 63461.0 63462.0 63463.0 63464.0 63465.0 63466.0 63467.0 63468.0 63469.0 63470.0 63471.0 63472.0 63473.0 63474.0 63475.0 63476.0 63477.0 63478.0 63479.0 63480.0 63481.0 63482.0 63483.0 63484.0 63485.0 63486.0 63487.0 63488.0 63489.0 63490.0 63491.0 63492.0 63493.0 63494.0 63495.0 63496.0 63497.0 63498.0 63499.0 63500.0 63501.0 63502.0 63503.0 63504.0 63505.0 63506.0 63507.0 63508.0 63509.0 63510.0 63511.0 63512.0 63513.0 63514.0 63515.0 63516.0 63517.0 63518.0 63519.0 63520.0 63521.0 63522.0 63523.0 63524.0 63525.0 63526.0 63527.0 63528.0 63529.0 63530.0 63531.0 63532.0 63533.0 63534.0 63535.0 63536.0 63537.0 63538.0 63539.0 63540.0 63541.0 63542.0 63543.0 63544.0 63545.0 63546.0 63547.0 63548.0 63549.0 63550.0 63551.0 63552.0 63553.0 63554.0 63555.0 63556.0 63557.0 63558.0 63559.0 63560.0 63561.0 63562.0 63563.0 63564.0 63565.0 63566.0 63567.0 63568.0 63569.0 63570.0 63571.0 63572.0 63573.0 63574.0 63575.0 63576.0 63577.0 63578.0 63579.0 63580.0 63581.0 63582.0 63583.0 63584.0 63585.0 63586.0 63587.0 63588.0 63589.0 63590.0 63591.0 63592.0 63593.0 63594.0 63595.0 63596.0 63597.0 63598.0 63599.0 63600.0 63601.0 63602.0 63603.0 63604.0 63605.0 63606.0 63607.0 63608.0 63609.0 63610.0 63611.0 63612.0 63613.0 63614.0 63615.0 63616.0 63617.0 63618.0 63619.0 63620.0 63621.0 63622.0 63623.0 63624.0 63625.0 63626.0 63627.0 63628.0 63629.0 63630.0 63631.0 63632.0 63633.0 63634.0 63635.0 63636.0 63637.0 63638.0 63639.0 63640.0 63641.0 63642.0 63643.0 63644.0 63645.0 63646.0 63647.0 63648.0 63649.0 63650.0 63651.0 63652.0 63653.0 63654.0 63655.0 63656.0 63657.0 63658.0 63659.0 63660.0 63661.0 63662.0 63663.0 63664.0 63665.0 63666.0 63667.0 63668.0 63669.0 63670.0 63671.0 63672.0 63673.0 63674.0 63675.0 63676.0 63677.0 63678.0 63679.0 63680.0 63681.0 63682.0 63683.0 63684.0 63685.0 63686.0 63687.0 63688.0 63689.0 63690.0 63691.0 63692.0 63693.0 63694.0 63695.0 63696.0 63697.0 63698.0 63699.0 63700.0 63701.0 63702.0 63703.0 63704.0 63705.0 63706.0 63707.0 63708.0 63709.0 63710.0 63711.0 63712.0 63713.0 63714.0 63715.0 63716.0 63717.0 63718.0 63719.0 63720.0 63721.0 63722.0 63723.0 63724.0 63725.0 63726.0 63727.0 63728.0 63729.0 63730.0 63731.0 63732.0 63733.0 63734.0 63735.0 63736.0 63737.0 63738.0 63739.0 63740.0 63741.0 63742.0 63743.0 63744.0 63745.0 63746.0 63747.0 63748.0 63749.0 63750.0 63751.0 63752.0 63753.0 63754.0 63755.0 63756.0 63757.0 63758.0 63759.0 63760.0 63761.0 63762.0 63763.0 63764.0 63765.0 63766.0 63767.0 63768.0 63769.0 63770.0 63771.0 63772.0 63773.0 63774.0 63775.0 63776.0 63777.0 63778.0 63779.0 63780.0 63781.0 63782.0 63783.0 63784.0 63785.0 63786.0 63787.0 63788.0 63789.0 63790.0 63791.0 63792.0 63793.0 63794.0 63795.0 63796.0 63797.0 63798.0 63799.0 63800.0 63801.0 63802.0 63803.0 63804.0 63805.0 63806.0 63807.0 63808.0 63809.0 63810.0 63811.0 63812.0 63813.0 63814.0 63815.0 63816.0 63817.0 63818.0 63819.0 63820.0 63821.0 63822.0 63823.0 63824.0 63825.0 63826.0 63827.0 63828.0 63829.0 63830.0 63831.0 63832.0 63833.0 63834.0 63835.0 63836.0 63837.0 63838.0 63839.0 63840.0 63841.0 63842.0 63843.0 63844.0 63845.0 63846.0 63847.0 63848.0 63849.0 63850.0 63851.0 63852.0 63853.0 63854.0 63855.0 63856.0 63857.0 63858.0 63859.0 63860.0 63861.0 63862.0 63863.0 63864.0 63865.0 63866.0 63867.0 63868.0 63869.0 63870.0 63871.0 63872.0 63873.0 63874.0 63875.0 63876.0 63877.0 63878.0 63879.0 63880.0 63881.0 63882.0 63883.0 63884.0 63885.0 63886.0 63887.0 63888.0 63889.0 63890.0 63891.0 63892.0 63893.0 63894.0 63895.0 63896.0 63897.0 63898.0 63899.0 63900.0 63901.0 63902.0 63903.0 63904.0 63905.0 63906.0 63907.0 63908.0 63909.0 63910.0 63911.0 63912.0 63913.0 63914.0 63915.0 63916.0 63917.0 63918.0 63919.0 63920.0 63921.0 63922.0 63923.0 63924.0 63925.0 63926.0 63927.0 63928.0 63929.0 63930.0 63931.0 63932.0 63933.0 63934.0 63935.0 63936.0 63937.0 63938.0 63939.0 63940.0 63941.0 63942.0 63943.0 63944.0 63945.0 63946.0 63947.0 63948.0 63949.0 63950.0 63951.0 63952.0 63953.0 63954.0 63955.0 63956.0 63957.0 63958.0 63959.0 63960.0 63961.0 63962.0 63963.0 63964.0 63965.0 63966.0 63967.0 63968.0 63969.0 63970.0 63971.0 63972.0 63973.0 63974.0 63975.0 63976.0 63977.0 63978.0 63979.0 63980.0 63981.0 63982.0 63983.0 63984.0 63985.0 63986.0 63987.0 63988.0 63989.0 63990.0 63991.0 63992.0 63993.0 63994.0 63995.0 63996.0 63997.0 63998.0 63999.0 64000.0 64001.0 64002.0 64003.0 64004.0 64005.0 64006.0 64007.0 64008.0 64009.0 64010.0 64011.0 64012.0 64013.0 64014.0 64015.0 64016.0 64017.0 64018.0 64019.0 64020.0 64021.0 64022.0 64023.0 64024.0 64025.0 64026.0 64027.0 64028.0 64029.0 64030.0 64031.0 64032.0 64033.0 64034.0 64035.0 64036.0 64037.0 64038.0 64039.0 64040.0 64041.0 64042.0 64043.0 64044.0 64045.0 64046.0 64047.0 64048.0 64049.0 64050.0 64051.0 64052.0 64053.0 64054.0 64055.0 64056.0 64057.0 64058.0 64059.0 64060.0 64061.0 64062.0 64063.0 64064.0 64065.0 64066.0 64067.0 64068.0 64069.0 64070.0 64071.0 64072.0 64073.0 64074.0 64075.0 64076.0 64077.0 64078.0 64079.0 64080.0 64081.0 64082.0 64083.0 64084.0 64085.0 64086.0 64087.0 64088.0 64089.0 64090.0 64091.0 64092.0 64093.0 64094.0 64095.0 64096.0 64097.0 64098.0 64099.0 64100.0 64101.0 64102.0 64103.0 64104.0 64105.0 64106.0 64107.0 64108.0 64109.0 64110.0 64111.0 64112.0 64113.0 64114.0 64115.0 64116.0 64117.0 64118.0 64119.0 64120.0 64121.0 64122.0 64123.0 64124.0 64125.0 64126.0 64127.0 64128.0 64129.0 64130.0 64131.0 64132.0 64133.0 64134.0 64135.0 64136.0 64137.0 64138.0 64139.0 64140.0 64141.0 64142.0 64143.0 64144.0 64145.0 64146.0 64147.0 64148.0 64149.0 64150.0 64151.0 64152.0 64153.0 64154.0 64155.0 64156.0 64157.0 64158.0 64159.0 64160.0 64161.0 64162.0 64163.0 64164.0 64165.0 64166.0 64167.0 64168.0 64169.0 64170.0 64171.0 64172.0 64173.0 64174.0 64175.0 64176.0 64177.0 64178.0 64179.0 64180.0 64181.0 64182.0 64183.0 64184.0 64185.0 64186.0 64187.0 64188.0 64189.0 64190.0 64191.0 64192.0 64193.0 64194.0 64195.0 64196.0 64197.0 64198.0 64199.0 64200.0 64201.0 64202.0 64203.0 64204.0 64205.0 64206.0 64207.0 64208.0 64209.0 64210.0 64211.0 64212.0 64213.0 64214.0 64215.0 64216.0 64217.0 64218.0 64219.0 64220.0 64221.0 64222.0 64223.0 64224.0 64225.0 64226.0 64227.0 64228.0 64229.0 64230.0 64231.0 64232.0 64233.0 64234.0 64235.0 64236.0 64237.0 64238.0 64239.0 64240.0 64241.0 64242.0 64243.0 64244.0 64245.0 64246.0 64247.0 64248.0 64249.0 64250.0 64251.0 64252.0 64253.0 64254.0 64255.0 64256.0 64257.0 64258.0 64259.0 64260.0 64261.0 64262.0 64263.0 64264.0 64265.0 64266.0 64267.0 64268.0 64269.0 64270.0 64271.0 64272.0 64273.0 64274.0 64275.0 64276.0 64277.0 64278.0 64279.0 64280.0 64281.0 64282.0 64283.0 64284.0 64285.0 64286.0 64287.0 64288.0 64289.0 64290.0 64291.0 64292.0 64293.0 64294.0 64295.0 64296.0 64297.0 64298.0 64299.0 64300.0 64301.0 64302.0 64303.0 64304.0 64305.0 64306.0 64307.0 64308.0 64309.0 64310.0 64311.0 64312.0 64313.0 64314.0 64315.0 64316.0 64317.0 64318.0 64319.0 64320.0 64321.0 64322.0 64323.0 64324.0 64325.0 64326.0 64327.0 64328.0 64329.0 64330.0 64331.0 64332.0 64333.0 64334.0 64335.0 64336.0 64337.0 64338.0 64339.0 64340.0 64341.0 64342.0 64343.0 64344.0 64345.0 64346.0 64347.0 64348.0 64349.0 64350.0 64351.0 64352.0 64353.0 64354.0 64355.0 64356.0 64357.0 64358.0 64359.0 64360.0 64361.0 64362.0 64363.0 64364.0 64365.0 64366.0 64367.0 64368.0 64369.0 64370.0 64371.0 64372.0 64373.0 64374.0 64375.0 64376.0 64377.0 64378.0 64379.0 64380.0 64381.0 64382.0 64383.0 64384.0 64385.0 64386.0 64387.0 64388.0 64389.0 64390.0 64391.0 64392.0 64393.0 64394.0 64395.0 64396.0 64397.0 64398.0 64399.0 64400.0 64401.0 64402.0 64403.0 64404.0 64405.0 64406.0 64407.0 64408.0 64409.0 64410.0 64411.0 64412.0 64413.0 64414.0 64415.0 64416.0 64417.0 64418.0 64419.0 64420.0 64421.0 64422.0 64423.0 64424.0 64425.0 64426.0 64427.0 64428.0 64429.0 64430.0 64431.0 64432.0 64433.0 64434.0 64435.0 64436.0 64437.0 64438.0 64439.0 64440.0 64441.0 64442.0 64443.0 64444.0 64445.0 64446.0 64447.0 64448.0 64449.0 64450.0 64451.0 64452.0 64453.0 64454.0 64455.0 64456.0 64457.0 64458.0 64459.0 64460.0 64461.0 64462.0 64463.0 64464.0 64465.0 64466.0 64467.0 64468.0 64469.0 64470.0 64471.0 64472.0 64473.0 64474.0 64475.0 64476.0 64477.0 64478.0 64479.0 64480.0 64481.0 64482.0 64483.0 64484.0 64485.0 64486.0 64487.0 64488.0 64489.0 64490.0 64491.0 64492.0 64493.0 64494.0 64495.0 64496.0 64497.0 64498.0 64499.0 64500.0 64501.0 64502.0 64503.0 64504.0 64505.0 64506.0 64507.0 64508.0 64509.0 64510.0 64511.0 64512.0 64513.0 64514.0 64515.0 64516.0 64517.0 64518.0 64519.0 64520.0 64521.0 64522.0 64523.0 64524.0 64525.0 64526.0 64527.0 64528.0 64529.0 64530.0 64531.0 64532.0 64533.0 64534.0 64535.0 64536.0 64537.0 64538.0 64539.0 64540.0 64541.0 64542.0 64543.0 64544.0 64545.0 64546.0 64547.0 64548.0 64549.0 64550.0 64551.0 64552.0 64553.0 64554.0 64555.0 64556.0 64557.0 64558.0 64559.0 64560.0 64561.0 64562.0 64563.0 64564.0 64565.0 64566.0 64567.0 64568.0 64569.0 64570.0 64571.0 64572.0 64573.0 64574.0 64575.0 64576.0 64577.0 64578.0 64579.0 64580.0 64581.0 64582.0 64583.0 64584.0 64585.0 64586.0 64587.0 64588.0 64589.0 64590.0 64591.0 64592.0 64593.0 64594.0 64595.0 64596.0 64597.0 64598.0 64599.0 64600.0 64601.0 64602.0 64603.0 64604.0 64605.0 64606.0 64607.0 64608.0 64609.0 64610.0 64611.0 64612.0 64613.0 64614.0 64615.0 64616.0 64617.0 64618.0 64619.0 64620.0 64621.0 64622.0 64623.0 64624.0 64625.0 64626.0 64627.0 64628.0 64629.0 64630.0 64631.0 64632.0 64633.0 64634.0 64635.0 64636.0 64637.0 64638.0 64639.0 64640.0 64641.0 64642.0 64643.0 64644.0 64645.0 64646.0 64647.0 64648.0 64649.0 64650.0 64651.0 64652.0 64653.0 64654.0 64655.0 64656.0 64657.0 64658.0 64659.0 64660.0 64661.0 64662.0 64663.0 64664.0 64665.0 64666.0 64667.0 64668.0 64669.0 64670.0 64671.0 64672.0 64673.0 64674.0 64675.0 64676.0 64677.0 64678.0 64679.0 64680.0 64681.0 64682.0 64683.0 64684.0 64685.0 64686.0 64687.0 64688.0 64689.0 64690.0 64691.0 64692.0 64693.0 64694.0 64695.0 64696.0 64697.0 64698.0 64699.0 64700.0 64701.0 64702.0 64703.0 64704.0 64705.0 64706.0 64707.0 64708.0 64709.0 64710.0 64711.0 64712.0 64713.0 64714.0 64715.0 64716.0 64717.0 64718.0 64719.0 64720.0 64721.0 64722.0 64723.0 64724.0 64725.0 64726.0 64727.0 64728.0 64729.0 64730.0 64731.0 64732.0 64733.0 64734.0 64735.0 64736.0 64737.0 64738.0 64739.0 64740.0 64741.0 64742.0 64743.0 64744.0 64745.0 64746.0 64747.0 64748.0 64749.0 64750.0 64751.0 64752.0 64753.0 64754.0 64755.0 64756.0 64757.0 64758.0 64759.0 64760.0 64761.0 64762.0 64763.0 64764.0 64765.0 64766.0 64767.0 64768.0 64769.0 64770.0 64771.0 64772.0 64773.0 64774.0 64775.0 64776.0 64777.0 64778.0 64779.0 64780.0 64781.0 64782.0 64783.0 64784.0 64785.0 64786.0 64787.0 64788.0 64789.0 64790.0 64791.0 64792.0 64793.0 64794.0 64795.0 64796.0 64797.0 64798.0 64799.0 64800.0 64801.0 64802.0 64803.0 64804.0 64805.0 64806.0 64807.0 64808.0 64809.0 64810.0 64811.0 64812.0 64813.0 64814.0 64815.0 64816.0 64817.0 64818.0 64819.0 64820.0 64821.0 64822.0 64823.0 64824.0 64825.0 64826.0 64827.0 64828.0 64829.0 64830.0 64831.0 64832.0 64833.0 64834.0 64835.0 64836.0 64837.0 64838.0 64839.0 64840.0 64841.0 64842.0 64843.0 64844.0 64845.0 64846.0 64847.0 64848.0 64849.0 64850.0 64851.0 64852.0 64853.0 64854.0 64855.0 64856.0 64857.0 64858.0 64859.0 64860.0 64861.0 64862.0 64863.0 64864.0 64865.0 64866.0 64867.0 64868.0 64869.0 64870.0 64871.0 64872.0 64873.0 64874.0 64875.0 64876.0 64877.0 64878.0 64879.0 64880.0 64881.0 64882.0 64883.0 64884.0 64885.0 64886.0 64887.0 64888.0 64889.0 64890.0 64891.0 64892.0 64893.0 64894.0 64895.0 64896.0 64897.0 64898.0 64899.0 64900.0 64901.0 64902.0 64903.0 64904.0 64905.0 64906.0 64907.0 64908.0 64909.0 64910.0 64911.0 64912.0 64913.0 64914.0 64915.0 64916.0 64917.0 64918.0 64919.0 64920.0 64921.0 64922.0 64923.0 64924.0 64925.0 64926.0 64927.0 64928.0 64929.0 64930.0 64931.0 64932.0 64933.0 64934.0 64935.0 64936.0 64937.0 64938.0 64939.0 64940.0 64941.0 64942.0 64943.0 64944.0 64945.0 64946.0 64947.0 64948.0 64949.0 64950.0 64951.0 64952.0 64953.0 64954.0 64955.0 64956.0 64957.0 64958.0 64959.0 64960.0 64961.0 64962.0 64963.0 64964.0 64965.0 64966.0 64967.0 64968.0 64969.0 64970.0 64971.0 64972.0 64973.0 64974.0 64975.0 64976.0 64977.0 64978.0 64979.0 64980.0 64981.0 64982.0 64983.0 64984.0 64985.0 64986.0 64987.0 64988.0 64989.0 64990.0 64991.0 64992.0 64993.0 64994.0 64995.0 64996.0 64997.0 64998.0 64999.0 65000.0 65001.0 65002.0 65003.0 65004.0 65005.0 65006.0 65007.0 65008.0 65009.0 65010.0 65011.0 65012.0 65013.0 65014.0 65015.0 65016.0 65017.0 65018.0 65019.0 65020.0 65021.0 65022.0 65023.0 65024.0 65025.0 65026.0 65027.0 65028.0 65029.0 65030.0 65031.0 65032.0 65033.0 65034.0 65035.0 65036.0 65037.0 65038.0 65039.0 65040.0 65041.0 65042.0 65043.0 65044.0 65045.0 65046.0 65047.0 65048.0 65049.0 65050.0 65051.0 65052.0 65053.0 65054.0 65055.0 65056.0 65057.0 65058.0 65059.0 65060.0 65061.0 65062.0 65063.0 65064.0 65065.0 65066.0 65067.0 65068.0 65069.0 65070.0 65071.0 65072.0 65073.0 65074.0 65075.0 65076.0 65077.0 65078.0 65079.0 65080.0 65081.0 65082.0 65083.0 65084.0 65085.0 65086.0 65087.0 65088.0 65089.0 65090.0 65091.0 65092.0 65093.0 65094.0 65095.0 65096.0 65097.0 65098.0 65099.0 65100.0 65101.0 65102.0 65103.0 65104.0 65105.0 65106.0 65107.0 65108.0 65109.0 65110.0 65111.0 65112.0 65113.0 65114.0 65115.0 65116.0 65117.0 65118.0 65119.0 65120.0 65121.0 65122.0 65123.0 65124.0 65125.0 65126.0 65127.0 65128.0 65129.0 65130.0 65131.0 65132.0 65133.0 65134.0 65135.0 65136.0 65137.0 65138.0 65139.0 65140.0 65141.0 65142.0 65143.0 65144.0 65145.0 65146.0 65147.0 65148.0 65149.0 65150.0 65151.0 65152.0 65153.0 65154.0 65155.0 65156.0 65157.0 65158.0 65159.0 65160.0 65161.0 65162.0 65163.0 65164.0 65165.0 65166.0 65167.0 65168.0 65169.0 65170.0 65171.0 65172.0 65173.0 65174.0 65175.0 65176.0 65177.0 65178.0 65179.0 65180.0 65181.0 65182.0 65183.0 65184.0 65185.0 65186.0 65187.0 65188.0 65189.0 65190.0 65191.0 65192.0 65193.0 65194.0 65195.0 65196.0 65197.0 65198.0 65199.0 65200.0 65201.0 65202.0 65203.0 65204.0 65205.0 65206.0 65207.0 65208.0 65209.0 65210.0 65211.0 65212.0 65213.0 65214.0 65215.0 65216.0 65217.0 65218.0 65219.0 65220.0 65221.0 65222.0 65223.0 65224.0 65225.0 65226.0 65227.0 65228.0 65229.0 65230.0 65231.0 65232.0 65233.0 65234.0 65235.0 65236.0 65237.0 65238.0 65239.0 65240.0 65241.0 65242.0 65243.0 65244.0 65245.0 65246.0 65247.0 65248.0 65249.0 65250.0 65251.0 65252.0 65253.0 65254.0 65255.0 65256.0 65257.0 65258.0 65259.0 65260.0 65261.0 65262.0 65263.0 65264.0 65265.0 65266.0 65267.0 65268.0 65269.0 65270.0 65271.0 65272.0 65273.0 65274.0 65275.0 65276.0 65277.0 65278.0 65279.0 65280.0 65281.0 65282.0 65283.0 65284.0 65285.0 65286.0 65287.0 65288.0 65289.0 65290.0 65291.0 65292.0 65293.0 65294.0 65295.0 65296.0 65297.0 65298.0 65299.0 65300.0 65301.0 65302.0 65303.0 65304.0 65305.0 65306.0 65307.0 65308.0 65309.0 65310.0 65311.0 65312.0 65313.0 65314.0 65315.0 65316.0 65317.0 65318.0 65319.0 65320.0 65321.0 65322.0 65323.0 65324.0 65325.0 65326.0 65327.0 65328.0 65329.0 65330.0 65331.0 65332.0 65333.0 65334.0 65335.0 65336.0 65337.0 65338.0 65339.0 65340.0 65341.0 65342.0 65343.0 65344.0 65345.0 65346.0 65347.0 65348.0 65349.0 65350.0 65351.0 65352.0 65353.0 65354.0 65355.0 65356.0 65357.0 65358.0 65359.0 65360.0 65361.0 65362.0 65363.0 65364.0 65365.0 65366.0 65367.0 65368.0 65369.0 65370.0 65371.0 65372.0 65373.0 65374.0 65375.0 65376.0 65377.0 65378.0 65379.0 65380.0 65381.0 65382.0 65383.0 65384.0 65385.0 65386.0 65387.0 65388.0 65389.0 65390.0 65391.0 65392.0 65393.0 65394.0 65395.0 65396.0 65397.0 65398.0 65399.0 65400.0 65401.0 65402.0 65403.0 65404.0 65405.0 65406.0 65407.0 65408.0 65409.0 65410.0 65411.0 65412.0 65413.0 65414.0 65415.0 65416.0 65417.0 65418.0 65419.0 65420.0 65421.0 65422.0 65423.0 65424.0 65425.0 65426.0 65427.0 65428.0 65429.0 65430.0 65431.0 65432.0 65433.0 65434.0 65435.0 65436.0 65437.0 65438.0 65439.0 65440.0 65441.0 65442.0 65443.0 65444.0 65445.0 65446.0 65447.0 65448.0 65449.0 65450.0 65451.0 65452.0 65453.0 65454.0 65455.0 65456.0 65457.0 65458.0 65459.0 65460.0 65461.0 65462.0 65463.0 65464.0 65465.0 65466.0 65467.0 65468.0 65469.0 65470.0 65471.0 65472.0 65473.0 65474.0 65475.0 65476.0 65477.0 65478.0 65479.0 65480.0 65481.0 65482.0 65483.0 65484.0 65485.0 65486.0 65487.0 65488.0 65489.0 65490.0 65491.0 65492.0 65493.0 65494.0 65495.0 65496.0 65497.0 65498.0 65499.0 65500.0 65501.0 65502.0 65503.0 65504.0 65505.0 65506.0 65507.0 65508.0 65509.0 65510.0 65511.0 65512.0 65513.0 65514.0 65515.0 65516.0 65517.0 65518.0 65519.0 65520.0 65521.0 65522.0 65523.0 65524.0 65525.0 65526.0 65527.0 65528.0 65529.0 65530.0 65531.0 65532.0 65533.0 65534.0 65535.0 \ No newline at end of file diff --git a/src/GTSAMIntegration/AugmentedScatter.cpp b/src/GTSAMIntegration/AugmentedScatter.cpp new file mode 100644 index 0000000..c85c058 --- /dev/null +++ b/src/GTSAMIntegration/AugmentedScatter.cpp @@ -0,0 +1,112 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ +// This code file is based on the file Scatter.cpp from the project GTSAM, which has been released under the following conditions: +/* ---------------------------------------------------------------------------- + * 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) + * +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ +#include "AugmentedScatter.hpp" +#include + +using namespace gtsam; + +dmvio::AugmentedScatter::AugmentedScatter(const GaussianFactorGraph& gfg, boost::optional ordering, const std::map& keyDimMap) +{ + // If we have an ordering, pre-fill the ordered variables first + if (ordering) { + for (Key key : *ordering) { + std::map::const_iterator it = keyDimMap.find(key); + unsigned long dim = 0; + if(it != keyDimMap.end()) + { + dim = it->second; + } + add(key, dim); + } + } + + // Now, find dimensions of variables and/or extend + 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()); + if (asJacobian && asJacobian->cols() <= 1) continue; + + // loop over variables + for (GaussianFactor::const_iterator variable = factor->begin(); + variable != factor->end(); ++variable) { + const Key key = *variable; + iterator it = findNew(key); // theoretically expensive, yet cache friendly + if (it!=end()) + it->dimension = factor->getDim(variable); + else + add(key, factor->getDim(variable)); + } + } + + // To keep the same behavior as before, sort the keys after the ordering + iterator first = begin(); + if (ordering) first += ordering->size(); + if (first != end()) std::sort(first, end()); + + // Filter out keys with zero dimensions (if ordering had more keys) + erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); +} + +FastVector::iterator dmvio::AugmentedScatter::findNew(Key key) { + iterator it = begin(); + while(it != end()) { + if (it->key == key) + return it; + ++it; + } + return it; // end() +} + +std::pair dmvio::AugmentedScatter::computeHessian(const GaussianFactorGraph& gfg) +{ + gtsam::HessianFactor combined(gfg, *this); + gtsam::Matrix augmented = combined.info().selfadjointView(); + size_t n = augmented.rows() - 1; + return std::make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1)); +} + +gtsam::Matrix dmvio::AugmentedScatter::computeAugmentedHessian(const gtsam::GaussianFactorGraph &gfg) +{ + gtsam::HessianFactor combined(gfg, *this); + gtsam::Matrix augmented = combined.info().selfadjointView(); + return augmented; +} diff --git a/src/GTSAMIntegration/AugmentedScatter.hpp b/src/GTSAMIntegration/AugmentedScatter.hpp new file mode 100644 index 0000000..f5bc9c5 --- /dev/null +++ b/src/GTSAMIntegration/AugmentedScatter.hpp @@ -0,0 +1,52 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef AugmentedScatter_hpp +#define AugmentedScatter_hpp + +#include + +#include +#include + +#include "util/NumType.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +#include + +namespace dmvio +{ +class AugmentedScatter : public gtsam::Scatter +{ +public: + // Scatter that can additionally handle keys that don't exist in the factor graph. For those keys the dimension must be specified in keyDimMap + AugmentedScatter(const gtsam::GaussianFactorGraph& gfg,boost::optional ordering, const std::map& keyDimMap); + + iterator findNew(gtsam::Key key); + + std::pair computeHessian(const gtsam::GaussianFactorGraph& gfg); + gtsam::Matrix computeAugmentedHessian(const gtsam::GaussianFactorGraph& gfg); +}; +} + + +#endif /* AugmentedScatter_hpp */ diff --git a/src/GTSAMIntegration/BAGTSAMIntegration.cpp b/src/GTSAMIntegration/BAGTSAMIntegration.cpp new file mode 100644 index 0000000..347513f --- /dev/null +++ b/src/GTSAMIntegration/BAGTSAMIntegration.cpp @@ -0,0 +1,570 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "BAGTSAMIntegration.h" + +#include + +#include +#include +#include +#include "Marginalization.h" +#include "AugmentedScatter.hpp" +#include +#include +#include + +#include "FullSystem/HessianBlocks.h" +#include "dso/util/FrameShell.h" +#include "GTSAMIntegration/ExtUtils.h" +#include "GTSAMUtils.h" + + +using namespace dmvio; +using dso::VecX, dso::MatXX, dso::EFFrame; +using std::vector; +using gtsam::Symbol; + + +BAGTSAMIntegration::BAGTSAMIntegration(std::unique_ptr baGraphs, + std::unique_ptr transformationDSOToBA, + const GTSAMIntegrationSettings& integrationSettings, dso::CalibHessian* HCalib) + : baGraphs(std::move(baGraphs)), transformationDSOToBA(std::move(transformationDSOToBA)), HCalib(HCalib), + settings(integrationSettings) +{ + baValues.reset(new gtsam::Values); + baEvalValues.reset(new gtsam::Values); +} + +// called from addKeyframeToBA and from outside (FullSystem::makeKeyFrame) +void BAGTSAMIntegration::updateBAOrdering(std::vector& frames) +{ + baOrdering.clear(); + + gtsam::Symbol calib('c', 0); + baOrdering.push_back(calib); + baDimMap[calib] = CPARS; + + int i = 0; + for(dso::EFFrame* h : frames) + { + int id = h->idx; + assert(id == i); + + long fullId = h->data->shell->id; + + gtsam::Symbol poseKey('p', fullId); + gtsam::Symbol affineKey('a', fullId); + + baOrdering.push_back(poseKey); + baOrdering.push_back(affineKey); + + baDimMap[poseKey] = 6; + baDimMap[affineKey] = 2; // two affine parameters. + + ++i; + } + + baOrderingSmall.clear(); + baOrderingSmall.insert(baOrderingSmall.end(), baOrdering.begin(), baOrdering.end()); + + // Let extensions add their variables + for(auto& extension : extensions) + { + extension->updateBAOrdering(frames, &baOrdering, baDimMap); + } +} + +void BAGTSAMIntegration::updateBAValues(std::vector& frames) +{ + dmvio::TimeMeasurement timeMeasurement("updateBAValues"); + + // Filling baValues. + dso::VecC calibVal = HCalib->value; + gtsam::Symbol calibKey('c', 0); + + eraseAndInsert(baValues, calibKey, calibVal); + + for(dso::EFFrame* h : frames) + { + dso::Vec10 state = h->data->get_state(); + // Transform pose with (even though at the moment this is just the identity). + gtsam::Pose3 pose(transformationDSOToBA->transformPose(h->data->PRE_worldToCam.matrix())); + gtsam::Vector2 affine = state.segment(6, 2); + + long fullId = h->data->shell->id; + gtsam::Symbol poseKey('p', fullId); + gtsam::Symbol affineKey('a', fullId); + + eraseAndInsert(baValues, poseKey, pose); + eraseAndInsert(baValues, affineKey, affine); + } +} + +dso::VecX +BAGTSAMIntegration::computeBAUpdate(const dso::MatXX& inputH, const VecX& inputB, double lambda, + vector& frames, + const MatXX& HNoLambda) +{ + dmvio::TimeMeasurement timeMeasurement("computeBAUpdate"); + + updateBAValues(frames); + *baEvalValues = *baValues; + computeEvaluationPointValues(frames, baEvalValues); + baGraphs->updateEvalValues(*baEvalValues); + + dmvio::TimeMeasurement timeMeasGraphs("baUpdateBuildHessians"); + lastDSOH = HNoLambda; + lastDSOB = inputB; + baValuesAfterBAUpdate = baValues; + + // Compute DSO Hessian and b. + // -------------------------------------------------- + // Convert H and b from DSO to GTSAM (swapping rotation and translation, converting left increment to right increment). + auto convertedHAndB = convertHAndBFromDSO(inputH, inputB, *transformationDSOToBA, computeDSOWeight(), baOrdering, + *baEvalValues, baDimMap); + gtsam::Matrix HFromDSO = convertedHAndB.first; + gtsam::Vector bFromDSO = convertedHAndB.second; + + int n = HFromDSO.rows(); + int smallN = n; + + // Compute GTSAM Hessian and b + // -------------------------------------------------- + gtsam::Ordering additionalKeys; // keys in the graphs but not in the ordering. + auto gtsamHb = baGraphs->getHAndB(*baValues, baOrdering, baDimMap, &additionalKeys); + + // Fix all keys which were not in the ordering. + auto accumFun = [this](int num, const gtsam::Key& key) + { + return num + baDimMap.at(key); + }; + int nonFixedSize = std::accumulate(baOrdering.begin(), baOrdering.end(), 0, accumFun); + gtsam::Matrix HFull = gtsamHb.first.topLeftCorner(nonFixedSize, nonFixedSize); + gtsam::Vector bFull = gtsamHb.second.head(nonFixedSize); + + // The H from DSO is already multiplied with lambda correctly. We have to multiply our Hessian as well though: + int fullN = HFull.rows(); + for(int i = 0; i < fullN; ++i) + { + HFull(i, i) *= (1 + lambda); + } + + // Because the photometric factors computed by DSO and the factors in the GTSAM graph are independent we can simply add them. + // -------------------------------------------------- + HFull.block(0, 0, n, n) += HFromDSO; + bFull.segment(0, n) += bFromDSO; + + timeMeasGraphs.end(); + + for(auto& extension : extensions) + { + extension->preSolve(HFull, bFull, n); + } + + // Solve system (with preconditioning). + dso::VecX SVecI = (HFull.diagonal() + dso::VecX::Constant(HFull.cols(), 10)).cwiseSqrt().cwiseInverse(); + gtsam::Matrix H_scaled = SVecI.asDiagonal() * HFull * SVecI.asDiagonal(); + + dmvio::TimeMeasurement matrixInversionMeasurement("baMatrixInversion"); + gtsam::Vector inc = SVecI.asDiagonal() * H_scaled.ldlt().solve(SVecI.asDiagonal() * bFull); + matrixInversionMeasurement.end(); + + // Update values based on the computed increment. + newBAValues.reset(new gtsam::Values()); + int current_pos = 0; + for(size_t i = 0; i < baOrdering.size(); i++) + { + gtsam::Key k = baOrdering[i]; + size_t s = baDimMap[k]; + newBAValues->insert(k, *(baValues->at(k).retract_(inc.segment(current_pos, s)))); + current_pos += s; + } + + canBreakOptimization = true; + for(auto& extension : extensions) + { + canBreakOptimization = + canBreakOptimization && extension->postSolve(baValues, newBAValues, inc, baOrdering, baDimMap); + } + + // Go through keys that were not in the ordering + // These will be fixed during optimization, but we display a warning to alert the user. + for(auto&& key : additionalKeys) + { + if(!newBAValues->exists(key)) + { + std::cout << "WARNING: Key was not in the ordering! " << gtsam::Symbol(key).chr() + << gtsam::Symbol(key).index() << std::endl; + // Backup: Insert old values! + newBAValues->insert(key, baValues->at(key)); + } + } + + // Prepare increment for DSO (which will be used to update the point estimates). + dso::VecX returning = inc.segment(0, smallN); + + // Exchange R and T (to convert it back to dso convention). + for(dso::EFFrame* h : frames) + { + int id = CPARS + 8 * h->idx; + + long fullId = h->data->shell->id; + gtsam::Symbol poseKey('p', fullId); + + // In practice this is usually the identity transformation. + dso::Mat44 worldToCam = transformationDSOToBA->transformPoseInverse( + newBAValues->at(poseKey).matrix()); + Sophus::SE3d newVal(worldToCam); + + Sophus::SE3d oldVal = h->data->PRE_worldToCam; + + // Note that there might be a more efficient way to compute the increment using adjoints! + // newVal = exp(inc) * oldVal -> inc = log(newVal * oldVal^{-1}) + dso::Vec6 increment = (newVal * oldVal.inverse()).log(); + + returning.segment(id, 6) = increment; + } + + // In DSO actually -inc is used at this point... + return -returning; +} + +void +BAGTSAMIntegration::addMarginalizedPointsBA(const dso::MatXX& H_in, const dso::VecX& b_in, + std::vector& frames) +{ + updateBAValues(frames); + *baEvalValues = *baValues; + computeEvaluationPointValues(frames, baEvalValues); + + auto convertedHAndB = convertHAndBFromDSO(H_in, b_in, *transformationDSOToBA, computeDSOWeight(), baOrdering, + *baEvalValues, baDimMap); + + gtsam::LinearContainerFactor::shared_ptr lcf = convertedDSOHAndBToFactor(convertedHAndB.first, + convertedHAndB.second, *baEvalValues); + + baGraphs->addFactor(lcf, false); + +} + +gtsam::LinearContainerFactor::shared_ptr +BAGTSAMIntegration::convertedDSOHAndBToFactor(const gtsam::Matrix& H, const gtsam::Vector& b, + const gtsam::Values& values, double energy) +{ + // Make Hessian symmetric for numeric reasons. + int n = H.rows(); + + gtsam::Matrix Hbaa_new(n + 1, n + 1); + Hbaa_new.setZero(); + Hbaa_new.topLeftCorner(n, n) = 0.5 * (H.transpose() + H); + Hbaa_new.topRightCorner(n, 1) = b; + Hbaa_new.bottomLeftCorner(1, n) = b.transpose(); + Hbaa_new(n, n) = energy; + + gtsam::Ordering ordering = baOrderingSmall; + gtsam::FastVector connected_dims; + + for(gtsam::Key& key : ordering) + { + connected_dims.push_back(baDimMap[key]); + } + + gtsam::SymmetricBlockMatrix sm(connected_dims, true); + + sm.setFullMatrix(Hbaa_new); + gtsam::LinearContainerFactor::shared_ptr lcf( + new gtsam::LinearContainerFactor(gtsam::HessianFactor(ordering, sm), values)); + return lcf; +} + +// Adds a prior to a frame of the BA. +void BAGTSAMIntegration::addPriorBA(dso::EFFrame* h, const dso::Vec8& H_diag, const dso::Vec8& b_in) +{ + long fullId = h->data->shell->id; + + // Convert H and b to GTSAM + dso::MatXX H_in = dso::Mat88::Zero(); + H_in.diagonal() = H_diag; + + gtsam::Ordering ordering; + gtsam::Symbol poseKey('p', fullId); + gtsam::Symbol affineKey('a', fullId); + ordering.push_back(poseKey); + ordering.push_back(affineKey); + + auto convertedHAndB = convertHAndBFromDSO(H_in, b_in, *transformationDSOToBA, computeDSOWeight(), ordering, + *baEvalValues, baDimMap); + dso::Mat66 H_pose = convertedHAndB.first.topLeftCorner<6, 6>(); + dso::Mat22 H_affine = convertedHAndB.first.bottomRightCorner<2, 2>(); + dso::Vec6 b_pose = convertedHAndB.second.segment(0, 6); + dso::Vec2 b_affine = convertedHAndB.second.segment(6, 2); + + gtsam::LinearContainerFactor::shared_ptr lcf( + new gtsam::LinearContainerFactor(gtsam::HessianFactor(poseKey, H_pose, b_pose, 0.0), *baEvalValues)); + gtsam::LinearContainerFactor::shared_ptr lcfA( + new gtsam::LinearContainerFactor(gtsam::HessianFactor(affineKey, H_affine, b_affine, 0.0), *baEvalValues)); + + baGraphs->addFactor(lcf, false); + baGraphs->addFactor(lcfA, false); +} + +// Computes values where the FEJValue is used for DSO variables (pose and affine brightness) **only**. +void BAGTSAMIntegration::computeEvaluationPointValues(const std::vector& frames, + gtsam::Values::shared_ptr values) +{ + dso::VecC calib = HCalib->value_zero; + gtsam::Symbol calibKey('c', 0); + eraseAndInsert(values, calibKey, calib); + + // Replace current value with FEJ value for poses and affine brightness. + for(dso::EFFrame* h : frames) + { + dso::Vec10 stateZero = h->data->get_state_zero(); + Sophus::SE3d evalPoint = h->data->get_worldToCam_evalPT(); + gtsam::Pose3 pose(evalPoint.matrix()); + assert(stateZero.segment(0, 6).norm() == 0); + gtsam::Vector2 affine = stateZero.segment(6, 2); + + long fullId = h->data->shell->id; + + gtsam::Symbol poseKey('p', fullId); + gtsam::Symbol affineKey('a', fullId); + + eraseAndInsert(values, poseKey, pose); + eraseAndInsert(values, affineKey, affine); + } +} + +void BAGTSAMIntegration::acceptBAUpdate(double energy) +{ + for(auto& extension : extensions) + { + extension->acceptUpdate(baValues, newBAValues); + } + + baValues = newBAValues; + lastDSOEnergy = energy; +} + +// The method assumes that baValues was updated (e.g. by a previous call to addMarginalizedPointsBA. +void BAGTSAMIntegration::marginalizeBAFrame(dso::EFFrame* fh) +{ + gtsam::FastVector keysToMarginalize; + long fullId = fh->data->shell->id; + + gtsam::Symbol poseKey('p', fullId); + gtsam::Symbol affineKey('a', fullId); + + keysToMarginalize.push_back(poseKey); + keysToMarginalize.push_back(affineKey); + + for(auto& extension : extensions) + { + extension->addKeysToMarginalize(fullId, keysToMarginalize); + } + + // baGraphs contains the logic for the actual marginalization. + // This method will also erase the marginalized keys from baEvalValues. + baGraphs->marginalizeFrame(keysToMarginalize, baEvalValues, baDimMap, + currBATimestamp, baValues); + + for(auto&& key : keysToMarginalize) + { + // Marginalize erases the keys from baEvalValues, but not from baValues. + baValues->erase(key); + } +} + +void BAGTSAMIntegration::addFirstBAFrame(int keyframeId) +{ + assert(keyframeId == 0); + for(auto& extension : extensions) + { + extension->addFirstBAFrame(keyframeId, baGraphs.get(), baValues); + } +} + +void +BAGTSAMIntegration::addKeyframeToBA(int keyframeId, const Sophus::SE3d& keyframePose, vector& frames) +{ + dmvio::TimeMeasurement timeMeasurement("addKeyframeToBA"); + + // Note: a keyframe in DSO has two IDs: + // fh->shell->id: The number of the frame (counting all frames). + // fh->frameId: The number of the keyframe (counting only keyframes). + // Here we use the former. + + currBATimestamp = frames.back()->data->shell->timestamp; + // Forward to extensions. + for(auto& extension : extensions) + { + extension->addKeyframe(baGraphs.get(), baValues, keyframeId, keyframePose, frames); + } + + updateBAOrdering(frames); +} + +double BAGTSAMIntegration::getCurrBaTimestamp() const +{ + return currBATimestamp; +} + +const gtsam::Values* BAGTSAMIntegration::getBaValues() const +{ + return baValues.get(); +} + +const gtsam::Values* BAGTSAMIntegration::getBaEvalValues() const +{ + return baEvalValues.get(); +} + +double BAGTSAMIntegration::getBAEnergy(bool useNewValues) +{ + return 2.0 * baGraphs->getError(useNewValues ? *newBAValues : *baValues) / + settings.weightDSOToGTSAM/* / computeDSOWeight()*/; // caller is responsible for dividing by dynamic weight +} + +void BAGTSAMIntegration::addExtension(std::shared_ptr extension) +{ + extensions.push_back(extension); +} + +bool BAGTSAMIntegration::canBreak() const +{ + return canBreakOptimization; +} + +void BAGTSAMIntegration::removeExtension(BAExtension* extension) +{ + for(auto it = extensions.begin(); it != extensions.end(); ++it) + { + if(it->get() == extension) + { + extensions.erase(it); + return; + } + } + std::cerr << "Error: couldn't delete extension!" << std::endl; + std::cout << "Error: couldn't delete extension!" << std::endl; + assert(0); +} + +gtsam::NonlinearFactor::shared_ptr BAGTSAMIntegration::getActiveDSOFactor() +{ + // We recompute so that no lambda is added. + auto convertedHAndB = convertHAndBFromDSO(lastDSOH, lastDSOB, *transformationDSOToBA, computeDSOWeight(), + baOrdering, *baEvalValues, baDimMap); + return convertedDSOHAndBToFactor(convertedHAndB.first, convertedHAndB.second, *baValuesAfterBAUpdate, + lastDSOEnergy * + computeDSOWeight()); // note that we need to use baValues for the active Hessian. +} + +gtsam::Values* BAGTSAMIntegration::getMutableValues() +{ + return baValues.get(); +} + +BAGraphs& BAGTSAMIntegration::getBaGraphs() +{ + return *baGraphs; +} + +const std::map& BAGTSAMIntegration::getBaDimMap() const +{ + return baDimMap; +} + +const gtsam::Ordering& BAGTSAMIntegration::getBaOrdering() const +{ + return baOrdering; +} + +const gtsam::Ordering& BAGTSAMIntegration::getBaOrderingSmall() const +{ + return baOrderingSmall; +} + +void BAGTSAMIntegration::postOptimization(vector& frames) +{ + baEvalValues.reset(new gtsam::Values()); + *baEvalValues = *baValues; + computeEvaluationPointValues(frames, baEvalValues); + baGraphs->updateEvalValues(*baEvalValues); +} + +void BAGTSAMIntegration::setDynamicDSOWeightCallback(BAGTSAMIntegration::DynamicWeightCallback callback) +{ + dynamicWeightCallback = std::move(callback); +} + +double BAGTSAMIntegration::computeDSOWeight() const +{ + return settings.weightDSOToGTSAM * dynamicDSOWeight; +} + +double BAGTSAMIntegration::updateDynamicWeight(double energy, double rmse, bool coarseTrackingWasGood) +{ + if(dynamicWeightCallback) + { + dynamicDSOWeight = dynamicWeightCallback(energy, rmse, coarseTrackingWasGood); + }else + { + dynamicDSOWeight = 1.0; + } + lastDSOEnergy = energy; + return dynamicDSOWeight; +} + +void dmvio::fillAdditionalKeysFromScatter(const gtsam::Ordering& originalOrdering, gtsam::Ordering& orderingToFill, + const AugmentedScatter& scatter) +{ + if(originalOrdering.size() < scatter.size()) + { + auto it = scatter.begin(); + it += originalOrdering.size(); + std::transform(it, scatter.end(), std::back_inserter(orderingToFill), + [](const gtsam::SlotEntry& entry) -> gtsam::Key + { return entry.key; }); + } +} + +// Default implementation has just one factor graph and simply calls add. +void BAGraphs::addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group) +{ + graph->push_back(factor); +} + +// Default implementation has just one factor graph and simply linearizes and computes the Hessian. +std::pair +BAGraphs::getHAndB(const gtsam::Values& values, const gtsam::Ordering& ordering, + const std::map& keyDimMap, gtsam::Ordering* fillAdditionalKeys) +{ + gtsam::GaussianFactorGraph::shared_ptr gfg = graph->linearize(values); + AugmentedScatter scatter(*gfg, ordering, keyDimMap); + if(fillAdditionalKeys) + { + fillAdditionalKeysFromScatter(ordering, *fillAdditionalKeys, scatter); + } + return scatter.computeHessian(*gfg); +} \ No newline at end of file diff --git a/src/GTSAMIntegration/BAGTSAMIntegration.h b/src/GTSAMIntegration/BAGTSAMIntegration.h new file mode 100644 index 0000000..d3efe9a --- /dev/null +++ b/src/GTSAMIntegration/BAGTSAMIntegration.h @@ -0,0 +1,251 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_BAGTSAMINTEGRATION_H +#define DMVIO_BAGTSAMINTEGRATION_H + + +#include +#include +#include +#include +#include +#include +#include + +#include "PoseTransformation.h" +#include "AugmentedScatter.hpp" + + +// This source file, and in particular the class BAGTSAMIntegration is responsible for integrating the Bundle Adjustment for DSO into GTSAM. +// While the photometric factors are still computed using the original DSO code, new factors can easily be added +// as standard GTSAM factors, as long as they do not depend on points depths. +// This works by registering a BAExtension, which can add new factors and keys. +namespace dmvio +{ + +typedef std::map KeyDimMap; + +class BAGraphs; + +// Interface for an extension to the Bundle Adjustment of DSO. +// It can add additional keys and factors which will be organized. +class BAExtension +{ +public: + + virtual ~BAExtension() = default; + + // Called when the first keyframe is added. + virtual void addFirstBAFrame(int keyframeId, BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues) = 0; + // Called when a keyframe (except the first one) is added. + // The method is supposed to add its factors to baGraphs, and new variables into baValues. + virtual void addKeyframe(BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues, int keyframeId, + const Sophus::SE3d& keyframePose, std::vector& frames) = 0; + + // The extension can update the passed ordering with more variables. + virtual void + updateBAOrdering(std::vector& frames, gtsam::Ordering* ordering, KeyDimMap& baDimMap) = 0; + + // Called when a KF gets marginalized. The method is supposed to add keys which shall be marginalized. + virtual void addKeysToMarginalize(int fullId, gtsam::FastVector& keysToMarginalize) = 0; + + // These methods are called before and after solving the linear system in computeBAUpdate. + virtual void preSolve(gtsam::Matrix& HFull, gtsam::Vector& bFull, int dimensionDSOH) = 0; + // called after solve the system. Should return true if the optimization can break. + virtual bool + postSolve(gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr newValues, const gtsam::Vector& inc, + const gtsam::Ordering& ordering, const KeyDimMap& baDimMap) = 0; + + // Called when the BA update has been accepted and values will be replaced with newValues shortly. + virtual void acceptUpdate(gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr newValues) = 0; + +}; + +// Interface for either a gtsam::NonlinearFactorGraph or a collection of them. +// This exists to abstract techniques like Delayed Marginalization or Dynamic Marginalization from the BAExtension. +// Currently the only implementation is DelayedMarginalizationGraphs. +class BAGraphs +{ +public: + virtual ~BAGraphs() = default; + + // Adds factor to the right factor graphs. + // Factors can have different groups, which can later be used to perform operations only on specific groups. + virtual void addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group) = 0; + + // Return H and b (Hessian and gradient) at a linearization point defined by values. + virtual std::pair + getHAndB(const gtsam::Values& values, const gtsam::Ordering& ordering, + const std::map& keyDimMap, gtsam::Ordering* fillAdditionalKeys) = 0; + + // Marginalize the given keys from the factor graph. + virtual void + marginalizeFrame(const gtsam::FastVector& keysToMarginalize, gtsam::Values::shared_ptr values, + std::map& keyDimMap, double currBATimestamp, + gtsam::Values::shared_ptr currValues) = 0; + + virtual double getError(const gtsam::Values& values) = 0; + + // Called when the evalValues (given by DSO) are updated. + virtual void updateEvalValues(const gtsam::Values& evalValues) + {} + +private: + gtsam::NonlinearFactorGraph::shared_ptr graph; +}; + + +// Settings for the GTSAM Integration. +class GTSAMIntegrationSettings +{ +public: + // All these settings are typically overwritten by IMUSettings member. Therefore not registered as arg. + double weightDSOToGTSAM = 1.0; +}; + +// This class interacts with the DSO Bundle Adjustment and integrates GTSAM into it. +// This works by intercepting the DSO Hessian and gradient vector right before the update is computed. +// Then the Hessian and gradient vector computed from the GTSAM factors is added. +// Special care is taken of the different conventions between DSO and GTSAM (left vs right sided update, different +// pose types), and on making the keyframe marginalization works (which is taken over by GTSAM now). +// This way the GTSAM integration can be decoupled from the Bundle-Adjustment code of DSO, mathematically the result +// is equal to a joint optimization of all variables, as long as no GTSAM factor depends on the point depths. +class BAGTSAMIntegration +{ +public: + // Will take ownership of passed unique_ptrs. + BAGTSAMIntegration(std::unique_ptr baGraphs, std::unique_ptr transformationDSOToBA, + const GTSAMIntegrationSettings& integrationSettings, dso::CalibHessian* HCalib); + + // Add an extension which can add new factors and keys to the optimization. + void addExtension(std::shared_ptr extension); + + // In order to set a dynamic weight for the DSO variables a DynamicWeightCallback can be registered, which + // should the new dynamic weight part. + using DynamicWeightCallback = std::function; + void setDynamicDSOWeightCallback(DynamicWeightCallback callback); + + // Get a factor which contains the active DSO part. + // Must only be called between an optimization iteration and the next marginalization (as otherwise the ordering does not match the matrices). + gtsam::NonlinearFactor::shared_ptr getActiveDSOFactor(); + + void removeExtension(BAExtension* extension); + + + // Getters + // -------------------------------------------------- + double getCurrBaTimestamp() const; + + // There are both, baValues and baEvalValues. The former are the current values obtained during the optimization, + // and the latter use the First Estimates values **for DSO variables only** (i.e. poses and affine brightness). + // FEJ values for all other variables are handled by the class FEJValues. + const gtsam::Values* getBaValues() const; + gtsam::Values* getMutableValues(); + const gtsam::Values* getBaEvalValues() const; + BAGraphs& getBaGraphs(); + const std::map& getBaDimMap() const; + const gtsam::Ordering& getBaOrdering() const; + const gtsam::Ordering& getBaOrderingSmall() const; + + // -------------------------------------------------- + // Most other methods will be called by DSO code, and except for the getters they should not be called from an extension. + // -------------------------------------------------- + + void addFirstBAFrame(int keyframeId); + + void addKeyframeToBA(int keyframeId, const Sophus::SE3d& keyframePose, std::vector& frames); + + // called from addKeyframeToBA and from outside (FullSystem::makeKeyFrame) + void updateBAOrdering(std::vector& frames); + + // updates the baValues with the data from DSO. + void updateBAValues(std::vector& frames); + + // inserts the data at the evaluation points from DSO into values. + void computeEvaluationPointValues(const std::vector& frames, gtsam::Values::shared_ptr values); + + // This is the main method which intercepts the DSO update: It gets the Hessian (H) and gradient vector (b) computed by DSO, and outputs the update. + dso::VecX computeBAUpdate(const dso::MatXX& inputH, const dso::VecX& inputB, double lambda, + std::vector& frames, const dso::MatXX& HNoLambda); + + void acceptBAUpdate(double energy); + double updateDynamicWeight(double energy, double rmse, bool coarseTrackingWasGood); + + // returns true if the step was small enough for the optimization to break. + bool canBreak() const; + + // called after the optimization finished. + void postOptimization(std::vector& frames); + + void marginalizeBAFrame(dso::EFFrame* fh); + + void addMarginalizedPointsBA(const dso::MatXX& H, const dso::VecX& b, std::vector& frames); + + void addPriorBA(dso::EFFrame* h, const dso::Vec8& H_diag, const dso::Vec8& b_in); + + double getBAEnergy(bool useNewValues); + +protected: + std::vector> extensions; + +private: + + std::unique_ptr baGraphs; + + // Will be the identity transformation for now, but convert epsilon from right to left. + std::unique_ptr transformationDSOToBA; + + GTSAMIntegrationSettings settings; + + boost::shared_ptr baValues, baEvalValues, newBAValues, baValuesAfterBAUpdate; + gtsam::Ordering baOrdering, baOrderingSmall; // baOrdering contains all keys and baOrderingSmall only the ones known by DSO (only poses). + std::map baDimMap; + + bool canBreakOptimization = false; + + dso::CalibHessian* HCalib; + + double currBATimestamp{-1}; + + // stored for getActiveDSOFactor. + dso::MatXX lastDSOH; + dso::VecX lastDSOB; + double lastDSOEnergy = 0.0; + + gtsam::LinearContainerFactor::shared_ptr + convertedDSOHAndBToFactor(const gtsam::Matrix& H, const gtsam::Vector& b, + const gtsam::Values& values, double energy = 0.0); + + DynamicWeightCallback dynamicWeightCallback; + double computeDSOWeight() const; + double dynamicDSOWeight = 1.0; +}; + +void fillAdditionalKeysFromScatter(const gtsam::Ordering& originalOrdering, gtsam::Ordering& orderingToFill, + const AugmentedScatter& scatter); + +} + + +#endif //DMVIO_BAGTSAMINTEGRATION_H diff --git a/src/GTSAMIntegration/DelayedMarginalization.cpp b/src/GTSAMIntegration/DelayedMarginalization.cpp new file mode 100644 index 0000000..8ee021e --- /dev/null +++ b/src/GTSAMIntegration/DelayedMarginalization.cpp @@ -0,0 +1,367 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "DelayedMarginalization.h" +#include "Marginalization.h" +#include "util/TimeMeasurement.h" +#include "GTSAMUtils.h" +#include +#include + +using namespace dmvio; + +DelayedMarginalizationGraphs::DelayedMarginalizationGraphs(int mainGraphDelay, int maxGroupInMainGraph) +{ + addMainGraph(std::make_shared(mainGraphDelay, maxGroupInMainGraph)); +} + +std::shared_ptr dmvio::DelayedMarginalizationGraphs::addDelayedGraph(int delayN, int maxGroupInGraph) +{ + delayedGraphs.emplace_back(new DelayedGraph(delayN, maxGroupInGraph)); + return delayedGraphs.back(); +} + +void DelayedMarginalizationGraphs::addDelayedGraph(std::shared_ptr graph) +{ + delayedGraphs.emplace_back(std::move(graph)); +} + +void dmvio::DelayedMarginalizationGraphs::addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group) +{ + for(auto&& graph : delayedGraphs) + { + graph->addFactor(factor, group); + } + for(auto&& graph : disconnectedGraphs) + { + graph->addFactor(factor, group); + } +} + +std::pair +dmvio::DelayedMarginalizationGraphs::getHAndB(const gtsam::Values& values, const gtsam::Ordering& ordering, + const std::map& keyDimMap, + gtsam::Ordering* fillAdditionalKeys) +{ + auto graph = getMainGraph()->getGraph(); + + // Make sure that the GTSAM factors use the FEJValues. + getMainGraph()->setFEJValuesForFactors(true); + // Lienarize the graph + gtsam::GaussianFactorGraph::shared_ptr gfg = graph->linearize(values); + getMainGraph()->setFEJValuesForFactors(false); + + // Compute the Hessian and gradient vector. We use the AugmentedScatter which also works for keys which don't exist in the graph. + AugmentedScatter scatter(*gfg, ordering, keyDimMap); + if(fillAdditionalKeys) + { + fillAdditionalKeysFromScatter(ordering, *fillAdditionalKeys, scatter); + } + return scatter.computeHessian(*gfg); +} + +void dmvio::DelayedMarginalizationGraphs::marginalizeFrame(const gtsam::FastVector& keysToMarginalize, + gtsam::Values::shared_ptr values, + std::map& keyDimMap, + double currBATimestamp, + gtsam::Values::shared_ptr currValues) +{ + dmvio::TimeMeasurement meas("DelayedMarginalization"); + + // First only marginalize the main graph so that we have separate time measurements. + dmvio::TimeMeasurement mainMeas( + "MarginalizeMainGraph"); // This saves only the time necessary for marginalizing the main graph. + auto* mainGraph = getMainGraph().get(); + mainGraph->marginalize(keysToMarginalize, values, currValues); + mainMeas.end(); + + dmvio::TimeMeasurement measDelayed("DelayedMarginalizationOnly"); + for(auto&& graph : delayedGraphs) + { + if(graph.get() == mainGraph) continue; + graph->marginalize(keysToMarginalize, values, currValues); + } + for(auto&& graph : disconnectedGraphs) + { + graph->marginalize(keysToMarginalize, values, currValues); + } + meas.end(); +} + +double dmvio::DelayedMarginalizationGraphs::getError(const gtsam::Values& values) +{ + return getMainGraph()->getGraph()->error(values); +} + +void DelayedMarginalizationGraphs::addMainGraph(std::shared_ptr delayedGraph) +{ + mainGraphInd = delayedGraphs.size(); + delayedGraphs.emplace_back(std::move(delayedGraph)); + for(auto&& callback : mainGraphCallbacks) + { + callback(delayedGraph); + } +} + +void DelayedMarginalizationGraphs::replaceMainGraph(std::shared_ptr delayedGraph) +{ + delayedGraphs[mainGraphInd] = std::move(delayedGraph); + for(auto&& callback : mainGraphCallbacks) + { + callback(delayedGraphs[mainGraphInd]); + } +} + +std::shared_ptr DelayedMarginalizationGraphs::getMainGraph() +{ + return delayedGraphs[mainGraphInd]; +} + +void DelayedMarginalizationGraphs::removeDelayedGraph(const DelayedGraph* graph) +{ + auto it = std::find_if(delayedGraphs.begin(), delayedGraphs.end(), + [graph](const std::shared_ptr& comp) + { return comp.get() == graph; }); + delayedGraphs.erase(it); +} + +void DelayedMarginalizationGraphs::registerMainGraphReplacementCallback( + DelayedMarginalizationGraphs::GraphReplacementCallback callback) +{ + mainGraphCallbacks.push_back(std::move(callback)); +} + +void DelayedMarginalizationGraphs::updateEvalValues(const gtsam::Values& evalValues) +{ + for(auto&& graph : delayedGraphs) + { + graph->fejValues->insertConnectedKeys(gtsam::Ordering(), evalValues); + } +} + +std::shared_ptr +DelayedMarginalizationGraphs::addDisconnectedGraph(int maxGroupInGraph) +{ + disconnectedGraphs.emplace_back(new DisconnectedDelayedGraph(maxGroupInGraph)); + return disconnectedGraphs.back(); +} + +void DelayedMarginalizationGraphs::removeDisconnectedGraph(const DisconnectedDelayedGraph* graph) +{ + auto it = std::find_if(disconnectedGraphs.begin(), disconnectedGraphs.end(), + [graph](const std::shared_ptr& comp) + { return comp.get() == graph; }); + disconnectedGraphs.erase(it); +} + +dmvio::DelayedGraph::DelayedGraph(int delayN, int maxGroupInGraph) : delayN(delayN), maxGroupInGraph(maxGroupInGraph) +{ + graph.reset(new gtsam::NonlinearFactorGraph()); + fejValues.reset(new FEJValues); +} + +void dmvio::DelayedGraph::addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group) +{ + if(group <= maxGroupInGraph) + { + graph->add(factor); + } +} + +void +dmvio::DelayedGraph::marginalize(const gtsam::FastVector& keysToMarginalize, + gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr currValues) +{ + marginalizationOrder.push_back(keysToMarginalize); + + // update values + for(auto&& val : *values) + { + eraseAndInsert(delayedValues, val.key, val.value); + } + if(currValues) + { + for(auto&& val : *currValues) + { + eraseAndInsert(delayedCurrValues, val.key, val.value); + } + } + + readvanceUntilDelay(); +} + +void dmvio::DelayedGraph::readvanceUntilDelay() +{ + if(marginalizationPaused) return; + while(marginalizationOrder.size() > delayN) + { + auto&& keysToMarg = marginalizationOrder.front(); + + // Insert values for keysToMarginalize, even though they will be removed below, but they are necessary + // so that all fejValues contains all values needed for the linearization below. + fejValues->insertConnectedKeys(keysToMarg, delayedValues); + + // The connected keys have to be inserted into the fejMap before the linearization inside marginalizeOut. + auto connectedKeyCallback = [this](const gtsam::FastSet& connectedKeys) + { + // Get connected keys and insert fejMap. + // delayedValues should be baEvalValues, i.e. evaluation point values for poses and affine, and the current values for everything else. + fejValues->insertConnectedKeys(connectedKeys, delayedValues); + }; + + if(!keysToMarg.empty()) + { + // Note that the connectedKeyCallback fills fejValues.fejValues with the needed values before the linearization happens. + graph = marginalizeOut(*graph, fejValues->fejValues, keysToMarg, connectedKeyCallback); + + for(auto&& key : keysToMarg) + { + if(delayedCurrValues.exists(key)) + { + delayedCurrValues.erase(key); + } + if(delayedValues.exists(key)) + { + delayedValues.erase(key); + } + } + + fejValues->keysRemoved(keysToMarg); + } + + marginalizationOrder.pop_front(); + } +} + +void DelayedGraph::readvanceGraph(int newDelay) +{ + assert(newDelay <= delayN); + delayN = newDelay; + if(marginalizationPaused) + { + std::cout << "WARNING: Trying to readvance graph while marginalization is paused!" << std::endl; + } + readvanceUntilDelay(); +} + +int DelayedGraph::getDelayN() const +{ + return delayN; +} + +gtsam::NonlinearFactorGraph::shared_ptr DelayedGraph::getGraph() const +{ + return graph; +} + +const gtsam::Values& DelayedGraph::getDelayedValues() const +{ + return delayedValues; +} + +const gtsam::Values& DelayedGraph::getDelayedCurrValues() const +{ + return delayedCurrValues; +} + +int DelayedGraph::getMaxGroupInGraph() const +{ + return maxGroupInGraph; +} + +const std::deque>& DelayedGraph::getMarginalizationOrder() const +{ + return marginalizationOrder; +} + +DelayedGraph::DelayedGraph(int delayN, int maxGroupInGraph, const gtsam::NonlinearFactorGraph::shared_ptr& graph, + std::deque> marginalizationOrder, + gtsam::Values delayedValues, gtsam::Values delayedCurrValues, + std::shared_ptr fejValuesPassed) + : delayN(delayN), maxGroupInGraph(maxGroupInGraph), graph(graph), + marginalizationOrder(std::move(marginalizationOrder)), delayedValues(std::move(delayedValues)), + delayedCurrValues(std::move(delayedCurrValues)) +{ + fejValues = std::move(fejValuesPassed); +} + +DelayedGraph::DelayedGraph(const DelayedGraph& other) + : delayN(other.delayN), maxGroupInGraph(other.maxGroupInGraph), + graph(boost::make_shared(other.graph->clone())), + marginalizationOrder(other.marginalizationOrder), delayedValues(other.delayedValues), + delayedCurrValues(other.delayedCurrValues), fejValues(new FEJValues(*(other.fejValues))) +{} + +void DelayedGraph::setMarginalizationPaused(bool marginalizationPausedPassed) +{ + marginalizationPaused = marginalizationPausedPassed; + if(!marginalizationPaused) + { + readvanceUntilDelay(); + } +} + +void DelayedGraph::setMaxGroupInGraph(int maxGroupInGraph) +{ + DelayedGraph::maxGroupInGraph = maxGroupInGraph; +} + +void DelayedGraph::setDelayN(int delayN) +{ + DelayedGraph::delayN = delayN; +} + +void DelayedGraph::setFEJValuesForFactors(bool useFEJ) +{ + setFEJMapForGraph(*graph, useFEJ ? fejValues : nullptr); +} + +DisconnectedDelayedGraph::DisconnectedDelayedGraph(int maxGroupInGraph) : maxGroupInGraph(maxGroupInGraph) +{} + +void DisconnectedDelayedGraph::addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group) +{ + if(group <= maxGroupInGraph) + { + addedFactors.push_back(factor); + } +} + +void DisconnectedDelayedGraph::marginalize(const gtsam::FastVector& keysToMarginalize, + gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr currValues) +{ + + marginalizationOrder.push_back(keysToMarginalize); + + // Also save values + for(auto&& val : *values) + { + eraseAndInsert(delayedValues, val.key, val.value); + } + if(currValues) + { + for(auto&& val : *currValues) + { + eraseAndInsert(delayedCurrValues, val.key, val.value); + } + } +} diff --git a/src/GTSAMIntegration/DelayedMarginalization.h b/src/GTSAMIntegration/DelayedMarginalization.h new file mode 100644 index 0000000..c7c3a23 --- /dev/null +++ b/src/GTSAMIntegration/DelayedMarginalization.h @@ -0,0 +1,167 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_DELAYEDMARGINALIZATION_H +#define DMVIO_DELAYEDMARGINALIZATION_H + +#include "GTSAMIntegration/BAGTSAMIntegration.h" +#include "GTSAMIntegration/PoseTransformation.h" +#include "GTSAMIntegration/FEJValues.h" + +namespace dmvio +{ + +// This is a delayed graph. It has a delay (can also be 0) and maxGroupInGraph which defines which factors are added to it. +// It is usually managed by DelayedMarginalizationGraphs. +class DelayedGraph +{ +public: + // Constructor taking the delay and the maximum factor group which will be added to the graph. + DelayedGraph(int delayN, int maxGroupInGraph); + + DelayedGraph(int delayN, int maxGroupInGraph, const gtsam::NonlinearFactorGraph::shared_ptr& graph, + std::deque> marginalizationOrder, + gtsam::Values delayedValues, gtsam::Values delayedCurrValues, std::shared_ptr fejValues); + + DelayedGraph(const DelayedGraph& other); // copy constructor. + + // changes the delay, but doesn't readvance. + void setDelayN(int delayN); + + // readvance graph to have a smaller delay now. + void readvanceGraph(int newDelay); + + // add factor if group <= maxGroupInGraph. + void addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group); + + void marginalize(const gtsam::FastVector& keysToMarginalize, gtsam::Values::shared_ptr values, + gtsam::Values::shared_ptr currValues); + + void setMaxGroupInGraph(int maxGroupInGraph); + + void setMarginalizationPaused(bool marginalizationPausedPassed); + + int getDelayN() const; + gtsam::NonlinearFactorGraph::shared_ptr getGraph() const; + const gtsam::Values& getDelayedValues() const; + const gtsam::Values& getDelayedCurrValues() const; + int getMaxGroupInGraph() const; + const std::deque>& getMarginalizationOrder() const; + + std::shared_ptr fejValues; // These contain FEJValues for all variables connected to a marginalization factor (including all DSO poses). + + void setFEJValuesForFactors(bool useFEJ); + + friend class PoseGraphBundleAdjustment; +protected: + // perform marginalization until we match the wanted delay. + void readvanceUntilDelay(); + + int delayN; + + // Only add factors with group <= maxGroupInGraph + int maxGroupInGraph; + bool marginalizationPaused = false; // While true, no readvancing is performed (even if the delay becomes larger than delayN). + + gtsam::NonlinearFactorGraph::shared_ptr graph; + + std::deque> marginalizationOrder; + + // delayedValues contain the baEvalValues, meaning FEJValues for DSO variables, and current values for all other variables. + // delayedCurrValues contain baValues, meaning current values for all variables. + gtsam::Values delayedValues, delayedCurrValues; // contain the delayed values (including new keys). +}; + +// This is a delayed graph which just saves the factors and marginalization commands in the right order. +// It can be used to "reconnect" a DelayedGraph which has not been updated for a while. +// Used e.g. for the realtime version of the PGBA (which has to run in a separate thread decoupled from the main BA). +class DisconnectedDelayedGraph +{ +public: + explicit DisconnectedDelayedGraph(int maxGroupInGraph); + + void addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group); + void marginalize(const gtsam::FastVector& keysToMarginalize, gtsam::Values::shared_ptr values, + gtsam::Values::shared_ptr currValues); + + std::deque> marginalizationOrder; + std::vector addedFactors; + gtsam::Values delayedValues, delayedCurrValues; + + int maxGroupInGraph; +}; + + +// Main class responsible for the DelayedMarginalization. +class DelayedMarginalizationGraphs : public BAGraphs +{ +public: + // Constructor, pass arguments for the main DelayedGraph (usually has delay 0). + DelayedMarginalizationGraphs(int mainGraphDelay, int maxGroupInMainGraph); + + // Should usually be called before operation starts. + // Returns shared_ptr to the created graph. + std::shared_ptr addDelayedGraph(int delayN, int maxGroupInGraph); + void addDelayedGraph(std::shared_ptr graph); + void removeDelayedGraph(const DelayedGraph* graph); + + std::shared_ptr addDisconnectedGraph(int maxGroupInGraph); + void removeDisconnectedGraph(const DisconnectedDelayedGraph* graph); + + // Add a new graph which becomes the main graph. + void addMainGraph(std::shared_ptr delayedGraph); + // like addMainGraph, but also delete old main graph. delayedGraph must not be in the delayedGraphs yet! + void replaceMainGraph(std::shared_ptr delayedGraph); + + std::shared_ptr getMainGraph(); + + using GraphReplacementCallback = std::function& graph)>; + // Register a callback which will be called when the main graph is replaced. + void registerMainGraphReplacementCallback(GraphReplacementCallback callback); + + // Override from BAGraphs. + // -------------------------------------------------- + void addFactor(gtsam::NonlinearFactor::shared_ptr factor, int group) override; + std::pair getHAndB(const gtsam::Values& values, const gtsam::Ordering& ordering, + const std::map& keyDimMap, + gtsam::Ordering* fillAdditionalKeys) override; + void marginalizeFrame(const gtsam::FastVector& keysToMarginalize, gtsam::Values::shared_ptr values, + std::map& keyDimMap, double currBATimestamp, + gtsam::Values::shared_ptr currValues) override; + double getError(const gtsam::Values& values) override; + + void updateEvalValues(const gtsam::Values& evalValues) override; + +private: + // Delayed graphs to use. (doesn't contain main graph). + std::vector> delayedGraphs; + // disconnected delayed graphs. + std::vector> disconnectedGraphs; + int mainGraphInd = -1; + + std::vector mainGraphCallbacks; + +}; + +} + +#endif //DMVIO_DELAYEDMARGINALIZATION_H diff --git a/src/GTSAMIntegration/ExtUtils.h b/src/GTSAMIntegration/ExtUtils.h new file mode 100644 index 0000000..45b97bf --- /dev/null +++ b/src/GTSAMIntegration/ExtUtils.h @@ -0,0 +1,92 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_EXTUTILS_H +#define DMVIO_EXTUTILS_H + +#include +#include +#include + +#ifdef STACKTRACE +#include +#endif + +namespace dmvio +{ + +template void assertAlmostEq(const T& first, const T& second, double epsilon = 0.00001) +{ + T diff = std::abs(first - second); + if(diff > epsilon) + { + std::cout << "Error: Not Eq: " << diff << " first: " << first << " second" << second << std::endl; +#ifdef STACKTRACE + std::cout << boost::stacktrace::stacktrace(); +#endif + assert(0); + } + +} + +template void assertEqEigen(const T& first, const T& second, double epsilon = 0.00001) +{ + T diff = first - second; + if(!diff.isZero(epsilon)) + { + std::cout << "Error: Not Eq:\n" << diff << "\nfirst:\n" << first << "\nsecond\n" << second << std::endl; + std::cout << "Max diff: " << diff.cwiseAbs().maxCoeff() << std::endl; +#ifdef STACKTRACE + std::cout << boost::stacktrace::stacktrace(); +#endif + assert(0); + } +} + + +template class MeanAccumulator +{ +public: + void add(const T& t) + { + sum += t; + num++; + } + + T getMean() + { + if(num == 0) + { + return 0; + } + return sum / num; + } + +private: + T sum = 0.0; + int num = 0; +}; +typedef MeanAccumulator MeanAccumulatorD; + +} + +#endif //DMVIO_EXTUTILS_H diff --git a/src/GTSAMIntegration/FEJNoiseModelFactor.h b/src/GTSAMIntegration/FEJNoiseModelFactor.h new file mode 100644 index 0000000..7d060a6 --- /dev/null +++ b/src/GTSAMIntegration/FEJNoiseModelFactor.h @@ -0,0 +1,116 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ +// This code file is based on the file NonlinearFactor.cpp from the project GTSAM, which has been released under the following conditions: +/* ---------------------------------------------------------------------------- + * 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) + * +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#ifndef DMVIO_FEJNOISEMODELFACTOR_H +#define DMVIO_FEJNOISEMODELFACTOR_H + +#include "FEJValues.h" +#include +#include + +namespace dmvio +{ + +// Can be used to transform any NoiseModelFactor into a factor handling First-Estimates Jacobians correctly. +// Before optimizing setFEJMapForGraph has to be called for the Factor Graph. +// The class is based on the gtsam::NonlinearFactor. +// For using it can be wrapped around any type of NoiseModelFactor (e.g. IMUFactor, BetweenFactor, etc.) and +// arguments of the constructor will be forwarded. +template::value>::type> +class FEJNoiseModelFactor : public T, public FactorHandlingFEJ +{ +public: + template explicit FEJNoiseModelFactor(Args&& ... args) + : T(std::forward(args) ...) + {} + + void setFEJValues(std::shared_ptr fejValues) override + { + fej = std::move(fejValues); + } + + // Modified version of NoiseModelFactor::linearize of the project GTSAM. For license, see above. + boost::shared_ptr linearize(const gtsam::Values& x) const override + { + // TODO: update linearize if I update GTSAM. + // Only linearize if the factor is active + if(!T::active(x)) + return boost::shared_ptr(); + + // Call evaluate error to get Jacobians and RHS vector b + std::vector A(T::size()); + gtsam::Vector b; + if(fej) + { + gtsam::Values fejVals = fej->buildValues(T::keys(), x); + -T::unwhitenedError(fejVals, A); // compute derivatives with first estimates values. + b = -T::unwhitenedError(x); // Compute residual with current values. + }else + { + b = -T::unwhitenedError(x, A); + } + + // Whiten the corresponding system now + if(T::noiseModel_) + T::noiseModel_->WhitenSystem(A, b); + + // Fill in terms, needed to create JacobianFactor below + std::vector > terms(T::size()); + for(size_t j = 0; j < T::size(); ++j) + { + terms[j].first = T::keys()[j]; + terms[j].second.swap(A[j]); + } + + // TODO pass unwhitened + noise model to Gaussian factor + using gtsam::noiseModel::Constrained; + if(T::noiseModel_ && T::noiseModel_->isConstrained()) + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(terms, b, + boost::static_pointer_cast(T::noiseModel_)->unit())); + else + return gtsam::GaussianFactor::shared_ptr(new gtsam::JacobianFactor(terms, b)); + } + +protected: + std::shared_ptr fej; +}; + +} +#endif //DMVIO_FEJNOISEMODELFACTOR_H diff --git a/src/GTSAMIntegration/FEJValues.cpp b/src/GTSAMIntegration/FEJValues.cpp new file mode 100644 index 0000000..9b295dc --- /dev/null +++ b/src/GTSAMIntegration/FEJValues.cpp @@ -0,0 +1,36 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include "FEJValues.h" + +void dmvio::setFEJMapForGraph(gtsam::NonlinearFactorGraph& graph, const std::shared_ptr& fejValues) +{ + for(auto&& factor : graph) + { + auto* casted = dynamic_cast(factor.get()); + if(casted) + { + casted->setFEJValues(fejValues); + } + } +} diff --git a/src/GTSAMIntegration/FEJValues.h b/src/GTSAMIntegration/FEJValues.h new file mode 100644 index 0000000..240b23b --- /dev/null +++ b/src/GTSAMIntegration/FEJValues.h @@ -0,0 +1,106 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_FEJVALUES_H +#define DMVIO_FEJVALUES_H + +#include +#include +#include +#include +#include "dso/util/settings.h" +#include "Marginalization.h" +#include "GTSAMUtils.h" + +namespace dmvio +{ + +// Handles First-Estimates Jacobians (FEJ) for GTSAM. +class FEJValues +{ +public: + gtsam::Values fejValues; + + // Called when keys are become connected to marginalization factors. Their values will be inserted into fejValues. + template void insertConnectedKeys(const T& connectedKeys, const gtsam::Values& currentValues) + { + // always insert current poses and affine brightness (no matter if connected). + for(auto&& val : currentValues) + { + auto chr = gtsam::Symbol(val.key).chr(); + if(chr == 'p' || chr == 'a') + { + eraseAndInsert(fejValues, val.key, val.value); + } + } + for(auto&& key : connectedKeys) + { + if(!fejValues.exists(key)) + { + fejValues.insert(key, currentValues.at(key)); + } + } + } + + // Remove keys from FEJMap, as they have been marginalized / removed. + template void keysRemoved(const T& keysRemoved) + { + for(auto&& key : keysRemoved) + { + fejValues.erase(key); + } + } + + // Returns values containing neededKeys, using fejValues where available, otherwise current values. + template gtsam::Values buildValues(const T& neededKeys, const gtsam::Values& currentValues) + { + gtsam::Values ret; + for(auto&& key : neededKeys) + { + bool use = fejValues.exists(key); + if(use) + { + ret.insert(key, fejValues.at(key)); + }else + { + ret.insert(key, currentValues.at(key)); + } + } + return ret; + } + +}; + +// Interface for factors which can handle FEJ. +// Implemented by PoseTransformationFactor and FEJNoiseModelFactor. +class FactorHandlingFEJ +{ +public: + virtual void setFEJValues(std::shared_ptr fej) = 0; +}; + +// calls setFEJValues for all factors in graph which implement FactorHandlingFEJ. +void setFEJMapForGraph(gtsam::NonlinearFactorGraph& graph, const std::shared_ptr& fejValues); + +} + +#endif //DMVIO_FEJVALUES_H diff --git a/src/GTSAMIntegration/GTSAMUtils.cpp b/src/GTSAMIntegration/GTSAMUtils.cpp new file mode 100644 index 0000000..041f626 --- /dev/null +++ b/src/GTSAMIntegration/GTSAMUtils.cpp @@ -0,0 +1,78 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "GTSAMUtils.h" + +using namespace gtsam; + +gtsam::Matrix dmvio::augmentedHessianFromPair(const std::pair& pair) +{ + int n = pair.first.rows(); + + gtsam::Matrix returning = gtsam::zeros(n + 1, n + 1); + returning.block(0, 0, n, n) = pair.first; + returning.block(n, 0, 1, n) = pair.second.transpose(); + returning.block(0, n, n, 1) = pair.second; + return returning; +} + +std::pair dmvio::pairFromAugmentedHessian(const gtsam::Matrix& matrix) +{ + int n = matrix.rows() - 1; + return std::pair(matrix.block(0, 0, n, n), matrix.block(0, n, n, 1)); +} + +void dmvio::removeKeysFromGraph(gtsam::NonlinearFactorGraph& graph, const std::set& keysToRemove, + int stopAfterNoRemoval) +{ + if(keysToRemove.empty()) + { + return; + } + int noRemoval = 0; + // We use the fact that we inserted the factors in order. + for(auto it = graph.begin(); it != graph.end(); ++it) + { + auto&& keys = (*it)->keys(); + bool keyContained = false; + for(auto&& key : keys) + { + if(keysToRemove.find(key) != keysToRemove.end()) + { + keyContained = true; + break; + } + } + if(keyContained) + { + it = graph.erase(it); + it--; + noRemoval = 0; + }else + { + noRemoval++; + if(stopAfterNoRemoval > 0 && noRemoval >= stopAfterNoRemoval) break; + } + } +} + + diff --git a/src/GTSAMIntegration/GTSAMUtils.h b/src/GTSAMIntegration/GTSAMUtils.h new file mode 100644 index 0000000..ada9f22 --- /dev/null +++ b/src/GTSAMIntegration/GTSAMUtils.h @@ -0,0 +1,82 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_GTSAMUTILS_H +#define DMVIO_GTSAMUTILS_H + +#include +#include +#include +#include + +namespace dmvio +{ + +std::pair pairFromAugmentedHessian(const gtsam::Matrix& matrix); + +gtsam::Matrix augmentedHessianFromPair(const std::pair& pair); + +void removeKeysFromGraph(gtsam::NonlinearFactorGraph& graph, const std::set& keysToRemove, + int stopAfterNoRemoval = -1); + +template void eraseAndInsert(gtsam::Values& values, gtsam::Key key, const T& value) +{ + if(values.exists(key)) + { + values.update(key, value); + }else + { + values.insert(key, value); + } +} + +template void eraseAndInsert(gtsam::Values::shared_ptr values, gtsam::Key key, const T& value) +{ + eraseAndInsert(*values, key, value); +} + +template gtsam::Key getMinKeyWithChr(const C& keys, unsigned char chr) +{ + auto minIt = std::min_element(keys.begin(), keys.end(), [chr](const gtsam::Key& key1, const gtsam::Key& key2) + { + gtsam::Symbol sym1(key1); + gtsam::Symbol sym2(key2); + return (sym1.chr() == chr && sym1.index() < sym2.index()) || sym2.chr() != chr; + }); + return *minIt; +} + +template gtsam::Key getMaxKeyWithChr(const C& keys, unsigned char chr) +{ + // we still use min_element but reverse the comparison + auto minIt = std::min_element(keys.begin(), keys.end(), [chr](const gtsam::Key& key1, const gtsam::Key& key2) + { + gtsam::Symbol sym1(key1); + gtsam::Symbol sym2(key2); + return (sym1.chr() == chr && sym1.index() > sym2.index()) || sym2.chr() != chr; + }); + return *minIt; +} + +} + +#endif //DMVIO_GTSAMUTILS_H diff --git a/src/GTSAMIntegration/Marginalization.cpp b/src/GTSAMIntegration/Marginalization.cpp new file mode 100644 index 0000000..d82bb1c --- /dev/null +++ b/src/GTSAMIntegration/Marginalization.cpp @@ -0,0 +1,181 @@ +/** +* This file is part of DM-VIO. +* The code in this file is in part based on code written by Vladyslav Usenko for the paper "Direct Visual-Inertial Odometry with Stereo Cameras". +* +* Copyright (c) 2022 Lukas von Stumberg , Vladyslav Usenko +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include +#include +#include "Marginalization.h" +#include "GTSAMUtils.h" + +gtsam::NonlinearFactorGraph::shared_ptr +dmvio::marginalizeOut(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::FastVector& keysToMarginalize, + std::function&)> connectedKeyCallback) +{ + if(keysToMarginalize.empty()) + { + std::cout << "WARNING: Calling marginalizeOut with empty keysToMarginalize." << std::endl; + return boost::shared_ptr(new gtsam::NonlinearFactorGraph(graph)); + } + + boost::shared_ptr newGraph(new gtsam::NonlinearFactorGraph); + + gtsam::NonlinearFactorGraph marginalizedOutGraph; + + gtsam::FastSet setOfKeysToMarginalize(keysToMarginalize); + gtsam::FastSet connectedKeys; + + extractKeysToMarginalize(graph, *newGraph, marginalizedOutGraph, setOfKeysToMarginalize, connectedKeys); + + if(connectedKeyCallback) + { + connectedKeyCallback(connectedKeys); + } + + gtsam::GaussianFactorGraph::shared_ptr linearizedFactorsToMarginalize = marginalizedOutGraph.linearize(values); + std::map keyDimMap = linearizedFactorsToMarginalize->getKeyDimMap(); + + int mSize = 0; + int aSize = 0; + + gtsam::Ordering ordering; + + gtsam::Ordering connectedOrdering; + gtsam::FastVector connectedDims; + for(const gtsam::Key& k : setOfKeysToMarginalize) + { + ordering.push_back(k); + mSize += keyDimMap[k]; + } + for(const gtsam::Key& k : connectedKeys) + { + ordering.push_back(k); + connectedOrdering.push_back(k); + connectedDims.push_back(keyDimMap[k]); + aSize += keyDimMap[k]; + } + + gtsam::Matrix hessian = linearizedFactorsToMarginalize->augmentedHessian(ordering); + + gtsam::Matrix HAfterSchurComplement = computeSchurComplement(hessian, mSize, aSize); + + gtsam::SymmetricBlockMatrix sm(connectedDims, true); + sm.setFullMatrix(HAfterSchurComplement); + + gtsam::LinearContainerFactor::shared_ptr lcf(new gtsam::LinearContainerFactor( + gtsam::HessianFactor(connectedOrdering, sm), values)); + newGraph->add(lcf); + + return newGraph; +} + +gtsam::NonlinearFactorGraph::shared_ptr +dmvio::marginalizeOut(const gtsam::NonlinearFactorGraph& graph, gtsam::Values& values, + const gtsam::FastVector& keysToMarginalize, + std::function&)> connectedKeyCallback, + bool deleteFromValues) +{ + auto ret = marginalizeOut(graph, values, keysToMarginalize, connectedKeyCallback); + if(deleteFromValues) + { + for(size_t i = 0; i < keysToMarginalize.size(); i++) + { + values.erase(keysToMarginalize[i]); + } + } + return ret; +} + +void dmvio::extractKeysToMarginalize(const gtsam::NonlinearFactorGraph& graph, gtsam::NonlinearFactorGraph& newGraph, + gtsam::NonlinearFactorGraph& marginalizedOutGraph, + gtsam::FastSet& setOfKeysToMarginalize, + gtsam::FastSet& connectedKeys) +{ + for(size_t i = 0; i < graph.size(); i++) + { + gtsam::NonlinearFactor::shared_ptr factor = graph.at(i); + + gtsam::FastSet set_of_factor_keys(factor->keys()); + + gtsam::FastSet intersection; + + std::set_intersection(setOfKeysToMarginalize.begin(), setOfKeysToMarginalize.end(), + set_of_factor_keys.begin(), set_of_factor_keys.end(), + std::inserter(intersection, intersection.begin())); + + if(!intersection.empty()) + { + std::set_difference(set_of_factor_keys.begin(), set_of_factor_keys.end(), + setOfKeysToMarginalize.begin(), setOfKeysToMarginalize.end(), + std::inserter(connectedKeys, connectedKeys.begin())); + + marginalizedOutGraph.add(factor); + }else + { + newGraph.add(factor); + } + } +} + +gtsam::Matrix dmvio::computeSchurComplement(const gtsam::Matrix& augmentedHessian, int mSize, int aSize) +{ + auto pair = dmvio::pairFromAugmentedHessian(augmentedHessian); + + // Preconditioning like in DSO code. + gtsam::Vector SVec = (pair.first.diagonal().cwiseAbs() + + gtsam::Vector::Constant(pair.first.cols(), 10)).cwiseSqrt(); + gtsam::Vector SVecI = SVec.cwiseInverse(); + + gtsam::Matrix hessianScaled = SVecI.asDiagonal() * pair.first * SVecI.asDiagonal(); + gtsam::Vector bScaled = SVecI.asDiagonal() * pair.second; + + gtsam::Matrix Hmm = hessianScaled.block(0, 0, mSize, mSize); + gtsam::Matrix Hma = hessianScaled.block(0, mSize, mSize, aSize); + gtsam::Matrix Haa = hessianScaled.block(mSize, mSize, aSize, aSize); + + gtsam::Vector bm = bScaled.segment(0, mSize); + gtsam::Vector ba = bScaled.segment(mSize, aSize); + + // Compute inverse. + gtsam::Matrix HmmInv = Hmm.completeOrthogonalDecomposition().pseudoInverse(); + + gtsam::Matrix HaaNew = Haa - Hma.transpose() * HmmInv * Hma; + gtsam::Vector baNew = ba - Hma.transpose() * HmmInv * bm; + + // Unscale + gtsam::Vector SVecUpdated = SVec.segment(mSize, aSize); + gtsam::Matrix HNewUnscaled = SVecUpdated.asDiagonal() * HaaNew * SVecUpdated.asDiagonal(); + gtsam::Matrix bNewUnscaled = SVecUpdated.asDiagonal() * baNew; + + // Make Hessian symmetric for numeric reasons. + HNewUnscaled = 0.5 * (HNewUnscaled.transpose() + HNewUnscaled).eval(); + + gtsam::Matrix augmentedHRes(aSize + 1, aSize + 1); + augmentedHRes.setZero(); + augmentedHRes.topLeftCorner(aSize, aSize) = HNewUnscaled; + augmentedHRes.topRightCorner(aSize, 1) = bNewUnscaled; + augmentedHRes.bottomLeftCorner(1, aSize) = bNewUnscaled.transpose(); + + return augmentedHRes; +} + diff --git a/src/GTSAMIntegration/Marginalization.h b/src/GTSAMIntegration/Marginalization.h new file mode 100644 index 0000000..68534c0 --- /dev/null +++ b/src/GTSAMIntegration/Marginalization.h @@ -0,0 +1,58 @@ +/** +* This file is part of DM-VIO. +* The code in this file is in part based on code written by Vladyslav Usenko for the paper "Direct Visual-Inertial Odometry with Stereo Cameras". +* +* Copyright (c) 2022 Lukas von Stumberg , Vladyslav Usenko +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_MARGINALIZATION_H +#define DMVIO_MARGINALIZATION_H + +#include +#include + +namespace dmvio +{ +// Returns a factor graph where the specified keys are marginalized. +// connectedKeyCallback (can also be nullptr) will be called as soon as the connected variables are computed (and before the actual marginalization is performed). +gtsam::NonlinearFactorGraph::shared_ptr +marginalizeOut(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::FastVector& keysToMarginalize, + std::function&)> connectedKeyCallback); + +// Like the above method, but can also delete the marginalized variables from the passed values. +gtsam::NonlinearFactorGraph::shared_ptr marginalizeOut(const gtsam::NonlinearFactorGraph& graph, gtsam::Values& values, + const gtsam::FastVector& keysToMarginalize, + std::function&)> connectedKeyCallback, + bool deleteFromValues); + +// Fills newGraph with factors which are not connected and marginalizedOutGraph with all factors which will be marginalized out, +// Also fills setOfKeysToMarginalize, and connectedKeys. +void extractKeysToMarginalize(const gtsam::NonlinearFactorGraph& graph, gtsam::NonlinearFactorGraph& newGraph, + gtsam::NonlinearFactorGraph& marginalizedOutGraph, + gtsam::FastSet& setOfKeysToMarginalize, + gtsam::FastSet& connectedKeys); + +// Compute the Schur complement with the given dimension of marginalized factors and other factors. +gtsam::Matrix computeSchurComplement(const gtsam::Matrix& augmentedHessian, int mSize, int aSize); + +} + +#endif //DMVIO_MARGINALIZATION_H diff --git a/src/GTSAMIntegration/PoseTransformation.cpp b/src/GTSAMIntegration/PoseTransformation.cpp new file mode 100644 index 0000000..826f485 --- /dev/null +++ b/src/GTSAMIntegration/PoseTransformation.cpp @@ -0,0 +1,311 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include "PoseTransformation.h" +#include "dso/util/FrameShell.h" +#include "ExtUtils.h" +#include "Sim3GTSAM.h" + +using namespace dmvio; +using dso::Vec8; +using gtsam::Matrix, gtsam::Vector; +using gtsam::Symbol; + +// Default implementation for child classes uses numeric Jacobians and should work out of the box. +gtsam::Matrix66 PoseTransformation::getPoseDerivative(const PoseType& posePassed, DerivativeDirection direction) +{ + // copy, because it needs to be changed by computeNumericJacobian. + Sophus::SE3d pose(posePassed); + return computeNumericJacobian(*this, pose, &pose, direction); +} + +std::vector +PoseTransformation::getAllDerivatives(const PoseType& pose, DerivativeDirection direction) +{ + std::vector returning; + returning.push_back(getPoseDerivative(pose, direction)); + return returning; +} + +std::vector PoseTransformation::getAllOptimizedSymbols() const +{ + return std::vector(); +} + +gtsam::Matrix66 TransformIdentity::getPoseDerivative(const PoseType& pose, DerivativeDirection direction) +{ + // Analytic derivative only implemented for RIGHT_to_LEFT at the moment. + if(direction == DerivativeDirection::RIGHT_TO_LEFT) + { + gtsam::Matrix66 returning = gtsam::Pose3(pose).AdjointMap(); +#ifdef DEBUG + // Check numeric jacobian. + Sophus::SE3d poseForNum(pose); + gtsam::Matrix numJac = computeNumericJacobian(*this, poseForNum, &poseForNum, direction); + assertNumericJac(numJac, returning); +#endif + return returning; + } + return PoseTransformation::getPoseDerivative(pose, direction); // Use numeric derivative if not implemented. +} + +// Exchanges rotation and translation (the first 6 rows/columns) in a Jacobian matrix. +Eigen::MatrixXd dmvio::convertJacobianToGTSAM(const Eigen::MatrixXd& jacobian) +{ + Eigen::MatrixXd ret = jacobian; + int rows = jacobian.rows(); + int cols = jacobian.cols(); + + // first exchange rows + ret.block(0, 0, 3, cols) = jacobian.block(3, 0, 3, cols); + ret.block(3, 0, 3, cols) = jacobian.block(0, 0, 3, cols); + + // then exchange columns (unless the derivative is w.r.t an SO(3) element). + if(cols > 3) + { + Eigen::MatrixXd tmp = ret.block(0, 0, rows, 3); + ret.block(0, 0, rows, 3) = ret.block(0, 3, rows, 3); + ret.block(0, 3, rows, 3) = tmp; + } + + return ret; +} + +void dmvio::convertAllPosesWithTransform(const gtsam::Values& values, const PoseTransformation& transformation, + const gtsam::KeyVector& keysToInclude, gtsam::Values& insertInto) +{ + for(int i = 0; i < keysToInclude.size(); ++i) + { + gtsam::Key key = keysToInclude.at(i); + if(gtsam::Symbol(key).chr() == 'p') + { + gtsam::Pose3 pose = values.at(key); + insertInto.insert(key, gtsam::Pose3(transformation.transformPose(pose.matrix()))); + }else + { + insertInto.insert(key, values.at(key)); + } + } +} + +std::pair +dmvio::convertHAndBFromDSO(const dso::MatXX& HInput, const dso::VecX& bInput, PoseTransformation& poseTransformation, + double weightDSOToGTSAM, const gtsam::Ordering& ordering, const gtsam::Values& values, + const std::map& keyDimMap) +{ + dmvio::TimeMeasurement timeMeasurement("convertHAndBFromDSO"); + int n = HInput.rows(); + assert(HInput.cols() == n); + + gtsam::Matrix H = HInput; + gtsam::Vector b = bInput; + + // Here we use the fact that the Hessian from DSO will always contain first calibration parameters, and then poses (each followed by a and b). + int numCPARS = 0; + for(auto&& key : ordering) + { + if(Symbol(key).chr() != 'c') break; + numCPARS += keyDimMap.at(key); + } + + int numFrames = (n - numCPARS) / 8; + assert(n == numFrames * 8 + numCPARS); + + // Exchange rotation with translation for all frames (because the order is different in GTSAM!) + // First exchange the rows. + for(int i = 0; i < numFrames; ++i) + { + int id = numCPARS + 8 * i; + + H.block(id, 0, 3, n) = HInput.block(id + 3, 0, 3, n); + H.block(id + 3, 0, 3, n) = HInput.block(id, 0, 3, n); + + b.segment(id, 3) = bInput.segment(id + 3, 3); + b.segment(id + 3, 3) = bInput.segment(id, 3); + } + + // Then exchange the column + for(int i = 0; i < numFrames; ++i) + { + int id = numCPARS + 8 * i; + dso::MatXX tmp = H.block(0, id, n, 3); + H.block(0, id, n, 3) = H.block(0, id + 3, n, 3); + H.block(0, id + 3, n, 3) = tmp; + } + + // multiply with weight + H *= weightDSOToGTSAM; + b *= weightDSOToGTSAM; + + // DSO has left sided increment and GTSAM has right sided increment. + // For converting the Hessian the derivative is always the other way round, so here we have to pass RIGHT_TO_LEFT. + auto pair = convertHAndBWithPoseTransformation(std::make_pair(H, b), ordering, keyDimMap, values, + poseTransformation, + DerivativeDirection::RIGHT_TO_LEFT); + // DSO uses -b compared to GTSAM. + pair.second = -pair.second; + return pair; +} + + +std::pair +dmvio::convertHAndBWithPoseTransformation(const std::pair& input, + const gtsam::Ordering& ordering, + const std::map& keyDimMap, + const gtsam::Values& values, PoseTransformation& poseTransformation, + DerivativeDirection derivativeDirection) +{ + // Find out positions of optimized transformation variables in the Hessian. + auto&& optimizedVariables = poseTransformation.getAllOptimizedSymbols(); + int numOpt = optimizedVariables.size(); + std::vector optPositions(numOpt, -1); + if(numOpt > 0) + { + int numToFindOut = numOpt; + int pos = 0; + for(auto&& key : ordering) + { + int size = keyDimMap.at(key); + for(int i = 0; i < optimizedVariables.size(); ++i) + { + if(key == optimizedVariables[i]) + { + optPositions[i] = pos; + numToFindOut--; + } + } + if(numToFindOut == 0) break; + pos += size; + } + assert(numToFindOut == 0); + } + + // buildRelativeJacobian expects vector of sizes. + std::vector sizes; + sizes.reserve(ordering.size()); + for(auto&& key : ordering) + { + sizes.push_back(keyDimMap.at(key)); + } + + // Build relative Jacobian + int n = input.first.rows(); + gtsam::Matrix bigJ = buildRelativeJacobian(n, ordering, sizes, values, poseTransformation, + derivativeDirection, numOpt, + optPositions); + + gtsam::Matrix H = gtsam::zeros(n, n); + gtsam::Vector b = gtsam::zeros(n, 1); + + // Multiply Hessian and b with the relative Jacobian. + H = bigJ.transpose() * input.first * bigJ; + b = bigJ.transpose() * input.second; + + return std::make_pair(H, b); +} + +gtsam::Matrix +dmvio::buildRelativeJacobian(int n, const std::vector& ordering, const std::vector& dimensions, + const gtsam::Values& values, PoseTransformation& poseTransformation, + const DerivativeDirection& derivativeDirection, int numOpt, + const std::vector& optPositions) +{ + gtsam::Matrix bigJ = gtsam::eye(n, n); + + poseTransformation.precomputeForDerivatives(); + int pos = 0; + for(int i = 0; i < ordering.size(); ++i) + { + gtsam::Key key = ordering[i]; + int size = dimensions[i]; + unsigned char chr = gtsam::Symbol(key).chr(); + if(chr == 'p') // Only convert poses. + { + gtsam::Pose3 pose = values.at(key); + // Get all derivatives from the poseTransformation. + std::vector derivatives = poseTransformation.getAllDerivatives(pose.matrix(), derivativeDirection); + + // First put in pose derivative: + bigJ.block(pos, pos, 6, 6) = derivatives[0]; + + for(int j = 0; j < numOpt; ++j) + { + int optPos = optPositions[j]; + // At pos 1 because first one is pose derivative. + auto&& deriv = derivatives[j + 1]; + assert(optPos >= 0); + bigJ.block(pos, optPos, 6, deriv.cols()) = deriv; + } + } + + pos += size; + } + return bigJ; +} + +std::pair +dmvio::convertCoarseHToGTSAM(PoseTransformation& transform, const dso::Mat88& HInput, const dso::Vec8& bInput, + const gtsam::Pose3& currentPose) +{ + // Correct order (switch translation and rotation and put the affine parameters to the front!) + // The Sophus twist constains first translation and then rotation. + // The GTSAM Pose3 contains first rotation and then translation! + gtsam::Matrix8 H; + gtsam::Vector8 b; + + H.block(2, 2, 3, 3) = HInput.block(3, 3, 3, 3); + H.block(5, 5, 3, 3) = HInput.block(0, 0, 3, 3); + + H.block(5, 2, 3, 3) = HInput.block(0, 3, 3, 3); + H.block(2, 5, 3, 3) = HInput.block(3, 0, 3, 3); + + H.block(0, 2, 2, 3) = HInput.block(6, 3, 2, 3); + H.block(0, 5, 2, 3) = HInput.block(6, 0, 2, 3); + + H.block(2, 0, 3, 2) = HInput.block(3, 6, 3, 2); + H.block(5, 0, 3, 2) = HInput.block(0, 6, 3, 2); + + H.block(0, 0, 2, 2) = HInput.block(6, 6, 2, 2); + + b.segment(2, 3) = bInput.segment(3, 3); + b.segment(5, 3) = bInput.segment(0, 3); + b.segment(0, 2) = bInput.segment(6, 2); + + // Compute Jacobian of frame and reference pose with respect to T_f_r + gtsam::Matrix J(6, 12); + std::vector derivatives = transform.getAllDerivatives(currentPose.matrix(), + DerivativeDirection::RIGHT_TO_LEFT); + + J.block(0, 6, 6, 6) = derivatives[0]; // J with respect to T_w_f + J.block(0, 0, 6, 6) = derivatives[1]; // J with respect to T_w_r + + gtsam::Matrix JReal(8, 14); + JReal.block(2, 2, 6, 12) = J; + JReal.block(0, 2, 2, 12) = gtsam::zeros(2, 12); + JReal.block(0, 0, 2, 2) = gtsam::eye(2, 2); + JReal.block(2, 0, 6, 2) = gtsam::zeros(6, 2); + + gtsam::Matrix H_full = JReal.transpose() * H * JReal; + gtsam::Vector b_full = JReal.transpose() * b; + return std::make_pair(H_full, b_full); +} \ No newline at end of file diff --git a/src/GTSAMIntegration/PoseTransformation.h b/src/GTSAMIntegration/PoseTransformation.h new file mode 100644 index 0000000..62eebf5 --- /dev/null +++ b/src/GTSAMIntegration/PoseTransformation.h @@ -0,0 +1,292 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_POSETRANSFORMATION_H +#define DMVIO_POSETRANSFORMATION_H + + +#include +#include +#include +#include +#include +#include + +namespace dmvio +{ + +// This file contains methods to convert poses betwen coordinate systems, and also compute relative derivatives (Jacobians) +// to convert Hessians and GTSAM factors between the systems. +// Usage: Define a child class of PoseTransformation which can transform poses and also compute relative derivatives. +// Then either convert a Hessian (H) and gradient vector (b) using convertHAndBWithPoseTransformation, or simply +// use a PoseTransformationFactor. + +// Defines on which side of of the pose the epsilon is for the input (first) and output (second) transformation. +// When computing derivatives w.r.t. poses using Lie Groups, the increment can either be placed on the left or the right side. +// DSO places the increment on the left side. i.e. newPose = exp(increment) * oldPose. +// GTSAM places the increment on the right side, i.e. newPose = oldPose * exp(increment). +// Both approaches are equally valid, but the Jacobians need to be computed consistently with the increment side. +// With the relative Jacobians computed by our PoseTransformations we can convert between the two. +// Note: that for computing relative Jacobians they work on the inverse direction as the PoseTransformation, e.g. +// to convert Jacobians from DSO style to GTSAM style, one has to pass RIGHT_TO_LEFT. +// When working only with GTSAM style increments one can simply pass RIGHT_TO_RIGHT. +enum class DerivativeDirection +{ + LEFT_TO_LEFT, + LEFT_TO_RIGHT, + RIGHT_TO_LEFT, + RIGHT_TO_RIGHT +}; + +// Abstract base class for transformations from one coordinate system to another. +// Main functionality is to transformm a pose to a different coordinate system. +// Also contains methods to compute relative derivatives. These will transform Jacobians according to the transformation. +// An example implementation is TransformDSOToIMU (in PoseTransformationIMU.h). +// -------------------------------------------------- +// By default only derivatives w.r.t. to the pose are computed (by the method getPoseDerivative). +// However some transformations have additional variables which are optimized (e.g. the scale, or the transform T_cam_imu), +// which can influence the transformed pose. +// These additional variables can be queried with getAllOptimizedSymbols, and derivatives w.r.t. these variables can +// be computed with getAllDerivatives. +// When optimizing these additional variables (e.g. in a factor graph), they can be updated with updateẂithValues. +// -------------------------------------------------- +// This class also contains a default implementation for getPoseDerivative using numeric Jacobians, which should work out of the box, +// in case the child class does not override the method with an analytic Jacobian computation. +// Child classes which optimize additional symbols need to always reimplement getAllDerivatives themself. +class PoseTransformation +{ +public: + typedef dso::Mat44 PoseType; + + virtual ~PoseTransformation() = default; + + // Transform pose with the transformation. + virtual PoseType transformPose(const PoseType& pose) const = 0; + // Perform the inverse transformation. + virtual PoseType transformPoseInverse(const PoseType& pose) const = 0; + + // Perform precomputation which might be necessary for the derivative computation. Should be called once before + // calling getPoseDerivative for a batch of variables. + virtual void precomputeForDerivatives() + {} + + // Compute the derivative w.r.t the pose. + // This can be used to transform Jacobians from the target coordinate system to the source coordinate system. + virtual gtsam::Matrix66 getPoseDerivative(const PoseType& pose, DerivativeDirection direction); + // Compute the derivatives for all variables which are optimized, first the pose and then all optimized symbols (e.g. scale, T_cam_imu, etc.). + virtual std::vector getAllDerivatives(const PoseType& pose, DerivativeDirection direction); + // Returns the symbols of the additional variables (except the pose) which are optimized. + virtual std::vector getAllOptimizedSymbols() const; + + // Updates all optimized symbols using the value in values (if available). + virtual void updateWithValues(const gtsam::Values& values) + {} + + int getOptimizedDim(gtsam::Key key) const + { return keyDimMap.at(key); } // get dimension of optimized symbol + + virtual std::unique_ptr clone() const = 0; + +protected: + std::map keyDimMap; // only for optimized symbols, often times empty. +// virtual std::unique_ptr clone() = 0; +}; + +// Identity transformation. Used get the Jacobians to transform from left sided increment (DSO) to right sided increment (GTSAM), without changing the poses. +class TransformIdentity : public PoseTransformation +{ +public: + virtual PoseType transformPose(const PoseType& pose) const override + { + return pose; + } + + virtual PoseType transformPoseInverse(const PoseType& pose) const override + { + return pose; + } + + std::unique_ptr clone() const override + { + return std::unique_ptr(new TransformIdentity(*this)); + } + + virtual gtsam::Matrix66 getPoseDerivative(const PoseType& pose, DerivativeDirection direction) override; +}; + +// Create the inverse of a PoseTransformation. +template class InversePoseTransform : public PoseTransformation +{ +public: + explicit InversePoseTransform(T& originalTransform) + : transform(originalTransform) + {} + + PoseType transformPose(const PoseType& pose) const override + { + return transform.transformPoseInverse(pose); + } + + PoseType transformPoseInverse(const PoseType& pose) const override + { + return transform.transformPose(pose); + } + + std::unique_ptr clone() const override + { + return std::unique_ptr(new InversePoseTransform(*this)); + } + +private: + T& transform; +}; + +// Convert Hessian (and gradient vector b) from DSO Bundle Adjustment to GTSAM. Also uses the poseTransformation (typically just identity) to transform the poses. +// ordering and keyDimMap are used to identify which column / row of the Hessian corresponds to which variable. +std::pair convertHAndBFromDSO(const dso::MatXX& H, const dso::VecX& b, + PoseTransformation& poseTransformation, + double weightDSOToGTSAM, + const gtsam::Ordering& ordering, + const gtsam::Values& values, + const std::map& keyDimMap); + +// Convert H and b between 2 GTSAM factor graphs. +// Note that the transformation has to be defined "the other way round" compared to the Hessians: +// To convert a Hessian that is defined in the IMU space to the DSO space you have to provide the TransformDSOToIMU. +// The reason is that the relative Jacobians have to be defined this way. +std::pair +convertHAndBWithPoseTransformation(const std::pair& input, + const gtsam::Ordering& ordering, const std::map& keyDimMap, + const gtsam::Values& values, PoseTransformation& poseTransformation, + DerivativeDirection derivativeDirection); + +// Method to build the relative Jacobian used for converting H,b in convertHAndBWithPoseTransformation and +// for the PoseTransformationFactor. +gtsam::Matrix buildRelativeJacobian(int n, const std::vector& ordering, const std::vector& dimensions, + const gtsam::Values& values, PoseTransformation& poseTransformation, + const DerivativeDirection& derivativeDirection, int numOpt, + const std::vector& optPositions); + +// Converts the DSO coarse tracking Hessian to GTSAM, transforming it with the PoseTransformation. +// The passed transformIMUToCoarse needs to provide derivatives for frameToWorld and for referenceToWorld (see TransformIMUToDSOForCoarse as an example). +std::pair +convertCoarseHToGTSAM(PoseTransformation& transformIMUToCoarse, const dso::Mat88& HInput, const dso::Vec8& bInput, + const gtsam::Pose3& currentPose); + +// Helper function to convert Jacobians from DSO style to GTSAM: This switches rotation and translation because +// Sophus (used by DSO) and GTSAM have different conventions here. +Eigen::MatrixXd convertJacobianToGTSAM(const Eigen::MatrixXd& jacobian); + +// Convert all poses in keysToInclude with the transformation. +void convertAllPosesWithTransform(const gtsam::Values& values, const PoseTransformation& transformation, + const gtsam::KeyVector& keysToInclude, gtsam::Values& insertInto); + +inline gtsam::Values convertAllPosesWithTransform(const gtsam::Values& values, const PoseTransformation& transformation, + const gtsam::KeyVector& keysToInclude) +{ + gtsam::Values returning; + convertAllPosesWithTransform(values, transformation, keysToInclude, returning); + return returning; +} + +inline gtsam::Values +convertAllPosesWithTransform(gtsam::Values::shared_ptr values, const PoseTransformation& transformation) +{ + return convertAllPosesWithTransform(*values, transformation, values->keys()); +} + +// Returns true if all symbols optimized by the poseTransformation are contained inside values. +inline bool allOptimizedSymbolsInside(const gtsam::Values& values, const PoseTransformation& poseTransformation) +{ + for(auto&& key : poseTransformation.getAllOptimizedSymbols()) + { + if(!values.exists(key)) return false; + } + return true; +} + + +// Assert that a numeric Jacobian is similar to analytic Jacobian. +template inline void assertNumericJac(const gtsam::Matrix& numericJac, const T& analyticJac) +{ + if(!(analyticJac - numericJac).isZero(0.0001)) + { + std::cout << "AnalyticJ:\n" << analyticJac << "\nNumJ: \n" << numericJac << std::endl; + std::cout << "Diff:\n" << (analyticJac - numericJac) << std::endl; + assert(0); + } +} + +// Compute the numeric Jacobian w.r.t the given variable, which can either be the argument (pose) itself or one of the members of transformation. +// Note that this method modifies the passed variableToChange (which can be an internal member of the PoseTransformation in some cases), +// so this method is typically very far from thread-safe! +template gtsam::Matrix +computeNumericJacobian(PoseTransformation& transformation, const Sophus::SE3d& pose, T* variableToChange, + DerivativeDirection direction) +{ +#ifndef DEBUG + std::cout << "Using Numeric Jacobian!" << std::endl; +#endif + double epsilon = 0.00001; + + gtsam::Matrix fullDerivative = gtsam::Matrix::Zero(6, T::DoF); + + // Numeric Jacobians work by slightly changing the pose in each direction, and then computing how much it effects the + // converted pose. + dso::Mat44 transformedPose = transformation.transformPose(pose.matrix()); + Sophus::SE3d transformedPoseInv = Sophus::SE3d(transformedPose).inverse(); + for(int i = 0; i < T::DoF; ++i) + { + gtsam::Vector incVec = gtsam::Vector::Zero(T::DoF); + incVec(i) = epsilon; + + T variableBackup = *variableToChange; + if(direction == dmvio::DerivativeDirection::RIGHT_TO_RIGHT || + direction == dmvio::DerivativeDirection::RIGHT_TO_LEFT) + { + *variableToChange = *variableToChange * T::exp(incVec); + }else + { + *variableToChange = T::exp(incVec) * *variableToChange; + } + + Sophus::SE3d transformedPoseNew(transformation.transformPose(pose.matrix())); + *variableToChange = variableBackup; + Sophus::SE3d relPose; + if(direction == dmvio::DerivativeDirection::LEFT_TO_LEFT || + direction == dmvio::DerivativeDirection::RIGHT_TO_LEFT) + { + relPose = transformedPoseNew * transformedPoseInv; + }else + { + relPose = transformedPoseInv * transformedPoseNew; + } + + dso::Vec6 derivative = Sophus::SE3d::log(relPose); + fullDerivative.col(i) = derivative / epsilon; + } + + return convertJacobianToGTSAM(fullDerivative); +} +} + +#endif //DMVIO_POSETRANSFORMATION_H diff --git a/src/GTSAMIntegration/PoseTransformationFactor.cpp b/src/GTSAMIntegration/PoseTransformationFactor.cpp new file mode 100644 index 0000000..dad656b --- /dev/null +++ b/src/GTSAMIntegration/PoseTransformationFactor.cpp @@ -0,0 +1,264 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include "PoseTransformationFactor.h" +#include "Marginalization.h" +#include +#include +#include "GTSAMUtils.h" +#include "util/TimeMeasurement.h" +#include "ExtUtils.h" + +dmvio::PoseTransformationFactor::PoseTransformationFactor(const gtsam::NonlinearFactor::shared_ptr& factor, + const PoseTransformation& poseTransformationPassed, + ConversionType conversionType, + const gtsam::Values& fixedValuesPassed) + : NonlinearFactor(factor->keys()), factor(factor), conversionType(conversionType) +{ + fixedValues.insert(fixedValuesPassed); + if(!fixedValues.empty()) + { + keys_.clear(); + fixedKeys = fixedValues.keys(); + fixedKeySet = std::set(fixedKeys.begin(), fixedKeys.end()); + + // Only insert non-fixed child keys! + for(auto&& childKey : factor->keys()) + { + if(fixedKeySet.find(childKey) == fixedKeySet.end()) + { + keys_.push_back(childKey); + } + } + } + + // Clone the pose transformation so we can update it with new values. + poseTransformation = poseTransformationPassed.clone(); + + auto&& optimizedSymbols = poseTransformation->getAllOptimizedSymbols(); + keys_.insert(keys_.end(), optimizedSymbols.begin(), optimizedSymbols.end()); + + // assumes that the factor does not optimize any of the symbols optimized by poseTransformation! + for(auto&& key : optimizedSymbols) + { + additionalDim += poseTransformation->getOptimizedDim(key); + } +} + +dmvio::PoseTransformationFactor::PoseTransformationFactor(const dmvio::PoseTransformationFactor& o) + : gtsam::NonlinearFactor(o), factor(o.factor->clone()), poseTransformation(o.poseTransformation->clone()), + conversionType(o.conversionType), + additionalDim(o.additionalDim), fixedValues(o.fixedValues), fixedKeys(o.fixedKeys), fixedKeySet(o.fixedKeySet) +{} + +dmvio::PoseTransformationFactor::~PoseTransformationFactor() += default; + +gtsam::Values dmvio::PoseTransformationFactor::convertValues(const gtsam::Values& c) const +{ + // Update the poseTransformation. + poseTransformation->updateWithValues(c); + + // And convert all values. + gtsam::Values ret; + convertAllPosesWithTransform(c, *poseTransformation, keys_, ret); + convertAllPosesWithTransform(fixedValues, *poseTransformation, fixedKeys, ret); + return ret; +} + +double dmvio::PoseTransformationFactor::error(const gtsam::Values& c) const +{ + return factor->error(convertValues(c)); +} + +size_t dmvio::PoseTransformationFactor::dim() const +{ + return factor->dim() + additionalDim; +} + +void dmvio::PoseTransformationFactor::setFEJValues(std::shared_ptr fejPassed) +{ + // Called if we are supposed to use FEJ values for derivative computatio. + + // Important: Also forward to child factor if necessary! + auto* casted = dynamic_cast(factor.get()); + if(casted) + { + if(fejPassed == nullptr) + { + childFej.reset(); + }else + { + // We have to forward the transformed fej values though! + // The transformation might change during the optimization, so we do it during linearize. + childFej = std::make_shared(); + } + casted->setFEJValues(childFej); + } + + fej = std::move(fejPassed); +} + +boost::shared_ptr dmvio::PoseTransformationFactor::linearize(const gtsam::Values& c) const +{ + // First convert FEJValues for child factor. + if(childFej) + { + // We need optimized symbols + the child factor non-fixed keys. + gtsam::Values fejVals = fej->buildValues(keys_, c); + childFej->fejValues = convertValues(fejVals); + } + + // Linearize child factor. + gtsam::GaussianFactor::shared_ptr gaussian = factor->linearize(convertValues(c)); + // Note that convertValues already updates the poseTransformation. + + auto&& optimizedSymbols = poseTransformation->getAllOptimizedSymbols(); + + gtsam::JacobianFactor* jacobianFac = nullptr; + if(conversionType == JACOBIAN_FACTOR) + { + jacobianFac = dynamic_cast(gaussian.get()); + if(jacobianFac == nullptr) + { + std::cout << "WARNING: Using ConversionType JACOBIAN_FACTOR, but a different GaussianFactor was passed!" + << std::endl; + } + } + // We have to multiple the Jacobian (A) with our relative Jacobian. + std::pair Ab = std::pair(); + if(jacobianFac == nullptr) + { + Ab = gaussian->jacobian(); + }else + { + Ab = jacobianFac->jacobianUnweighted(); + } + + // Use FEJ values for derivatives if passed. + if(fej) + { + gtsam::Values fejVals = fej->buildValues(optimizedSymbols, c); + poseTransformation->updateWithValues(fejVals); + } + + poseTransformation->precomputeForDerivatives(); + + // Computation of terms: + // We only iterate the child keys, but including fixed keys. + std::vector > terms(keys_.size()); + int pos = 0; + int i = 0; + int rows = Ab.first.rows(); + bool firstPose = true; + int firstOptPos = keys_.size() - optimizedSymbols.size(); // size of non-fixed child keys + for(auto it = gaussian->keys().begin(); it != gaussian->keys().end(); ++it) + { + int dim = gaussian->getDim(it); + gtsam::Key key = *it; + bool fixed = fixedKeySet.find(key) != fixedKeySet.end(); + + if(gtsam::Symbol(key).chr() == 'p') // We only need to convert poses. + { + gtsam::Pose3 pose; + if(fixed) + { + pose = fixedValues.at(key); + }else if(fej && fej->fejValues.exists(key)) + { + pose = fej->fejValues.at(key); + }else + { + pose = c.at(key); + } + std::vector derivatives = poseTransformation->getAllDerivatives(pose.matrix(), + DerivativeDirection::RIGHT_TO_RIGHT); + + if(!fixed) + { + terms[i].first = key; + terms[i].second = + Ab.first.block(0, pos, rows, dim) * derivatives[0]; // multiply with relative pose Jacobian. + i++; + } + + int j = 0; + for(auto&& optKey : optimizedSymbols) + { + auto&& deriv = derivatives[j + 1]; + int optPos = j + firstOptPos; + + if(firstPose) + { + terms[optPos].first = optKey; + terms[optPos].second = Ab.first.block(0, pos, rows, dim) * deriv; + }else + { + terms[optPos].second += Ab.first.block(0, pos, rows, dim) * deriv; + } + + j++; + } + firstPose = false; + }else if(!fixed) + { + terms[i].first = key; + terms[i].second = Ab.first.block(0, pos, rows, dim); + i++; + } + + pos += dim; + } + + if(jacobianFac == nullptr) + { + return boost::shared_ptr( + new gtsam::JacobianFactor(terms, Ab.second)); + }else + { + return boost::shared_ptr( + new gtsam::JacobianFactor(terms, Ab.second, jacobianFac->get_model())); + } + +} + +std::ostream& dmvio::operator<<(std::ostream& os, dmvio::PoseTransformationFactor::ConversionType& conversion) +{ + os << static_cast::type>(conversion); + return os; +} + +std::istream& dmvio::operator>>(std::istream& is, dmvio::PoseTransformationFactor::ConversionType& conversion) +{ + int num; + is >> num; + conversion = static_cast(num); + return is; +} + +template<> +void dmvio::defaultYAMLHandler(void* pointer, const YAML::Node& node) +{ + auto* typedPointer = static_cast(pointer); + *typedPointer = static_cast(node.as()); +} \ No newline at end of file diff --git a/src/GTSAMIntegration/PoseTransformationFactor.h b/src/GTSAMIntegration/PoseTransformationFactor.h new file mode 100644 index 0000000..ea6fd10 --- /dev/null +++ b/src/GTSAMIntegration/PoseTransformationFactor.h @@ -0,0 +1,115 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_POSETRANSFORMATIONFACTOR_H +#define DMVIO_POSETRANSFORMATIONFACTOR_H + +#include "PoseTransformation.h" +#include +#include "yaml-cpp/yaml.h" +#include "util/SettingsUtil.h" +#include "FEJValues.h" + +namespace dmvio +{ + +// Transform another factor using the PoseTransformation +// Note that the transformation works in the inverse direction: +// To pass a factor which works in coordinate system IMU, to a graph working in coordinate system DSO, +// we have to pass wrap the factor in TransformDSOToIMU. +// The class can also fix variables of the child factor. +// The factor will also apply First-Estimates Jacobians if setFEJMapForGraph has been called for the graph before optimization. +// Note that the child factor must not optimize any of the additional symbols optimized by the PoseTransformation. +// Assumes that all (but only) poses use the symbol 'p' +class PoseTransformationFactor : public gtsam::NonlinearFactor, public FactorHandlingFEJ +{ +public: + // Defines how the factor is converted (use JACOBIAN_FACTOR for JacobianFactor, and JACOBIAN_BAKED_IN for other factor types) + // It might make a small difference for performance but is not too relevant. + enum ConversionType + { + JACOBIAN_FACTOR, JACOBIAN_BAKED_IN + }; + + // factor is the child factor which is transformed. + // poseTransformation is the transformation which will be applied to values to to convert from the graph to the child factor. + // conversionType defines how the linear system is converted (mainly relevant for performance). + // All symbols in fixedValues will be fixed for the child factor during optimization. + PoseTransformationFactor(const gtsam::NonlinearFactor::shared_ptr& factor, + const PoseTransformation& poseTransformation, + ConversionType conversionType, + const gtsam::Values& fixedValues = gtsam::Values{}); + + PoseTransformationFactor(const PoseTransformationFactor& o); + + virtual ~PoseTransformationFactor(); + + // -------------------- + // Methods overriden from NonlinearFactor + double error(const gtsam::Values& c) const override; + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const override; + + /** linearize to a GaussianFactor */ + boost::shared_ptr linearize(const gtsam::Values& c) const override; + + virtual gtsam::NonlinearFactor::shared_ptr clone() const + { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PoseTransformationFactor(*this))); + } + + void setFEJValues(std::shared_ptr fej) override; + + gtsam::Values fixedValues; +private: + // convert the values using the PoseTransformation + // The resulting values will only contain values for the symbols optimized by the child factor, or the TransformationFactor. + gtsam::Values convertValues(const gtsam::Values& c) const; + + + gtsam::NonlinearFactor::shared_ptr factor; // child factor. + + mutable std::unique_ptr poseTransformation; + + ConversionType conversionType; + + int additionalDim = 0; // dimension of symbols optimized by the poseTransformation. + + gtsam::FastVector fixedKeys; + std::set fixedKeySet; + + std::shared_ptr fej; + std::shared_ptr childFej; +}; +std::ostream& operator<<(std::ostream& os, dmvio::PoseTransformationFactor::ConversionType& conversion); +std::istream& operator>>(std::istream& is, dmvio::PoseTransformationFactor::ConversionType& conversion); +} + +namespace dmvio +{ +template<> +void defaultYAMLHandler(void* pointer, const YAML::Node& node); +} + +#endif //DMVIO_POSETRANSFORMATIONFACTOR_H diff --git a/src/GTSAMIntegration/PoseTransformationIMU.cpp b/src/GTSAMIntegration/PoseTransformationIMU.cpp new file mode 100644 index 0000000..25e941e --- /dev/null +++ b/src/GTSAMIntegration/PoseTransformationIMU.cpp @@ -0,0 +1,451 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "PoseTransformationIMU.h" +#include +#include "dso/util/FrameShell.h" +#include "ExtUtils.h" +#include "Sim3GTSAM.h" + +using namespace dmvio; +using dso::Vec8; +using gtsam::Matrix, gtsam::Vector; +using gtsam::Symbol; + +TransformDSOToIMU::TransformDSOToIMU(const gtsam::Pose3& T_cam_imu, std::shared_ptr optScale, + std::shared_ptr optGravity, std::shared_ptr optT_cam_imu, + bool fixZ, + int symbolInd) + : T_cam_imu(T_cam_imu.matrix()), optScale(optScale), optGravity(optGravity), optT_cam_imu(optT_cam_imu), + symbolInd(symbolInd), + fixZ(fixZ) +{ + fillKeyDimMap(); +} + +TransformDSOToIMU::TransformDSOToIMU(const TransformDSOToIMU& other, std::shared_ptr optScalePassed, + std::shared_ptr optGravityPassed, + std::shared_ptr optT_cam_imuPassed) + : TransformDSOToIMU(other) // first call default copy constructor +{ + optScale = optScalePassed; + optGravity = optGravityPassed; + optT_cam_imu = optT_cam_imuPassed; +} + +// pose is T_cam_dsoW in DSO scale. +PoseTransformation::PoseType TransformDSOToIMU::transformPose(const PoseTransformation::PoseType& pose) const +{ + Sophus::Sim3d scaledT_w_cam = T_S_DSO * Sophus::Sim3d(pose).inverse() * T_S_DSO.inverse(); // in metric scale. + assert(std::abs(scaledT_w_cam.scale() - 1.0) < 0.0001); + Sophus::SE3d T_metricW_imu; + T_metricW_imu = Sophus::SE3d(R_dsoW_metricW.inverse(), Sophus::Vector3d::Zero()) * + Sophus::SE3d(scaledT_w_cam.matrix()) * T_cam_imu; + PoseType returning = T_metricW_imu.matrix(); + +#ifdef DEBUG + // Check that inverse is the actual inverse. + assertEqEigen(pose, transformPoseInverse(returning), 1e-4); +#endif + + return returning; +} + +// pose is T_metricW_imu in metric scale. +PoseTransformation::PoseType TransformDSOToIMU::transformPoseInverse(const PoseTransformation::PoseType& pose) const +{ + // dso world to cam in metric scale: + Sophus::SE3d T_cam_dsoW_metric = Sophus::SE3d(); + T_cam_dsoW_metric = T_cam_imu * Sophus::SE3d(pose).inverse() * + Sophus::SE3d(R_dsoW_metricW.inverse(), Sophus::Vector3d::Zero()); + // in DSO scale: + Sophus::Sim3d T_cam_dsoW = T_S_DSO.inverse() * Sophus::Sim3d(T_cam_dsoW_metric.matrix()) * T_S_DSO; + if(!(std::abs(T_cam_dsoW.scale() - 1.0) < 0.0001)) + { + std::cout << T_cam_dsoW.matrix() << std::endl; + } + assert(std::abs(T_cam_dsoW.scale() - 1.0) < 0.0001 || + std::isnan(T_cam_dsoW.scale())); // Scale should be close to 1 (unless it is Nan which can happen sometimes). + return T_cam_dsoW.matrix(); +} + +void TransformDSOToIMU::precomputeForDerivatives() +{ + if(precomputedValid) return; + precomputedValid = true; + precomputed = Sophus::Sim3d(T_cam_imu.inverse().matrix()) * T_S_DSO; + precomputedAdj = precomputed.Adj(); +} + +gtsam::Matrix66 +TransformDSOToIMU::getPoseDerivative(const PoseTransformation::PoseType& pose, DerivativeDirection direction) +{ + if(direction == DerivativeDirection::RIGHT_TO_RIGHT) + { + // Analytic derivatives + assert(precomputedValid); + Sophus::Sim3d intermediateRes = precomputed * Sophus::Sim3d(pose); + auto firstAdj = intermediateRes.Adj(); + gtsam::Matrix66 poseJ = convertJacobianToGTSAM(-firstAdj).block<6, 6>(0, 0); + return poseJ; + } + return PoseTransformation::getPoseDerivative(pose, direction); +} + +std::vector +TransformDSOToIMU::getAllDerivatives(const PoseTransformation::PoseType& pose, DerivativeDirection direction) +{ + std::vector analyticDerivs; + bool analyticDerivsFilled = false; + if(direction == DerivativeDirection::RIGHT_TO_RIGHT) + { + // Analytic derivatives: + assert(precomputedValid); + analyticDerivsFilled = true; + // Intermediate res is: T_cam_imu^-1 * T_S_DSO * T_cam_world + Sophus::Sim3d intermediateRes = precomputed * Sophus::Sim3d(pose); + auto firstAdj = intermediateRes.Adj(); + gtsam::Matrix66 poseJ = convertJacobianToGTSAM(-firstAdj).block<6, 6>(0, 0); + analyticDerivs.push_back(poseJ); + + if(*optScale) + { + gtsam::Matrix scaleJ = convertJacobianToGTSAM(firstAdj - precomputedAdj); + analyticDerivs.push_back(scaleJ.topRightCorner<6, 1>()); + } + if(*optGravity) + { + Sophus::SE3d innerAdjoint((intermediateRes * T_S_DSO.inverse()).matrix()); + gtsam::Matrix66 gravityJac; + // J = -(T_cam_imu.inverse() * T_S_DSO * pose * T_S_DSO.inverse() * R_dsoW_metricW).Adj(); + gravityJac = convertJacobianToGTSAM( + -(innerAdjoint * Sophus::SE3d(R_dsoW_metricW, Sophus::Vector3d::Zero())).Adj()); + if(fixZ) + { + // Set the yaw derivative to zero here. + gravityJac.block<6, 1>(0, 2).setZero(); + } + analyticDerivs.push_back(gravityJac.topLeftCorner<6, 3>()); + } + if(*optT_cam_imu) + { + // Derivative is one. + analyticDerivs.push_back(gtsam::Matrix66::Identity()); + } + +#ifndef DEBUG + // In debug mode don't return yet, but later after we compared to numeric derivatives! + return analyticDerivs; +#endif + } + + // Compute numeric Jacobians. + std::vector returning; + returning.push_back(PoseTransformation::getPoseDerivative(pose, direction)); + if(*optScale) + { + gtsam::Matrix numJac = computeNumericJacobian(*this, Sophus::SE3d(pose), &T_S_DSO, direction); + // Set translational and rotational part to zero, because we only want to optimize scale! + numJac.topLeftCorner<6, 6>().setZero(); + returning.push_back(numJac.topRightCorner<6, 1>()); + } + if(*optGravity) + { + gtsam::Matrix numJac = gtsam::Matrix(); + numJac = computeNumericJacobian(*this, Sophus::SE3d(pose), &R_dsoW_metricW, direction); + if(fixZ) + { + // Set the yaw derivative to zero here. But I'm not sure which one it is yet! + numJac.block<6, 1>(0, 2).setZero(); + } + returning.push_back(numJac); + } + if(*optT_cam_imu) + { + gtsam::Matrix numJac = computeNumericJacobian(*this, Sophus::SE3d(pose), &T_cam_imu, direction); + returning.push_back(numJac); + } + + if(analyticDerivsFilled) + { + assert(analyticDerivs.size() == returning.size()); + for(int i = 0; i < analyticDerivs.size(); ++i) + { + assertNumericJac(returning[i], analyticDerivs[i]); + } + return analyticDerivs; + } + return returning; + +} + +std::vector TransformDSOToIMU::getAllOptimizedSymbols() const +{ + std::vector returning; + if(*optScale) + { + returning.push_back(gtsam::Symbol('s', symbolInd)); + } + if(*optGravity) + { + returning.push_back(gtsam::Symbol('g', symbolInd)); + } + if(*optT_cam_imu) + { + // i for IMU intrinsics. + returning.push_back(gtsam::Symbol('i', symbolInd)); + } + return returning; +} + +void TransformDSOToIMU::updateWithValues(const gtsam::Values& values) +{ + if(*optScale) + { + double scaleBefore = T_S_DSO.scale(); + T_S_DSO = values.at(Symbol('s', symbolInd)).sim(); + if(fabs(scaleBefore - T_S_DSO.scale()) >= 1e-9) + { + precomputedValid = false; + } + assert(T_S_DSO.rotationMatrix().isIdentity(0.000001)); + assert(T_S_DSO.translation().isZero(0.000001)); + } + if(*optGravity) + { + gtsam::Rot3 rot = values.at(Symbol('g', symbolInd)); + if(!rot.equals(gtsam::Rot3(R_dsoW_metricW.matrix()))) + { + precomputedValid = false; + } + R_dsoW_metricW = Sophus::SO3d(rot.matrix()); + } + if(*optT_cam_imu) + { + gtsam::Pose3 newExtr = values.at(Symbol('i', symbolInd)); + if(!newExtr.equals(gtsam::Pose3(T_cam_imu.matrix()))) precomputedValid = false; + T_cam_imu = Sophus::SE3d(newExtr.matrix()); + } +} + +void TransformDSOToIMU::setScale(double variable) +{ + precomputedValid = false; + T_S_DSO.setScale(variable); +} + +double TransformDSOToIMU::getScale() const +{ + return T_S_DSO.scale(); +} + +std::unique_ptr TransformDSOToIMU::clone() const +{ + return std::unique_ptr(new TransformDSOToIMU(*this)); +} + +void TransformDSOToIMU::resetGravityDirection() +{ + R_dsoW_metricW = Sophus::SO3d{}; +} + +template +TransformIMUToDSOForCoarse::TransformIMUToDSOForCoarse(std::shared_ptr transformDSOToIMU, int keyframeId) + : transformToIMU(transformDSOToIMU), keyframeId(keyframeId) +{ + keyDimMap[Symbol('p', keyframeId)] = 6; +} + +template +dmvio::PoseTransformation::PoseType TransformIMUToDSOForCoarse::transformPose(const PoseType& pose) const +{ + // First convert from IMU to camera frame. + // pose is imuToWorld, and we also have reference to world (both IMU metric frame). + PoseType T_f_w = transformToIMU->transformPoseInverse(pose); + PoseType T_r_w = transformToIMU->transformPoseInverse(referenceToWorld.matrix()); + + // We want to output T_f_r (reference to frame). + PoseType T_f_r = T_f_w * T_r_w.inverse(); + +#ifdef DEBUG + // Check that inverse is the actual inverse. + if(!T_f_r.hasNaN()) + { + assertEqEigen(pose, transformPoseInverse(T_f_r)); + } +#endif + + return T_f_r; +} + +template +dmvio::PoseTransformation::PoseType TransformIMUToDSOForCoarse::transformPoseInverse(const PoseType& poseT_f_r) const +{ + // First get worldToReference + PoseType T_r_w = transformToIMU->transformPoseInverse(referenceToWorld.matrix()); + // We got T_f_r as input -> + PoseType T_f_w = poseT_f_r * T_r_w; + + return transformToIMU->transformPose(T_f_w); +} + +// Helper functions for the derivatives, so that we can specialize them to compute analytic derivatives for each class type. +template +gtsam::Matrix66 dmvio::getCoarsePoseDerivative(const PoseTransformation::PoseType& pose, + const DerivativeDirection& direction, T& transform, + TransformIMUToDSOForCoarse& transformForCoarse) +{ + // Default is numeric Jacobian. + return transformForCoarse.PoseTransformation::getPoseDerivative(pose, direction); +} + +// Analytic derivatives for TransformIMUToDSOForCoarse +template<> gtsam::Matrix66 dmvio::getCoarsePoseDerivative(const PoseTransformation::PoseType& pose, + const DerivativeDirection& direction, + TransformDSOToIMU& transform, + TransformIMUToDSOForCoarse& transformForCoarse) +{ + gtsam::Matrix poseJac = convertJacobianToGTSAM( + -(transform.T_S_DSO.inverse() * Sophus::Sim3d(transform.T_cam_imu.matrix())).Adj()).topLeftCorner( + 6, 6); + return poseJac; +} + +template gtsam::Matrix dmvio::getCoarseReferenceDerivative(const PoseTransformation::PoseType& pose, + DerivativeDirection direction, T& transform, + TransformIMUToDSOForCoarse& transformForCoarse) +{ + // Default to numeric Jacobian. + gtsam::Matrix numJac = computeNumericJacobian(transformForCoarse, Sophus::SE3d(pose), + &transformForCoarse.referenceToWorld, direction); + return numJac; +} + +// Analytic derivatives for TransformIMUToDSOForCoarse +template<> gtsam::Matrix dmvio::getCoarseReferenceDerivative(const PoseTransformation::PoseType& pose, + DerivativeDirection direction, + TransformDSOToIMU& transform, + TransformIMUToDSOForCoarse& transformForCoarse) +{ + // compute derivative w.r.t reference to world. + Sophus::Sim3d T_w_f_imu(pose); + Sophus::Sim3d T_w_r_imu(transformForCoarse.referenceToWorld.matrix()); + gtsam::Matrix referenceJac = convertJacobianToGTSAM( + (transform.T_S_DSO.inverse() * Sophus::Sim3d(transform.T_cam_imu.matrix()) * + T_w_f_imu.inverse() * + T_w_r_imu).Adj()).topLeftCorner(6, 6); + return referenceJac; +} + +const Sophus::SE3d& TransformDSOToIMU::getT_cam_imu() const +{ + return T_cam_imu; +} + +void TransformDSOToIMU::setSymbolInd(int symbolInd) +{ + TransformDSOToIMU::symbolInd = symbolInd; + keyDimMap.clear(); + fillKeyDimMap(); +} + +int TransformDSOToIMU::getSymbolInd() const +{ + return symbolInd; +} + +void TransformDSOToIMU::fillKeyDimMap() +{ + keyDimMap[Symbol('s', symbolInd)] = 1; + keyDimMap[Symbol('g', symbolInd)] = 3; + keyDimMap[Symbol('i', symbolInd)] = 6; +} + +const Sophus::SO3d& TransformDSOToIMU::getR_dsoW_metricW() const +{ + return R_dsoW_metricW; +} + +template +gtsam::Matrix66 TransformIMUToDSOForCoarse::getPoseDerivative(const PoseType& pose, DerivativeDirection direction) +{ + if(direction == DerivativeDirection::RIGHT_TO_LEFT) + { + gtsam::Matrix poseJac = getCoarsePoseDerivative(pose, direction, *transformToIMU, *this); +#ifdef DEBUG + assertNumericJac(PoseTransformation::getPoseDerivative(pose, direction), poseJac); +#endif + return poseJac; + } + return PoseTransformation::getPoseDerivative(pose, direction); +} + +// Computes the derivative w.r.t T_w_f and also T_w_r. +template std::vector +TransformIMUToDSOForCoarse::getAllDerivatives(const PoseType& pose, DerivativeDirection direction) +{ + std::vector returning; + returning.push_back(getPoseDerivative(pose, direction)); + + if(direction == DerivativeDirection::RIGHT_TO_LEFT) + { + // compute derivative w.r.t reference to world. + Sophus::Sim3d T_w_f_imu(pose); + Sophus::Sim3d T_w_r_imu(referenceToWorld.matrix()); + gtsam::Matrix referenceJac = getCoarseReferenceDerivative(pose, direction, *transformToIMU, *this); +#ifdef DEBUG + gtsam::Matrix numJac = computeNumericJacobian(*this, Sophus::SE3d(pose), &referenceToWorld, direction); + assertNumericJac(numJac, referenceJac); +#endif + returning.push_back(referenceJac); + }else + { + gtsam::Matrix numJac = computeNumericJacobian(*this, Sophus::SE3d(pose), &referenceToWorld, direction); + returning.push_back(numJac); + } + return returning; +} + +template std::vector TransformIMUToDSOForCoarse::getAllOptimizedSymbols() const +{ + std::vector returning; + // We also optimize the reference pose. + returning.push_back(gtsam::Symbol('p', keyframeId)); + return returning; +} + +template +void TransformIMUToDSOForCoarse::updateWithValues(const gtsam::Values& values) +{ + referenceToWorld = Sophus::SE3d(values.at(Symbol('p', keyframeId)).matrix()); +} + +template +std::unique_ptr TransformIMUToDSOForCoarse::clone() const +{ + return std::unique_ptr(new TransformIMUToDSOForCoarse(*this)); +} + +namespace dmvio +{ +template class TransformIMUToDSOForCoarse; +} diff --git a/src/GTSAMIntegration/PoseTransformationIMU.h b/src/GTSAMIntegration/PoseTransformationIMU.h new file mode 100644 index 0000000..276d143 --- /dev/null +++ b/src/GTSAMIntegration/PoseTransformationIMU.h @@ -0,0 +1,175 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_POSETRANSFORMATIONIMU_H +#define DMVIO_POSETRANSFORMATIONIMU_H + +#include +#include "PoseTransformation.h" + +namespace dmvio +{ + +template class TransformIMUToDSOForCoarse; + +// Helper methods which compute the analytic derivatives for TransformIMUToDSOForCoarse +template +gtsam::Matrix66 getCoarsePoseDerivative(const PoseTransformation::PoseType& pose, + const DerivativeDirection& direction, T& transform, + TransformIMUToDSOForCoarse& transformForCoarse); +template gtsam::Matrix getCoarseReferenceDerivative(const PoseTransformation::PoseType& pose, + DerivativeDirection direction, T& transform, + TransformIMUToDSOForCoarse& transformForCoarse); + +// PoseTransformation which transforms from DSO frame (world to cam in DSO scale) to metric IMU frame (imu to world in metric scale). +// This also needs to be used to convert Hessians and factors the other way round (from metric frame to DSO frame). +class TransformDSOToIMU : public PoseTransformation +{ +public: + // optScale, optGravity, and optT_Cam_imu define which additional variables shall be optimized. + // We pass them as a pointers, so that they can be set to false for all instances of this class simultaneously when optimization of them shall be stopped. + // if fixZ is true, the z-axis of gravity direction is fixed (which makes sense because yaw is not observable with an IMU). + // symbol ind is the GTSAM symbol index of the additionally optimized variables (typically 0). + TransformDSOToIMU(const gtsam::Pose3& T_cam_imu, std::shared_ptr optScale, + std::shared_ptr optGravity, std::shared_ptr optT_cam_imu, bool fixZ, + int symbolInd = 0); + + // Copy constructor which sets different pointers for bools which determine what is optimized. + TransformDSOToIMU(const TransformDSOToIMU& other, std::shared_ptr optScale, std::shared_ptr optGravity, + std::shared_ptr optT_cam_imu); + + + // Override from PoseTransformation + // ------------------------------------------------------------ + + // We transform from worldToCam to imuToWorld; + PoseType transformPose(const PoseType& pose) const override; + PoseType transformPoseInverse(const PoseType& pose) const override; + + void precomputeForDerivatives() override; + + // Compute the derivative w.r.t the pose. + gtsam::Matrix66 getPoseDerivative(const PoseType& pose, DerivativeDirection direction) override; + // Compute the derivatives for all variables which are optimized, first the pose and then all optimized symbols (e.g. scale, T_cam_imu, etc.). + std::vector getAllDerivatives(const PoseType& pose, DerivativeDirection direction) override; + // Returns the symbols of the additional variables (except the pose) which are optimized. + std::vector getAllOptimizedSymbols() const override; + // Updated all optimized symbols using the value in values (if available). + void updateWithValues(const gtsam::Values& values) override; + + // Setters and getters. + // -------------------------------------------------- + void setScale(double variable); + double getScale() const; + + const Sophus::SE3d& getT_cam_imu() const; + void resetGravityDirection(); + + std::unique_ptr clone() const override; + + bool optimizeScale() const + { return *optScale; } + + bool optimizeGravity() const + { return *optGravity; } + + bool optimizeExtrinsics() const + { return *optT_cam_imu; } + + int getSymbolInd() const; + void setSymbolInd(int symbolInd); + + const Sophus::SO3d& getR_dsoW_metricW() const; + +private: + std::shared_ptr optScale, optGravity, optT_cam_imu; + + void fillKeyDimMap(); + + Sophus::Sim3d T_S_DSO; // Scale: Sim(3) transformation from DSO scale to metric scale. + + Sophus::SO3d R_dsoW_metricW; // The dso world (dsoW) is a rotated version of the metric world (metricW). + // Note that we represent rotation this way (not with the inverse) because we have a right-sided increment, + // and this way we can fix the z axis of this rotation. + + Sophus::SE3d T_cam_imu; + + bool precomputedValid{false}; + Sophus::Sim3d precomputed; + gtsam::Matrix77 precomputedAdj; + + int symbolInd = 0; + + bool fixZ; + + friend gtsam::Matrix66 dmvio::getCoarsePoseDerivative<>(const PoseTransformation::PoseType& pose, + const DerivativeDirection& direction, + TransformDSOToIMU& transform, + TransformIMUToDSOForCoarse& transformForCoarse); + + friend gtsam::Matrix dmvio::getCoarseReferenceDerivative<>(const PoseTransformation::PoseType& pose, + DerivativeDirection direction, + TransformDSOToIMU& transform, + TransformIMUToDSOForCoarse& transformForCoarse); +}; + +// DSO Coarse tracking does not optimize global poses (camToWorld), but the relative pose T_f_r (reference to frame). +// To convert between these and the global poses (frame to world and reference to world) we use this transform. +// Converts poses from T_w_f (frame to world) (while also using reference to world T_w_r) in IMU frame to T_f_r in DSO frame (and therefore to convert Hessians from DSO frame to IMU frame). +// Uses the fields of the original transform transformDSOToIMU for the conversion from camera to metric frame. +template class TransformIMUToDSOForCoarse : public PoseTransformation +{ +public: + // Pass the used transformDSOToIMU and the id of the keyframe (reference frame). + // Note, that a pointer to transformDSOToIMU is kept, so changes to it will also effect this object. + TransformIMUToDSOForCoarse(std::shared_ptr transformDSOToIMU, int keyframeId); + + // pose is the T_w_f in IMU frame, output is T_f_r in DSO frame. For this the additionally optimized symbol T_w_r is used. + PoseType transformPose(const PoseType& pose) const override; + PoseType transformPoseInverse(const PoseType& pose) const override; + + gtsam::Matrix66 getPoseDerivative(const PoseType& pose, DerivativeDirection direction) override; + + // Computes the derivative w.r.t T_w_f and also T_w_r. + std::vector getAllDerivatives(const PoseType& pose, DerivativeDirection direction) override; + // Returns the symbol of the reference frame as this is also optimized in the coarse tracking. + std::vector getAllOptimizedSymbols() const override; + void updateWithValues(const gtsam::Values& values) override; + + std::unique_ptr clone() const override; +private: + std::shared_ptr transformToIMU; + Sophus::SE3d referenceToWorld; + int keyframeId = -1; + + friend gtsam::Matrix66 dmvio::getCoarsePoseDerivative<>(const PoseTransformation::PoseType& pose, + const DerivativeDirection& direction, T& transform, + TransformIMUToDSOForCoarse& transformForCoarse); + + friend gtsam::Matrix dmvio::getCoarseReferenceDerivative<>(const PoseTransformation::PoseType& pose, + DerivativeDirection direction, T& transform, + TransformIMUToDSOForCoarse& transformForCoarse); +}; + +} + +#endif //DMVIO_POSETRANSFORMATIONIMU_H diff --git a/src/GTSAMIntegration/Sim3GTSAM.cpp b/src/GTSAMIntegration/Sim3GTSAM.cpp new file mode 100644 index 0000000..c63f9bf --- /dev/null +++ b/src/GTSAMIntegration/Sim3GTSAM.cpp @@ -0,0 +1,181 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include +#include +#include "Sim3GTSAM.h" + +using namespace gtsam; + +Sim3GTSAM::Sim3GTSAM(double scale) : sim() +{ + sim.setScale(scale); +} + +Sim3GTSAM::Sim3GTSAM(const Sophus::Sim3d& sim) : sim(sim) +{ + +} + +void Sim3GTSAM::print(const std::string& str) const +{ + std::cout << str << "Sim3: " << std::endl << sim.matrix() << std::endl; +} + +size_t Sim3GTSAM::dim() const +{ + return 7; +} + +bool Sim3GTSAM::equals(const Sim3GTSAM& other, double tol) const +{ + Rot3 test(sim.rotationMatrix()); + return (Rot3(sim.rotationMatrix())).equals(Rot3(other.sim.rotationMatrix()), tol) && + (Point3(sim.translation())).equals(Point3(other.sim.translation()), tol) && + (fabs(sim.scale() - other.sim.scale()) < tol); +} + +Sim3GTSAM Sim3GTSAM::retract(const gtsam::Vector& inc) const +{ + // Switch rotation and translation to match the GTSAM convention! + gtsam::Vector myInc = inc; + myInc.segment(0, 3) = inc.segment(3, 3); + myInc.segment(3, 3) = inc.segment(0, 3); + // For larger values the exp would get too large. + if(std::abs(myInc(6)) > 10) + { + myInc(6) *= 10 / std::abs(myInc(6)); + } + Sophus::Sim3d after = sim * Sophus::Sim3d::exp(myInc); + return Sim3GTSAM(after); +} + + +// Basically we have to solve the following for inc: : this->retract(inc)=other +// -> sim * exp(inc) = other -> exp(inc) = sim^{-1} * other -> inc = log(sim^{-1} * other) +gtsam::Vector7 Sim3GTSAM::localCoordinates(const Sim3GTSAM& other) const +{ + // Switch rotation and translation to match the GTSAM convention! + Sophus::Sim3d::Tangent tangent = Sophus::Sim3d::log(sim.inverse() * other.sim); + Sophus::Sim3d::Tangent returning = tangent; + returning.segment(0, 3) = tangent.segment(3, 3); + returning.segment(3, 3) = tangent.segment(0, 3); + return returning; +} + +Sim3GTSAM::Sim3GTSAM(const Sim3GTSAM& other) : sim(other.sim) +{ + +} + + +bool gtsam::traits::Equals(const Sim3GTSAM& val1, const Sim3GTSAM& val2, double tol) +{ + return val1.equals(val2, tol); +} + +int gtsam::traits::GetDimension(const Sim3GTSAM& sim) +{ + return 7; +} + +gtsam::Vector7 gtsam::traits::Local(Sim3GTSAM origin, Sim3GTSAM other) +{ + return origin.localCoordinates(other); +} + +Sim3GTSAM traits::Retract(const Sim3GTSAM& origin, const gtsam::Vector7& v) +{ + return origin.retract(v); +} + +ScaleGTSAM::ScaleGTSAM(double scale) + : scale(scale) +{ +} + +Sophus::Sim3d ScaleGTSAM::sim() const +{ + Sophus::Sim3d ret; + ret.setScale(scale); + return ret; +} + +void ScaleGTSAM::print(const std::string& str) const +{ + std::cout << str << " Scale: " << scale << std::endl; +} + +size_t ScaleGTSAM::dim() const +{ + return 1; +} + +bool ScaleGTSAM::equals(const ScaleGTSAM& other, double tol) const +{ + return fabs(scale - other.scale) < tol; +} + +ScaleGTSAM ScaleGTSAM::identity() +{ + return ScaleGTSAM(0); +} + +ScaleGTSAM ScaleGTSAM::operator*(const ScaleGTSAM& T) const +{ + return ScaleGTSAM((sim() * T.sim()).scale()); +} + +ScaleGTSAM ScaleGTSAM::inverse() const +{ + return ScaleGTSAM(sim().inverse().scale()); +} + +gtsam::Vector1 ScaleGTSAM::Logmap(const ScaleGTSAM& s, gtsam::OptionalJacobian<1, 1> Hm) +{ + Sophus::Sim3d::Tangent tangent = Sophus::Sim3d::log(s.sim()); + gtsam::Vector1 ret; + ret(0) = tangent(6); + return ret; +} + +ScaleGTSAM ScaleGTSAM::Expmap(const Vector1& v, gtsam::OptionalJacobian<1, 1> Hm) +{ + double scaleInc = v(0); + // For larger values the exp would get too large. + if(std::abs(scaleInc) > 10) + { + scaleInc *= 10 / std::abs(scaleInc); + } + gtsam::Vector7 myInc = gtsam::Vector7::Zero(); + myInc(6) = scaleInc; + Sophus::Sim3d simAfter = Sophus::Sim3d::exp(myInc); + return ScaleGTSAM(simAfter.scale()); +} + +gtsam::Matrix1 ScaleGTSAM::AdjointMap() const +{ + // This is probably just one always, which could be used to make it faster. + gtsam::Matrix77 adj = sim().Adj(); + return adj.block(6, 6, 1, 1); +} diff --git a/src/GTSAMIntegration/Sim3GTSAM.h b/src/GTSAMIntegration/Sim3GTSAM.h new file mode 100644 index 0000000..0b7eedf --- /dev/null +++ b/src/GTSAMIntegration/Sim3GTSAM.h @@ -0,0 +1,137 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_SIM3GTSAM_H +#define DMVIO_SIM3GTSAM_H + + +#include +#include +#include +#include + +// Class for Sim(3)-transformations based on Sophus for GTSAM. +class Sim3GTSAM : public gtsam::DerivedValue +{ +public: + + Sim3GTSAM(const Sim3GTSAM& other); + + Sim3GTSAM(double scale); + + Sim3GTSAM(const Sophus::Sim3d& sim); + + void print(const std::string& str) const override; + + size_t dim() const override; + + Sim3GTSAM retract(const gtsam::Vector& inc) const; + + gtsam::Vector7 localCoordinates(const Sim3GTSAM& other) const; + + bool equals(const Sim3GTSAM& other, double tol) const; + + Sophus::Sim3d sim; +private: +}; + +// In contrast to Sim3GTSAM this contains only the scale. +// Could probably be made faster by not basing it on Sophus. +class ScaleGTSAM : public gtsam::LieGroup +{ +public: + ScaleGTSAM(double scale); + + Sophus::Sim3d sim() const; + double scale = 1.0; + + // For the LieGroup. + static ScaleGTSAM identity(); + + /// Composition + ScaleGTSAM operator*(const ScaleGTSAM& T) const; + + /// Return the inverse + ScaleGTSAM inverse() const; + + static gtsam::Vector1 Logmap(const ScaleGTSAM& s, + gtsam::OptionalJacobian<1, 1> Hm = boost::none); + + static ScaleGTSAM Expmap(const gtsam::Vector1& v, + gtsam::OptionalJacobian<1, 1> Hm = boost::none); + + /// Chart at the origin + struct ChartAtOrigin + { + static ScaleGTSAM Retract(const gtsam::Vector1& v, ChartJacobian H = boost::none) + { + return ScaleGTSAM::Expmap(v, H); + } + + static gtsam::Vector1 Local(const ScaleGTSAM& other, ChartJacobian H = boost::none) + { + return ScaleGTSAM::Logmap(other, H); + } + }; + using LieGroup::inverse; + + gtsam::Matrix1 AdjointMap() const; + + + void print(const std::string& str) const; + size_t dim() const; + bool equals(const ScaleGTSAM& other, double tol) const; +}; + +namespace gtsam +{ +template<> +struct traits +{ + static void Print(const Sim3GTSAM& sim, const std::string& str = "") + { + sim.print(str); + } + + static bool Equals(const Sim3GTSAM& val1, const Sim3GTSAM& val2, double tol = 1e-8); + + static int GetDimension(const Sim3GTSAM& sim); + + static gtsam::Vector7 Local(Sim3GTSAM origin, Sim3GTSAM other); + + static Sim3GTSAM Retract(const Sim3GTSAM& origin, const gtsam::Vector7& v); +}; + + +template<> +struct traits : public internal::LieGroup +{ +}; + +template<> +struct traits : public internal::LieGroup +{ +}; + +} + +#endif //DMVIO_SIM3GTSAM_H diff --git a/src/IMU/BAIMULogic.cpp b/src/IMU/BAIMULogic.cpp new file mode 100644 index 0000000..74bca54 --- /dev/null +++ b/src/IMU/BAIMULogic.cpp @@ -0,0 +1,805 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "BAIMULogic.h" +#include "dso/util/FrameShell.h" +#include "GTSAMIntegration/ExtUtils.h" +#include "IMUInitialization/GravityInitializer.h" +#include "dso/util/settings.h" +#include "IMUUtils.h" +#include +#include +#include +#include +#include "GTSAMIntegration/FEJNoiseModelFactor.h" + +using namespace dmvio; +using gtsam::Symbol; +using gtsam::symbol_shorthand::S, gtsam::symbol_shorthand::G; + +constexpr char end = '\n'; +// Version for flushing always (slower, but useful for crashes). +//template +//auto end(std::basic_ostream& os) -> decltype(std::endl(os))& +//{ +// return std::endl(os); +//} + +BAIMULogic::BAIMULogic(PreintegrationProviderBA* preintegrationProvider, BAGTSAMIntegration* baIntegration, + const IMUCalibration& imuCalibration, + IMUSettings& imuSettings) + : preintegrationProvider(preintegrationProvider), baIntegration(baIntegration), imuSettings(imuSettings), + imuCalibration(imuCalibration), scaleQueue(imuSettings.generalScaleIntervalSize), + optimizeScalePtr(new bool()), optimizeGravityPtr(new bool()), optimizedIMUExtrinsicsPtr(new bool()), + optimizeScale(*optimizeScalePtr), optimizeGravity(*optimizeGravityPtr), + optimizeIMUExtrinsics(*optimizedIMUExtrinsicsPtr) +{ + if(!imuSettings.initSettings.disableVIOUntilFirstInit) + { + optimizeScale = imuSettings.setting_optScaleBA; + optimizeGravity = imuSettings.setting_optGravity; + optimizeIMUExtrinsics = imuSettings.setting_optIMUExtrinsics; + optimizeTransform = optimizeScale || optimizeGravity || optimizeIMUExtrinsics; + }else + { + optimizeScale = optimizeGravity = optimizeIMUExtrinsics = false; + optimizeTransform = true; + } + + // Create PoseTransformation + transformDSOToIMU.reset(new TransformDSOToIMU(gtsam::Pose3(imuCalibration.T_cam_imu.matrix()), optimizeScalePtr, + optimizeGravityPtr, optimizedIMUExtrinsicsPtr, + imuSettings.gravityDirectionFixZ)); + + baIntegration->setDynamicDSOWeightCallback([this](double lastDSOEnergy, double lastRMSE, bool coarseTrackingWasGood) + { + return computeDynamicDSOWeight(lastDSOEnergy, lastRMSE, + coarseTrackingWasGood); + }); + + scaleFile.open(imuSettings.resultsPrefix + "scalesdso.txt"); // contains the current scale for all times. + baBiasFile.open(imuSettings.resultsPrefix + "babiasdso.txt"); // contains estimated and groundtruth bias. + baGravDirFile.open(imuSettings.resultsPrefix + "bagravdir.txt"); // contains gravity directions. + baVelFile.open(imuSettings.resultsPrefix + "bavel.txt"); // contains estimated velocities. +} + +bool BAIMULogic::addIMUVarsForKey(int keyframeId) +{ + bool addIMUKeys = !imuSettings.skipFirstKeyframe || keyframeId != firstFrameId; + if(disableFromKF > 0 && keyframeId > disableFromKF) + { + addIMUKeys = false; + } + if(noIMUInOrderingUntilKFId >= 0 && keyframeId < noIMUInOrderingUntilKFId) + { + addIMUKeys = false; + } + return addIMUKeys; +} + +void +dmvio::BAIMULogic::updateBAOrdering(std::vector& frames, gtsam::Ordering* ordering, KeyDimMap& baDimMap) +{ + // Add IMU related variables to the BAOrdering. + + // First the variables from the transform. + int index = transformDSOToIMU->getSymbolInd(); // == 0 usually. + bool addScaleKey = optimizeScale && !scaleFixed; + // To fix the scale we simply don't add it to the ordering. The BAGTSAMIntegration will fix all keys which are + // not in the ordering. + if(addScaleKey) + { + gtsam::Symbol scaleKey('s', index); + ordering->push_back(scaleKey); + baDimMap[scaleKey] = 1; + } + if(optimizeGravity) + { + gtsam::Symbol gravityKey('g', index); + ordering->push_back(gravityKey); + baDimMap[gravityKey] = 3; + } + if(imuSettings.setting_optIMUExtrinsics && optimizeIMUExtrinsics) + { + gtsam::Symbol extrinsicsKey('i', index); + ordering->push_back(extrinsicsKey); + baDimMap[extrinsicsKey] = 6; + } + + // Add velocities and biases. + int i = 0; + for(dso::EFFrame* h : frames) + { + int id = h->idx; + + assert(id == i); + + long fullId = h->data->shell->id; + + gtsam::Symbol velKey('v', fullId); + gtsam::Symbol biasKey('b', fullId); + + if(addIMUVarsForKey(fullId)) + { + ordering->push_back(velKey); + ordering->push_back(biasKey); + } + + baDimMap[velKey] = 3; + baDimMap[biasKey] = 6; + + ++i; + } +} + +void dmvio::BAIMULogic::addKeysToMarginalize(int fullId, gtsam::FastVector& keysToMarginalize) +{ + gtsam::Symbol velKey('v', fullId); + gtsam::Symbol biasKey('b', fullId); + + if(addIMUVarsForKey(fullId)) + { + keysToMarginalize.push_back(velKey); + keysToMarginalize.push_back(biasKey); + } +} + +// This method is only relevant if the legacy setting disableVIOUntilFirstInit is set to false. +// Called from addKeyframe, and from initializeFromInitializer! +// It is first called from initializeFromInitializer, but then only the first 5 lines of the method are executed. +// Then it will be called from the first calling of addKeyframe. +void dmvio::BAIMULogic::addFirstBAFrame(int keyframeId, BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues) +{ + if(imuSettings.skipFirstKeyframe && firstFrameId == -1) + { + firstFrameId = keyframeId; + previousKeyframeId = -1; + return; + } + gtsam::Key vel_current_key = gtsam::Symbol('v', keyframeId); + gtsam::Key bias_current_key = gtsam::Symbol('b', keyframeId); + + std::cout << "Add first keyframe: Add priors for keyframe " << keyframeId << std::endl; + gtsam::Vector3 initialVelocity = (gtsam::Vector(3) << 0, 0, 0).finished(); + gtsam::imuBias::ConstantBias initialBias; + + baValues->insert(vel_current_key, initialVelocity); + baValues->insert(bias_current_key, initialBias); + + if(imuSettings.setting_prior_bias) + { + gtsam::noiseModel::Diagonal::shared_ptr bias_prior_model = gtsam::noiseModel::Diagonal::Variances( + (gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-3, 1e-3, 1e-3).finished()); + baGraphs->addFactor( + boost::make_shared>(bias_current_key, initialBias, + bias_prior_model), + BIAS_AND_PRIOR_GROUP); + } + + if(imuSettings.setting_prior_velocity) + { + gtsam::noiseModel::Diagonal::shared_ptr velocity_prior_model = gtsam::noiseModel::Isotropic::Sigma(3, 1e-1); + baGraphs->addFactor(boost::make_shared>(vel_current_key, initialVelocity, + velocity_prior_model), + BIAS_AND_PRIOR_GROUP); + } + + // Add priors for gravity direction and extrinsics + insert them and the scale into values. + auto factors = getPriorsAndAddValuesForTransform(*transformDSOToIMU, imuSettings.transformPriors, *baValues); + for(auto&& factor : factors) + { + baGraphs->addFactor(factor, BIAS_AND_PRIOR_GROUP); + } + + previousKeyframeId = keyframeId; // Set so that the next call of addKeyframeToBA knows about this first frame. + + maxScaleInterval = 0.0; + minScaleInterval = 1000.0; + +} + +void dmvio::BAIMULogic::setNextBAVel(const gtsam::Vector3& velocity, int frameId) +{ + nextVelocity = velocity; + nextVelocityFrameId = frameId; +} + +void dmvio::BAIMULogic::addKeyframe(BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues, int keyframeId, + const Sophus::SE3& keyframePose, std::vector& frames) +{ + if(disableFromKF > 0 && keyframeId > disableFromKF) + { + return; + } + + double currBATimestamp = baIntegration->getCurrBaTimestamp(); + if(firstBATimestamp < 0) + { + firstBATimestamp = currBATimestamp; + } + + int lastKeyframeId = previousKeyframeId; + currKeyframeId = keyframeId; + + if(lastKeyframeId == -1 || lastKeyframeId == keyframeId) + { + assert(imuSettings.skipFirstKeyframe); // should not happen unless we want to skip the first keyframe! + std::cout << "First keyframe was successfully skipped." << std::endl; + std::cout << lastKeyframeId << std::endl; + addFirstBAFrame(keyframeId, baGraphs, baValues); + }else + { + // Insert IMU factor. + + gtsam::Key currPoseKey = gtsam::Symbol('p', keyframeId); + gtsam::Key currVelKey = gtsam::Symbol('v', keyframeId); + gtsam::Key currBiasKey = gtsam::Symbol('b', keyframeId); + + gtsam::Key prevPoseKey = gtsam::Symbol('p', lastKeyframeId); + gtsam::Key prevVelKey = gtsam::Symbol('v', lastKeyframeId); + gtsam::Key prevBiasKey = gtsam::Symbol('b', lastKeyframeId); + + auto&& imuMeasurements = preintegrationProvider->getPreintegratedMeasurements(keyframeId); + + // Alternative: Example how the GTSAM IMU Factor can be modified to handle First Estimates Jacobians in the + // correct way. We disable it because it does not seem to improve the results, as it is sufficient to handle + // FEJ in the PoseTransformationFactor only). + // The same technique with our FEJNoiseModelFator can also be used for the bias random walk factors. +// gtsam::NonlinearFactor::shared_ptr imuFactorWithFEJ( +// new FEJNoiseModelFactor(pose_prev_key, vel_prev_key, +// pose_current_key, vel_current_key, bias_prev_key, +// imuMeasurements)); + + gtsam::NonlinearFactor::shared_ptr imuFactor( + new gtsam::ImuFactor(prevPoseKey, prevVelKey, + currPoseKey, currVelKey, prevBiasKey, + imuMeasurements)); + + // Note: The graph optimizes poses with worldToCam in DSO scale. The IMU factor works on metric poses with imuToWorld. + // To transform between the two we use our PoseTransformationFactor and the transformDSOToIMU. + // This will also add the scale and gravity direction as optimizable variables to our factor graph. + auto transformedFactor = boost::make_shared(imuFactor, + *transformDSOToIMU, + PoseTransformationFactor::JACOBIAN_FACTOR); + baGraphs->addFactor(transformedFactor, METRIC_GROUP); + + gtsam::noiseModel::Diagonal::shared_ptr biasNoiseModel = computeBiasNoiseModel(imuCalibration, imuMeasurements); + + gtsam::NonlinearFactor::shared_ptr biasFactor( + new gtsam::BetweenFactor( + prevBiasKey, currBiasKey, + gtsam::imuBias::ConstantBias(gtsam::Vector3::Zero(), + gtsam::Vector3::Zero()), biasNoiseModel)); + + baGraphs->addFactor(biasFactor, BIAS_AND_PRIOR_GROUP); + + // Insert previous bias as initial new bias. + gtsam::imuBias::ConstantBias currentBias = baValues->at(prevBiasKey); + baValues->insert(currBiasKey, currentBias); + + gtsam::Vector3 currentVelocity = baValues->at(prevVelKey); + // If available use velocity from coarse tracking. + if(nextVelocityFrameId != -1) + { + if(nextVelocityFrameId == keyframeId) + { + currentVelocity = nextVelocity; + nextVelocityFrameId = -1; + }else + { + std::cout << "ERROR: nextVelocity frame not as expected! " << nextVelocityFrameId << " " << nextVelocity + << std::endl; + } + } + baValues->insert(currVelKey, currentVelocity); + } +} + +void dmvio::BAIMULogic::preSolve(gtsam::Matrix& HFull, gtsam::Vector& bFull, int dimensionDSOH) +{ + if(disableFromKF > 0) return; + + if(imuSettings.useScaleDiagonalHack && optimizeScale && !scaleFixed) + { + // Hack which is useful in case we initialize immediately without a scale prior. This stops the scale from converging + // even if it is far from the optimum. + // Usually disabled because with the IMU initializer of DM-VIO we don't need it. + int scalePos = dimensionDSOH; + HFull(scalePos, scalePos) += 5.0; + HFull(scalePos, scalePos) *= (1 + 1.0); + } +} + +bool dmvio::BAIMULogic::postSolve(gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr newValues, + const gtsam::Vector& inc, const gtsam::Ordering& ordering, const KeyDimMap& baDimMap) +{ + if(disableFromKF > 0) return true; + + bool dontBreak = false; + + if(imuSettings.alwaysCanBreakIMU) + { + return true; + } + // Compute mean squared increment norm for velocities, biases, scale, etc. + std::map accums; + accums['v'] = MeanAccumulatorD{}; + accums['b'] = MeanAccumulatorD{}; + accums['s'] = MeanAccumulatorD{}; + accums['g'] = MeanAccumulatorD{}; + accums['i'] = MeanAccumulatorD{}; + // Thresholds analogous to computation in FullSystem::doStepFromBackup. + std::map thresholds = {{'v', 0.0001}, + {'b', 0.0005}, + {'s', 0.0005}, + {'g', 0.0005}, + {'i', 0.0005}}; + int pos = 0; + for(auto&& key : ordering) + { + Symbol s(key); + auto&& accum = accums.find(s.chr()); + if(accum != accums.end()) + { + gtsam::Vector increment = inc.segment(pos, baDimMap.at(key)); + accum->second.add(increment.squaredNorm()); + } + pos += baDimMap.at(key); + } + bool canBreak = true; + for(auto&& pair : accums) + { + double val = std::sqrt(pair.second.getMean()); + double thresh = thresholds[pair.first] * dso::setting_thOptIterations; + canBreak = canBreak && val < thresh; + } + return canBreak && !dontBreak; +} + +void dmvio::BAIMULogic::acceptUpdate(gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr newValues) +{ + if(optimizeTransform) + { + // Update scale and gravity direction in the transform. + transformDSOToIMU->updateWithValues(*newValues); + } + + gtsam::Key scaleKey = gtsam::Symbol('s', 0); + if(optimizeScale && !scaleFixed) + { + double newScale = transformDSOToIMU->getScale(); + std::cout << "Optimized scale: " << newScale << end; + if(newScale > maxScaleInterval) + { + maxScaleInterval = newScale; + } + if(newScale < minScaleInterval) + { + minScaleInterval = newScale; + } + } + if(optimizeIMUExtrinsics) + { + gtsam::Pose3 newT_cam_imu = newValues->at(Symbol('i', 0)); + std::cout << "Optimized T_cam_imu: " << newT_cam_imu.translation().transpose() << end; + } +} + +// Signals that the BA and marginalization operations for this keyframe are finished (might be after the KF is already the new tracking ref) +void dmvio::BAIMULogic::finishKeyframeOperations(int keyframeId) +{ + if(disableFromKF > 0) return; + + auto&& baValues = baIntegration->getBaValues(); + gtsam::imuBias::ConstantBias currentBias = baValues->at( + gtsam::Symbol('b', keyframeId)); + + double currBATimestamp = baIntegration->getCurrBaTimestamp(); + + if(currGTData) + { + // Print estimated bias and corresponding GT bias to file. + Eigen::Vector3d gtTrans = currGTData->biasTranslation; + Eigen::Vector3d gtRot = currGTData->biasRotation; + Eigen::Vector3d errorTrans = currentBias.accelerometer() - gtTrans; + Eigen::Vector3d errorRot = currentBias.gyroscope() - gtRot; + currGTData = 0; // Note that this will not work in realtime mode... + + baBiasFile << std::fixed << std::setprecision(6) << currBATimestamp << ' ' << std::setprecision(20) + << gtTrans.transpose() << ' ' << gtRot.transpose() << ' ' << currentBias.accelerometer().transpose() + << ' ' << currentBias.gyroscope().transpose(); + }else + { + Eigen::Vector3d gtTrans = Eigen::Vector3d::Zero(); + Eigen::Vector3d gtRot = Eigen::Vector3d::Zero(); + baBiasFile << std::fixed << std::setprecision(6) << currBATimestamp << ' ' << std::setprecision(20) + << gtTrans.transpose() << ' ' << gtRot.transpose() << ' ' << currentBias.accelerometer().transpose() + << ' ' << currentBias.gyroscope().transpose(); + } + if(biasCovForKF == keyframeId) + { + // We also have computed bias covariance. + baBiasFile << ' ' << biasCovariance.diagonal().transpose(); + } + baBiasFile << end; + + if(optimizeScale && !scaleFixed) + { + // Compute how much the scale has changed lately to determine if it should be fixed. + scaleQueue.push_back(std::make_pair(maxScaleInterval, minScaleInterval)); + scaleQueue.pop_front(); + maxScaleInterval = minScaleInterval = transformDSOToIMU->getScale(); + double realMaxScale = maxScaleInterval, realMinScale = minScaleInterval; + // Compute and minimum and maximum scale over the last n keyframes optimizations. + for(auto&& pair : scaleQueue) + { + if(pair.first > realMaxScale) + { + realMaxScale = pair.first; + } + if(pair.second < realMinScale) + { + realMinScale = pair.second; + } + } + double diff = realMaxScale / realMinScale - 1.0; + + if(diff < imuSettings.setting_scaleFixTH) + { + // Fix scale! + scaleFixed = true; + std::cout << "Fixing scale at time: " << std::fixed << std::setprecision(6) << currBATimestamp << std::endl; + + // Useful for testing. Allows to evaluate the visual-only system, for the majority of the time but with the metric scale obtained from IMU. + if(imuSettings.setting_visualOnlyAfterScaleFixing == 1) + { + std::cout << "DISABLING IMU AND THE GTSAM INTEGRATION COMPLETELY!" << std::endl; + dso::setting_useIMU = false; + dso::setting_useGTSAMIntegration = false; + }else if(imuSettings.setting_visualOnlyAfterScaleFixing == 2) + { + std::cout << "DISABLING IMU COMPLETELY!" << std::endl; + dso::setting_useIMU = false; + disableFromKF = keyframeId; + } + } + } +} + + +// computes the factor. Also computes uncertainty for some more variables for saving to file. +gtsam::LinearContainerFactor::shared_ptr BAIMULogic::computeFactorForCoarseGraphAndMarginalCovariances() +{ + dmvio::TimeMeasurement meas("computeFactorForCoarseGraphAndMarginalCovariances"); + + // The idea behind this method is that we want both, a factor for the coarse tracking, and marginal uncertainty for some more variables. + // These need to do similar processing, hence we combine them: + // First we marginalize everything that is neither in factorOrdering or uncertOrdering, and compute marginal covariances. + // Then we also marginalize everything in uncertOrdering to obtain the factor for the coarse graph. + + gtsam::Ordering factorOrdering; // This contains all keys which shall be in the factor. + + gtsam::Key biasKey = gtsam::Symbol('b', currKeyframeId); + gtsam::Key velKey = gtsam::Symbol('v', currKeyframeId); + factorOrdering.push_back(biasKey); + factorOrdering.push_back(velKey); + + gtsam::Ordering uncertOrdering; // This contains all keys for which we need uncertainty (but which shall not be in the factor). + gtsam::Key scaleKey = gtsam::Symbol('s', transformDSOToIMU->getSymbolInd()); + gtsam::Key gravKey = gtsam::Symbol('g', transformDSOToIMU->getSymbolInd()); + gtsam::Key newestPoseKey = gtsam::Symbol('p', currKeyframeId); + uncertOrdering.push_back(scaleKey); + uncertOrdering.push_back(gravKey); + + gtsam::Ordering ordering; + ordering.insert(ordering.end(), uncertOrdering.begin(), uncertOrdering.end()); + ordering.insert(ordering.end(), factorOrdering.begin(), factorOrdering.end()); + + auto& keyDimMap = baIntegration->getBaDimMap(); + gtsam::Ordering additionalKeys; + + // We first compute the Hessian H and gradient vector b using the new values. + auto graphPair = baIntegration->getBaGraphs().getHAndB(*(baIntegration->getBaValues()), ordering, + keyDimMap, &additionalKeys); + + gtsam::Ordering completeOrdering; + completeOrdering.insert(completeOrdering.begin(), ordering.begin(), ordering.end()); + completeOrdering.insert(completeOrdering.end(), additionalKeys.begin(), additionalKeys.end()); + + // Also add the active DSO hessian. + auto activeDSOFactor = baIntegration->getActiveDSOFactor(); + gtsam::NonlinearFactorGraph activeDSOGraph; + activeDSOGraph.push_back(activeDSOFactor); + auto linearizedDSOGraph = activeDSOGraph.linearize(*(baIntegration->getBaValues())); + + AugmentedScatter scatter(*linearizedDSOGraph, completeOrdering, keyDimMap); + std::pair pair = scatter.computeHessian(*linearizedDSOGraph); + + gtsam::Ordering newAdditionalKeys; + fillAdditionalKeysFromScatter(completeOrdering, newAdditionalKeys, scatter); + additionalKeys.insert(additionalKeys.end(), newAdditionalKeys.begin(), newAdditionalKeys.end()); + + assert(graphPair.first.size() <= pair.first.size()); + pair.first.block(0, 0, graphPair.first.rows(), graphPair.first.cols()) += graphPair.first; + pair.second.segment(0, graphPair.second.size()) += graphPair.second; + + auto accumFun = [&keyDimMap](int num, const gtsam::Key& key) + { + return num + keyDimMap.at(key); + }; + int aSize = std::accumulate(ordering.begin(), ordering.end(), 0, accumFun); + int mSize = std::accumulate(additionalKeys.begin(), additionalKeys.end(), 0, accumFun); + + // Then we first marginalize everything except the necessary variables. + + // Marginalize with preconditioning: + // First create the proper input (first variables to marginalize, then rest) for computeSchurComplement. + gtsam::Matrix margInputH(aSize + mSize, aSize + mSize); + margInputH.block(0, 0, mSize, mSize) = pair.first.block(aSize, aSize, mSize, mSize); + margInputH.block(mSize, mSize, aSize, aSize) = pair.first.block(0, 0, aSize, aSize); + margInputH.block(0, mSize, mSize, aSize) = pair.first.block(aSize, 0, mSize, aSize); + margInputH.block(mSize, 0, aSize, mSize) = pair.first.block(0, aSize, aSize, mSize); + gtsam::Vector margInputB(aSize + mSize); + margInputB.segment(0, mSize) = pair.second.segment(aSize, mSize); + margInputB.segment(mSize, aSize) = pair.second.segment(0, aSize); + + gtsam::Matrix marginalized = computeSchurComplement( + augmentedHessianFromPair(std::make_pair(margInputH, margInputB)), mSize, aSize); + + // Compute covariances + gtsam::Matrix margH = marginalized.block(0, 0, aSize, aSize); + + // Covariance is the inverse of the Hessian. + gtsam::Matrix inversed = margH.inverse(); + + std::vector uncertainties; + int pos = 0; + for(auto&& key : ordering) + { + int dim = keyDimMap.at(key); + uncertainties.emplace_back(inversed.block(pos, pos, dim, dim)); + pos += dim; + } + + gtsam::Matrix biasCov = uncertainties[uncertOrdering.size()]; +#ifdef DEBUG + gtsam::Matrix testMat = pair.first.inverse().block(4, 4, 6, 6); + assertEqEigen(testMat, biasCov); +#endif + biasCovariance = std::move(biasCov); + biasCovForKF = currKeyframeId; + + // Print covariances to file. + scaleFile << std::fixed << std::setprecision(6) << baIntegration->getCurrBaTimestamp() << ' ' + << transformDSOToIMU->getScale() << ' ' << uncertainties[0](0, 0) << end; + + Eigen::Vector3d rotLog = transformDSOToIMU->getR_dsoW_metricW().log(); + baGravDirFile << std::fixed << std::setprecision(6) << baIntegration->getCurrBaTimestamp() << ' ' + << rotLog.transpose() << ' ' << uncertainties[1].diagonal().transpose() << end; + + baVelFile << std::fixed << std::setprecision(6) << baIntegration->getCurrBaTimestamp() << ' ' << + baIntegration->getBaValues()->at(velKey).transpose() << ' ' + << uncertainties[uncertOrdering.size() + 1].diagonal().transpose() << end; + + // Now marginalize everything in uncertOrdering, because these variables shall not be in the factor for the coarse graph. + int secondASize = std::accumulate(factorOrdering.begin(), factorOrdering.end(), 0, accumFun); + int secondMSize = std::accumulate(uncertOrdering.begin(), uncertOrdering.end(), 0, accumFun); + gtsam::Matrix margForFactor = computeSchurComplement(marginalized, secondMSize, + secondASize); // This method assumes that the variables to marginalize are first. + + gtsam::FastVector connectedDims; + for(auto&& key : factorOrdering) + { + connectedDims.push_back(keyDimMap.at(key)); + } + gtsam::SymmetricBlockMatrix sm(connectedDims, true); + sm.setFullMatrix(margForFactor * imuSettings.transferCovToCoarseMultiplier); + + gtsam::LinearContainerFactor::shared_ptr lcf( + new gtsam::LinearContainerFactor(gtsam::HessianFactor(factorOrdering, sm), + *(baIntegration->getBaValues()))); + return lcf; + +} + +void BAIMULogic::postOptimization(int keyframeId) +{ + factorForCoarseGraph = computeFactorForCoarseGraphAndMarginalCovariances(); +} + +std::unique_ptr BAIMULogic::finishKeyframeOptimization(int keyframeId) +{ + // Prepare information for the coarse tracking. + std::unique_ptr returning(new InformationBAToCoarse); + returning->latestBAScale = transformDSOToIMU->getScale(); + auto&& baValues = baIntegration->getBaValues(); + + returning->latestBAVel = baValues->at(gtsam::Symbol('v', keyframeId)); + returning->latestBABias = baValues->at(gtsam::Symbol('b', keyframeId)); + returning->latestBAPose = baValues->at(gtsam::Symbol('p', keyframeId)); + returning->latestBAPosePrevKeyframe = baValues->at(gtsam::Symbol('p', previousKeyframeId)); + + assert(transformDSOToIMU != nullptr); + std::shared_ptr copy(new TransformDSOToIMU(*transformDSOToIMU)); + returning->transformBAToIMU = copy; + returning->transformIMUToDSOForCoarse = std::make_unique>(copy, + keyframeId); + + if(imuSettings.setting_transferCovToCoarse) + { + returning->priorFactor = factorForCoarseGraph; + } + + previousKeyframeId = currKeyframeId; + + return returning; +} + +void BAIMULogic::setCurrGtData(dmvio::GTData* currGtData, int frameId) +{ + currGTData = currGtData; + gtFrameId = frameId; +} + +std::shared_ptr BAIMULogic::getTransformDSOToIMU() const +{ + return transformDSOToIMU; +} + +// Takeover result from the IMU initializer. Values contains the new values (DSO variables will not replaced though). +// If reinit this is a reinitialization, meaning that the IMU is being used already. +// If willReplaceGraph the IMU initializer will replace the marginalization prior (typically obtained by readvancing). +void BAIMULogic::initFromIMUInit(const gtsam::Values& values, bool reinit, bool willReplaceGraph) +{ + auto* targValues = baIntegration->getMutableValues(); + int latestKFId = -1; + + if(!willReplaceGraph && !reinit) // For reinit we don't need to do this. + { + // If the IMU initializer does not provide a new marginalization prior (e.g. for the ablation studies), we + // have to manually add the priors. + + // find out the latestKFId + for(auto&& val : values) + { + gtsam::Symbol sym(val.key); + if(sym.chr() == 'v' && static_cast(sym.index()) > latestKFId) + { + latestKFId = sym.index(); + } + } + + // Add zero prior for rotation! + gtsam::Values nullValues; + auto factors = getPriorsAndAddValuesForTransform(*transformDSOToIMU, imuSettings.transformPriors, nullValues); + for(auto&& factor : factors) + { + baIntegration->getBaGraphs().addFactor(factor, BIAS_AND_PRIOR_GROUP); + } + + if(imuSettings.initSettings.scalePriorAfterInit > 0) + { + // Add scale prior. + gtsam::Vector1 scaleModel; + scaleModel(0) = imuSettings.initSettings.scalePriorAfterInit; + gtsam::PriorFactor::shared_ptr scalePrior( + new gtsam::PriorFactor(S(0), values.at(S(0)), + gtsam::noiseModel::Diagonal::Sigmas(scaleModel))); + baIntegration->getBaGraphs().addFactor(scalePrior, BIAS_AND_PRIOR_GROUP); + } + + // don't add keys before this key, because there will not be respective IMU factors in the graph. + noIMUInOrderingUntilKFId = latestKFId; + }else if(!reinit) + { + // If we skipped KFs in the PGBA we must skip them now as well. + // In general it might happen that there is still a very old KF in the active window for which did not get + // included in the PGBA. + long long minKFId = std::numeric_limits::max(); + for(auto&& val : values) + { + gtsam::Symbol sym(val.key); + if(sym.chr() == 'v' && static_cast(sym.index()) < minKFId) + { + minKFId = sym.index(); + } + } + noIMUInOrderingUntilKFId = minKFId; + } + + // Replace the values. + for(auto&& val : values) + { + bool replaceValue = true; + gtsam::Symbol sym(val.key); + if(sym.chr() == 'v' && static_cast(sym.index()) > latestKFId) + { + latestKFId = sym.index(); + } + if(!imuSettings.initSettings.initDSOParams) + { + // Don't replace the values of poses (and affine brightness). + replaceValue = sym.chr() != 'p' && sym.chr() != 'a'; + } + if(noIMUInOrderingUntilKFId >= 0) + { + if((sym.chr() == 'v' || sym.chr() == 'b') && sym.index() < noIMUInOrderingUntilKFId) + { + replaceValue = false; + } + } + if(replaceValue) + { + eraseAndInsert(*targValues, val.key, val.value); + } + } + + // When initializing from CoarseIMUInit (ablation study) we might have optimized only a single bias. + if(!targValues->exists(gtsam::Symbol('b', latestKFId))) + { + targValues->insert(gtsam::Symbol('b', latestKFId), values.at(gtsam::Symbol('b', 0))); + } + + if(!reinit) + { + std::cout << "INITIALIZED IMU Integration. Using IMU data in main optimization from now on!" << std::endl; + + optimizeScale = imuSettings.setting_optScaleBA; + optimizeGravity = imuSettings.setting_optGravity; + optimizeIMUExtrinsics = imuSettings.setting_optIMUExtrinsics; + + previousKeyframeId = latestKFId; + }else + { + std::cout << "REINIT IMU Integration." << std::endl; + } + + // Set transform from values + transformDSOToIMU->updateWithValues(values); + + + maxScaleInterval = 0.0; + minScaleInterval = 1000.0; +} + +bool BAIMULogic::isScaleFixed() const +{ + return scaleFixed; +} + +double BAIMULogic::computeDynamicDSOWeight(double lastDSOEnergy, double lastRMSE, bool coarseTrackingWasGood) +{ + // Compute dynamic photometric weight. Basically a threshold robust cost function. + double rmseThresh = imuSettings.dynamicWeightRMSEThresh; + if(lastRMSE < rmseThresh || std::isnan(lastRMSE)) return 1.0; + double sqrtWeight = rmseThresh / lastRMSE; + return sqrtWeight * sqrtWeight; +} diff --git a/src/IMU/BAIMULogic.h b/src/IMU/BAIMULogic.h new file mode 100644 index 0000000..6e4eb74 --- /dev/null +++ b/src/IMU/BAIMULogic.h @@ -0,0 +1,199 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_BAIMULOGIC_H +#define DMVIO_BAIMULOGIC_H + +#include +#include +#include "GTSAMIntegration/BAGTSAMIntegration.h" +#include "IMUTypes.h" +#include "IMUSettings.h" +#include "GTSAMIntegration/DelayedMarginalization.h" + +namespace dmvio +{ + +// Information the Coarse tracking needs from the previous BA optimization when the KF changes. +struct InformationBAToCoarse +{ + double latestBAScale; + gtsam::Vector3 latestBAVel; + gtsam::imuBias::ConstantBias latestBABias; + gtsam::Pose3 latestBAPose, latestBAPosePrevKeyframe; + + // Transform poses from BA to IMU frame. Typically of type TransformDSOToIMU + std::shared_ptr transformBAToIMU; + // Transform poses from IMU to coarse DSO poses. Typically of type TransformIMUToDSOForCoarse. + std::unique_ptr transformIMUToDSOForCoarse; + + gtsam::LinearContainerFactor::shared_ptr priorFactor; // Factor with priors to add to the graph. +}; + +// Provides the preintegrated BA data for BAIMULogic. +// Responsible for making sure that it always contains the preintegrated data for the frame which actually became a KF +// (which can change in realtime mode because DSO can change its mind which frame becomes a keyframe). +class PreintegrationProviderBA +{ +public: + virtual const gtsam::PreintegratedImuMeasurements& getPreintegratedMeasurements(int keyframeId) = 0; +}; + +class IMUInitializer; + +// Responsible for integrating IMU data into the DSO Bundle Adjustment (BA). +// This class adds velocity, bias, scale and gravity direction as additional variables to be optimized. +// It adds IMU factors and bias random walk factors between successive keyframes. +class BAIMULogic : public BAExtension +{ +public: + // We have three groups of factors. NO_IMU_GROUP contains all DSO factors, BIAS_AND_PRIOR_group contains bias + // random walk factors and prior factors. METRIC_GROUP contains IMU factors. + // This is used e.g. for adding only visual factors (group 0) to the delayed graph for IMU reinitialization. + enum FactorGroups + { + NO_IMU_GROUP = 0, BIAS_AND_PRIOR_GROUP, METRIC_GROUP + }; + + // Note: A reference to preintegrationProvider, imuCalibration, imuSettings, and baIntegration is kept, so they all must be kept alive. + BAIMULogic(PreintegrationProviderBA* preintegrationProvider, BAGTSAMIntegration* baIntegration, + const IMUCalibration& imuCalibration, IMUSettings& imuSettings); + + // Methods called by BAGTSAMIntegration: + virtual void addFirstBAFrame(int keyframeId, BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues) override; + + virtual void addKeyframe(BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues, int keyframeId, + const Sophus::SE3d& keyframePose, std::vector& frames) override; + + virtual void + updateBAOrdering(std::vector& frames, gtsam::Ordering* ordering, KeyDimMap& baDimMap) override; + + virtual void addKeysToMarginalize(int fullId, gtsam::FastVector& keysToMarginalize) override; + + virtual void preSolve(gtsam::Matrix& HFull, gtsam::Vector& bFull, int dimensionDSOH) override; + + virtual bool + postSolve(gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr newValues, const gtsam::Vector& inc, + const gtsam::Ordering& ordering, const KeyDimMap& baDimMap) override; + + virtual void acceptUpdate(gtsam::Values::shared_ptr values, gtsam::Values::shared_ptr newValues) override; + + // -------------------------------------------------- + + // Takeover result from the IMU initializer. Values contains the new values (DSO variables will not replaced though). + // If reinit this is a reinitialization, meaning that the IMU is being used already. + // If willReplaceGraph the IMU initializer will replace the marginalization prior (typically obtained by readvancing). + void initFromIMUInit(const gtsam::Values& values, bool reinit, bool willReplaceGraph); + + // Methods called by IMUIntegration: + + // Called when all keyframe operations (including marginalization) are finished. + // Note that this can be after the tracking reference has already changed + // e.g. the order can be: BA finished (finishKeyframeOptimization) -> tracking reference changed -> + // -> in other thread marginalization and post BA stuff -> finishKeyframeOperations()) + void finishKeyframeOperations(int keyframeId); + + // Called before finishKeyframeOptimization (and before the IMU init might be called). + void postOptimization(int keyframeId); + + // Called just before the new KF will become the new coarse tracking reference. + // (called from the BA thread but with mutex on coarseTrackerSwapMutex.) + std::unique_ptr finishKeyframeOptimization(int keyframeId); + + // Called to set the velocity of the next keyframe (to transfer the velocity from the coarse tracking). + void setNextBAVel(const gtsam::Vector3& velocity, int frameId); + + // Set groundtruth data if available (for printing result to file). + // Should only be used in non-RT mode for now, because it keeps a reference and the objected might be deleted otherwise. + void setCurrGtData(dmvio::GTData* currGtData, int frameId); + + // Return the (optimized and regularly updated) transform from DSO to IMU. + // Should only used inside the BA thread, otherwise there might be a race condition. + std::shared_ptr getTransformDSOToIMU() const; + + bool isScaleFixed() const; + + double computeDynamicDSOWeight(double lastDSOEnergy, double lastRMSE, bool coarseTrackingWasGood); + +private: + bool addIMUVarsForKey(int keyframeId); + + BAGTSAMIntegration* baIntegration; + PreintegrationProviderBA* preintegrationProvider; + + // Shared with parent IMUIntegration. + IMUSettings& imuSettings; + const IMUCalibration& imuCalibration; + + // Pose transformation used for the IMU factors. + // Transforms from DSO frame to IMU (metric) frame. + std::shared_ptr transformDSOToIMU; + + // If set to a positive value, the IMU integration is disabled starting from the KF with id (used for debug purposes): + int disableFromKF = -1; + int noIMUInOrderingUntilKFId = -1; // if set, we don't add IMU keys to the ordering for kfid < noIMUInOrderingUntilKFId + + // True if the variables inside the transformDSOToIMU are optimized at all. + bool optimizeTransform; + // Pointers are shared with transformDSOToIMUNew. + std::shared_ptr optimizeScalePtr, optimizeGravityPtr, optimizedIMUExtrinsicsPtr; + // For convenience: references to the pointers above. + bool& optimizeScale; + bool& optimizeGravity; + bool& optimizeIMUExtrinsics; + + int previousKeyframeId{-1}; + int currKeyframeId{-1}; + double firstBATimestamp{-1}; + + // Used for skipping the first keyframe for IMU data. + int firstFrameId{-1}; + + // Variables for determining when to fix the scale. + bool scaleFixed = false; + // Maximum and minimum scale during this keyframe optimization. + double maxScaleInterval = 0.0, minScaleInterval = 1000; + std::deque > scaleQueue; // Saves maximum and minimum scale for the last keyframes. + + dmvio::GTData* currGTData = nullptr; + int gtFrameId = -1; + + // Files to which results are saved. + std::ofstream scaleFile, baBiasFile, baGravDirFile, baVelFile; + + // Prior factor to be used by the CoarseIMULogic. It is obtained by marginalizing everything in the BA graph, except + // the newest bias and velocity. This is a useful prior for the coarse tracking. + gtsam::LinearContainerFactor::shared_ptr factorForCoarseGraph; + // computes the factor. Also computes uncertainty for some more variables for saving to file. + gtsam::LinearContainerFactor::shared_ptr computeFactorForCoarseGraphAndMarginalCovariances(); + + gtsam::Matrix biasCovariance; + int biasCovForKF = -1; + + // Used to get the velocity from the coarse tracking. + gtsam::Vector3 nextVelocity; + int nextVelocityFrameId = -1; +}; + +} + +#endif //DMVIO_BAIMULOGIC_H diff --git a/src/IMU/CoarseIMULogic.cpp b/src/IMU/CoarseIMULogic.cpp new file mode 100644 index 0000000..e0d538a --- /dev/null +++ b/src/IMU/CoarseIMULogic.cpp @@ -0,0 +1,467 @@ +/** +* This file is part of DM-VIO. +* The code in this file is in part based on code written by Vladyslav Usenko for the paper "Direct Visual-Inertial Odometry with Stereo Cameras". +* +* Copyright (c) 2022 Lukas von Stumberg , Vladyslav Usenko +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include +#include "CoarseIMULogic.h" +#include "IMUInitialization/GravityInitializer.h" +#include "IMUUtils.h" +#include "GTSAMIntegration/GTSAMUtils.h" + + +dmvio::CoarseIMULogic::CoarseIMULogic(std::unique_ptr transformBAToIMU, + boost::shared_ptr preintegrationParams, + const IMUCalibration& imuCalibration, dmvio::IMUSettings& imuSettings) + : transformBAToIMU(std::move(transformBAToIMU)), + preintegrationParams(preintegrationParams), + imuSettings(imuSettings), + imuCalibration(imuCalibration) +{ + coarseBiasFile.open(imuSettings.resultsPrefix + "coarsebiasdso.txt"); +} + + +Sophus::SE3d dmvio::CoarseIMULogic::addIMUData(const dmvio::IMUData& imuData, int frameId, double frameTimestamp, + int lastFrameId, + boost::shared_ptr additionalMeasurements, + int dontMargFrame) +{ + dmvio::TimeMeasurement timeMeasurement("addIMUData"); + if(lastFrameId < 0) + { + lastFrameId = frameId - 1; + } + + int keyframeId = currentKeyframeId; + + currCoarseTimestamp = frameTimestamp; + + // add symbols to graph: + gtsam::Pose3 currentPose = coarseValues->at(gtsam::Symbol('p', lastFrameId)); + gtsam::imuBias::ConstantBias currentBias = coarseValues->at( + gtsam::Symbol('b', lastFrameId)); + gtsam::Vector3 currentVelocity = coarseValues->at(gtsam::Symbol('v', lastFrameId)); + + // Select factors to marginalize out. We want to keep in the graph keyframe pose, previous and current states. We also want to keep the prepared keyframe pose + gtsam::FastVector keysToMarginalize; + gtsam::FastSet keysInGraph = coarseGraph->keys(); + + gtsam::FastSet setOfKeysToMarginalize; + + for(const gtsam::Key& k : keysInGraph) + { + gtsam::Symbol s(k); + + int idx = s.index(); + + if(s.chr() == 's') + continue; + + if(idx == frameId || idx == lastFrameId) + continue; + + if((idx == keyframeId || idx == dontMargFrame) && s.chr() == 'p') + continue; + + if(setOfKeysToMarginalize.find(k) == setOfKeysToMarginalize.end()) + { + keysToMarginalize.push_back(k); + setOfKeysToMarginalize.insert(k); + } + } + + if(!keysToMarginalize.empty()) + { + coarseGraph = marginalizeOut(*coarseGraph, *coarseValues, keysToMarginalize, nullptr, true); + } + + // Define keys to be used in this iteration + gtsam::Key poseKeyframeKey = gtsam::Symbol('p', + keyframeId); + + gtsam::Key poseCurrentKey = gtsam::Symbol('p', frameId); + gtsam::Key velCurrentKey = gtsam::Symbol('v', frameId); + gtsam::Key biasCurrentKey = gtsam::Symbol('b', frameId); + + gtsam::Key posePrevKey = gtsam::Symbol('p', lastFrameId); + gtsam::Key velPrevKey = gtsam::Symbol('v', lastFrameId); + gtsam::Key biasPrevKey = gtsam::Symbol('b', lastFrameId); + + // Integrate IMU data + boost::shared_ptr imuMeasurements; + if(additionalMeasurements) + { + imuMeasurements = additionalMeasurements; + }else + { + imuMeasurements.reset(new gtsam::PreintegratedImuMeasurements(preintegrationParams, currentBias)); + } + for(size_t i = 0; i < imuData.size(); i++) + { + auto& measurement = imuData[i]; + if(measurement.getIntegrationTime() == 0.0) continue; + imuMeasurements->integrateMeasurement(gtsam::Vector(measurement.getAccData()), + gtsam::Vector(measurement.getGyrData()), + measurement.getIntegrationTime()); + } + + // Create IMU factor. + gtsam::ImuFactor::shared_ptr imuFactor( + new gtsam::ImuFactor(posePrevKey, velPrevKey, + poseCurrentKey, velCurrentKey, biasPrevKey, + *imuMeasurements)); + + if(imuMeasurements->preintMeasCov().hasNaN() || imuFactor->noiseModel()->sigmas().hasNaN()) + { + std::cout << "Exiting because of bad measurement covariance." << std::endl; + exit(1); + } + + gtsam::noiseModel::Diagonal::shared_ptr biasNoiseModel = computeBiasNoiseModel(imuCalibration, *imuMeasurements); + + // Add bias random walk factor. + gtsam::NonlinearFactor::shared_ptr bias_factor( + new gtsam::BetweenFactor( + biasPrevKey, biasCurrentKey, + gtsam::imuBias::ConstantBias(gtsam::Vector3::Zero(), + gtsam::Vector3::Zero()), biasNoiseModel)); + + // In the coarse graph we optimize poses in metric frame (imu to world), so we don't need any PoseTransformationFactors. + // Instead, we transform the DSO Hessian to the metric frame. + coarseGraph->push_back(imuFactor); + coarseGraph->push_back(bias_factor); + + coarseValues->insert(poseCurrentKey, currentPose); + coarseValues->insert(velCurrentKey, currentVelocity); + coarseValues->insert(biasCurrentKey, currentBias); + + if(currentPose.matrix().hasNaN() || currentVelocity.hasNaN() || currentBias.vector().hasNaN()) + { + std::cout << "ERROR: NaNs in the system, exiting!" << std::endl; + exit(1); + } + + // Predict the new pose based on the IMU data (will be used as an initialization). + gtsam::LevenbergMarquardtOptimizer::shared_ptr optimizer( + new gtsam::LevenbergMarquardtOptimizer(*coarseGraph, *coarseValues)); + gtsam::Values optimizedValues = optimizer->optimize(); + gtsam::Values newValues; + for(gtsam::Values::iterator it = optimizedValues.begin(); it != optimizedValues.end(); ++it) + { + if(gtsam::Symbol((*it).key).index() == currentKeyframeId && imuSettings.fixKeyframeDuringCoarseTracking) + { + // Don't change the values of the keyframe... + newValues.insert(it->key, coarseValues->at(it->key)); + }else + { + newValues.insert(it->key, it->value); + } + } + *coarseValues = newValues; + + transformIMUToDSOForCoarse->updateWithValues(*coarseValues); + // Convert T_w_f to T_f_r: + Sophus::SE3d referenceToFrame( + transformIMUToDSOForCoarse->transformPose(coarseValues->at(poseCurrentKey).matrix())); + + currentPoseKey = poseCurrentKey; + refPoseKey = poseKeyframeKey; + + coarseOrdering.clear(); + coarseOrdering.push_back(refPoseKey); + coarseOrdering.push_back(currentPoseKey); + + gtsam::KeySet set; + set.insert(refPoseKey); + set.insert(currentPoseKey); + + for(const gtsam::Key& k : coarseGraph->keys()) + { + if(k != refPoseKey && k != currentPoseKey) + { + coarseOrdering.push_back(k); + set.insert(k); + } + } + + // The returned prediction will be used as an initialization for the coarse direct image alignment. + return referenceToFrame; +} + +Sophus::SE3d +dmvio::CoarseIMULogic::initCoarseGraph(int keyframeId, std::unique_ptr informationBAToCoarse) +{ + currentKeyframeId = keyframeId; + + gtsam::Key poseKey0 = gtsam::Symbol('p', keyframeId); + gtsam::Key velocityKey0 = gtsam::Symbol('v', keyframeId); + gtsam::Key biasKey0 = gtsam::Symbol('b', keyframeId); + + // Take over transforms from BA. + if(informationBAToCoarse) + { + transformBAToIMU = std::move(informationBAToCoarse->transformBAToIMU); + transformIMUToDSOForCoarse = std::move(informationBAToCoarse->transformIMUToDSOForCoarse); + scale = informationBAToCoarse->latestBAScale; + } + + coarseGraph.reset(new gtsam::NonlinearFactorGraph()); + coarseValues.reset(new gtsam::Values); + + currentKeyframeId = keyframeId; + + gtsam::Vector3 initialVelocity = (gtsam::Vector(3) << 0, 0, 0).finished(); + gtsam::imuBias::ConstantBias initialBias = gtsam::imuBias::ConstantBias(); + gtsam::Pose3 poseFromBA; + bool gotBABias = !(imuSettings.skipFirstKeyframe && firstCoarseInit); + + if(informationBAToCoarse) + { + initialVelocity = informationBAToCoarse->latestBAVel; + initialBias = informationBAToCoarse->latestBABias; + poseFromBA = informationBAToCoarse->latestBAPose; + } + + // Transform DSO pose to IMU. + gtsam::Pose3 initialPose(transformBAToIMU->transformPose(poseFromBA.matrix())); + + // Add pose prior on the keyframe + double rotVariance = imuSettings.baToCoarseRotVariance; + double poseVariance = imuSettings.baToCoarsePoseVariance; + double velVariance = imuSettings.baToCoarseVelVariance; + double accBiasVariance = imuSettings.baToCoarseAccBiasVariance; + double gyrBiasVariance = imuSettings.baToCoarseGyrBiasVariance; + + gtsam::noiseModel::Diagonal::shared_ptr pose_prior_model = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) + << rotVariance, rotVariance, rotVariance, poseVariance, poseVariance, poseVariance).finished()); + coarseGraph->add(gtsam::PriorFactor(poseKey0, initialPose, pose_prior_model)); + + // Add prior on bias and velocity. + if(gotBABias) + { + if(imuSettings.setting_transferCovToCoarse) + { + coarseGraph->add(informationBAToCoarse->priorFactor); + }else + { + gtsam::noiseModel::Diagonal::shared_ptr vel_prior_model = gtsam::noiseModel::Diagonal::Variances( + (gtsam::Vector(3) << velVariance, velVariance, velVariance).finished()); + coarseGraph->add(gtsam::PriorFactor(velocityKey0, initialVelocity, vel_prior_model)); + + gtsam::noiseModel::Diagonal::shared_ptr bias_prior_model = gtsam::noiseModel::Diagonal::Variances( + (gtsam::Vector(6) + << accBiasVariance, accBiasVariance, accBiasVariance, gyrBiasVariance, gyrBiasVariance, gyrBiasVariance).finished()); + coarseGraph->add( + gtsam::PriorFactor(biasKey0, initialBias, bias_prior_model)); + } + } + + coarseValues->insert(poseKey0, initialPose); + coarseValues->insert(velocityKey0, initialVelocity); + coarseValues->insert(biasKey0, initialBias); + + firstCoarseInit = false; + + // BA gtsam poses are cam to world + gtsam::Pose3 lastKFToCurr; + if(informationBAToCoarse) + { + lastKFToCurr = informationBAToCoarse->latestBAPose.inverse() * informationBAToCoarse->latestBAPosePrevKeyframe; + } + return Sophus::SE3d(lastKFToCurr.matrix()); +} + +Sophus::SE3d +dmvio::CoarseIMULogic::computeCoarseUpdate(const dso::Mat88& H_in, const dso::Vec8& b_in, float extrapFac, float lambda, + double& incA, double& incB, double& incNorm) +{ + dmvio::TimeMeasurement timeMeasurement("computeCoarseUpdate"); + + PoseTransformation& transformIMUToCoarse = *transformIMUToDSOForCoarse; + transformIMUToCoarse.updateWithValues(*coarseValues); // Set reference pose. + // Convert Hessian and b to absolute poses. + auto dsoHAndB = convertCoarseHToGTSAM(transformIMUToCoarse, H_in * imuSettings.setting_weightDSOCoarse, + b_in * imuSettings.setting_weightDSOCoarse, + coarseValues->at(currentPoseKey)); + + // Linearize factor graph. + gtsam::GaussianFactorGraph::shared_ptr gfg = coarseGraph->linearize(*coarseValues); + std::map keyDimMap = gfg->getKeyDimMap(); + + std::pair gtsamHAndB = gfg->hessian(coarseOrdering); + + int nrowsGT = gtsamHAndB.first.rows(); + gtsam::Matrix HComplete(nrowsGT + 2, nrowsGT + 2); + gtsam::Vector bComplete(nrowsGT + 2); + + HComplete.block(2, 2, nrowsGT, nrowsGT) = gtsamHAndB.first; // Fill correct part with the matrix from GTSAM + HComplete.block(0, 0, nrowsGT + 2, 2) = gtsam::zeros(nrowsGT + 2, 2); // Fill the rest with zeros. + HComplete.block(0, 2, 2, nrowsGT) = gtsam::zeros(2, nrowsGT); + + // Add DSO part of the Hessian. + HComplete.block(0, 0, 14, 14) += dsoHAndB.first; + + bComplete.segment(0, 2) = gtsam::zeros(2, 1); + bComplete.segment(2, nrowsGT) = -gtsamHAndB.second; // The b in GTSAM resembles -b in DSO! + bComplete.segment(0, 14) += dsoHAndB.second; + + // Use lambda multiplication... + for(int i = 0; i < nrowsGT + 2; i++) HComplete(i, i) *= (1 + lambda); + + // -------------------------------------------------- + // Compute update step + // -------------------------------------------------- + gtsam::Vector inc = HComplete.ldlt().solve(-bComplete); + + inc *= extrapFac; + + if(imuSettings.fixKeyframeDuringCoarseTracking) + { + // GTSAM Pose contains first rotation, then translation -> only remove the translational part. + inc.segment(5, 3) = gtsam::zeros(3, 1); + } + + // Apply update. + newCoarseValues.reset(new gtsam::Values()); + int current_pos = 2; + for(size_t i = 0; i < coarseOrdering.size(); i++) + { + gtsam::Key k = coarseOrdering[i]; + size_t s = keyDimMap[k]; + newCoarseValues->insert(k, *(coarseValues->at(k).retract_(inc.segment(current_pos, s)))); + current_pos += s; + } + + // Compute increment, norm, and updated relative pose for CoarseTracker. + incA = inc(0); + incB = inc(1); + + incNorm = inc.norm(); + transformIMUToCoarse.updateWithValues(*newCoarseValues); // Set reference pose. + Sophus::SE3d newReferenceToFrame( + transformIMUToCoarse.transformPose(newCoarseValues->at(currentPoseKey).matrix())); + + return newReferenceToFrame; + +} + +Sophus::SE3d dmvio::CoarseIMULogic::getCoarseKFPose() +{ + return Sophus::SE3d(coarseValues->at(gtsam::Symbol('p', currentKeyframeId)).matrix()); +} + +void dmvio::CoarseIMULogic::updateCoarsePose(const Sophus::SE3& refToFrame) +{ + // GTSAM expects currentImu to world, we passed referenceCamera to currentCamera. + PoseTransformation& transformIMUToCoarse = *transformIMUToDSOForCoarse; + transformIMUToCoarse.updateWithValues(*coarseValues); // Set reference pose. + + gtsam::Pose3 currentIMUToWorld(transformIMUToCoarse.transformPoseInverse(refToFrame.matrix())); + + eraseAndInsert(coarseValues, currentPoseKey, currentIMUToWorld); +} + +void dmvio::CoarseIMULogic::acceptCoarseUpdate() +{ + coarseValues = newCoarseValues; +} + +// Our factor graph contains (and marginalizes old frames), so we need to add the linearized direct image alignment factor. +void dmvio::CoarseIMULogic::addVisualToCoarseGraph(const dso::Mat88& H, const dso::Vec8& b, bool trackingIsGood) +{ + if(!imuSettings.addVisualToCoarseGraphIfTrackingBad && !trackingIsGood) return; + + PoseTransformation& transformIMUToCoarse = *transformIMUToDSOForCoarse; + transformIMUToCoarse.updateWithValues(*coarseValues); // Set reference pose. + auto dsoHAndB = convertCoarseHToGTSAM(transformIMUToCoarse, H * imuSettings.setting_weightDSOCoarse, + b * imuSettings.setting_weightDSOCoarse, + coarseValues->at(currentPoseKey)); + gtsam::Matrix HFull = std::move(dsoHAndB.first); + gtsam::Vector bFull = std::move(dsoHAndB.second); + + bFull = -bFull; // The b in GTSAM resembles -b in DSO! + + // Marginalize out a, b as they shall not be included in the factor graph... + gtsam::Matrix Hmm = HFull.block(0, 0, 2, 2); + gtsam::Matrix Hma = HFull.block(0, 2, 2, 12); + gtsam::Matrix Haa = HFull.block(2, 2, 12, 12); + + gtsam::Vector bm = bFull.segment(0, 2); + gtsam::Vector ba = bFull.segment(2, 12); + + gtsam::Matrix HmmInv = Hmm.completeOrthogonalDecomposition().pseudoInverse(); + + gtsam::Matrix HaaNew = Haa - Hma.transpose() * HmmInv * Hma; + gtsam::Vector baNew = ba - Hma.transpose() * HmmInv * bm; + + gtsam::LinearContainerFactor::shared_ptr lcf(new gtsam::LinearContainerFactor( + gtsam::HessianFactor(refPoseKey, currentPoseKey, HaaNew.block(0, 0, 6, 6), HaaNew.block(0, 6, 6, 6), + baNew.segment(0, 6), HaaNew.block(6, 6, 6, 6), baNew.segment(6, 6), 0), + *coarseValues)); + + coarseGraph->add(lcf); +} + +gtsam::imuBias::ConstantBias dmvio::CoarseIMULogic::getBias(int frameId) +{ + gtsam::imuBias::ConstantBias currentBias; + if(coarseValues) + { + currentBias = coarseValues->at(gtsam::Symbol('b', frameId)); + } + return currentBias; +} + +gtsam::Vector3 dmvio::CoarseIMULogic::getVelocity(int frameId) +{ + gtsam::Vector3 velocity = gtsam::Vector3::Identity(); + if(coarseValues) + { + velocity = coarseValues->at(gtsam::Symbol('v', frameId)); + } + return velocity; +} + + +void dmvio::CoarseIMULogic::printCoarseBiases(const dmvio::GTData* gtData, int frameId) +{ + if(gtData && coarseValues) + { + gtsam::imuBias::ConstantBias currentBias = coarseValues->at( + gtsam::Symbol('b', frameId)); + Eigen::Vector3d gtTrans = gtData->biasTranslation; + Eigen::Vector3d gtRot = gtData->biasRotation; + Eigen::Vector3d errorTrans = currentBias.accelerometer() - gtTrans; + Eigen::Vector3d errorRot = currentBias.gyroscope() - gtRot; + + coarseBiasFile << std::fixed << std::setprecision(6) << currCoarseTimestamp << ' ' << std::setprecision(20) + << gtTrans.transpose() << ' ' << gtRot.transpose() << ' ' + << currentBias.accelerometer().transpose() << ' ' << currentBias.gyroscope().transpose() << '\n'; + } +} + +double dmvio::CoarseIMULogic::getScale() const +{ + return scale; +} + diff --git a/src/IMU/CoarseIMULogic.h b/src/IMU/CoarseIMULogic.h new file mode 100644 index 0000000..ba1cff6 --- /dev/null +++ b/src/IMU/CoarseIMULogic.h @@ -0,0 +1,120 @@ +/** +* This file is part of DM-VIO. +* The code in this file is in part based on code written by Vladyslav Usenko for the paper "Direct Visual-Inertial Odometry with Stereo Cameras". +* +* Copyright (c) 2022 Lukas von Stumberg , Vladyslav Usenko +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_COARSEIMULOGIC_H +#define DMVIO_COARSEIMULOGIC_H + +#include "IMUTypes.h" +#include "IMUSettings.h" +#include "BAIMULogic.h" + +#include +#include +#include +#include + +namespace dmvio +{ +// Contains logic how to integrate IMU data into the coarse tracking (see CoarseTracker.h) +// To this end it changes the optimized variables from the relative pose from referenceToFrame to absolute poses +// in IMU frame. It adds IMU-Factors between successive frames. +class CoarseIMULogic +{ +public: + // Typically initCoarseGraph should also be called before using this. + // Note that a reference to imuCalibration and imuSettings is kept, so they need to be kept alive. + CoarseIMULogic(std::unique_ptr transformBAToIMU, + boost::shared_ptr preintegrationParams, + const IMUCalibration& imuCalibration, + IMUSettings& imuSettings); + + // (Re-)initialize the coarse tracking graph after a new reference frame has been activated. + Sophus::SE3d initCoarseGraph(int keyframeId, std::unique_ptr informationBAToCoarse); + + // Adds an new frame with IMU data to the coarse factor graph, marginalizes old variables, and returns an estimate + // for the relative pose of the newly added frame. + // dontMargFrame is the id of a frame (usually a prepared KF) which should not be marginalized. + Sophus::SE3d addIMUData(const IMUData& imuData, + int frameId, double frameTimestamp, + int lastFrameId, + boost::shared_ptr additionalMeasurements = nullptr, + int dontMargFrame = -1); + + // ------------------------------ + // Called by CoarseTracker (at the moment they are forwarded through IMUIntegration): + + // Passes the new coarse pose. + void updateCoarsePose(const Sophus::SE3d& pose); + + // This method integrates the CoarseTracker optimization with GTSAM. It is called in each iteration, and + // will compute the increment for the optimization iteration. + // returns new refToFrame, gets H and b as an input. incA and incB are output parameters that contain the + // increment of the affine lightning transforms after the method call, incNorm is the norm of the increment. + // b contains the following parameters: 3 for the rotation ref_to_frame, 3 for the translation ref_to_frame, and + // 2 for affine lightning parameters. + Sophus::SE3d computeCoarseUpdate(const dso::Mat88& H, const dso::Vec8& b, float extrapFac, float lambda, + double& incA, double& incB, double& incNorm); + + // Apply the update computed by the last call of computeCoarseUpdate. + void acceptCoarseUpdate(); + + // Add linearized visual factor to the coarse graph. + void addVisualToCoarseGraph(const dso::Mat88& H, const dso::Vec8& b, bool trackingIsGood); + + + Sophus::SE3d getCoarseKFPose(); + gtsam::imuBias::ConstantBias getBias(int frameId); + gtsam::Vector3 getVelocity(int frameId); + void printCoarseBiases(const dmvio::GTData* gtData, int frameId); + double getScale() const; + +private: + // Shared with parent IMUIntegration. + IMUSettings& imuSettings; + const IMUCalibration& imuCalibration; + + std::shared_ptr transformBAToIMU; + // Usually of type TransformDSOToIMU + std::unique_ptr transformIMUToDSOForCoarse; // Usually of type TransformIMUToDSOForCoarse + double scale = 1.0; + + boost::shared_ptr preintegrationParams; + + boost::shared_ptr coarseGraph; + boost::shared_ptr coarseValues; + + gtsam::Key currentPoseKey, refPoseKey; + + gtsam::Ordering coarseOrdering; + gtsam::Values::shared_ptr newCoarseValues; + + int currentKeyframeId = -1; + double currCoarseTimestamp; + bool firstCoarseInit = true; + + std::ofstream coarseBiasFile; +}; + + +} +#endif //DMVIO_COARSEIMULOGIC_H diff --git a/src/IMU/IMUIntegration.cpp b/src/IMU/IMUIntegration.cpp new file mode 100644 index 0000000..02a880d --- /dev/null +++ b/src/IMU/IMUIntegration.cpp @@ -0,0 +1,389 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "IMUIntegration.hpp" + +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +#include "FullSystem/HessianBlocks.h" +#include "dso/util/FrameShell.h" + + +#include +#include +#include "GTSAMIntegration/ExtUtils.h" +#include "IMUUtils.h" +#include "GTSAMIntegration/DelayedMarginalization.h" + +using namespace dmvio; +using std::cout; +using std::endl; + +IMUIntegration::IMUIntegration(dso::CalibHessian* HCalib, const IMUCalibration& imuCalibrationPassed, + IMUSettings& imuSettingsPassed, bool linearizeOperationPassed) + : linearizeOperation(linearizeOperationPassed), preparedKeyframe(-1), preparedKFCreated(false), + imuCalibration(imuCalibrationPassed), imuSettings(imuSettingsPassed) +{ + // Create preintegrationParams + double accelVar = imuCalibration.accel_sigma * imuCalibration.accel_sigma; + double gyroVar = imuCalibration.gyro_sigma * imuCalibration.gyro_sigma; + double integrationVar = imuCalibration.integration_sigma * imuCalibration.integration_sigma; + + // -------------------------------------------------- + preintegrationParams.reset(new gtsam::PreintegrationParams(imuCalibrationPassed.gravity)); + preintegrationParams->setIntegrationCovariance(integrationVar * Eigen::Matrix3d::Identity()); + preintegrationParams->setAccelerometerCovariance(accelVar * Eigen::Matrix3d::Identity()); + preintegrationParams->setGyroscopeCovariance(gyroVar * Eigen::Matrix3d::Identity()); + + + // Create Delayed Marginalization Graphs. + std::unique_ptr baGraphs; + DelayedMarginalizationGraphs* delayedGraphs = new DelayedMarginalizationGraphs(0, BAIMULogic::METRIC_GROUP); + baGraphs.reset(delayedGraphs); + + // Create BAGTSAMIntegration. + // Pass empty transformation, because DSO and baGraph have the same coordinate system (except for the side of epsilon). + std::unique_ptr transformationDSOToBA(new TransformIdentity()); + GTSAMIntegrationSettings baGTSAMSettings; + baGTSAMSettings.weightDSOToGTSAM = imuSettings.setting_weightDSOToGTSAM; + baGTSAMIntegration.reset( + new BAGTSAMIntegration(std::move(baGraphs), std::move(transformationDSOToBA), baGTSAMSettings, HCalib)); + + // Create classes handling the IMUIntegration in BA and Coarse tracking respectively. + baLogic.reset(new BAIMULogic(this, baGTSAMIntegration.get(), imuCalibration, imuSettings)); + std::unique_ptr coarsePoseTransformation = baLogic->getTransformDSOToIMU()->clone(); + coarseLogic.reset( + new CoarseIMULogic(std::move(coarsePoseTransformation), preintegrationParams, imuCalibration, imuSettings)); + + if(!imuSettings.initSettings.disableVIOUntilFirstInit) // Only add the extension right away if we start with VIO immediately. + { + baGTSAMIntegration->addExtension(baLogic); + coarseInitialized = true; + baInitialized = true; + } + + // IMUInitializer: + imuInitializer.reset(new IMUInitializer(imuSettings.resultsPrefix, preintegrationParams, imuCalibration, + imuSettings.initSettings, delayedGraphs, linearizeOperation, + [this](const gtsam::Values& values, bool willReplaceGraph) + { + // Callback called upon IMU initialization. + bool reinit = baInitialized; + if(!reinit) + { + baGTSAMIntegration->addExtension(baLogic); + } + baLogic->initFromIMUInit(values, reinit, willReplaceGraph); + baInitialized = true; // Note: not threadsafe for RT yet if we + // initialize from CoarseIMUInit (which we do not do in the normal + // transition mode). + })); + + // -------------------------------------------------- + TS_cam_imu = imuCalibration.T_cam_imu; + + preintegratedBA.reset(new gtsam::PreintegratedImuMeasurements(preintegrationParams)); + preintegratedBACurr.reset(new gtsam::PreintegratedImuMeasurements(preintegrationParams)); +} + +// return lastKeyframe to newKeyframe. +Sophus::SE3d IMUIntegration::initCoarseGraph() +{ + std::cout << "Prepared keyframe id: " << preparedKeyframe << std::endl; + int keyframeId = preparedKeyframe; + preparedKeyframe = -1; + + if(!informationBAToCoarse) + { + return Sophus::SE3d{}; + } + coarseInitialized = true; + return coarseLogic->initCoarseGraph(keyframeId, std::move(informationBAToCoarse)); +} + +// updateBAValues should be called before this... +void IMUIntegration::finishKeyframeOperations(int keyframeId) +{ + if(baInitialized) + { + dmvio::TimeMeasurement timeMeasurement("IMUIntegration::finishKeyframeOperations"); + + // Forward to baLogic which does the work. + baLogic->finishKeyframeOperations(keyframeId); + } + if(imuInitializer) + { + imuInitializer->initializeIfReady(); + } +} + +void IMUIntegration::addIMUDataToBA(const IMUData& imuData) +{ + dmvio::TimeMeasurement timeMeasurement("addIMUDataToBA"); + integrateIMUData(imuData, *preintegratedBACurr); + lastIMUData = imuData; +} + +// returns estimated referenceToFrame. +Sophus::SE3 IMUIntegration::addIMUData(const IMUData& imuData, int frameId, double frameTimestamp, + bool firstFrameAfterKFChange, + int lastFrameId, bool onlyForHint) +{ + boost::shared_ptr additionalMeasurements; + if(firstFrameAfterKFChange) + { + additionalMeasurements = preintegratedForNextCoarse; + preintegratedForNextCoarse.reset(); + } + + if(preparedKeyframe != -1 && !onlyForHint) + { + // Currently a new bundle adjustment is in progress -> Also put the imu data into the preintegration for the next coarse tracking. + if(imuData.size() > 0) + { + integrateIMUData(imuData, *preintegratedForNextCoarse); + imuDataPreintegrated = true; + } + } + + if(imuInitializer) + { + imuInitializer->addIMUData(imuData, frameId); + } + + if(!isCoarseInitialized()) return Sophus::SE3d{}; + + return coarseLogic->addIMUData(imuData, frameId, frameTimestamp, lastFrameId, additionalMeasurements, + preparedKeyframe); +} + + +void IMUIntegration::updateCoarsePose(const Sophus::SE3& refToFrame) +{ + if(!coarseInitialized) return; + coarseLogic->updateCoarsePose(refToFrame); +} + +Sophus::SE3 +IMUIntegration::computeCoarseUpdate(const dso::Mat88& H_in, const dso::Vec8& b_in, float extrapFac, float lambda, + double& incA, double& incB, double& incNorm) +{ + + assert(isCoarseInitialized()); // Caller is responsible for not calling if not initialized. + Sophus::SE3d newReferenceToFrame = coarseLogic->computeCoarseUpdate(H_in, b_in, extrapFac, lambda, incA, incB, + incNorm); + + return newReferenceToFrame; +} + +void IMUIntegration::acceptCoarseUpdate() +{ + if(!isCoarseInitialized()) return; + coarseLogic->acceptCoarseUpdate(); +} + +void IMUIntegration::addVisualToCoarseGraph(const dso::Mat88& H, const dso::Vec8& b, bool trackingIsGood) +{ + if(!isCoarseInitialized()) return; + coarseLogic->addVisualToCoarseGraph(H, b, trackingIsGood); +} + +void IMUIntegration::setGTData(dmvio::GTData* gtData, int frameId) +{ + dmvio::TimeMeasurement timeMeasurement("printBiases"); + + // This must be done only for linearizeOperation! (because otherwise it's not the correct gtData for the KF + it might be deleted...) + baLogic->setCurrGtData(gtData, frameId); + + // Note: At the moment coarse biases are only printed for frames that will become a KF (because otherwise this method is not called). + coarseLogic->printCoarseBiases(gtData, frameId); +} + +IMUIntegration::~IMUIntegration() = default; + +// Called in the coarse tracking thread. +// This contains code relevant for both, CoarseTracking and BA. Mainly to make it work in realtime mode. +void IMUIntegration::prepareKeyframe(int frameId) +{ + bool previouslyPrepared = false; // The last frame was already prepared to be a KF. + + // Make sure that the previous keyframe was finished! + if(preparedKeyframe != -1 && !linearizeOperation) + { + std::cout << "Note: there is already a keyframe prepared! " << preparedKeyframe << std::endl; + assert(frameId == preparedKeyframe + 1); + assert(!preparedKFCreated); + + previouslyPrepared = true; + } + assert(!linearizeOperation || preparedKeyframe == -1); + + preparedKeyframe = frameId; + preparedKFCreated = false; + + gtsam::imuBias::ConstantBias currentBias = coarseLogic->getBias(frameId); + preparedCoarseVel = coarseLogic->getVelocity( + frameId); // We can call this without mutex because we are in the coarse tracking thread. + + preintegratedForNextCoarse.reset(new gtsam::PreintegratedImuMeasurements(preintegrationParams, currentBias)); + imuDataPreintegrated = false; + + // We solve that sometimes a new keyframe shall be prepared even though the old one hasn't finished BA yet. + // There are two ways this can happen: + // 1. The old keyframe is currently in BA and the system already needs a new KF. + // 2. The system has just prepared the previous preparedKeyframe, but hasn't started BA yet. In this case, normal DSO + // would optimize the new frame and the old preparedKeyframe would not become a KF. + + if(previouslyPrepared) + { + // If the last KF was also prepared the two buffers were already swapped. Then the latest IMU-data has to be added to the current buffer. + // In the multithreaded case we need the preintegratedBACurr + integrateIMUData(lastIMUData, *preintegratedBA); + }else + { + boost::shared_ptr swap = preintegratedBA; + preintegratedBA = preintegratedBACurr; + preintegratedBACurr = swap; + } + // Take latest bias from BA. Maybe this should actually locked. Currently however the BA must be finished + // already, as else the assertion would have failed. Therefore the latestBias cannot be changed right now. + preintegratedBACurr->resetIntegrationAndSetBias(latestBias); +} + +const gtsam::PreintegratedImuMeasurements& IMUIntegration::getPreintegratedMeasurements(int keyframeId) +{ + assert(linearizeOperation || keyframeId == preparedKeyframe); + + return *preintegratedBA; +} + +// called right before finishKeyframeOptimization, but outside the mutex. +void IMUIntegration::postOptimization(int keyframeId) +{ + initializedBeforePostOptimization = baInitialized; + if(initializedBeforePostOptimization) + { + baLogic->postOptimization(keyframeId); + } + if(imuInitializer) + { + imuInitializer->postBAInit(keyframeId, baGTSAMIntegration->getActiveDSOFactor(), + *(baGTSAMIntegration->getBaValues()), baGTSAMIntegration->getCurrBaTimestamp(), + getPreintegratedMeasurements(keyframeId)); + } +} + +// This method gets the latest BA values for the coarse tracker (for the next graph). +void IMUIntegration::finishKeyframeOptimization(int keyframeId) +{ + if(!initializedBeforePostOptimization) + { + // Get latestBias from initializer until initialized + latestBias = imuInitializer->getLatestBias(); + return; + } + informationBAToCoarse = baLogic->finishKeyframeOptimization(keyframeId); + latestBias = informationBAToCoarse->latestBABias; +} + +bool IMUIntegration::newTrackingRefNeedsHandling() +{ + return imuDataPreintegrated; +} + +int IMUIntegration::getPreparedKeyframe() const +{ + return preparedKeyframe; +} + +void IMUIntegration::skipPreparedKeyframe() +{ + preparedKeyframe = -1; +} + +void IMUIntegration::keyframeCreated(int frameId) +{ + assert(frameId == preparedKeyframe); + preparedKFCreated = true; + // This is locked with mutex, meaning that there cannot be a BA, or a call to prepareKeyframe at the same time. + baLogic->setNextBAVel(preparedCoarseVel, preparedKeyframe); +} + +bool IMUIntegration::isPreparedKFCreated() const +{ + return preparedKFCreated; +} + +Sophus::SE3d IMUIntegration::getCoarseKFPose() +{ + if(!isCoarseInitialized()) return Sophus::SE3d{}; + Sophus::SE3d pose = coarseLogic->getCoarseKFPose(); + return pose; +} + +IMUSettings& IMUIntegration::getImuSettings() const +{ + return imuSettings; +} + +const std::unique_ptr& IMUIntegration::getBAGTSAMIntegration() const +{ + return baGTSAMIntegration; +} + +TransformDSOToIMU& IMUIntegration::getTransformDSOToIMU() +{ + return *(baLogic->getTransformDSOToIMU()); +} + +void IMUIntegration::newFrameEnergyTH(float& energyThreshold) +{ + lastDSOEnergyTH = energyThreshold; + float th = imuSettings.maxFrameEnergyThreshold; + if(th > 0.0 && energyThreshold > th) + { + energyThreshold = th; + } +} + +void IMUIntegration::finishCoarseTracking(const dso::FrameShell& frameShell, bool willBecomeKeyframe) +{ + if(imuInitializer) + { + imuInitializer->addPose(frameShell, willBecomeKeyframe); + } +} + +bool IMUIntegration::isCoarseInitialized() +{ + return coarseInitialized; +} + +void IMUIntegration::resetBAPreintegration() +{ + preintegratedBACurr->resetIntegration(); +} + +double IMUIntegration::getCoarseScale() +{ + return coarseLogic->getScale(); +} \ No newline at end of file diff --git a/src/IMU/IMUIntegration.hpp b/src/IMU/IMUIntegration.hpp new file mode 100644 index 0000000..8553715 --- /dev/null +++ b/src/IMU/IMUIntegration.hpp @@ -0,0 +1,208 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef IMUIntegration_hpp +#define IMUIntegration_hpp + +#include +#include +#include + +#include "IMUTypes.h" +#include "IMUSettings.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "dso/util/NumType.h" + +#include "OptimizationBackend/EnergyFunctional.h" +#include "util/GTData.hpp" +#include "BAIMULogic.h" +#include +#include + +#include "CoarseIMULogic.h" +#include "IMUInitialization/IMUInitializer.h" + +namespace dmvio +{ + + +// Main class for the integration of IMU data into DSO. Mostly a facade as the implementation logic is forwarded to +// either CoarseIMULogic (for the coarse tracking), BAIMULogic (for the integration of IMU into the main bundle +// adjustment), and IMUInitializer (for the IMU initialization). +// This class is responsible for making sure that the preintegration and interactions between CoarseIMULogic +// and BAIMULogic work correctly in realtime mode (where they are in different threads). +class IMUIntegration : public PreintegrationProviderBA +{ +public: + // linearizeOperation is true in non-realtime mode (means that there is only a single thread used). + // Note that a reference to imuSettings is kept, so it needs to stay alive. + IMUIntegration(dso::CalibHessian* HCalib, const IMUCalibration& imuCalibrationPassed, + IMUSettings& imuSettingsPassed, bool linearizeOperationPassed); + + ~IMUIntegration(); + + // Return true if the CoarseTracking logic is initialized. + bool isCoarseInitialized(); + + // called when the coarse tracking reference is switched. + // Returns transformation from last keyframe to new keyframe. + Sophus::SE3d initCoarseGraph(); + + // Called to add IMU data for the coarse tracking (and the IMU initializer). + // Adds a new frame with IMU data to the coarse factor graph, marginalizes old variables, and returns an estimate + // for the relative pose of the newly added frame. + Sophus::SE3 addIMUData(const IMUData& imuData, + int frameId, double frameTimestamp, bool firstFrameAfterKFChange, + int lastFrameId, bool onlyForHint = false); + + + // Add IMU data for the bundle adjustment + void addIMUDataToBA(const IMUData& imuData); + + // Called when the first initializer frame changes. + void resetBAPreintegration(); + + // Passes the new coarse tracking pose. + void updateCoarsePose(const Sophus::SE3& pose); + + // This method integrates the CoarseTracker optimization with GTSAM. It is called in each iteration, and + // will compute the increment for the optimization iteration. + // returns new refToFrame, gets H and b as an input. incA and incB are output parameters that contain the + // increment of the affine lightning transforms after the method call, incNorm is the norm of the increment. + // b contains the following parameters: 3 for the rotation ref_to_frame, 3 for the translation ref_to_frame, and + // 2 for affine lightning parameters. + Sophus::SE3 computeCoarseUpdate(const dso::Mat88& H, const dso::Vec8& b, float extrapFac, float lambda, + double& incA, double& incB, double& incNorm); + + // Apply the update computed by the last call of computeCoarseUpdate. + void acceptCoarseUpdate(); + + void addVisualToCoarseGraph(const dso::Mat88& H, const dso::Vec8& b, bool trackingIsGood); + + // Returns the pose of the current keyframe as computed by the coarse tracking as a gtsam Pose (imu to world) + Sophus::SE3d getCoarseKFPose(); + + // Called when DSO finishes coarse tracking. + void finishCoarseTracking(const dso::FrameShell& frameShell, bool willBecomeKeyframe); + + // prepareKeyframe tells the IMU-Integration that this frame will probably become a keyframe. (-> don' marginalize it during addIMUData...) + // Also resets the IMU preintegration for the BA. + void prepareKeyframe(int frameId); + + virtual const gtsam::PreintegratedImuMeasurements& getPreintegratedMeasurements(int keyframeId) override; + + bool isPreparedKFCreated() const; + + int getPreparedKeyframe() const; + + // tells the IMU-Integration that this frame has become a keyframe that will shortly be bundle-adjusted. + void keyframeCreated(int frameId); + + void skipPreparedKeyframe(); + + bool newTrackingRefNeedsHandling(); + + // These methods are both called right after the BA optimization, the former outside and the latter inside the + // mutex for changing the coarse tracking ref. + void postOptimization(int keyframeId); + void finishKeyframeOptimization(int keyframeId); // updateBAValues should be called before this. + + // Called when all keyframe operations (including marginalization) are finished. + // Note that this can be after the tracking reference has already changed + // e.g. the order can be: BA finished (finishKeyframeOptimization) -> tracking reference changed -> in other + // thread marginalization and post BA stuff -> finishKeyframeOperations() + void finishKeyframeOperations(int keyframeId); + + Sophus::SE3 TS_cam_imu; + + // Sets groundtruth data for a frame for printing out information. Should only be used in non-rt mode as it currently + // does not handle multiple threads correctly. + void setGTData(dmvio::GTData* gtData, int frameId); + + IMUSettings& getImuSettings() const; + + // Called when a new energy threshold is computed by DSO. Can optionally update the threshold. + // We use this to make sure the energy threshold cannot get arbitrarily high (and allow extremely bad points + // into the optimization). + void newFrameEnergyTH(float& energyThreshold); + + const std::unique_ptr& getBAGTSAMIntegration() const; + + // Return current transform between IMU and DSO frame (updated during the BA). Should only be called from BA thread. + TransformDSOToIMU& getTransformDSOToIMU(); + // Get the scale of TransformDSOToIMU used for the coarse tracking currently (can be called from any thread). + double getCoarseScale(); + +private: + IMUCalibration imuCalibration; + IMUSettings& imuSettings; + + bool linearizeOperation; + + boost::shared_ptr preintegrationParams; + + boost::shared_ptr preintegratedBA, preintegratedBACurr, preintegratedForNextCoarse; + + int preparedKeyframe; + gtsam::Vector3 preparedCoarseVel; + bool preparedKFCreated; + + // Changed (only) in the coarse tracker's thread. + bool imuDataPreintegrated = false; + + IMUData lastIMUData; + + std::shared_ptr baLogic; + std::unique_ptr coarseLogic; + std::unique_ptr baGTSAMIntegration; + gtsam::imuBias::ConstantBias latestBias; + + std::unique_ptr imuInitializer; + + // These variables are set when the BA optimization is finished, so the coarse tracking can later initialize with these values. + std::unique_ptr informationBAToCoarse; + + bool baInitialized = false; + bool coarseInitialized = false; + bool initializedBeforePostOptimization = false; + + float lastDSOEnergyTH; +}; + +} + +#endif /* IMUIntegration_hpp */ + + diff --git a/src/IMU/IMUSettings.cpp b/src/IMU/IMUSettings.cpp new file mode 100644 index 0000000..6591ad1 --- /dev/null +++ b/src/IMU/IMUSettings.cpp @@ -0,0 +1,156 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "IMUSettings.h" +#include "yaml-cpp/yaml.h" +#include + +using namespace dmvio; + +IMUCalibration::IMUCalibration() +{ + initDefault(); +} + +// Directly pass the camchain.yaml (EuRoC format). +IMUCalibration::IMUCalibration(std::string settingsFilename) +{ + initDefault(); + + loadFromFile(settingsFilename); +} + +void IMUCalibration::initDefault() +{ + // Default Settings are for EuRoC. + // Init T_cam_imu + Eigen::Matrix3d m; + Eigen::Vector3d p; + m << 0.0148655429818, -0.999880929698, 0.00414029679422, + 0.999557249008, 0.0149672133247, 0.025715529948, + -0.0257744366974, 0.00375618835797, 0.999660727178; + p << -0.0216401454975, -0.064676986768, 0.00981073058949; + + Sophus::SE3d imu_cam(m, p); + T_cam_imu = imu_cam.inverse(); +} + +void IMUCalibration::registerArgs(dmvio::SettingsUtil& set) +{ + set.registerArg("accelerometer_random_walk", sigma_between_b_a); + set.registerArg("gyroscope_random_walk", sigma_between_b_g); + set.registerArg("accelerometer_noise_density", accel_sigma); + set.registerArg("gyroscope_noise_density", gyro_sigma); + set.registerArg("integration_sigma", integration_sigma); +} + +void IMUCalibration::loadFromFile(std::string settingsFilename) +{ + if(settingsFilename == "") + { + return; + } + + std::cout << "Loading IMU parameter file at: " << settingsFilename << std::endl; + YAML::Node config = YAML::LoadFile(settingsFilename)["cam0"]; + std::vector > theVector = config["T_cam_imu"].as > >(); + Eigen::Matrix4d matrix; + for(int x = 0; x < 4; ++x) + { + for(int y = 0; y < 4; ++y) + { + matrix(x, y) = theVector[x][y]; + } + } + std::cout << "Used T_cam_imu: " << std::endl << matrix << std::endl; + T_cam_imu = Sophus::SE3d(matrix); + + if(config["accelerometer_random_walk"]) + { + sigma_between_b_a = config["accelerometer_random_walk"].as(); // * 0.5; + } + if(config["gyroscope_random_walk"]) + { + sigma_between_b_g = config["gyroscope_random_walk"].as();// * 0.5; + } + if(config["accelerometer_noise_density"]) + { + accel_sigma = config["accelerometer_noise_density"].as();// / deltaTime; + } + if(config["gyroscope_noise_density"]) + { + gyro_sigma = config["gyroscope_noise_density"].as();// / deltaTime; + } + + std::cout << "Used noise values: " << sigma_between_b_a << " " << sigma_between_b_g << " " << accel_sigma << " " + << gyro_sigma << std::endl; +} + +void IMUSettings::registerArgs(dmvio::SettingsUtil& set) +{ + set.registerArg("resultsPrefix", resultsPrefix); + + set.registerArg("maxTimeBetweenInitFrames", maxTimeBetweenInitFrames); + + set.registerArg("skipFirstKeyframe", skipFirstKeyframe); + + set.registerArg("setting_weightDSOCoarse", setting_weightDSOCoarse); + set.registerArg("setting_weightDSOToGTSAM", setting_weightDSOToGTSAM); + set.registerArg("maxFrameEnergyThreshold", maxFrameEnergyThreshold); + + set.registerArg("dynamicWeightRMSEThresh", dynamicWeightRMSEThresh); + set.registerArg("updateDynamicWeightDuringOptimization", updateDynamicWeightDuringOptimization); + + set.registerArg("setting_scaleFixTH", setting_scaleFixTH); + set.registerArg("generalScaleIntervalSize", generalScaleIntervalSize); + + set.registerArg("numMeasurementsGravityInit", numMeasurementsGravityInit); + + set.registerArg("setting_optScaleBA", setting_optScaleBA); + set.registerArg("setting_optGravity", setting_optGravity); + set.registerArg("setting_optIMUExtrinsics", setting_optIMUExtrinsics); + + set.registerArg("setting_prior_bias", setting_prior_bias); + set.registerArg("setting_prior_velocity", setting_prior_velocity); + transformPriors.registerArgs(set, ""); + set.registerArg("gravityDirectionFixZ", gravityDirectionFixZ); + + set.registerArg("alwaysCanBreakIMU", alwaysCanBreakIMU); + + set.registerArg("useScaleDiagonalHack", useScaleDiagonalHack); + + set.registerArg("fixKeyframeDuringCoarseTracking", fixKeyframeDuringCoarseTracking); + set.registerArg("addVisualToCoarseGraphIfTrackingBad", addVisualToCoarseGraphIfTrackingBad); + + set.registerArg("baToCoarseRotVariance", baToCoarseRotVariance); + set.registerArg("baToCoarsePoseVariance", baToCoarsePoseVariance); + set.registerArg("baToCoarseVelVariance", baToCoarseVelVariance); + set.registerArg("baToCoarseAccBiasVariance", baToCoarseAccBiasVariance); + set.registerArg("baToCoarseGyrBiasVariance", baToCoarseGyrBiasVariance); + + set.registerArg("setting_transferCovToCoarse", setting_transferCovToCoarse); + set.registerArg("transferCovToCoarseMultiplier", transferCovToCoarseMultiplier); + + set.registerArg("setting_visualOnlyAfterScaleFixing", setting_visualOnlyAfterScaleFixing); + + initSettings.registerArgs(set); +} diff --git a/src/IMU/IMUSettings.h b/src/IMU/IMUSettings.h new file mode 100644 index 0000000..f2dbf48 --- /dev/null +++ b/src/IMU/IMUSettings.h @@ -0,0 +1,142 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUSETTINGS_H +#define DMVIO_IMUSETTINGS_H + +#include +#include +#include +#include +#include +#include "IMUInitialization/IMUInitSettings.h" + +namespace dmvio +{ + +// Contains various IMU related settings. +// There are the following ways to set them (where the latter ones overwrite the former ones): +// - Default values set in the code (here or in the constructor). +// - Values set using a settings.yaml file. +// - Values set using commandline arguments. +class IMUSettings +{ +public: + void registerArgs(dmvio::SettingsUtil& set); + + // Prefix for all results files. + std::string resultsPrefix = ""; + + IMUInitSettings initSettings; // settings for the IMU initializer. + double maxTimeBetweenInitFrames = 100000.0; // Maximum time between the first 2 frames for DSO. + + // Don't add IMU data between the first two keyframes. Not relevant when the IMU initializer is active (unless + // disableVIOUntilFirstInit=false). + bool skipFirstKeyframe = true; + + // Weight wrt DSO. + double setting_weightDSOCoarse = 1.0 / 1000; // DSO weight for coarse tracking. + double setting_weightDSOToGTSAM = 1.0 / 60000;// DSO weight for BA. + float maxFrameEnergyThreshold = 5000; // Maximum energy threshold for DSO. + + // ----------- BA Settings ----------- + // Settings regarding dynamic photometric weight. + double dynamicWeightRMSEThresh = 8.0; + bool updateDynamicWeightDuringOptimization = true; + + // When the scale changes less than this threshold over generalScaleIntervalSize optimizations we fix it. + // Disabled by default but could be usefull for some applications. 0.005 or 0.007 are good values for it. + double setting_scaleFixTH = 0; + int generalScaleIntervalSize = 60; + + // Maximum number of measurements to include for the simple gravity initializer. + int numMeasurementsGravityInit = 40; + + // Settings what to optimize in the main BA. + bool setting_optScaleBA = true; + bool setting_optGravity = true; + bool setting_optIMUExtrinsics = false; + + // Settings regarding priors. + bool setting_prior_bias = false; // Only relevant if disableVIOUntilFirstInit=false + bool setting_prior_velocity = false; // Only relevant if disableVIOUntilFirstInit=false + IMUTransformPriorSettings transformPriors; // Prior settings for gravity and IMU extrinsics. + bool gravityDirectionFixZ = true; // Fix z-axis of gravity direction (as yaw is not observable). + + // Don't include IMU variables when calculating whether the BA optimization can break. + bool alwaysCanBreakIMU = false; + + bool useScaleDiagonalHack = false; // This can be used to improve performance when the initial scale is very far from optimum. + + // ----------- Settings for Coarse Tracking ----------- + bool fixKeyframeDuringCoarseTracking = true; + bool addVisualToCoarseGraphIfTrackingBad = false; // Add visual factor even if tracking is bad. + + // Priors from BA when initialization CoarseGraph: + double baToCoarseRotVariance = 1.0; + double baToCoarsePoseVariance = 1e-1; + double baToCoarseVelVariance = 1e-1; + double baToCoarseAccBiasVariance = 1000.0; + double baToCoarseGyrBiasVariance = 5e-2; + + // Settings regarding bias transfer between coarse tracking and BA. + bool setting_transferCovToCoarse = true; // Transfer covariance from BA to tracking. + double transferCovToCoarseMultiplier = 1.0; + + // ----------- Settings for debugging. ----------- + // Use the visual only system after scale has been fixed, which can be useful for debugging (only makes sense together with setting_scaleFixTH). + // 1 means that also the gtsamIntegration is not used anymore, while 2 means that the gtsamIntegration is still used with IMUExtension removed. + int setting_visualOnlyAfterScaleFixing = 0; +}; + +// Contains IMU-Calibration and can read them from file. +// Default contains values for EuRoC. +class IMUCalibration +{ +public: + IMUCalibration(); + + IMUCalibration(std::string settingsFilename); + + void loadFromFile(std::string settingsFilename); + + // The noise values are registered as settings so they can also be set from commandline and from the settings yaml. + void registerArgs(dmvio::SettingsUtil& set); + + Sophus::SE3d T_cam_imu; + // Old defaults for EuRoC. + double sigma_between_b_a = 0.00447213; + double sigma_between_b_g = 0.0014142; + double accel_sigma = 0.316227; + double gyro_sigma = 0.1; + double integration_sigma = 0.316227; + + // Currently not read from settings. + gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8082).finished();; + +private: + void initDefault(); +}; + +} + +#endif //DMVIO_IMUSETTINGS_H diff --git a/src/IMU/IMUTypes.cpp b/src/IMU/IMUTypes.cpp new file mode 100644 index 0000000..0852d7c --- /dev/null +++ b/src/IMU/IMUTypes.cpp @@ -0,0 +1,44 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "IMUTypes.h" + +using namespace dmvio; + +IMUMeasurement::IMUMeasurement(const Eigen::Vector3d& accData, const Eigen::Vector3d& gyrData, double integrationTime) + : accData(accData), gyrData(gyrData), integrationTime(integrationTime) +{} + +const Eigen::Vector3d& IMUMeasurement::getAccData() const +{ + return accData; +} + +const Eigen::Vector3d& IMUMeasurement::getGyrData() const +{ + return gyrData; +} + +double IMUMeasurement::getIntegrationTime() const +{ + return integrationTime; +} diff --git a/src/IMU/IMUTypes.h b/src/IMU/IMUTypes.h new file mode 100644 index 0000000..3fa53b9 --- /dev/null +++ b/src/IMU/IMUTypes.h @@ -0,0 +1,51 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUTYPES_H +#define DMVIO_IMUTYPES_H + +#include +#include + +namespace dmvio +{ +class IMUMeasurement +{ +public: + IMUMeasurement(const Eigen::Vector3d& accData, const Eigen::Vector3d& gyrData, double integrationTime); + + const Eigen::Vector3d& getAccData() const; + + const Eigen::Vector3d& getGyrData() const; + + double getIntegrationTime() const; + +private: + Eigen::Vector3d accData{}; + Eigen::Vector3d gyrData{}; + double integrationTime; // time between this and previous IMU measurement. +}; + +typedef std::vector IMUData; +} + +#endif //DMVIO_IMUTYPES_H diff --git a/src/IMU/IMUUtils.cpp b/src/IMU/IMUUtils.cpp new file mode 100644 index 0000000..50e2d9e --- /dev/null +++ b/src/IMU/IMUUtils.cpp @@ -0,0 +1,104 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include +#include +#include "IMUUtils.h" +#include "IMUTypes.h" +#include "IMUSettings.h" + +using namespace dmvio; +using namespace gtsam; + +void dmvio::integrateIMUData(const IMUData& imuData, gtsam::PreintegratedImuMeasurements& preintegrated) +{ + for(const auto& measurement : imuData) + { + if(measurement.getIntegrationTime() == 0.0) continue; + preintegrated.integrateMeasurement(gtsam::Vector(measurement.getAccData()), + gtsam::Vector(measurement.getGyrData()), + measurement.getIntegrationTime()); + } +} + +gtsam::noiseModel::Diagonal::shared_ptr dmvio::computeBiasNoiseModel(const IMUCalibration& imuCalibration, + const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + double sigma_b_a = imuCalibration.sigma_between_b_a * sqrt(imuMeasurements.deltaTij()); + double sigma_b_g = imuCalibration.sigma_between_b_g * sqrt(imuMeasurements.deltaTij()); + return gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << sigma_b_a, sigma_b_a, sigma_b_a, sigma_b_g, sigma_b_g, sigma_b_g).finished()); +} + +void IMUTransformPriorSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) +{ + set.registerArg(prefix + "priorExtrinsicsRot", priorExtrinsicsRot); + set.registerArg(prefix + "priorExtrinsicsTrans", priorExtrinsicsTrans); + set.registerArg(prefix + "priorGravityDirection", priorGravityDirection); + set.registerArg(prefix + "priorGravityDirectionZ", priorGravityDirectionZ); +} + +std::vector dmvio::getPriorsAndAddValuesForTransform( + const TransformDSOToIMU& transform, const IMUTransformPriorSettings& settings, gtsam::Values& values) +{ + std::vector ret; + int symInd = transform.getSymbolInd(); + if(transform.optimizeScale()) + { + gtsam::Key scaleKey = gtsam::Symbol('s', symInd); + ScaleGTSAM sim(transform.getScale()); + values.insert(scaleKey, sim); + } + + if(transform.optimizeGravity()) + { + gtsam::Key gravityKey = Symbol('g', symInd); + gtsam::Rot3 initialRot(transform.getR_dsoW_metricW().matrix()); + gtsam::Rot3 zeroRot; // Initialize with current transform but set prior to zero transform. + values.insert(gravityKey, initialRot); + + Eigen::Vector3d gravityModel; + double rotationalSigma = settings.priorGravityDirection; + gravityModel.setConstant(rotationalSigma); + gravityModel(2) = settings.priorGravityDirectionZ; + gtsam::PriorFactor::shared_ptr rotationPrior( + new gtsam::PriorFactor(gravityKey, zeroRot, + gtsam::noiseModel::Diagonal::Sigmas(gravityModel))); + ret.push_back(rotationPrior); + } + + if(transform.optimizeExtrinsics()) + { + gtsam::Key extrinsicsKey = Symbol('i', symInd); + gtsam::Pose3 initialExtr(transform.getT_cam_imu().matrix()); + values.insert(extrinsicsKey, initialExtr); + + gtsam::Vector6 extrinsicsModel; + extrinsicsModel.segment(0, 3).setConstant(settings.priorExtrinsicsRot); + extrinsicsModel.segment(3, 3).setConstant(settings.priorExtrinsicsTrans); + gtsam::PriorFactor::shared_ptr extrinsicsPrior(new gtsam::PriorFactor( + extrinsicsKey, initialExtr, gtsam::noiseModel::Diagonal::Sigmas(extrinsicsModel) + )); + ret.push_back(extrinsicsPrior); + } + return ret; +} diff --git a/src/IMU/IMUUtils.h b/src/IMU/IMUUtils.h new file mode 100644 index 0000000..e9fc8bd --- /dev/null +++ b/src/IMU/IMUUtils.h @@ -0,0 +1,60 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUUTILS_H +#define DMVIO_IMUUTILS_H + +#include "IMUTypes.h" +#include +#include "GTSAMIntegration/PoseTransformationIMU.h" +#include "util/SettingsUtil.h" + +namespace dmvio +{ + +class IMUCalibration; +class TransformDSOToIMU; + +void integrateIMUData(const IMUData& imuData, gtsam::PreintegratedImuMeasurements& preintegrated); + +gtsam::noiseModel::Diagonal::shared_ptr +computeBiasNoiseModel(const IMUCalibration& imuCalibration, const gtsam::PreintegratedImuMeasurements& imuMeasurements); + +// Settings regarding prior on the symbols optimized by TransformDSOToIMU. +class IMUTransformPriorSettings +{ +public: + double priorExtrinsicsRot = 0.01; // rotational and translational prior for extrinsics. + double priorExtrinsicsTrans = 0.1; + double priorGravityDirection = 0.4; // Prior on gravity direction (first xy and then z). + double priorGravityDirectionZ = 0.0001; + + void registerArgs(dmvio::SettingsUtil& set, std::string prefix = ""); +}; + +// Add values for the variables optimized by TransformDSOToIMU and return a list of prior factors, +// according to the IMUTransformPriorSettings. +std::vector getPriorsAndAddValuesForTransform( + const TransformDSOToIMU& transform, const IMUTransformPriorSettings& settings, gtsam::Values& values); + +} +#endif //DMVIO_IMUUTILS_H diff --git a/src/IMUInitialization/CoarseIMUInitOptimizer.cpp b/src/IMUInitialization/CoarseIMUInitOptimizer.cpp new file mode 100644 index 0000000..4870280 --- /dev/null +++ b/src/IMUInitialization/CoarseIMUInitOptimizer.cpp @@ -0,0 +1,252 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "CoarseIMUInitOptimizer.h" +#include "IMU/IMUUtils.h" +#include +#include +#include "GTSAMIntegration/PoseTransformationFactor.h" +#include "GTSAMIntegration/ExtUtils.h" +#include "dso/util/FrameShell.h" +#include "GTSAMIntegration/GTSAMUtils.h" + +using namespace dmvio; +using namespace gtsam; +using symbol_shorthand::P, symbol_shorthand::S, symbol_shorthand::V, symbol_shorthand::B; + +dmvio::CoarseIMUInitOptimizer::CoarseIMUInitOptimizer(std::shared_ptr transformDSOToIMU, + const IMUCalibration& imuCalibration, + const CoarseIMUInitOptimizerSettings& settingsPassed) + : transformDSOToIMU(transformDSOToIMU), imuCalibration(imuCalibration), settings(settingsPassed) +{ + gtsam::Vector6 posePriorVector; + posePriorVector.segment(0, 3).setConstant(settings.priorRotSigma); + posePriorVector.segment(3, 3).setConstant(settings.priorTransSigma); + posePriorModel = noiseModel::Diagonal::Sigmas(posePriorVector); + + params.lambdaLowerBound = settings.lambdaLowerBound; +} + +void CoarseIMUInitOptimizer::handleFirstFrame(int frameId) +{ + // Note: the owner of this class is responsible for adding initial values and priors for the variables optimized + // by transformDSOToIMU (e.g S0, G0). + gtsam::Vector3 initialVel = Vector3::Zero(); + values.insert(V(frameId), initialVel); + gtsam::Key biasKey; + if(settings.multipleBiases) + { + biasKey = B(frameId); + }else + { + biasKey = B(0); + } + values.insert(biasKey, imuBias::ConstantBias(gtsam::Vector6::Zero())); +} + +void dmvio::CoarseIMUInitOptimizer::addPose(int frameId, const Sophus::SE3d& camToWorld, + const gtsam::PreintegratedImuMeasurements* imuData) +{ + // Note that we are optimizing worldToCam! + gtsam::Pose3 framePose(camToWorld.inverse().matrix()); + + if(prevFrameId > 0) + { + assert(imuData != nullptr); + gtsam::Key prevBiasKey; + if(settings.multipleBiases) + { + // Add random walk factor. + prevBiasKey = B(prevFrameId); + auto biasNoiseModel = computeBiasNoiseModel(imuCalibration, *imuData); + + gtsam::NonlinearFactor::shared_ptr bias_factor( + new BetweenFactor( + prevBiasKey, B(frameId), + gtsam::imuBias::ConstantBias(gtsam::Vector3::Zero(), + gtsam::Vector3::Zero()), biasNoiseModel)); + graph.add(bias_factor); + values.insert(B(frameId), values.at(B(prevFrameId))); + }else + { + prevBiasKey = B(0); + } + // Add IMU factor + gtsam::NonlinearFactor::shared_ptr imuFactor( + new gtsam::ImuFactor(P(prevFrameId), V(prevFrameId), + P(frameId), V(frameId), prevBiasKey, + *imuData)); + + // The IMUFactor needs to be transformed (from IMU frame to DSO frame). + gtsam::Values fixedValues; + if(settings.fixPoses) + { + fixedValues.insert(P(prevFrameId), prevFramePose); + fixedValues.insert(P(frameId), framePose); + } + auto transformedFactor = boost::make_shared(imuFactor, + *transformDSOToIMU, + settings.conversionType, fixedValues); + + graph.add(transformedFactor); + + values.insert(V(frameId), values.at(V(prevFrameId))); + }else + { + handleFirstFrame(frameId); + } + + if(!settings.fixPoses) + { + graph.push_back(gtsam::PriorFactor(P(frameId), framePose, posePriorModel)); + values.insert(P(frameId), framePose); + } + + poseIds.push_back(frameId); + + // Remove factors if the maximum number of frames is reached. + if(settings.maxNumPoses > 0) + { + std::set keysToRemove; + while(poseIds.size() > settings.maxNumPoses) + { + // Remove a pose and the corresponding factors from the graph. + int toRemove = poseIds[0]; + if(!settings.fixPoses) + { + keysToRemove.insert(P(toRemove)); + } + keysToRemove.insert(V(toRemove)); + if(settings.multipleBiases) + { + keysToRemove.insert(B(toRemove)); + } + poseIds.pop_front(); + numFrames--; + + } + removeKeysFromGraph(graph, keysToRemove, 5); + for(auto&& key : keysToRemove) + { + values.erase(key); + } + } + + numFrames++; + prevFrameId = frameId; + prevFramePose = framePose; +} + +dmvio::CoarseIMUInitOptimizer::OptimizationResult dmvio::CoarseIMUInitOptimizer::optimize() +{ + if(settings.updatePoses) + { + // Get the newest poses from DSO. + boost::unique_lock lock(dso::FrameShell::shellPoseMutex); + for(auto&& factor : graph) + { + PoseTransformationFactor* casted = dynamic_cast(factor.get()); + if(casted) + { + auto&& keys = casted->fixedValues.keys(); + for(auto&& key : keys) + { + gtsam::Symbol sym(key); + if(sym.chr() == 'p') + { + const auto* shell = activeShells.at(sym.index()); + // compute updated camToWorld + Sophus::SE3d camToWorld = shell->camToWorld; + if(shell->keyframeId == -1) + { + camToWorld = shell->trackingRef->camToWorld * shell->camToTrackingRef; + } + assert(sym.index() == shell->id); + eraseAndInsert(casted->fixedValues, key, gtsam::Pose3(camToWorld.inverse().matrix())); + } + } + } + } + } + + LevenbergMarquardtOptimizer optimizer(graph, values, params); + optimizedValues = optimizer.optimize(); + transformDSOToIMU->updateWithValues(optimizedValues); + double error = optimizer.error(); + double normalizedError = error / numFrames; + std::cout << "CoarseIMUInit error: " << error << std::endl; + std::cout << "Normalized CoarseIMUInit error: " << normalizedError << std::endl; + + bool good = true; + // If error is too high we assume that odometry failed and request a full reset. + if((settings.requestFullResetErrorThreshold > 0 && error > settings.requestFullResetErrorThreshold) || + (settings.requestFullResetNormalizedErrorThreshold > 0 && + normalizedError > settings.requestFullResetNormalizedErrorThreshold)) + { + std::cout << "Large CoarseIMUInitializer error! Requesting full reset!" << std::endl; + good = false; + dso::setting_fullResetRequested = true; + } + + return OptimizationResult(optimizer.iterations(), error, normalizedError, good); +} + +std::shared_ptr dmvio::CoarseIMUInitOptimizer::getUpdatedTransform() +{ + return transformDSOToIMU; +} + +gtsam::Key CoarseIMUInitOptimizer::getBiasKey() +{ + return settings.multipleBiases ? B(prevFrameId) : B(0); +} + +gtsam::imuBias::ConstantBias CoarseIMUInitOptimizer::getBias() +{ + return values.at(getBiasKey()); +} + +gtsam::Marginals CoarseIMUInitOptimizer::getMarginals() +{ + return gtsam::Marginals(graph, optimizedValues); +} + +void CoarseIMUInitOptimizer::takeOverOptimizedValues() +{ + values = optimizedValues; +} + +void CoarseIMUInitOptimizer::addPose(const dso::FrameShell& shell, const gtsam::PreintegratedImuMeasurements* imuData) +{ + boost::unique_lock lock(dso::FrameShell::shellPoseMutex); + if(settings.updatePoses) + { + activeShells[shell.id] = &shell; + } + addPose(shell.id, shell.camToWorld, imuData); +} + + +CoarseIMUInitOptimizer::OptimizationResult::OptimizationResult(int numIterations, double error, double normalizedError, + bool good) + : numIterations(numIterations), error(error), normalizedError(normalizedError), good(good) +{} diff --git a/src/IMUInitialization/CoarseIMUInitOptimizer.h b/src/IMUInitialization/CoarseIMUInitOptimizer.h new file mode 100644 index 0000000..73bb6e3 --- /dev/null +++ b/src/IMUInitialization/CoarseIMUInitOptimizer.h @@ -0,0 +1,113 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_COARSEIMUINITOPTIMIZER_H +#define DMVIO_COARSEIMUINITOPTIMIZER_H + + +#include "sophus/se3.hpp" +#include "GTSAMIntegration/PoseTransformationIMU.h" +#include "IMU/IMUTypes.h" +#include +#include +#include +#include +#include "IMU/IMUSettings.h" +#include + +namespace dso +{ +class FrameShell; +} + +namespace dmvio +{ +// This is the Coarse IMU Initializer from the paper +// It only optimizes IMU variables (scale, gravity direction, bias, velocities). +// To be exact: velocities, bias and all symbols optimized by the passed PoseTransformation are optimized. +// Owned (and methods called) by IMUInitializer, doesn't know DSO. +class CoarseIMUInitOptimizer +{ +public: + // transformDSOToIMU is updated during optimization. + // Note that a reference to the settings and calibration is kept! + // Note: the owner of this class is responsible for adding initial values and priors for the variables optimized + // by transformDSOToIMU (e.g s0, g0). + explicit CoarseIMUInitOptimizer(std::shared_ptr transformDSOToIMU, + const IMUCalibration& imuCalibration, + const CoarseIMUInitOptimizerSettings& settingsPassed); + + // Add frame to the optimizer. + void addPose(int frameId, const Sophus::SE3d& camToWorld, const gtsam::PreintegratedImuMeasurements* imuData); + void addPose(const dso::FrameShell& shell, const gtsam::PreintegratedImuMeasurements* imuData); + + struct OptimizationResult + { + OptimizationResult(int numIterations, double error, double normalizedError, bool good); + int numIterations; + double error; + double normalizedError; + bool good; + }; + + OptimizationResult optimize(); + std::shared_ptr getUpdatedTransform(); + gtsam::imuBias::ConstantBias getBias(); + gtsam::Key getBiasKey(); + + gtsam::Marginals getMarginals(); + + void takeOverOptimizedValues(); + + gtsam::NonlinearFactorGraph graph; + gtsam::Values values; + gtsam::Values optimizedValues; + int numFrames = 0; + int imuFactorsRemovedUntil = -1; +private: + void handleFirstFrame(int frameId); + + const CoarseIMUInitOptimizerSettings& settings; + const IMUCalibration& imuCalibration; + + std::shared_ptr transformDSOToIMU; + + int prevFrameId = -1; + gtsam::Pose3 prevFramePose; // Needed for fixPoses. + + gtsam::LevenbergMarquardtParams params = gtsam::LevenbergMarquardtParams::CeresDefaults(); + + // Only used if fixPoses == false; + gtsam::SharedNoiseModel posePriorModel; + + // For implementing maxNumPoses: + std::deque poseIds; // pose ids currently in the graph. + + // used to get updated poses from DSO before optimizing. + std::map activeShells; + +}; + + +} + +#endif //DMVIO_COARSEIMUINITOPTIMIZER_H diff --git a/src/IMUInitialization/GravityInitializer.cpp b/src/IMUInitialization/GravityInitializer.cpp new file mode 100644 index 0000000..2ffa6b9 --- /dev/null +++ b/src/IMUInitialization/GravityInitializer.cpp @@ -0,0 +1,85 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "GravityInitializer.h" +#include + +using dmvio::GravityInitializer; +using dmvio::IMUIntegration; + +GravityInitializer::GravityInitializer(int numMeasurementsToUse, const IMUCalibration& imuCalibration) + : maxNumMeasurements(numMeasurementsToUse) +{ + gravity = imuCalibration.gravity; +} + +Sophus::SE3d +GravityInitializer::addMeasure(const IMUData& imuData, const Sophus::SE3d& currToFirst) +{ + int numMeasure = 0; + Eigen::Vector3d measure(0.0, 0.0, 0.0); + for(int i = 0; i < imuData.size(); ++i) + { + Eigen::Vector3d curr = imuData[i].getAccData(); + measure += curr; + numMeasure++; + } + measure /= (double) numMeasure; + + measures.push_back(measure); + + if(measures.size() > maxNumMeasurements) + { + measures.pop_front(); + } + + Eigen::Vector3d filteredM(0.0, 0.0, 0.0); + for(auto&& m : measures) + { + filteredM += m; + } + filteredM /= (double) measures.size(); + + measure = filteredM; + + Eigen::Quaterniond quat; + quat.setFromTwoVectors(measure, -gravity); + + Sophus::SE3d imuToWorld(quat, Eigen::Vector3d::Zero()); + + return imuToWorld; +} + +double dmvio::getGravityError(const Sophus::SE3d& imuToWorld, const Sophus::SE3d& imuToWorldGT) +{ + Eigen::Vector3d g = (gtsam::Vector(3) + << 0, 0, -9.8082).finished(); // Only the direction actually matters so it's ok if this is not the actually used gravity. + // g is in world coordinates, so check what g is in drone coordinates. + Eigen::Vector3d gDrone = imuToWorld.inverse().rotationMatrix() * g; + Eigen::Vector3d gDroneGT = imuToWorldGT.inverse().rotationMatrix() * g; + + // Compute angle between the two vectors. + double angle = std::acos(gDrone.dot(gDroneGT) / (gDrone.norm() * gDroneGT.norm())); + double degrees = (angle * 180.0) / boost::math::constants::pi(); + + return degrees; +} diff --git a/src/IMUInitialization/GravityInitializer.h b/src/IMUInitialization/GravityInitializer.h new file mode 100644 index 0000000..804625f --- /dev/null +++ b/src/IMUInitialization/GravityInitializer.h @@ -0,0 +1,50 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_GRAVITYINITIALIZER_H +#define DMVIO_GRAVITYINITIALIZER_H + +#include "sophus/se3.hpp" +#include "IMU/IMUIntegration.hpp" + +namespace dmvio +{ +// Averages accelerometer measurements to provide a simple initialization for gravity direction. +class GravityInitializer +{ +public: + GravityInitializer(int numMeasurementsToUse, const IMUCalibration& imuCalibration); + + // returns an approximate imuToWorld transform (only rotation). + Sophus::SE3d addMeasure(const IMUData& imuData, const Sophus::SE3d& currToFirst); + +private: + int maxNumMeasurements; // Num of last gravity measurements to average. + std::deque measures; + Eigen::Vector3d gravity; +}; + +double getGravityError(const Sophus::SE3d& imuToWorld, const Sophus::SE3d& imuToWorldGT); + +} + +#endif //DMVIO_GRAVITYINITIALIZER_H diff --git a/src/IMUInitialization/IMUInitSettings.cpp b/src/IMUInitialization/IMUInitSettings.cpp new file mode 100644 index 0000000..db83013 --- /dev/null +++ b/src/IMUInitialization/IMUInitSettings.cpp @@ -0,0 +1,86 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "util/SettingsUtil.h" +#include "IMUInitSettings.h" + +using namespace dmvio; + + +void IMUInitSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) +{ + set.registerArg(prefix + "transitionModel", transitionModel); + set.registerArg(prefix + "onlyKFs", onlyKFs); + set.registerArg(prefix + "coarseScaleUncertaintyThresh", coarseScaleUncertaintyThresh); + set.registerArg(prefix + "initDSOParams", initDSOParams); + set.registerArg(prefix + "scalePriorAfterInit", scalePriorAfterInit); + set.registerArg(prefix + "disableVIOUntilFirstInit", disableVIOUntilFirstInit); + set.registerArg(prefix + "multithreadedInitDespiteNonRT", multithreadedInitDespiteNonRT); + + transformPriors.registerArgs(set, prefix); + coarseInitSettings.registerArgs(set, prefix); + pgbaSettings.registerArgs(set, prefix + "pgba_"); + + thresholdSettings.threshScale = 1.02; + thresholdSettings.registerArgs(set, prefix); + secondThresholdSettings.threshScale = 10000000.0; + secondThresholdSettings.registerArgs(set, prefix + "second"); +} + +void CoarseIMUInitOptimizerSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) +{ + set.registerArg(prefix + "maxNumPoses", maxNumPoses); + set.registerArg(prefix + "fixPoses", fixPoses); + set.registerArg(prefix + "multipleBiases", multipleBiases); + + set.registerArg(prefix + "priorRotSigma", priorRotSigma); + set.registerArg(prefix + "priorTransSigma", priorTransSigma); + + set.registerArg(prefix + "lambdaLowerBound", lambdaLowerBound); + + set.registerArg(prefix + "conversionType", conversionType); + + set.registerArg(prefix + "updatePoses", updatePoses); + + set.registerArg(prefix + "requestFullResetErrorThreshold", requestFullResetErrorThreshold); + set.registerArg(prefix + "requestFullResetNormalizedErrorThreshold", requestFullResetNormalizedErrorThreshold); +} + +void PGBASettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) +{ + set.registerArg(prefix + "delay", delay); + set.registerArg(prefix + "scaleUncertaintyThresh", scaleUncertaintyThresh); + set.registerArg(prefix + "reinitScaleUncertaintyThresh", reinitScaleUncertaintyThresh); + set.registerArg(prefix + "skipFirstKFs", skipFirstKFs); + set.registerArg(prefix + "conversionType", conversionType); + + set.registerArg(prefix + "prepareGraphAddFactors", prepareGraphAddFactors); + set.registerArg(prefix + "prepareGraphAddDelValues", prepareGraphAddDelValues); + + transformPriors.registerArgs(set, prefix); +} + +void IMUThresholdSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) +{ + set.registerArg(prefix + "threshScale", threshScale); + set.registerArg(prefix + "threshGravdir", threshGravdir); +} diff --git a/src/IMUInitialization/IMUInitSettings.h b/src/IMUInitialization/IMUInitSettings.h new file mode 100644 index 0000000..1aa9b6c --- /dev/null +++ b/src/IMUInitialization/IMUInitSettings.h @@ -0,0 +1,130 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUINITSETTINGS_H +#define DMVIO_IMUINITSETTINGS_H + +#include "util/SettingsUtil.h" +#include "IMU/IMUUtils.h" +#include "GTSAMIntegration/PoseTransformationFactor.h" + +namespace dmvio +{ + +class CoarseIMUInitOptimizerSettings +{ +public: + void registerArgs(dmvio::SettingsUtil& set, std::string prefix); + + int maxNumPoses = 100; // forgets older poses. + bool fixPoses = true, multipleBiases = false; + + // Prior on rotation and translation (not used if fixPoses=true). + double priorRotSigma = 0.00001; + double priorTransSigma = 0.00001; + + // Optimizer settings + double lambdaLowerBound = 1e-16; + + PoseTransformationFactor::ConversionType conversionType = PoseTransformationFactor::JACOBIAN_FACTOR; + + bool updatePoses = true; // if true we get the updated poses from DSO before optimizing. + + double requestFullResetErrorThreshold = -1; // if the error gets higher than this request a full reset. + double requestFullResetNormalizedErrorThreshold = -1; // if the normalized error gets higher than this request a full reset. +}; + +class PGBASettings +{ +public: + void registerArgs(dmvio::SettingsUtil& set, std::string prefix); + + // Delay of the graph used for the PGBA. + int delay = 100; + + double scaleUncertaintyThresh = 1.0; // Threshold for first init to succeed. + double reinitScaleUncertaintyThresh = 0.5; // Threshold to stop reinitializing. + + int skipFirstKFs = 0; // if positive the first n KFs are skipped. + + // Should theoretically be best set to true, but in practice false seems to be better. + bool prepareGraphAddFactors = false; + bool prepareGraphAddDelValues = false; + + PoseTransformationFactor::ConversionType conversionType = PoseTransformationFactor::JACOBIAN_FACTOR; + IMUTransformPriorSettings transformPriors; +}; + +class IMUThresholdSettings +{ +public: + void registerArgs(dmvio::SettingsUtil& set, std::string prefix = "thresh_"); + + double threshScale = 1000.0; + double threshGravdir = 1000.0; +}; + +// Settings related to the initializer +class IMUInitSettings +{ +public: + void registerArgs(dmvio::SettingsUtil& set, std::string prefix = "init_"); + + // For available options see enum InitTransitionMode in file IMUInitializerTransitions. + int transitionModel = 2; + + // Settings regarding coarse initializer. + CoarseIMUInitOptimizerSettings coarseInitSettings; + bool onlyKFs = true; // Only include keyframes in the coarse IMU initializer. + // Priors for the coarse optimizer. + IMUTransformPriorSettings transformPriors; + double coarseScaleUncertaintyThresh = 1.0; // Scale uncertainty must be below this to consider the coarse init to have succeeded. + + // Settings regarding PGBA based initializer. + PGBASettings pgbaSettings; + + // Threshold settings for the marginalization replacement. + IMUThresholdSettings thresholdSettings; // default for scale threshold is 1.02 + double percentageSwitchToSecondTH = 0.5; // switch to second threshold once this fraction of IMU factors would be lost. + IMUThresholdSettings secondThresholdSettings; // default for second scale threshold is effectively infinity (see cpp file). + + // Also init IMU params. + bool initDSOParams = true; + + // If positive add a scale prior with this stddev after the initialization (meant to be used with + // transitionModels 4 and 5 for the ablations). + double scalePriorAfterInit = 0.0; + + // If false, the visual-inertial system is initialized immediately (with scale 1). Then we + // still run the initializer for re-initialization though. A value of false will not work well if the objects in + // the scene are far away. This is mostly a legacy setting which might not work very well at the moment, but it + // could be useful for future extensions. + bool disableVIOUntilFirstInit = true; + + // Setting for debugging. Do IMU initialization in separate thread, even if we are in non-realtime mode. + bool multithreadedInitDespiteNonRT = false; + +}; + +} + +#endif //DMVIO_IMUINITSETTINGS_H diff --git a/src/IMUInitialization/IMUInitStateChanger.h b/src/IMUInitialization/IMUInitStateChanger.h new file mode 100644 index 0000000..d50eb36 --- /dev/null +++ b/src/IMUInitialization/IMUInitStateChanger.h @@ -0,0 +1,49 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUINITSTATECHANGER_H +#define DMVIO_IMUINITSTATECHANGER_H + +#include +#include + +namespace dmvio +{ + +class IMUInitializerState; + +// Provides methods to change the state. +class IMUInitStateChanger +{ +public: + // Can directly be called to change the state. + virtual void lockAndSetState(std::unique_ptr&& newState) = 0; + + // Aquire lock for calling setState. + virtual std::unique_lock acquireSetStateLock() = 0; + // Before calling setState a lock must be acquired with the previous method! + virtual void setState(std::unique_ptr&& newState) = 0; +}; + +} + +#endif //DMVIO_IMUINITSTATECHANGER_H diff --git a/src/IMUInitialization/IMUInitializer.cpp b/src/IMUInitialization/IMUInitializer.cpp new file mode 100644 index 0000000..bf354b4 --- /dev/null +++ b/src/IMUInitialization/IMUInitializer.cpp @@ -0,0 +1,151 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "IMUInitializer.h" +#include "dso/util/FrameShell.h" +#include "IMU/IMUUtils.h" +#include "util/TimeMeasurement.h" +#include "GTSAMIntegration/Marginalization.h" +#include "GTSAMIntegration/Sim3GTSAM.h" +#include "IMU/BAIMULogic.h" +#include "IMUInitializerLogic.h" +#include "IMUInitializerStates.h" +#include "IMUInitializerTransitions.h" + +dmvio::IMUInitializer::IMUInitializer(std::string resultsPrefix, + boost::shared_ptr preintegrationParams, + const IMUCalibration& imuCalibration, IMUInitSettings& settings, + DelayedMarginalizationGraphs* delayedMarginalization, bool linearizeOperation, + InitCallback callOnInit) +{ + logic = std::make_unique(resultsPrefix, preintegrationParams, + imuCalibration, settings, delayedMarginalization, linearizeOperation, + callOnInit, *this); + + transitionModel = createTransitionModel(InitTransitionMode(settings.transitionModel), *logic); + setState(transitionModel->getInitialState()); +} + +dmvio::IMUInitializer::~IMUInitializer() = default; + +void dmvio::IMUInitializer::addIMUData(const dmvio::IMUData& data, int frameId) +{ + currentCoarseFrameId = frameId; + currentCoarseIMUData = std::make_unique(data); +} + +void dmvio::IMUInitializer::addPose(const dso::FrameShell& shell, bool willBecomeKeyframe) +{ + assert(shell.id == currentCoarseFrameId || !currentCoarseIMUData); + std::unique_ptr newState; + { + // Forward to current state... + std::shared_lock lock(mutex); + newState = currentState->addPose(shell, willBecomeKeyframe, currentCoarseIMUData.get()); + } + // ... and change state if necessary. + // For changing the state we need a unique_lock. + lockAndSetState(std::move(newState)); +} + +bool dmvio::IMUInitializer::initializeIfReady() +{ + std::pair, bool> ret; + { + std::shared_lock lock(mutex); + ret = currentState->initializeIfReady(); + } + lockAndSetState(std::move(ret.first)); + return ret.second; +} + +void dmvio::IMUInitializer::postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, double timestamp, + const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + std::unique_ptr newState; + { + std::shared_lock lock(mutex); + newState = currentState->postBAInit(keyframeId, activeHBFactor, baValues, timestamp, imuMeasurements); + } + lockAndSetState(std::move(newState)); +} + +const gtsam::imuBias::ConstantBias& dmvio::IMUInitializer::getLatestBias() const +{ + return logic->latestBias; +} + +void dmvio::IMUInitializer::setState(std::unique_ptr&& newState) +{ + if(newState) + { + // Change state, the caller is responsible for making sure we have a lock. + currentState = std::move(newState); + std::cout << "Switching to initializer state: " << *currentState << std::endl; + } +} + +void dmvio::IMUInitializer::lockAndSetState(std::unique_ptr&& newState) +{ + if(newState) + { + // For changing the state we need a unique_lock. + std::unique_lock lock(mutex); + setState(std::move(newState)); + } +} + +std::unique_lock dmvio::IMUInitializer::acquireSetStateLock() +{ + return std::unique_lock(mutex); +} + +bool dmvio::thresholdVariableChanges(IMUThresholdSettings settings, const gtsam::Values& baValues, + const gtsam::Values& fejValues) +{ + gtsam::Key scaleKey = gtsam::Symbol('s', 0); + gtsam::Key gravKey = gtsam::Symbol('g', 0); + + if(fejValues.exists(scaleKey)) + { + double scale = baValues.at(scaleKey).scale; + double fejScale = fejValues.at(scaleKey).scale; + double scaleDiff = scale / fejScale; + if(scaleDiff < 1.0) + { + scaleDiff = 1.0 / scaleDiff; + } + if(scaleDiff > settings.threshScale) return true; + } + + if(fejValues.exists(gravKey)) + { + Sophus::SO3d R_dsoW_metricW(baValues.at(gravKey).matrix()); + Sophus::SO3d fejR_dsoW_metricW(fejValues.at(gravKey).matrix()); + double gravDiff = (R_dsoW_metricW.inverse() * fejR_dsoW_metricW).log().norm(); + if(gravDiff > settings.threshGravdir) return true; + } + + return false; +} + diff --git a/src/IMUInitialization/IMUInitializer.h b/src/IMUInitialization/IMUInitializer.h new file mode 100644 index 0000000..59fd387 --- /dev/null +++ b/src/IMUInitialization/IMUInitializer.h @@ -0,0 +1,123 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUINITIALIZER_H +#define DMVIO_IMUINITIALIZER_H + + +#include "GTSAMIntegration/PoseTransformationIMU.h" +#include +#include +#include "FullSystem/HessianBlocks.h" +#include "IMU/IMUTypes.h" +#include "CoarseIMUInitOptimizer.h" +#include "PoseGraphBundleAdjustment.h" +#include "IMU/BAIMULogic.h" +#include "GTSAMIntegration/DelayedMarginalization.h" +#include +#include "IMUInitStateChanger.h" + +namespace dmvio +{ + +class IMUInitializerLogic; +class IMUInitializerState; +class StateTransitionModel; + +// IMU Initializer as described in the DM-VIO paper. +// Implementation is performed with a state machine. +// This is more of a facade class which forwards most of the actual logic to: +// - IMUInitializerStates define the states of the state machine. +// - IMUInitializerTransitions define when to transition between which states (there are multiple transition models e.g. for ablation studies). +// - IMUInitializerLogic contains the main member variables and logic for calling the correct optimizers. +// - The actual optimizations are performed by CoarseIMUInitOptimizer and PoseGraphBundleAdjustment (PGBA) +class IMUInitializer : public IMUInitStateChanger +{ +public: + typedef std::function InitCallback; + + // Note that a reference to the settings and imuCalibration, and also delayedMarginalization is kept! + IMUInitializer(std::string resultsPrefix, boost::shared_ptr preintegrationParams, + const IMUCalibration& imuCalibration, IMUInitSettings& settings, + DelayedMarginalizationGraphs* delayedMarginalization, bool linearizeOperation, + InitCallback callOnInit); + + ~IMUInitializer(); + + // -------------------------------------------------- + // Called from coarse tracking thread. + // -------------------------------------------------- + // Called when IMU data for a frame is available. + void addIMUData(const IMUData& data, int frameId); + // Called when coarse tracking has finished. + // --> forwards to CoarseIMUInitOptimizer, can also filter out non-KFs. + void addPose(const dso::FrameShell& shell, bool willBecomeKeyframe); + + + // -------------------------------------------------- + // Called from BA thread. + // -------------------------------------------------- + // Called after all keyframe operations are finished. This is when the IMU in the main system should be + // initialized (if the initialized values are ready). + bool initializeIfReady(); + + // Called after the main BA. + // here the PGBA can be run. + void postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, double timestamp, + const gtsam::PreintegratedImuMeasurements& imuMeasurements); + + + const gtsam::imuBias::ConstantBias& getLatestBias() const; + + // -------------------------------------------------- + // Methods overridden from IMUInitStateChanger. These allow states and transitions to set the current state. + // -------------------------------------------------- + // Can directly be called to change the state. + void lockAndSetState(std::unique_ptr&& newState) final; + + // Aquire lock for calling setState. + std::unique_lock acquireSetStateLock() final; + // Before calling setState a lock must be acquired with the previous method! + void setState(std::unique_ptr&& newState) final; + +private: + // Variables for the state machine. + std::unique_ptr logic; // contains main initialization data and logic common for all states. + std::unique_ptr currentState; + std::unique_ptr transitionModel; // contains the logic which states transition to which new states. + + // requires a shared_lock for calls to the currentState, and a unique_lock to change the state. + std::shared_timed_mutex mutex; + + // Used to couple the IMU data with the corresponding frame pose. + std::unique_ptr currentCoarseIMUData; + int currentCoarseFrameId = -1; +}; + +// Returns true if if gravity direction and scale have changed more than a threshold (according to the settings). +bool thresholdVariableChanges(dmvio::IMUThresholdSettings settings, const gtsam::Values& baValues, + const gtsam::Values& fejValues); + + +} +#endif //DMVIO_IMUINITIALIZER_H diff --git a/src/IMUInitialization/IMUInitializerLogic.cpp b/src/IMUInitialization/IMUInitializerLogic.cpp new file mode 100644 index 0000000..3847f87 --- /dev/null +++ b/src/IMUInitialization/IMUInitializerLogic.cpp @@ -0,0 +1,129 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "util/TimeMeasurement.h" + +#include +#include "IMUInitializerLogic.h" +#include "IMUInitializerStates.h" + +dmvio::IMUInitializerLogic::IMUInitializerLogic(std::string resultsPrefix, + boost::shared_ptr preintegrationParams, + const dmvio::IMUCalibration& imuCalibration, + dmvio::IMUInitSettings& settings, + DelayedMarginalizationGraphs* delayedMarginalization, + bool linearizeOperation, InitCallback callOnInit, + IMUInitStateChanger& stateChanger) + : imuCalibration(imuCalibration), settings(settings), + imuMeasurements(preintegrationParams), + optScale(new bool(true)), optGravity(new bool(true)), optT_cam_imu(new bool(false)), + callOnInit(callOnInit), delayedMarginalizationGraphs(delayedMarginalization), + stateChanger(stateChanger) +{ + if(linearizeOperation) + { + realtimePGBA = settings.multithreadedInitDespiteNonRT; + realtimeCoarseIMUInit = settings.multithreadedInitDespiteNonRT; + }else + { + // Realtime mode + realtimePGBA = true; + realtimeCoarseIMUInit = true; + } + + transformDSOToIMU.reset(new TransformDSOToIMU(gtsam::Pose3(imuCalibration.T_cam_imu.matrix()), + optScale, optGravity, optT_cam_imu, true, 0)); + + transformDSOToIMUAfterPGBA.reset(new TransformDSOToIMU(*transformDSOToIMU)); + + // Initialize CoarseIMUInitOptimizer: + coarseIMUOptimizer = std::make_unique(transformDSOToIMU, imuCalibration, + settings.coarseInitSettings); + + // Add priors for transform related variables to the imuOptimGraph: + auto factors = getPriorsAndAddValuesForTransform(*transformDSOToIMU, settings.transformPriors, + coarseIMUOptimizer->values); + for(auto&& factor : factors) + { + coarseIMUOptimizer->graph.add(factor); + } + + assert(delayedMarginalization); + pgba = std::make_unique(delayedMarginalization, imuCalibration, settings.pgbaSettings, + transformDSOToIMUAfterPGBA); + +} + +void dmvio::IMUInitializerLogic::addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, const IMUData* imuData) +{ + bool imuDataAvailable = false; + if(imuData) + { + imuDataAvailable = true; + integrateIMUData(*imuData, imuMeasurements); + } + + if(settings.onlyKFs && !willBecomeKeyframe) return; + + dmvio::TimeMeasurement addPoseTimeMeas("IMUInitAddPose"); + if(imuDataAvailable) + { + coarseIMUOptimizer->addPose(shell, &imuMeasurements); + imuMeasurements.resetIntegrationAndSetBias(coarseIMUOptimizer->getBias()); + }else + { + // First frame: No IMU data yet. + coarseIMUOptimizer->addPose(shell, nullptr); + imuMeasurements.resetIntegration(); + } +} + +dmvio::IMUInitVariances dmvio::IMUInitializerLogic::performCoarseIMUInit(double timestamp) +{ + dmvio::TimeMeasurement optimTime("IMUInitOptimize"); + CoarseIMUInitOptimizer::OptimizationResult result = coarseIMUOptimizer->optimize(); + double time = optimTime.end(); + + IMUInitVariances variances; + if(result.good) + { + gtsam::Marginals marginals = coarseIMUOptimizer->getMarginals(); + variances = IMUInitVariances(marginals, gtsam::Symbol('s', 0), coarseIMUOptimizer->getBiasKey()); + } + + return variances; +} + + +dmvio::IMUInitVariances::IMUInitVariances(const gtsam::Marginals& marginals, gtsam::Key scaleKey, gtsam::Key biasKey) +{ + indetermined = false; + try + { + gtsam::Matrix scaleCovariance = marginals.marginalCovariance(scaleKey); + scaleVariance = scaleCovariance(0, 0); + biasCovariance = marginals.marginalCovariance(biasKey); + }catch(gtsam::IndeterminantLinearSystemException& exc) + { + indetermined = true; + } +} diff --git a/src/IMUInitialization/IMUInitializerLogic.h b/src/IMUInitialization/IMUInitializerLogic.h new file mode 100644 index 0000000..e35f3a2 --- /dev/null +++ b/src/IMUInitialization/IMUInitializerLogic.h @@ -0,0 +1,104 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUINITIALIZERLOGIC_H +#define DMVIO_IMUINITIALIZERLOGIC_H + + +#include +#include +#include +#include "CoarseIMUInitOptimizer.h" +#include "PoseGraphBundleAdjustment.h" +#include "IMUInitStateChanger.h" + +namespace dmvio +{ + +class IMUInitializerState; + +class IMUInitVariances +{ +public: + IMUInitVariances() = default; + IMUInitVariances(const gtsam::Marginals& marginals, gtsam::Key scaleKey, gtsam::Key biasKey); + + bool indetermined = true; + double scaleVariance; + gtsam::Matrix biasCovariance; +}; + +class StateTransitionModel; + +// Helper class which encapsulates common logic and data for the states of the IMUInitializer. +class IMUInitializerLogic +{ +public: + typedef std::function InitCallback; + + IMUInitializerLogic(std::string resultsPrefix, + boost::shared_ptr preintegrationParams, + const dmvio::IMUCalibration& imuCalibration, + dmvio::IMUInitSettings& settings, + DelayedMarginalizationGraphs* delayedMarginalization, + bool linearizeOperation, InitCallback callOnInit, + IMUInitStateChanger& stateChanger); + + // This is required to change the state from different threads. + IMUInitStateChanger& stateChanger; + + DelayedMarginalizationGraphs* delayedMarginalizationGraphs; // Used to replace the main graph on init. + + // if true the factory methods will create the CoarseIMUInitState (or the PGBAState respectively) in realtime mode. + bool realtimeCoarseIMUInit; + bool realtimePGBA; + + InitCallback callOnInit; + + std::shared_ptr transformDSOToIMU; + std::shared_ptr transformDSOToIMUAfterPGBA; + std::shared_ptr optScale; + std::shared_ptr optGravity; + std::shared_ptr optT_cam_imu; + + const IMUCalibration& imuCalibration; + IMUInitSettings& settings; + + // This is the bias used for the preintegration in the main system. + gtsam::imuBias::ConstantBias latestBias; + + // For CoarseIMUInit: + std::unique_ptr coarseIMUOptimizer; + gtsam::PreintegratedImuMeasurements imuMeasurements; + void addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, const IMUData* imuData); + IMUInitVariances performCoarseIMUInit(double timestamp); + + // For PGBA. + std::unique_ptr pgba; + +}; + + +} + + +#endif //DMVIO_IMUINITIALIZERLOGIC_H diff --git a/src/IMUInitialization/IMUInitializerStates.cpp b/src/IMUInitialization/IMUInitializerStates.cpp new file mode 100644 index 0000000..973a95c --- /dev/null +++ b/src/IMUInitialization/IMUInitializerStates.cpp @@ -0,0 +1,606 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "util/TimeMeasurement.h" +#include "IMUInitializerStates.h" +#include "IMUInitializerLogic.h" +#include "IMUInitializer.h" +#include "IMUInitializerTransitions.h" +#include "GTSAMIntegration/GTSAMUtils.h" + +using namespace dmvio; + +dmvio::DefaultActiveIMUInitializerState::DefaultActiveIMUInitializerState(IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel) + : logic(imuInitLogic), transitionModel(transitionModel) +{} + +std::unique_ptr +dmvio::DefaultActiveIMUInitializerState::addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) +{ + logic.addPose(shell, willBecomeKeyframe, imuData); + return nullptr; +} + +std::unique_ptr +dmvio::DefaultActiveIMUInitializerState::postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, double timestamp, + const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + // This is actually not called for the very first KF which is what we want in this case. + logic.pgba->addKeyframe(keyframeId, imuMeasurements); + return nullptr; +} + +std::pair, bool> dmvio::DefaultActiveIMUInitializerState::initializeIfReady() +{ + return std::make_pair(nullptr, false); +} + +dmvio::CoarseIMUInitState::CoarseIMUInitState(dmvio::IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel) +{} + +std::unique_ptr +dmvio::CoarseIMUInitState::addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) +{ + DefaultActiveIMUInitializerState::addPose(shell, willBecomeKeyframe, imuData); + + if(logic.coarseIMUOptimizer->numFrames > 5 && willBecomeKeyframe) + { + IMUInitVariances variances = logic.performCoarseIMUInit(shell.timestamp); + + if(!variances.indetermined) + { + // Maybe adding a bias covariance threshold here would be a good idea. + logic.latestBias = logic.coarseIMUOptimizer->getBias(); + } + + // Get new state from transition model. + auto newState = transitionModel.coarseIMUInitOptimized(logic.coarseIMUOptimizer->optimizedValues, variances); + if(newState) + { + // This means that the initialization was successful and the CoarseIMUOptimizer can take over the values + // to the new optimization. + logic.coarseIMUOptimizer->takeOverOptimizedValues(); + logic.pgba->removeIMUFactorsUntil = logic.coarseIMUOptimizer->imuFactorsRemovedUntil; + return std::move(newState); + } + } + return nullptr; +} + +void dmvio::CoarseIMUInitState::print(std::ostream& str) const +{ + str << "CoarseIMUInit"; +} + +dmvio::RealtimeCoarseIMUInitState::RealtimeCoarseIMUInitState(dmvio::IMUInitializerLogic& imuInitLogic, + dmvio::StateTransitionModel& transitionModel) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel) +{} + +std::unique_ptr +dmvio::RealtimeCoarseIMUInitState::addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) +{ + dmvio::TimeMeasurement meas("RealtimeCoarseIMUInitState::addPose"); + switch(status) + { + case NOT_RUNNING: + DefaultActiveIMUInitializerState::addPose(shell, willBecomeKeyframe, imuData); + if(logic.coarseIMUOptimizer->numFrames > 5 && willBecomeKeyframe && !dso::setting_fullResetRequested) + { + optimizingTimestamp = shell.timestamp; + // perform optimization in separate thread. + runthread = std::thread{&RealtimeCoarseIMUInitState::threadRun, this}; + status = RUNNING; + runthread.detach(); + } + break; + case RUNNING: + // Save data coming in while the thread is running. + cachedData.emplace_back(&shell, willBecomeKeyframe, *imuData); + break; + } + return nullptr; +} + +void dmvio::RealtimeCoarseIMUInitState::threadRun() +{ + dmvio::TimeMeasurement timeMeasurement("RealtimeCoarseIMUInitState::threadRun"); + IMUInitVariances variances = logic.performCoarseIMUInit(optimizingTimestamp); + + if(!variances.indetermined) + { + // Maybe adding a bias covariance threshold here would be a good idea. + logic.latestBias = logic.coarseIMUOptimizer->getBias(); + } + + auto newState = transitionModel.coarseIMUInitOptimized(logic.coarseIMUOptimizer->optimizedValues, variances); + if(newState) + { + // This means that the initialization was successful and the CoarseIMUInitOptimizer can take over the values to + // the new optimization. + logic.coarseIMUOptimizer->takeOverOptimizedValues(); + logic.pgba->removeIMUFactorsUntil = logic.coarseIMUOptimizer->imuFactorsRemovedUntil; + } + + // Acquire lock to change the state. This ensures that the addPose method will not be called for this state in + // the meantime, which would lead to IMU data being lost. + auto lock = logic.stateChanger.acquireSetStateLock(); + // Add cached imu data and poses. + for(auto&& data : cachedData) + { + DefaultActiveIMUInitializerState::addPose(*std::get<0>(data), std::get<1>(data), &std::get<2>(data)); + } + cachedData.clear(); + if(!newState) status = NOT_RUNNING; + logic.stateChanger.setState(std::move(newState)); + + std::cout << "Finished thread" << std::endl; +} + +void dmvio::RealtimeCoarseIMUInitState::print(std::ostream& str) const +{ + str << "RealtimeCoarseIMUInitState"; +} + +std::unique_ptr +dmvio::createCoarseIMUInitState(dmvio::IMUInitializerLogic& imuInitLogic, dmvio::StateTransitionModel& transitionModel) +{ + if(imuInitLogic.realtimeCoarseIMUInit) + { + return std::make_unique(imuInitLogic, transitionModel); + }else + { + return std::make_unique(imuInitLogic, transitionModel); + } +} + +std::unique_ptr +dmvio::createPGBAState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& initValues) +{ + if(imuInitLogic.realtimePGBA) + { + return std::make_unique(imuInitLogic, transitionModel, std::move(initValues)); + }else + { + return std::make_unique(imuInitLogic, transitionModel, std::move(initValues)); + } +} + +dmvio::PGBAState::PGBAState(dmvio::IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& initValuesPassed) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), initValues(std::move(initValuesPassed)) +{} + +gtsam::Values* prepareInitValuesForPGBA(std::unique_ptr& initValuesNew, + const std::unique_ptr& initValues, + const gtsam::Values& baValues, dmvio::IMUInitializerLogic& logic) +{ + + if(!initValues) + { + // If BA + if(baValues.exists(gtsam::Symbol('s', 0))) + { + // Note: These delayedCurrValues also contain IMU variables (even though the graph doesn't) (only if IMU + // is initialized in the main graph). + initValuesNew = std::make_unique(logic.pgba->getInputDelayedGraph()->getDelayedCurrValues()); + for(auto&& pair : baValues) + { + eraseAndInsert(*initValuesNew, pair.key, pair.value); + } + }else + { + initValuesNew = std::make_unique(); + } + return initValuesNew.get(); + }else + { + for(auto&& pair : baValues) + { + if(!initValues->exists(pair.key)) + { + initValues->insert(pair.key, pair.value); + } + } + return initValues.get(); + } +} + +std::unique_ptr +dmvio::PGBAState::postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + DefaultActiveIMUInitializerState::postBAInit(keyframeId, activeHBFactor, baValues, timestamp, imuMeasurements); + + if(logic.pgba->getNumKFs() < 3) + { + return nullptr; + } + + // Don't overwrite member variable to not influence the potential next optimization. + std::unique_ptr initValuesNew; + gtsam::Values* initValuesUsed = prepareInitValuesForPGBA(initValuesNew, initValues, baValues, logic); + + try + { + logic.pgba->prepareOptimization(); + auto optimizedValues = std::make_unique( + logic.pgba->optimize(activeHBFactor, baValues, *initValuesUsed, false)); + + logic.transformDSOToIMUAfterPGBA->updateWithValues(*optimizedValues); + + IMUInitVariances variances(logic.pgba->getMarginals(*optimizedValues), + gtsam::Symbol('s', logic.transformDSOToIMUAfterPGBA->getSymbolInd()), + logic.pgba->getBiasKey()); + + // Write out result! + auto&& bias = optimizedValues->at(logic.pgba->getBiasKey()); + if(!variances.indetermined) + { + std::cout << "PGBA: " << logic.transformDSOToIMUAfterPGBA->getScale() << " var: " << variances.scaleVariance + << " " << variances.biasCovariance.diagonal().transpose() << std::endl; + logic.latestBias = bias; + } + + auto pair = transitionModel.pgbaOptimized(optimizedValues, variances); + if(pair.first) + { + if(pair.second) return std::move(pair.second); + // If values are taken, and pair.second is not nullptr, over we are responsible for creating the new state. + auto newState = std::make_unique(logic, transitionModel, + std::move(optimizedValues)); + return std::move(newState); + }else + { + logic.pgba->optimizationResultNotUsed(); + return std::move(pair.second); + } + }catch(gtsam::IndeterminantLinearSystemException& exc) + { + std::cout << "ERROR during PGBA!" << std::endl; + logic.pgba->optimizationResultNotUsed(); + std::unique_ptr emptyVals; + return transitionModel.pgbaOptimized(emptyVals, IMUInitVariances()).second; + } + +} + +void dmvio::PGBAState::print(std::ostream& str) const +{ + str << "PGBAState"; +} + + +dmvio::RealtimePGBAState::RealtimePGBAState(dmvio::IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel, + std::unique_ptr&& initValuesPassed) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), initValues(std::move(initValuesPassed)) +{} + +std::unique_ptr +dmvio::RealtimePGBAState::postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + dmvio::TimeMeasurement meas("RealtimePGBAState::postBAInit"); + if(running) + { + cachedData.emplace_back(imuMeasurements, keyframeId); + }else + { + DefaultActiveIMUInitializerState::postBAInit(keyframeId, activeHBFactor, baValues, timestamp, imuMeasurements); + + if(logic.pgba->getNumKFs() < 3) + { + return nullptr; + } + + // Prepare optimization + initValuesUsed = prepareInitValuesForPGBA(initValuesNew, initValues, baValues, logic); + this->activeHBFactor = activeHBFactor; + optimizingTimestamp = timestamp; + this->baValues = baValues; + + logic.pgba->prepareOptimization(); + + // Start optimization thread + std::thread runthread{&RealtimePGBAState::threadRun, this}; + running = true; + runthread.detach(); + } + return nullptr; +} + +void RealtimePGBAState::threadRun() +{ + dmvio::TimeMeasurement meas("RealtimePGBAState::threadRun"); + std::pair newStatePair; + std::unique_ptr optimizedValues; + try + { + optimizedValues = std::make_unique( + logic.pgba->optimize(activeHBFactor, baValues, *initValuesUsed, false)); + logic.transformDSOToIMUAfterPGBA->updateWithValues(*optimizedValues); + + IMUInitVariances variances(logic.pgba->getMarginals(*optimizedValues), + gtsam::Symbol('s', logic.transformDSOToIMUAfterPGBA->getSymbolInd()), + logic.pgba->getBiasKey()); + + // Write out result! + auto&& bias = optimizedValues->at(logic.pgba->getBiasKey()); + if(!variances.indetermined) + { + std::cout << "PGBA: " << logic.transformDSOToIMUAfterPGBA->getScale() << " var: " << variances.scaleVariance + << " " << variances.biasCovariance.diagonal().transpose() << std::endl; + logic.latestBias = bias; + } + + newStatePair = transitionModel.pgbaOptimized(optimizedValues, variances); + }catch(gtsam::IndeterminantLinearSystemException& exc) + { + std::cout << "ERROR during PGBA!" << std::endl; + std::unique_ptr emptyVals; + newStatePair = transitionModel.pgbaOptimized(emptyVals, IMUInitVariances()); + } + initValuesNew.reset(); + + if(!newStatePair.first) // Don't take over result (but maybe switch to new state provided by transition) + { + logic.pgba->optimizationResultNotUsed(); + + // Lock and add new things. + // Acquire lock to change the state. This ensures that the postBAInit method will not be called for this + // state in the meantime, which would lead to data being lost. + auto lock = logic.stateChanger.acquireSetStateLock(); + + // Add cached imu data and poses. + for(auto&& data : cachedData) + { + logic.pgba->addKeyframe(data.second, std::move(data.first)); + } + cachedData.clear(); + + if(!newStatePair.second) running = false; // do one or more optimization. + + logic.stateChanger.setState(std::move(newStatePair.second)); + + }else // Takeover result. + { + assert(!newStatePair.second); // other state created not yet supported for RT mode. + + // Unroll graph as far as we can in this separate thread.. + logic.pgba->preparePreparation(*optimizedValues); + + // Lock before creating the new state (so that no cachedData gets lost)! + auto lock = logic.stateChanger.acquireSetStateLock(); + + // Pass cachedData to RealtimeInitializedState + auto newState = std::make_unique(logic, transitionModel, + std::move(optimizedValues), + std::move(cachedData)); + + logic.stateChanger.setState(std::move(newState)); + } + +} + +void dmvio::RealtimePGBAState::print(std::ostream& str) const +{ + str << "RealtimePGBAState"; +} + +dmvio::InitializedFromPGBAState::InitializedFromPGBAState(dmvio::IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValuesPassed) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), + optimizedValues(std::move(optimizedValuesPassed)) +{} + +std::pair, bool> dmvio::InitializedFromPGBAState::initializeIfReady() +{ + auto delayedGraph = logic.pgba->prepareGraphForMainOptimization(*optimizedValues); + + logic.callOnInit(delayedGraph->getDelayedCurrValues(), true); + delayedGraph->setMaxGroupInGraph(BAIMULogic::METRIC_GROUP); // From now on all factors should be added to this. + logic.delayedMarginalizationGraphs->replaceMainGraph(std::move(delayedGraph)); + return std::make_pair(transitionModel.initialized(), true); +} + +void dmvio::InitializedFromPGBAState::print(std::ostream& str) const +{ + str << "InitializedFromPGBAState"; +} + +dmvio::InitializedFromRealtimePGBAState::InitializedFromRealtimePGBAState(dmvio::IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValuesPassed, + PoseGraphBundleAdjustment::KeyframeDataContainer&& cachedDataPassed) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), + optimizedValues(std::move(optimizedValuesPassed)), + cachedData(std::move(cachedDataPassed)) +{} + +std::unique_ptr +InitializedFromRealtimePGBAState::postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, double timestamp, + const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + // First add the newest KF. + cachedData.emplace_back(imuMeasurements, keyframeId); + + optimizedValues = std::make_unique( + logic.pgba->extendGraph(activeHBFactor, std::move(*optimizedValues), baValues, cachedData)); + cachedData.clear(); + prepared = true; + return nullptr; +} + +std::pair, bool> dmvio::InitializedFromRealtimePGBAState::initializeIfReady() +{ + if(!prepared) + return std::make_pair(nullptr, + false); // postBAInit was not run yet, which means that the new factors have not been added. + dmvio::TimeMeasurement meas("InitializedFromRealtimePGBAState::initializeIfReady"); + auto delayedGraph = logic.pgba->prepareGraphForMainOptimization(*optimizedValues); + + logic.callOnInit(delayedGraph->getDelayedCurrValues(), true); + delayedGraph->setMaxGroupInGraph(BAIMULogic::METRIC_GROUP); // From now on all factors should be added to this. + logic.delayedMarginalizationGraphs->replaceMainGraph(std::move(delayedGraph)); + return std::make_pair(transitionModel.initialized(), true); +} + +void dmvio::InitializedFromRealtimePGBAState::print(std::ostream& str) const +{ + str << "InitializedFromRealtimePGBAState"; +} + +dmvio::NoMarginalizationReplacementInitializedFromPGBAState::NoMarginalizationReplacementInitializedFromPGBAState( + dmvio::IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValuesPassed) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), + optimizedValues(std::move(optimizedValuesPassed)) +{} + +std::pair, bool> +dmvio::NoMarginalizationReplacementInitializedFromPGBAState::initializeIfReady() +{ + // If this ablation is done in RT mode (which it was not in the paper) building of the graph should be removed as + // well. + auto delayedGraph = logic.pgba->prepareGraphForMainOptimization(*optimizedValues); + + logic.callOnInit(delayedGraph->getDelayedCurrValues(), false); + + return std::make_pair(transitionModel.initialized(), true); +} + +void dmvio::NoMarginalizationReplacementInitializedFromPGBAState::print(std::ostream& str) const +{ + str << "NoMarginalizationReplacementInitializedFromPGBAState"; +} + +dmvio::InitializedFromCoarseIMUInitState::InitializedFromCoarseIMUInitState(dmvio::IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel) +{} + +std::pair, bool> dmvio::InitializedFromCoarseIMUInitState::initializeIfReady() +{ + logic.callOnInit(logic.coarseIMUOptimizer->optimizedValues, false); + + return std::make_pair(transitionModel.initialized(), true); +} + +void dmvio::InitializedFromCoarseIMUInitState::print(std::ostream& str) const +{ + str << "InitializedFromCoarseIMUInitState"; +} + +dmvio::PotentialMarginalizationReplacementState::PotentialMarginalizationReplacementState( + dmvio::IMUInitializerLogic& imuInitLogic, dmvio::StateTransitionModel& transitionModel, int startIdForSecondTh) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), startIdForSecondTH(startIdForSecondTh) +{} + +std::unique_ptr dmvio::PotentialMarginalizationReplacementState::postBAInit(int keyframeId, + gtsam::NonlinearFactor::shared_ptr activeHBFactor, + const gtsam::Values& baValues, + double timestamp, + const gtsam::PreintegratedImuMeasurements& imuMeasurements) +{ + DefaultActiveIMUInitializerState::postBAInit(keyframeId, activeHBFactor, baValues, timestamp, imuMeasurements); + + auto* fejVals = logic.delayedMarginalizationGraphs->getMainGraph()->fejValues.get(); + if(!fejVals) return nullptr; + bool changed = thresholdVariableChanges( + useSecondTH ? logic.settings.secondThresholdSettings : logic.settings.thresholdSettings, baValues, + fejVals->fejValues); + + if(!changed) return nullptr; + + dmvio::TimeMeasurement meas("MarginalizationReplacementBuildGraph"); + + gtsam::Values initValues(logic.pgba->getInputDelayedGraph()->getDelayedCurrValues()); + for(auto&& pair : baValues) + { + eraseAndInsert(initValues, pair.key, pair.value); + } + logic.pgba->prepareOptimization(); + // We pass noOptimization=true, so that the graph is only built, but not actually optimized. + auto values = std::make_unique( + logic.pgba->optimize(activeHBFactor, baValues, initValues, true)); + + // Get minimum connected pose and maybe switch to second threshold! + int firstId = logic.pgba->getFirstIdWithIMUData(); + if(!useSecondTH && firstId >= startIdForSecondTH) + { + // From now on we need to use the second threshold. + useSecondTH = true; + // That means we need to check again if this threshold is met as well. + bool reallyChanged = thresholdVariableChanges(logic.settings.secondThresholdSettings, baValues, + fejVals->fejValues); + if(!reallyChanged) + { + std::cout << "IMUInitialization (switch): Found out that second threshold was not met!" << std::endl; + logic.pgba->optimizationResultNotUsed(); + return nullptr; + } + } + + return transitionModel.marginalizationReplacementReady(std::move(values)); +} + +void dmvio::PotentialMarginalizationReplacementState::print(std::ostream& str) const +{ + str << "PotentialMarginalizationReplacementState"; +} + +dmvio::MarginalizationReplacementReadyState::MarginalizationReplacementReadyState( + dmvio::IMUInitializerLogic& imuInitLogic, + dmvio::StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValues) + : DefaultActiveIMUInitializerState(imuInitLogic, transitionModel), + optimizedValues(std::move(optimizedValues)) +{} + +std::pair, bool> dmvio::MarginalizationReplacementReadyState::initializeIfReady() +{ + dmvio::TimeMeasurement meas("MarginalizationReplacementReplaceGraph"); + + auto delayedGraph = logic.pgba->prepareGraphForMainOptimization(*optimizedValues); + delayedGraph->setMaxGroupInGraph(BAIMULogic::METRIC_GROUP); // From now on all factors should be added to this. + logic.delayedMarginalizationGraphs->replaceMainGraph(std::move(delayedGraph)); + + return std::make_pair(transitionModel.marginalizationReplaced(), false); +} + +void dmvio::MarginalizationReplacementReadyState::print(std::ostream& str) const +{ + str << "MarginalizationReplacementReadyState"; +} diff --git a/src/IMUInitialization/IMUInitializerStates.h b/src/IMUInitialization/IMUInitializerStates.h new file mode 100644 index 0000000..2e3e415 --- /dev/null +++ b/src/IMUInitialization/IMUInitializerStates.h @@ -0,0 +1,309 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUINITIALIZERSTATES_H +#define DMVIO_IMUINITIALIZERSTATES_H + +#include "dso/util/FrameShell.h" +#include +#include "IMU/IMUTypes.h" +#include "IMUInitializerLogic.h" +#include +#include +#include "IMUInitStateChanger.h" + +namespace dmvio +{ + +// Abstract base class for the states of our state machine. +// States can be switched in two ways: +// - The called method can return the new state (or nullptr if the same state should be kept). +// - By manually calling the methods of IMUInitStateChanger (stored in IMUInitializerLogic). This is for changing the +// state in a separate thread and mainly used by the realtime states. +// IMPORTANT: This should not be called inside the methods defined by IMUInitializerState, as they already have a lock +// on the state (and it would result in a deadlock). They should instead just return the new state. +// Initialization is either performed in addPose (for the CoarseIMUInit) or in postBAInit (for the PGBA). +class IMUInitializerState +{ +public: + virtual ~IMUInitializerState() = default; + + // Add coarse pose for use in initialization. + // Potentially run coarse IMU init. + virtual std::unique_ptr addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) = 0; + + // Called after the Bundle Adjustment (BA), used by some states to run PGBA. + virtual std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) = 0; + + // Called after all keyframe operations are finished. This is when the IMU in the main system should be + // initialized (if the initialized values are ready). Returns true if an initialization is performed. + virtual std::pair, bool> initializeIfReady() = 0; + + friend std::ostream& operator<<(std::ostream& str, IMUInitializerState const& data) + { + data.print(str); + return str; + } + + using unique_ptr = std::unique_ptr; + +protected: + // print state name. + virtual void print(std::ostream& str) const = 0; +}; + +// State for doing no initialization anymore. +class InactiveIMUInitializerState : public IMUInitializerState +{ +public: + std::unique_ptr addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) override + { return nullptr; } + + std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) override + { return nullptr; } + + std::pair, bool> initializeIfReady() override + { + return std::make_pair(nullptr, false); + } + + void print(std::ostream& str) const override + { + str << "InactiveIMUInitializerState"; + } +}; + +// Default state which forwards poses and keyframes to the CoarseIMUInitOptimizer and PoseGraphBundleAdjustment +// respectively, but doesn't do anything else. +class DefaultActiveIMUInitializerState : public IMUInitializerState +{ +public: + // We keep a reference to IMUInitializerLogic and transitionModel without using shared_ptr, as the parent + // IMUInitializer is responsible for handling memory. + DefaultActiveIMUInitializerState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel); + + std::unique_ptr addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) override; + + std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) override; + + std::pair, bool> initializeIfReady() override; + +protected: + IMUInitializerLogic& logic; + StateTransitionModel& transitionModel; +}; + +// State when the CoarseIMUInitOptimizer is the next step. +class CoarseIMUInitState : public DefaultActiveIMUInitializerState +{ +public: + CoarseIMUInitState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel); + + std::unique_ptr addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) override; + + void print(std::ostream& str) const override; +}; + +// Realtime version of the CoarseIMUInitState: Performs optimization in a separate thread. +class RealtimeCoarseIMUInitState : public DefaultActiveIMUInitializerState +{ +public: + RealtimeCoarseIMUInitState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel); + + std::unique_ptr addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, + const IMUData* imuData) override; + + void print(std::ostream& str) const override; +private: + void threadRun(); + + enum ThreadStatus + { + NOT_RUNNING, RUNNING + }; + ThreadStatus status = NOT_RUNNING; + std::thread runthread; + + double optimizingTimestamp; + using AddPoseData = std::tuple; // We need to save the pointers because + // CoarseIMUInitOptimizer will use them to get the updated poses later. + std::vector cachedData; +}; + +// depending on the value of imuInitLogic.realtimeCoaresIMUInit this creates either a CoarseIMUInitState or a +// RealtimeCoarseIMUInitState +std::unique_ptr +createCoarseIMUInitState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel); +// same as last method, but for PGBA. +std::unique_ptr +createPGBAState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& initValues); + +class PGBAState : public DefaultActiveIMUInitializerState +{ +public: + PGBAState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& initValues); + std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) override; + + void print(std::ostream& str) const override; +private: + std::unique_ptr initValues; +}; + +class RealtimePGBAState : public DefaultActiveIMUInitializerState +{ +public: + RealtimePGBAState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& initValues); + + std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) override; + + void print(std::ostream& str) const override; + + void threadRun(); + +private: + // Variables for the optimization + std::unique_ptr initValues; + std::unique_ptr initValuesNew; // this one is deleted after every optimization. + gtsam::Values* initValuesUsed = nullptr; // points to one of the above vars. + gtsam::NonlinearFactor::shared_ptr activeHBFactor; + gtsam::Values baValues; + + bool running = false; + double optimizingTimestamp; + PoseGraphBundleAdjustment::KeyframeDataContainer cachedData; +}; + +class InitializedFromPGBAState : public DefaultActiveIMUInitializerState +{ +public: + InitializedFromPGBAState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValues); + + std::pair, bool> initializeIfReady() override; + + void print(std::ostream& str) const override; +private: + std::unique_ptr optimizedValues; +}; + +class InitializedFromRealtimePGBAState : public DefaultActiveIMUInitializerState +{ +public: + InitializedFromRealtimePGBAState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValues, + PoseGraphBundleAdjustment::KeyframeDataContainer&& cachedData); + + std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) override; + + std::pair, bool> initializeIfReady() override; + + void print(std::ostream& str) const override; +private: + std::unique_ptr optimizedValues; + PoseGraphBundleAdjustment::KeyframeDataContainer cachedData; + bool prepared = false; +}; + + +// For ablations: do not perform the initial marginalization replacement. +class NoMarginalizationReplacementInitializedFromPGBAState : public DefaultActiveIMUInitializerState +{ +public: + NoMarginalizationReplacementInitializedFromPGBAState(IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValues); + + std::pair, bool> initializeIfReady() override; + + void print(std::ostream& str) const override; +private: + std::unique_ptr optimizedValues; +}; + +class InitializedFromCoarseIMUInitState : public DefaultActiveIMUInitializerState +{ +public: + InitializedFromCoarseIMUInitState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel); + + std::pair, bool> initializeIfReady() override; + + void print(std::ostream& str) const override; +}; + +// State where the marginalization factor is replaced if the (scale) diff is larger than a threshold. +class PotentialMarginalizationReplacementState : public DefaultActiveIMUInitializerState +{ +public: + PotentialMarginalizationReplacementState(IMUInitializerLogic& imuInitLogic, StateTransitionModel& transitionModel, + int startIdForSecondTh); + + std::unique_ptr + postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr activeHBFactor, const gtsam::Values& baValues, + double timestamp, const gtsam::PreintegratedImuMeasurements& imuMeasurements) override; + + void print(std::ostream& str) const override; + +protected: + int startIdForSecondTH; + bool useSecondTH = false; +}; + +// Replaces the marginalization prior of the main graph, but doesn't call callOnInit as the values shall +// not be replaced (in contrast to a normal init). +class MarginalizationReplacementReadyState : public DefaultActiveIMUInitializerState +{ +public: + MarginalizationReplacementReadyState(IMUInitializerLogic& imuInitLogic, + StateTransitionModel& transitionModel, + std::unique_ptr&& optimizedValues); + + std::pair, bool> initializeIfReady() override; + + void print(std::ostream& str) const override; + +private: + std::unique_ptr optimizedValues; +}; + + +} + +#endif //DMVIO_IMUINITIALIZERSTATES_H diff --git a/src/IMUInitialization/IMUInitializerTransitions.cpp b/src/IMUInitialization/IMUInitializerTransitions.cpp new file mode 100644 index 0000000..02b3595 --- /dev/null +++ b/src/IMUInitialization/IMUInitializerTransitions.cpp @@ -0,0 +1,226 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "IMUInitializerTransitions.h" + +using namespace dmvio; + +std::unique_ptr +dmvio::createTransitionModel(InitTransitionMode mode, IMUInitializerLogic& logic) +{ + switch(mode) + { + case InitTransitionMode::COMBINED_MODEL: + return std::make_unique(logic); + case InitTransitionMode::COMBINED_REPLACEMENT_MODEL: + return std::make_unique(logic); + case InitTransitionMode::COMBINED_NO_INITIAL_REPLACEMENT: + return std::make_unique(logic); + case InitTransitionMode::ONLY_COARSE_IMU_INIT: + return std::make_unique(logic); + default: + std::cout << "ERROR: Trying to use non-implemented transition model!" << std::endl; + assert(false); + } +} + +dmvio::CombinedTransitionModel::CombinedTransitionModel(dmvio::IMUInitializerLogic& logic) + : logic(logic) +{} + +dmvio::IMUInitializerState::unique_ptr dmvio::CombinedTransitionModel::getInitialState() +{ + // start with coarse imu init. + return createCoarseIMUInitState(logic, *this); +} + +dmvio::IMUInitializerState::unique_ptr +dmvio::CombinedTransitionModel::coarseIMUInitOptimized(const gtsam::Values& optimizedValues, + dmvio::IMUInitVariances initVariances) +{ + // after coarse imu init continue with pgba. + if(!initVariances.indetermined && initVariances.scaleVariance < logic.settings.coarseScaleUncertaintyThresh) + { + return createPGBAState(logic, *this, std::make_unique(optimizedValues)); + } + return nullptr; +} + +std::pair +dmvio::CombinedTransitionModel::pgbaOptimized(std::unique_ptr& optimizedValues, + dmvio::IMUInitVariances initVariances) +{ + // We distinguish the first initialization vs reinitialization. + if(lastInitUncert.indetermined) + { + // Takeover initialization if < threshold. + if(!initVariances.indetermined && + initVariances.scaleVariance < logic.settings.pgbaSettings.scaleUncertaintyThresh) + { + return takeOverLargeBAOptim(optimizedValues, std::move(initVariances)); + }else + { + // If init failed we try again with the CoarseIMUInitOptimizer next time. + return std::make_pair(false, createCoarseIMUInitState(logic, *this)); + } + }else + { + // Takeover initialization if uncertainty < last init. + if(!initVariances.indetermined && initVariances.scaleVariance <= lastInitUncert.scaleVariance) + { + return takeOverLargeBAOptim(optimizedValues, std::move(initVariances)); + }else + { + // Try reinit again (note that we don't go back to the CoarseIMUInitOptimizer here). + return std::make_pair(false, nullptr); + } + } +} + +std::pair +dmvio::CombinedTransitionModel::takeOverLargeBAOptim(std::unique_ptr& optimizedValues, + IMUInitVariances&& initVariances) +{ + lastInitUncert = std::move(initVariances); + // The state is responsible for creating either the InitializedFromPGBAState or the InitializedFromRealtimePGBAState + return std::make_pair(true, nullptr); +} + +dmvio::IMUInitializerState::unique_ptr dmvio::CombinedTransitionModel::initialized() +{ + if(lastInitUncert.scaleVariance > logic.settings.pgbaSettings.reinitScaleUncertaintyThresh) + { + // Reinit as long as the reinit threshold is exceeded. + // Giving no initValues will try to use the newest values from BA. + return createPGBAState(logic, *this, nullptr); + }else + { + // Else we stop. + return std::make_unique(); + } +} + +dmvio::CombinedWithMarginalizationReplacementModel::CombinedWithMarginalizationReplacementModel( + dmvio::IMUInitializerLogic& logic) + : CombinedTransitionModel(logic) +{} + +dmvio::IMUInitializerState::unique_ptr dmvio::CombinedWithMarginalizationReplacementModel::initialized() +{ + if(lastInitUncert.scaleVariance > logic.settings.pgbaSettings.reinitScaleUncertaintyThresh) + { + // Reinit as long as the reinit threshold is exceeded. + // Giving no initValues will try to use the newest values from BA. + return createPGBAState(logic, *this, nullptr); + }else + { + // Else we continue with potential marginalization replacement. + assert(startIdSecondTh >= 0); + return std::make_unique(logic, *this, startIdSecondTh); + } +} + +std::pair +dmvio::CombinedWithMarginalizationReplacementModel::takeOverLargeBAOptim( + std::unique_ptr& optimizedValues, + IMUInitVariances&& initVariances) +{ + // We need to find out when more than 50% of IMU factors will be lost. + // From then on the second threshold will be used (which we set to infinity in practice, meaning that from then + // on no marginalization replacement will be done anymore). + int n = (int) (logic.settings.percentageSwitchToSecondTH * logic.settings.pgbaSettings.delay); + startIdSecondTh = logic.pgba->getIdOfNthIMUData(n); + return CombinedTransitionModel::takeOverLargeBAOptim(optimizedValues, std::move(initVariances)); +} + +dmvio::IMUInitializerState::unique_ptr +dmvio::CombinedWithMarginalizationReplacementModel::marginalizationReplacementReady( + std::unique_ptr&& optimizedValues) +{ + // Update startId for second TH. + int n = (int) (logic.settings.percentageSwitchToSecondTH * logic.settings.pgbaSettings.delay); + startIdSecondTh = logic.pgba->getIdOfNthIMUData(n); + return std::make_unique(logic, *this, std::move(optimizedValues)); +} + +dmvio::IMUInitializerState::unique_ptr dmvio::CombinedWithMarginalizationReplacementModel::marginalizationReplaced() +{ + // Potentially do more marginalization replacements in the future. + return std::make_unique(logic, *this, startIdSecondTh); +} + +dmvio::CombinedTransitionModelNoInitialMarginalizationReplacement::CombinedTransitionModelNoInitialMarginalizationReplacement( + dmvio::IMUInitializerLogic& logic) : CombinedTransitionModel(logic) +{} + +std::pair +dmvio::CombinedTransitionModelNoInitialMarginalizationReplacement::takeOverLargeBAOptim( + std::unique_ptr& optimizedValues, + IMUInitVariances&& initVariances) +{ + lastInitUncert = std::move(initVariances); + return std::make_pair(true, std::make_unique(logic, *this, + std::move( + optimizedValues))); +} + +dmvio::OnlyCoarseIMUInitTransitionModel::OnlyCoarseIMUInitTransitionModel(dmvio::IMUInitializerLogic& logic) + : logic(logic) +{} + +dmvio::IMUInitializerState::unique_ptr dmvio::OnlyCoarseIMUInitTransitionModel::getInitialState() +{ + return createCoarseIMUInitState(logic, *this); +} + +dmvio::IMUInitializerState::unique_ptr +dmvio::OnlyCoarseIMUInitTransitionModel::coarseIMUInitOptimized(const gtsam::Values& optimizedValues, + dmvio::IMUInitVariances initVariances) +{ + if(!initVariances.indetermined && initVariances.scaleVariance < logic.settings.coarseScaleUncertaintyThresh) + { + return std::make_unique(logic, *this); + } + return nullptr; +} + +std::pair +dmvio::OnlyCoarseIMUInitTransitionModel::pgbaOptimized(std::unique_ptr& optimizedValues, + dmvio::IMUInitVariances initVariances) +{ + assert(false); + return std::make_pair(false, nullptr); +} + +dmvio::IMUInitializerState::unique_ptr dmvio::OnlyCoarseIMUInitTransitionModel::initialized() +{ + if(lastInitUncert.scaleVariance > logic.settings.pgbaSettings.reinitScaleUncertaintyThresh) + { + // Reinit as long as the reinit threshold is exceeded. + // Giving no initValues will try to use the newest values from BA. + return createCoarseIMUInitState(logic, *this); + }else + { + // Else we stop. + return std::make_unique(); + } +} \ No newline at end of file diff --git a/src/IMUInitialization/IMUInitializerTransitions.h b/src/IMUInitialization/IMUInitializerTransitions.h new file mode 100644 index 0000000..17ed78e --- /dev/null +++ b/src/IMUInitialization/IMUInitializerTransitions.h @@ -0,0 +1,167 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_IMUINITIALIZERTRANSITIONS_H +#define DMVIO_IMUINITIALIZERTRANSITIONS_H + +#include "IMUInitializerStates.h" + +namespace dmvio +{ + +// Defines how to transition between states. There are multiple different transition models for the main method and +// ablation studies. +class StateTransitionModel +{ +public: + virtual ~StateTransitionModel() = default; + + // Return the initial state. + virtual IMUInitializerState::unique_ptr getInitialState() = 0; + + // Called when the coarse IMU init optimization has finished and returns the new state. + virtual IMUInitializerState::unique_ptr coarseIMUInitOptimized(const gtsam::Values& optimizedValues, + dmvio::IMUInitVariances initVariances) = 0; + + // Called when a PGBA has been finished. If return.first is true the result of this optimization shall + // be taken over. In this case, if return.second is nullptr the caller state is responsible for creating a proper new state itself. + // If return.first is true the also returned state shall always be used. + // Note that the passed optimizedValues can be moved away, if pair.second != nullptr. + virtual std::pair + pgbaOptimized(std::unique_ptr& optimizedValues, IMUInitVariances initVariances) = 0; + + virtual IMUInitializerState::unique_ptr initialized() = 0; + + virtual IMUInitializerState::unique_ptr + marginalizationReplacementReady(std::unique_ptr&& optimizedValues) + {}; + + virtual IMUInitializerState::unique_ptr marginalizationReplaced() + {}; +}; + +// Enumeration for the available transition models. +// (The legacy options were removed but are still in the enum so that the indices are the same as earlier version). +// - 2 (COMBINED_REPLACEMENT_MODEL) is the default +// - 1 is no marginalization replacement ablation +// - 4 is no initial readvancing ablation +// - 5 is no PGBA ablation. +enum class InitTransitionMode +{ + LEGACY_UNUSED, COMBINED_MODEL, COMBINED_REPLACEMENT_MODEL, LEGACY_UNUSED2, COMBINED_NO_INITIAL_REPLACEMENT, + ONLY_COARSE_IMU_INIT +}; + +std::unique_ptr +createTransitionModel(InitTransitionMode mode, IMUInitializerLogic& logic); + + +// This is similar to the transition model shown in the paper but without the marginalization replacement. +// TransitionModel with 2 thresholds, where reinitialization is performed as long as the scale uncertainty is greater +// than a second larger threshold. +class CombinedTransitionModel : public StateTransitionModel +{ +public: + // A reference to logic is kept and must be alive together with this object. + CombinedTransitionModel(IMUInitializerLogic& logic); + + IMUInitializerState::unique_ptr getInitialState() override; + + IMUInitializerState::unique_ptr + coarseIMUInitOptimized(const gtsam::Values& optimizedValues, dmvio::IMUInitVariances initVariances) override; + + std::pair pgbaOptimized( + std::unique_ptr& optimizedValues, + IMUInitVariances initVariances) override; + + IMUInitializerState::unique_ptr initialized() override; + +protected: + IMUInitializerLogic& logic; + IMUInitVariances lastInitUncert; + + virtual std::pair + takeOverLargeBAOptim(std::unique_ptr& optimizedValues, + IMUInitVariances&& initVariances); +}; + +// This is the full transition model as shown in the paper. +// We base it on the CombinedTransitionModel and only override the methods which need to be changed. +class CombinedWithMarginalizationReplacementModel : public CombinedTransitionModel +{ +public: + CombinedWithMarginalizationReplacementModel(IMUInitializerLogic& logic); + + IMUInitializerState::unique_ptr initialized() override; + + virtual IMUInitializerState::unique_ptr + marginalizationReplacementReady(std::unique_ptr&& optimizedValues) override; + + virtual IMUInitializerState::unique_ptr marginalizationReplaced() override; + +protected: + std::pair + takeOverLargeBAOptim(std::unique_ptr& optimizedValues, + IMUInitVariances&& initVariances) override; + + int startIdSecondTh = -1; +}; + +// For ablations: This transition model does not do a marginalization replacement after initializing (despite using +// PGBA). +class CombinedTransitionModelNoInitialMarginalizationReplacement : public CombinedTransitionModel +{ +public: + CombinedTransitionModelNoInitialMarginalizationReplacement(IMUInitializerLogic& logic); + +protected: + std::pair + takeOverLargeBAOptim(std::unique_ptr& optimizedValues, + IMUInitVariances&& initVariances) override; +}; + +// This transition model only uses the CoarseIMUInit. +class OnlyCoarseIMUInitTransitionModel : public StateTransitionModel +{ +public: + // A reference to logic is kept and must be alive together with this object. + OnlyCoarseIMUInitTransitionModel(IMUInitializerLogic& logic); + + IMUInitializerState::unique_ptr getInitialState() override; + + IMUInitializerState::unique_ptr + coarseIMUInitOptimized(const gtsam::Values& optimizedValues, dmvio::IMUInitVariances initVariances) override; + + std::pair pgbaOptimized( + std::unique_ptr& optimizedValues, + IMUInitVariances initVariances) override; + + IMUInitializerState::unique_ptr initialized() override; + +protected: + IMUInitializerLogic& logic; + IMUInitVariances lastInitUncert; +}; + +} + +#endif //DMVIO_IMUINITIALIZERTRANSITIONS_H diff --git a/src/IMUInitialization/PoseGraphBundleAdjustment.cpp b/src/IMUInitialization/PoseGraphBundleAdjustment.cpp new file mode 100644 index 0000000..91544a0 --- /dev/null +++ b/src/IMUInitialization/PoseGraphBundleAdjustment.cpp @@ -0,0 +1,613 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "PoseGraphBundleAdjustment.h" + +#include +#include "util/TimeMeasurement.h" +#include +#include "GTSAMIntegration/Sim3GTSAM.h" +#include "GTSAMIntegration/Marginalization.h" +#include "GTSAMIntegration/FEJNoiseModelFactor.h" +#include "GTSAMIntegration/GTSAMUtils.h" +#include +#include + +using namespace dmvio; +using namespace gtsam; + +using symbol_shorthand::P, symbol_shorthand::S, symbol_shorthand::V, symbol_shorthand::B, symbol_shorthand::A; + +PoseGraphBundleAdjustment::PoseGraphBundleAdjustment(DelayedMarginalizationGraphs* delayedMarginalizationPassed, + const IMUCalibration& imuCalibration, const PGBASettings& settings, + std::shared_ptr transformDSOToIMUPassed) + : delayedMarginalization(delayedMarginalizationPassed), + imuCalibration(imuCalibration), + settings(settings), + transformDSOToIMU(std::move(transformDSOToIMUPassed)) +{ + inputDelayedGraph = delayedMarginalization->addDelayedGraph(settings.delay, + BAIMULogic::NO_IMU_GROUP); // This graph will only contain visual factors. +} + +void PoseGraphBundleAdjustment::prepareOptimization() +{ + // clone DelayedGraph + delayedGraph = std::make_unique(*inputDelayedGraph); + disconnectedGraph = delayedMarginalization->addDisconnectedGraph(delayedGraph->getMaxGroupInGraph()); +} + +gtsam::Values dmvio::PoseGraphBundleAdjustment::optimize(gtsam::NonlinearFactor::shared_ptr activeDSOFactor, + const gtsam::Values& baValues, + const gtsam::Values& imuInitValues, bool noOptimization) +{ + dmvio::TimeMeasurement fullMeas("PGBAFull"); + + graph = delayedGraph->getGraph().get(); + + if(!imuInitValues.empty()) + { + transformDSOToIMU->updateWithValues(imuInitValues); + std::cout << "Updating transform with index " << transformDSOToIMU->getSymbolInd() << " to val: " + << transformDSOToIMU->getScale() << std::endl; + } + + graph->push_back(activeDSOFactor); + dsoFactorPos = graph->size() - 1; + + const gtsam::Values& inputValues = delayedGraph->getDelayedCurrValues(); + // Call buildGraph to fill the graph with the IMUFactors + gtsam::Values values = buildGraph(*graph, baValues, inputValues, imuInitValues, noOptimization); + + if(noOptimization) return values; + + dmvio::TimeMeasurement optMeas("PGBA"); + + // Enable FEJ for this optimization. + delayedGraph->setFEJValuesForFactors(true); + + // Call gtsam optimize and return the value. + gtsam::LevenbergMarquardtParams params = gtsam::LevenbergMarquardtParams::CeresDefaults(); + LevenbergMarquardtOptimizer optimizer(*graph, values, params); + Values newValues = optimizer.optimize(); + transformDSOToIMU->updateWithValues(newValues); + + double error = optimizer.error(); + std::cout << "PGBA Error: " << error << std::endl; + + delayedGraph->setFEJValuesForFactors(false); + + return newValues; +} + +gtsam::Marginals PoseGraphBundleAdjustment::getMarginals(const Values& values) +{ + return gtsam::Marginals(*graph, values); +} + +// Extend graph with the disconnected graph. +gtsam::Values dmvio::PoseGraphBundleAdjustment::extendGraph(gtsam::NonlinearFactor::shared_ptr activeDSOFactor, + gtsam::Values&& previouslyOptimizedValues, + const gtsam::Values& baValues, + const KeyframeDataContainer& cachedData) +{ + dmvio::TimeMeasurement fullMeas("ExtendGraph"); + // Note: We need to add the cachedData before calling insertIMUFactorsAndValues, because it accesses prevKFIds, which is updated here. + for(auto&& data : cachedData) + { + addKeyframe(data.second, data.first); + } + + // New factors added, this means that probably also new variables were added. + // --> Unless all variables are available otherwise we need to re-optimize. + + // Find last available bias and velocity from previouslyOptimizedValue + auto previouslyOptimizedKeys = previouslyOptimizedValues.keys(); + + gtsam::Vector3 velocity = Vector3::Zero(); + imuBias::ConstantBias imuBias; + + gtsam::Key latestVelKey = getMaxKeyWithChr(previouslyOptimizedKeys, 'v'); + gtsam::Key latestBiasKey = getMaxKeyWithChr(previouslyOptimizedKeys, 'b'); + velocity = previouslyOptimizedValues.at(latestVelKey); + imuBias = previouslyOptimizedValues.at(latestBiasKey); + + this->graph = delayedGraph->getGraph().get(); // also update member variable. + gtsam::NonlinearFactorGraph& graph = *(delayedGraph->graph); + // previouslyOptimizedValues must not be used below this! + gtsam::Values values(std::move(previouslyOptimizedValues)); + + + // -------------------- Extend graph with new IMU factors. + const gtsam::Values& inputValues2 = disconnectedGraph->delayedCurrValues; + + gtsam::KeyVector addedKeys(activeDSOFactor->keys()); + for(auto&& factor : disconnectedGraph->addedFactors) + { + graph.add(factor); + auto&& keys = factor->keys(); + addedKeys.insert(addedKeys.end(), keys.begin(), keys.end()); + } + // sort and make unique. + std::sort(addedKeys.begin(), addedKeys.end()); + addedKeys.erase(std::unique(addedKeys.begin(), addedKeys.end()), addedKeys.end()); + + disconnectedGraph->addedFactors.clear(); + + // add active DSO factor. + graph.push_back(activeDSOFactor); + dsoFactorPos = graph.size() - 1; + + + gtsam::Values imuInputValues = disconnectedGraph->delayedCurrValues; + // Use baValues if possible, otherwise new delayedCurrValues. + for(auto&& pair : baValues) + { + eraseAndInsert(imuInputValues, pair.key, pair.value); + } + + long long minConnectedPoseInd = prevKFIds.at(cachedData.front().second); + + // Add new values + for(auto&& key : addedKeys) + { + if(!values.exists(key)) + { + values.insert(key, imuInputValues.at(key)); + } + } + + // We don't do negative energy compensation here again, although maybe it might be useful. + if(!settings.prepareGraphAddDelValues) + { + for(auto&& val : disconnectedGraph->delayedValues) + { + if(!delayedGraph->delayedValues.exists(val.key)) + { + delayedGraph->delayedValues.insert(val.key, val.value); + } + } + } + + + // We want to use disconnectedGraph->delayedCurrValues, and eraseAndInsert all baValues. + numIMUFactors = insertIMUFactorsAndValues(graph, cachedData, imuInputValues, values, minConnectedPoseInd, imuBias, + velocity); + + // Optimize with the newly added variables. + delayedGraph->setFEJValuesForFactors(true); + + gtsam::LevenbergMarquardtParams params = gtsam::LevenbergMarquardtParams::CeresDefaults(); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values newValues = optimizer.optimize(); + transformDSOToIMU->updateWithValues(newValues); + + delayedGraph->setFEJValuesForFactors(false); + + return newValues; +} + +gtsam::Values +dmvio::PoseGraphBundleAdjustment::buildGraph(gtsam::NonlinearFactorGraph& graph, const gtsam::Values& poseInputValues, + const gtsam::Values& poseInputValues2, const gtsam::Values& imuInputValues, + bool noOptimization) +{ + gtsam::Values values; + // get Ordering of all poses. + auto&& keyVector = graph.keyVector(); // this method already sorts it! + + // find out first pose which is connected to the end! + long long minConnectedPoseInd = insertValuesAndGetMinConnectedPoseInd(poseInputValues, poseInputValues2, values, + keyVector); + + allPosesUsed = minConnectedPoseInd == 0; + + // First remove all preintegrated measurements before minConnectedPoseInd + while(preintegratedForKF.size() > 0 && preintegratedForKF.front().second <= minConnectedPoseInd) + { + preintegratedForKF.pop_front(); + } + minConnectedPoseInd = std::max((int) minConnectedPoseInd, prevKFIds[preintegratedForKF[0].second]); + assert(prevKFIds[preintegratedForKF[0].second] == minConnectedPoseInd); + + firstId = minConnectedPoseInd; + + if(!noOptimization) + { + // Workaround: Unfortunately the DSO marginalization factors can get a (large) negative energy which is problematic + // for the GTSAM optimizer. So we do one GN-iteration, and manually add this energy to the graph. + auto negFac = compensateNegativeEnergy(graph, values, *transformDSOToIMU); + if(negFac) + { + graph.push_back(negFac); + } + } + + // From there one insert all IMU factors. + imuBias::ConstantBias imuBias; + gtsam::Vector3 velocity = Vector3::Zero(); + if(imuInputValues.exists(B(0))) + { + imuBias = imuInputValues.at(B(0)); + } + + // Insert values for first KF: + if(imuInputValues.exists(V(firstId))) + { + velocity = imuInputValues.at(V(firstId)); + } + if(imuInputValues.exists(B(firstId))) + { + imuBias = imuInputValues.at(B(firstId)); + } + values.insert(B(firstId), imuBias); + values.insert(V(firstId), velocity); + + // Insert values for IMU data: If initial velocity or bias does not exist for a KF we use the values of the previous KF. + numIMUFactors = insertIMUFactorsAndValues(graph, preintegratedForKF, imuInputValues, values, minConnectedPoseInd, + imuBias, velocity); + + latestInd = preintegratedForKF.back().second; + + std::cout << "PGBA: Built graph with " << numIMUFactors << " IMU factors " << "and " << numOptimizedPoses << " " + "poses." + << std::endl; + + // Insert priors for TransformationFactor! + auto factors = getPriorsAndAddValuesForTransform(*transformDSOToIMU, settings.transformPriors, values); + for(auto&& factor : factors) + { + graph.add(factor); + } + return values; +} + +int dmvio::PoseGraphBundleAdjustment::insertIMUFactorsAndValues(NonlinearFactorGraph& graph, + const std::deque>& preintegratedMeasurements, + const Values& imuInputValues, + Values& values, long long int minConnectedPoseInd, + imuBias::ConstantBias& imuBias, Vector3& velocity) +{ + int numFactors = 0; + for(auto&& pair : preintegratedMeasurements) + { + assert(pair.second > minConnectedPoseInd); + int id = pair.second; + int prevId = prevKFIds.at(id); + auto&& imuData = pair.first; + + Key prevBiasKey = B(prevId); + NonlinearFactor::shared_ptr imuFactor( + new ImuFactor(P(prevId), V(prevId), + P(id), V(id), prevBiasKey, + imuData)); + + // The IMUFactor needs to be transformed (from IMU frame to DSO frame). + auto transformedFactor = boost::make_shared(imuFactor, + *transformDSOToIMU, + settings.conversionType); + graph.add(transformedFactor); + + auto biasNoiseModel = computeBiasNoiseModel(imuCalibration, imuData); + NonlinearFactor::shared_ptr bias_factor( + new BetweenFactor( + prevBiasKey, B(id), + imuBias::ConstantBias(gtsam::Vector3::Zero(), + gtsam::Vector3::Zero()), biasNoiseModel)); + graph.add(bias_factor); + + if(imuInputValues.exists(V(id))) + { + velocity = imuInputValues.at(V(id)); + } + if(imuInputValues.exists(B(id))) + { + imuBias = imuInputValues.at(B(id)); + } + + values.insert(B(id), imuBias); + values.insert(V(id), velocity); + + numFactors++; + } + return numFactors; +} + +long long int dmvio::PoseGraphBundleAdjustment::insertValuesAndGetMinConnectedPoseInd(const Values& poseInputValues, + const Values& poseInputValues2, + Values& values, + const KeyVector& keyVector) +{ + long long minConnectedPoseInd = -1; + long long lastPoseInd = -1; + numOptimizedPoses = 0; + for(auto&& key : keyVector) + { + // Use poseInputValues if possible otherwise poseInputValues2 + // The reason is that the newest pose is not yet in the values stored inside DelayedMarginalization. + auto firstIt = poseInputValues.find(key); + if(firstIt != poseInputValues.end()) + { + values.insert(key, firstIt->value); + }else + { + values.insert(key, poseInputValues2.at(key)); + } + Symbol sym(key); + if(sym.chr() == 'p') + { + long long ind = sym.index(); + if(lastPoseInd == -1) + { + minConnectedPoseInd = ind; + }else + { + assert(ind > lastPoseInd); + bool skipDueToMinIMUFactor = removeIMUFactorsUntil >= 0 && minConnectedPoseInd < removeIMUFactorsUntil; + if(prevKFIds[ind] != lastPoseInd || skipDueToMinIMUFactor) + { + minConnectedPoseInd = ind; + } + } + lastPoseInd = ind; + numOptimizedPoses++; + } + } + return minConnectedPoseInd; +} + +gtsam::NonlinearFactor::shared_ptr dmvio::compensateNegativeEnergy(NonlinearFactorGraph& graph, const Values& values, + const TransformDSOToIMU& transformForFakeFactor) +{ + GaussNewtonOptimizer gnOptim(graph, values); + double optimError = 0.0; + try + { + GaussNewtonOptimizer optim(graph, values); + optim.optimize(); + optimError = optim.error(); + }catch(IndeterminantLinearSystemException& exc) + { + std::cout << "WARNING: INDETERMINED LINEAR SYSTEM EXCEPTION" << std::endl; + + LevenbergMarquardtOptimizer lmOptim(graph, values); + lmOptim.iterate(); + optimError = lmOptim.error(); + } + if(optimError < 0) + { + optimError *= 2; // to be sure we add twice this cost... + Vector bConst(1); + bConst(0) = sqrt(-optimError * 2.0); + // GTSAM doesn't like factors with just a constant so we add the scale as well. + Key scaleKey = S(transformForFakeFactor.getSymbolInd()); + JacobianFactor::shared_ptr constantFactor(new JacobianFactor(scaleKey, gtsam::Matrix11::Zero(), bConst)); + Values linPoint; + linPoint.insert(scaleKey, ScaleGTSAM(transformForFakeFactor.getScale())); + auto lcf = boost::make_shared(constantFactor, linPoint); + return lcf; + } + return nullptr; +} + +void PoseGraphBundleAdjustment::addKeyframe(int id, gtsam::PreintegratedImuMeasurements imuMeasurements) +{ + if(skipped < settings.skipFirstKFs) + { + lastKFId = id; + skipped++; + return; + } + preintegratedForKF.emplace_back(std::move(imuMeasurements), id); + prevKFIds[id] = lastKFId; + lastKFId = id; +} + +void PoseGraphBundleAdjustment::updateGraphMarginalizationOrder() +{ + auto optSymbols = this->transformDSOToIMU->getAllOptimizedSymbols(); + gtsam::FastSet dontMargKeys; + dontMargKeys.insert(optSymbols.begin(), optSymbols.end()); + + std::deque>& marginalizationOrder = this->delayedGraph->marginalizationOrder; + std::set keySet; + // Add IMU variables to keys to marginalize. + for(auto& keyVector : marginalizationOrder) + { + keySet.clear(); + keySet.insert(keyVector.begin(), keyVector.end()); + + // Make sure that we don't marginalize symbols optimized by the current transform! + keyVector.erase(std::remove_if(keyVector.begin(), keyVector.end(), [&dontMargKeys](const gtsam::Key& key) + { return dontMargKeys.exists(key); }), keyVector.end()); + + for(int i = 0; i < keyVector.size(); ++i) + { + gtsam::Symbol sym(keyVector[i]); + int ind = sym.index(); + if(sym.chr() == 'p' && ind >= this->firstId) + { + // Add the IMU variables as well for all poses. + if(keySet.find(V(ind)) == keySet.end()) + { + keyVector.push_back(V(ind)); + } + if(keySet.find(B(ind)) == keySet.end()) + { + keyVector.push_back(B(ind)); + } + } + } + } +} + +// Prepare for prepareGraphForMainOptimization (usually called in a separate thread) +void PoseGraphBundleAdjustment::preparePreparation(const gtsam::Values& optimizedValues) +{ + dmvio::TimeMeasurement fullMeas("PreparePreparation"); + // Remove active DSO factor again! + removeDSOFactorIfNeeded(); + + mainPreparation(optimizedValues); +} + +void PoseGraphBundleAdjustment::mainPreparation(const gtsam::Values& optimizedValues) +{ + // Update Delayed Graph + updateGraphMarginalizationOrder(); + + // update delayedValues + gtsam::Values& delayedValues = delayedGraph->delayedValues; + for(auto&& pair : optimizedValues) + { + auto chr = gtsam::Symbol(pair.key).chr(); + if(chr != 'a' && + chr != 'p') // We don't overwrite DSO variables, as the evaluation point values shall be used for them. + { + eraseAndInsert(delayedValues, pair.key, pair.value); + } + } + + // Replace values with optimized values! + delayedGraph->delayedCurrValues = optimizedValues; + + // Change to delay 0. + { + dmvio::TimeMeasurement meas("GraphUnrolling"); + delayedGraph->readvanceGraph(0); + } +} + +void PoseGraphBundleAdjustment::removeDSOFactorIfNeeded() +{ + if(dsoFactorPos < 0) return; + graph->erase(graph->begin() + dsoFactorPos); + dsoFactorPos = -1; +} + +std::unique_ptr +PoseGraphBundleAdjustment::prepareGraphForMainOptimization(const gtsam::Values& optimizedValues) +{ + dmvio::TimeMeasurement fullMeas("PrepareGraphForMainOptimization"); + // Remove active DSO factor again! + removeDSOFactorIfNeeded(); + + // These should just be the factors arising from one call to addMarginalizedPointsBA + if(settings.prepareGraphAddFactors) + { + for(auto&& factor : disconnectedGraph->addedFactors) + { + delayedGraph->graph->add(factor); + } + disconnectedGraph->addedFactors.clear(); + } + + // delayedGraph->delayedCurrValues will be replaced with optimizedValues later on (in mainPreparation). + // Non-DSO variables in delayedGraph->delayedValues will be replaced with optimizedValues there as well. + // This means that we need to add the new DSO delayedValues here. + if(settings.prepareGraphAddDelValues) + { + for(auto&& val : disconnectedGraph->delayedValues) + { + if(!delayedGraph->delayedValues.exists(val.key)) + { + delayedGraph->delayedValues.insert(val.key, val.value); + } + } + } + + delayedGraph->marginalizationOrder.insert(delayedGraph->marginalizationOrder.end(), + disconnectedGraph->marginalizationOrder.begin(), + disconnectedGraph->marginalizationOrder.end()); + // This method must be called from within the BA thread, otherwise a mutex would have to be added here. + delayedMarginalization->removeDisconnectedGraph(disconnectedGraph.get()); + disconnectedGraph.reset(); + + mainPreparation(optimizedValues); + + return std::move(delayedGraph); +} + +int PoseGraphBundleAdjustment::getNumKFs() +{ + return preintegratedForKF.size(); +} + +gtsam::Key PoseGraphBundleAdjustment::getBiasKey() +{ + return B(latestInd); +} + +int PoseGraphBundleAdjustment::getNumOptimizedPoses() const +{ + return numOptimizedPoses; +} + +int PoseGraphBundleAdjustment::getNumImuFactors() const +{ + return numIMUFactors; +} + +int PoseGraphBundleAdjustment::getLatestInd() const +{ + return latestInd; +} + +std::shared_ptr PoseGraphBundleAdjustment::getInputDelayedGraph() const +{ + return inputDelayedGraph; +} + +void PoseGraphBundleAdjustment::optimizationResultNotUsed() +{ + delayedMarginalization->removeDisconnectedGraph(disconnectedGraph.get()); + disconnectedGraph.reset(); +} + +bool PoseGraphBundleAdjustment::isAllPosesUsed() const +{ + return allPosesUsed; +} + +int PoseGraphBundleAdjustment::getFirstIdWithIMUData() const +{ + return firstId; +} + +int PoseGraphBundleAdjustment::getIdOfNthIMUData(int n) const +{ + auto&& keys = graph->keyVector(); + int num = 0; + int lastInd = -1; + for(auto&& key : keys) + { + gtsam::Symbol sym(key); + if(sym.chr() == 'v') + { + if(num == n) return sym.index(); + lastInd = sym.index(); + num++; + } + } + assert(lastInd != -1); + return lastInd + (n - num) * 5; +} diff --git a/src/IMUInitialization/PoseGraphBundleAdjustment.h b/src/IMUInitialization/PoseGraphBundleAdjustment.h new file mode 100644 index 0000000..c713a01 --- /dev/null +++ b/src/IMUInitialization/PoseGraphBundleAdjustment.h @@ -0,0 +1,167 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_POSEGRAPHBUNDLEADJUSTMENT_H +#define DMVIO_POSEGRAPHBUNDLEADJUSTMENT_H + + +#include +#include "IMU/BAIMULogic.h" +#include + +namespace dmvio +{ + + +// PoseGraphBundleAdjustment (PGBA) as described in the paper. +class PoseGraphBundleAdjustment +{ +public: + PoseGraphBundleAdjustment(DelayedMarginalizationGraphs* delayedMarginalization, + const IMUCalibration& imuCalibration, const PGBASettings& settings, + std::shared_ptr transformDSOToIMU); + + + using AddKeyframeData = std::pair; + using KeyframeDataContainer = std::deque; + + // -------------------- Main methods which have to be called + + // Should be called for every keyframe added, providing the corresponding IMU measurements. + void addKeyframe(int id, gtsam::PreintegratedImuMeasurements imuMeasurements); + + // This must be called before calling optimize! + // This method must be called from the BA thread (between optimization and marginalization), wheres + // optimize can be called later in a different thread. + void prepareOptimization(); + + // Adds IMU factors to the delayed factor graph and then optimizes it (unless noOptimization==true). + // After calling optimize either optimizationResultNotUsed must be called or prepareGraphForMainOptimization. + // if noOptimization is true the graph is only built, but no optimization is performed (useful e.g. for + // marginalization replacement). + gtsam::Values optimize(gtsam::NonlinearFactor::shared_ptr activeDSOFactor, + const gtsam::Values& baValues, + const gtsam::Values& imuInitValues, bool noOptimization); // <-- called by IMUInitializer + + // Notifies that prepareGraphForMainOptimization will **not** be called for this optimization result. + void optimizationResultNotUsed(); + + // This method can optionally be called (usually in a separate thread) before prepareGraphForMainOptimization. + void preparePreparation(const gtsam::Values& optimizedValues); + + // If optimization has been performed in a separate thread this method must be called before prepareGraphForMainOptimization. + // It adds the cached IMU data which was added during optimization to the graph, and potentially performs another, + // smaller optimization to get biases and velocities for the newly added variables. + // This method must be called from the main BA thread. + gtsam::Values + extendGraph(gtsam::NonlinearFactor::shared_ptr activeDSOFactor, gtsam::Values&& previouslyOptimizedValues, + const gtsam::Values& baValues, const KeyframeDataContainer& cachedData); + + // This method must be called from the main BA thread. + // It unrolls the delayed graph, so that it can be used in the main optimization. + std::unique_ptr prepareGraphForMainOptimization(const gtsam::Values& optimizedValues); + + gtsam::Marginals getMarginals(const gtsam::Values& values); + + gtsam::Key getBiasKey(); + + int getNumKFs(); + + int getNumOptimizedPoses() const; + + int getNumImuFactors() const; + + int getLatestInd() const; + + // returns id of the nth pose that has IMU data. + int getIdOfNthIMUData(int n) const; + + int getFirstIdWithIMUData() const; + + std::shared_ptr getInputDelayedGraph() const; + + bool isAllPosesUsed() const; + + int removeIMUFactorsUntil = -1; +private: + gtsam::Values buildGraph(gtsam::NonlinearFactorGraph& graph, const gtsam::Values& poseInputValues, + const gtsam::Values& poseInputValues2, const gtsam::Values& imuInputValues, + bool noOptimization); // builds IMU graph for all factors connected to the latest KF in the delayedGraph. + + DelayedMarginalizationGraphs* delayedMarginalization; + + // Note that it needs to add IMUFactors using the TransformationFactor! + std::shared_ptr inputDelayedGraph; // this also specifies the delay of the larger optimization. + + // This graph is cloned + std::unique_ptr delayedGraph; + gtsam::NonlinearFactorGraph* graph = nullptr; // points to the graph of delayedGraph + std::shared_ptr disconnectedGraph; + + + const IMUCalibration& imuCalibration; + const PGBASettings& settings; + + std::shared_ptr transformDSOToIMU; + + // stores preintegrated, keyframeId. filled forward, read backwards. + KeyframeDataContainer preintegratedForKF; + std::map prevKFIds; // for each kf it stores the id of the previous KF. + int lastKFId = 0; // We use the fact that the very first KF always has index 0. + + int numOptimizedPoses = -1; + int numIMUFactors = -1; + int latestInd = -1; + int firstId = -1; // Id of first pose that is connected to IMU factors. + int dsoFactorPos = -1; + + int skipped = 0; + + bool allPosesUsed = false; + + // Add IMU variables to keys to marginalize. + void updateGraphMarginalizationOrder(); + + void removeDSOFactorIfNeeded(); + + void mainPreparation(const gtsam::Values& optimizedValues); + + long long int + insertValuesAndGetMinConnectedPoseInd(const gtsam::Values& poseInputValues, const gtsam::Values& poseInputValues2, + gtsam::Values& values, const gtsam::KeyVector& keyVector); + + int insertIMUFactorsAndValues(gtsam::NonlinearFactorGraph& graph, + const std::deque>& preintegratedMeasurements, + const gtsam::Values& imuInputValues, + gtsam::Values& values, long long int minConnectedPoseInd, + gtsam::imuBias::ConstantBias& imuBias, gtsam::Vector3& velocity); + +}; + +gtsam::NonlinearFactor::shared_ptr +compensateNegativeEnergy(gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const TransformDSOToIMU& transformForFakeFactor); + +} + + +#endif //DMVIO_POSEGRAPHBUNDLEADJUSTMENT_H diff --git a/src/dso/FullSystem/CoarseInitializer.cpp b/src/dso/FullSystem/CoarseInitializer.cpp new file mode 100644 index 0000000..7854794 --- /dev/null +++ b/src/dso/FullSystem/CoarseInitializer.cpp @@ -0,0 +1,1044 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include "FullSystem/CoarseInitializer.h" +#include "FullSystem/FullSystem.h" +#include "FullSystem/HessianBlocks.h" +#include "FullSystem/Residuals.h" +#include "FullSystem/PixelSelector.h" +#include "FullSystem/PixelSelector2.h" +#include "util/nanoflann.h" + +#include + +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#include "SSE2NEON.h" +#endif + +namespace dso +{ + +CoarseInitializer::CoarseInitializer(int ww, int hh) + : thisToNext_aff(0, 0), thisToNext(SE3()) +{ + for(int lvl=0; lvl &wraps) +{ + newFrame = newFrameHessian; + + for(IOWrap::Output3DWrapper* ow : wraps) + ow->pushLiveFrame(newFrameHessian); + + int maxIterations[] = {5,5,10,30,50}; + + + alphaK = 2.5*2.5;//*freeDebugParam1*freeDebugParam1; + alphaW = 150*150;//*freeDebugParam2*freeDebugParam2; + regWeight = 0.8;//*freeDebugParam4; + couplingWeight = 1;//*freeDebugParam5; + + if(!snapped) + { + thisToNext.translation().setZero(); + for(int lvl=0;lvlab_exposure>0 && newFrame->ab_exposure>0) + refToNew_aff_current = AffLight(logf(newFrame->ab_exposure / firstFrame->ab_exposure),0); // coarse approximation. + + + Vec3f latestRes = Vec3f::Zero(); + for(int lvl=pyrLevelsUsed-1; lvl>=0; lvl--) + { + + if(lvl %.3f+%.5f (%.3f->%.3f) (|inc| = %f)! \t", + lvl, 0, lambda, + "INITIA", + sqrtf((float)(resOld[0] / resOld[2])), + sqrtf((float)(resOld[1] / resOld[2])), + sqrtf((float)(resOld[0] / resOld[2])), + sqrtf((float)(resOld[1] / resOld[2])), + (resOld[0]+resOld[1]) / resOld[2], + (resOld[0]+resOld[1]) / resOld[2], + 0.0f); + std::cout << refToNew_current.log().transpose() << " AFF " << refToNew_aff_current.vec().transpose() <<"\n"; + } + + int iteration=0; + while(true) + { + Mat88f Hl = H; + for(int i=0;i<8;i++) Hl(i,i) *= (1+lambda); + Hl -= Hsc*(1/(1+lambda)); + Vec8f bl = b - bsc*(1/(1+lambda)); + + Hl = wM * Hl * wM * (0.01f/(w[lvl]*h[lvl])); + bl = wM * bl * (0.01f/(w[lvl]*h[lvl])); + + + Vec8f inc; + SE3 refToNew_new; + if (fixAffine) + { + // Note as we set the weights of rotation and translation to 1 the wM is just the identity in this case. + inc.head<6>() = -(wM.toDenseMatrix().topLeftCorner<6, 6>() * + (Hl.topLeftCorner<6, 6>().ldlt().solve(bl.head<6>()))); + inc.tail<2>().setZero(); + } else + inc = -(wM * (Hl.ldlt().solve(bl))); //=-H^-1 * b. + + double incNorm = inc.norm(); + + refToNew_new = SE3::exp(inc.head<6>().cast()) * refToNew_current; + + AffLight refToNew_aff_new = refToNew_aff_current; + refToNew_aff_new.a += inc[6]; + refToNew_aff_new.b += inc[7]; + doStep(lvl, lambda, inc); + + + Mat88f H_new, Hsc_new; Vec8f b_new, bsc_new; + Vec3f resNew = calcResAndGS(lvl, H_new, b_new, Hsc_new, bsc_new, refToNew_new, refToNew_aff_new, false); + Vec3f regEnergy = calcEC(lvl); + + float eTotalNew = (resNew[0]+resNew[1]+regEnergy[1]); + float eTotalOld = (resOld[0]+resOld[1]+regEnergy[0]); + + + bool accept = eTotalOld > eTotalNew; + + if(printDebug) + { + printf("lvl %d, it %d (l=%f) %s: %.5f + %.5f + %.5f -> %.5f + %.5f + %.5f (%.2f->%.2f) (|inc| = %f)! \t", + lvl, iteration, lambda, + (accept ? "ACCEPT" : "REJECT"), + sqrtf((float)(resOld[0] / resOld[2])), + sqrtf((float)(regEnergy[0] / regEnergy[2])), + sqrtf((float)(resOld[1] / resOld[2])), + sqrtf((float)(resNew[0] / resNew[2])), + sqrtf((float)(regEnergy[1] / regEnergy[2])), + sqrtf((float)(resNew[1] / resNew[2])), + eTotalOld / resNew[2], + eTotalNew / resNew[2], + incNorm); + std::cout << refToNew_new.log().transpose() << " AFF " << refToNew_aff_new.vec().transpose() <<"\n"; + } + + if(accept) + { + if(resNew[1] == alphaK*numPoints[lvl]) + snapped = true; + H = H_new; + b = b_new; + Hsc = Hsc_new; + bsc = bsc_new; + resOld = resNew; + refToNew_aff_current = refToNew_aff_new; + refToNew_current = refToNew_new; + applyStep(lvl); + optReg(lvl); + lambda *= 0.5; + fails=0; + if(lambda < 0.0001) lambda = 0.0001; + } + else + { + fails++; + lambda *= 4; + if(lambda > 10000) lambda = 10000; + } + + bool quitOpt = false; + + if(!(incNorm > eps) || iteration >= maxIterations[lvl] || fails >= 2) + { + Mat88f H,Hsc; Vec8f b,bsc; + + quitOpt = true; + } + + + if(quitOpt) break; + iteration++; + } + latestRes = resOld; + + } + + + + thisToNext = refToNew_current; + thisToNext_aff = refToNew_aff_current; + + for(int i=0;i snappedAt+5; +} + +void CoarseInitializer::debugPlot(int lvl, std::vector &wraps) +{ + bool needCall = false; + for(IOWrap::Output3DWrapper* ow : wraps) + needCall = needCall || ow->needPushDepthImage(); + if(!needCall) return; + + + int wl = w[lvl], hl = h[lvl]; + Eigen::Vector3f* colorRef = firstFrame->dIp[lvl]; + + MinimalImageB3 iRImg(wl,hl); + + for(int i=0;iisGood) + { + nid++; + sid += point->iR; + } + } + float fac = nid / sid; + + + for(int i=0;iisGood) + iRImg.setPixel9(point->u+0.5f,point->v+0.5f,Vec3b(0,0,0)); + + else + iRImg.setPixel9(point->u+0.5f,point->v+0.5f,makeRainbow3B(point->iR*fac)); + } + + + //IOWrap::displayImage("idepth-R", &iRImg, false); + for(IOWrap::Output3DWrapper* ow : wraps) + ow->pushDepthImage(&iRImg); +} + +// calculates residual, Hessian and Hessian-block neede for re-substituting depth. +Vec3f CoarseInitializer::calcResAndGS( + int lvl, Mat88f &H_out, Vec8f &b_out, + Mat88f &H_out_sc, Vec8f &b_out_sc, + const SE3 &refToNew, AffLight refToNew_aff, + bool plot) +{ + int wl = w[lvl], hl = h[lvl]; + Eigen::Vector3f* colorRef = firstFrame->dIp[lvl]; + Eigen::Vector3f* colorNew = newFrame->dIp[lvl]; + + Mat33f RKi = (refToNew.rotationMatrix() * Ki[lvl]).cast(); + Vec3f t = refToNew.translation().cast(); + Eigen::Vector2f r2new_aff = Eigen::Vector2f(exp(refToNew_aff.a), refToNew_aff.b); + + float fxl = fx[lvl]; + float fyl = fy[lvl]; + float cxl = cx[lvl]; + float cyl = cy[lvl]; + + + Accumulator11 E; + acc9.initialize(); + E.initialize(); + + + int npts = numPoints[lvl]; + Pnt* ptsl = points[lvl]; + for(int i=0;imaxstep = 1e10; + if(!point->isGood) + { + E.updateSingle((float)(point->energy[0])); + point->energy_new = point->energy; + point->isGood_new = false; + continue; + } + + VecNRf dp0; + VecNRf dp1; + VecNRf dp2; + VecNRf dp3; + VecNRf dp4; + VecNRf dp5; + VecNRf dp6; + VecNRf dp7; + VecNRf dd; + VecNRf r; + JbBuffer_new[i].setZero(); + + // sum over all residuals. + bool isGood = true; + float energy=0; + for(int idx=0;idxu+dx, point->v+dy, 1) + t*point->idepth_new; + float u = pt[0] / pt[2]; + float v = pt[1] / pt[2]; + float Ku = fxl * u + cxl; + float Kv = fyl * v + cyl; + float new_idepth = point->idepth_new/pt[2]; + + if(!(Ku > 1 && Kv > 1 && Ku < wl-2 && Kv < hl-2 && new_idepth > 0)) + { + isGood = false; + break; + } + + Vec3f hitColor = getInterpolatedElement33(colorNew, Ku, Kv, wl); + //Vec3f hitColor = getInterpolatedElement33BiCub(colorNew, Ku, Kv, wl); + + //float rlR = colorRef[point->u+dx + (point->v+dy) * wl][0]; + float rlR = getInterpolatedElement31(colorRef, point->u+dx, point->v+dy, wl); + + if(!std::isfinite(rlR) || !std::isfinite((float)hitColor[0])) + { + isGood = false; + break; + } + + + float residual = hitColor[0] - r2new_aff[0] * rlR - r2new_aff[1]; + float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual); + energy += hw *residual*residual*(2-hw); + + + float dxdd = (t[0]-t[2]*u)/pt[2]; + float dydd = (t[1]-t[2]*v)/pt[2]; + + if(hw < 1) hw = sqrtf(hw); + float dxInterp = hw*hitColor[1]*fxl; + float dyInterp = hw*hitColor[2]*fyl; + dp0[idx] = new_idepth*dxInterp; + dp1[idx] = new_idepth*dyInterp; + dp2[idx] = -new_idepth*(u*dxInterp + v*dyInterp); + dp3[idx] = -u*v*dxInterp - (1+v*v)*dyInterp; + dp4[idx] = (1+u*u)*dxInterp + u*v*dyInterp; + dp5[idx] = -v*dxInterp + u*dyInterp; + dp6[idx] = - hw*r2new_aff[0] * rlR; + dp7[idx] = - hw*1; + dd[idx] = dxInterp * dxdd + dyInterp * dydd; + r[idx] = hw*residual; + + float maxstep = 1.0f / Vec2f(dxdd*fxl, dydd*fyl).norm(); + if(maxstep < point->maxstep) point->maxstep = maxstep; + + // immediately compute dp*dd' and dd*dd' in JbBuffer1. + JbBuffer_new[i][0] += dp0[idx]*dd[idx]; + JbBuffer_new[i][1] += dp1[idx]*dd[idx]; + JbBuffer_new[i][2] += dp2[idx]*dd[idx]; + JbBuffer_new[i][3] += dp3[idx]*dd[idx]; + JbBuffer_new[i][4] += dp4[idx]*dd[idx]; + JbBuffer_new[i][5] += dp5[idx]*dd[idx]; + JbBuffer_new[i][6] += dp6[idx]*dd[idx]; + JbBuffer_new[i][7] += dp7[idx]*dd[idx]; + JbBuffer_new[i][8] += r[idx]*dd[idx]; + JbBuffer_new[i][9] += dd[idx]*dd[idx]; + } + + if(!isGood || energy > point->outlierTH*20) + { + E.updateSingle((float)(point->energy[0])); + point->isGood_new = false; + point->energy_new = point->energy; + continue; + } + + + // add into energy. + E.updateSingle(energy); + point->isGood_new = true; + point->energy_new[0] = energy; + + // update Hessian matrix. + for(int i=0;i+3>2)<<2); i < patternNum; i++) + acc9.updateSingle( + (float)dp0[i],(float)dp1[i],(float)dp2[i],(float)dp3[i], + (float)dp4[i],(float)dp5[i],(float)dp6[i],(float)dp7[i], + (float)r[i]); + + + } + + E.finish(); + acc9.finish(); + + + // calculate alpha energy, and decide if we cap it. + Accumulator11 EAlpha; + EAlpha.initialize(); + for(int i=0;iisGood_new) + { + E.updateSingle((float)(point->energy[1])); + } + else + { + point->energy_new[1] = (point->idepth_new-1)*(point->idepth_new-1); + E.updateSingle((float)(point->energy_new[1])); + } + } + EAlpha.finish(); + float alphaEnergy = alphaW*(EAlpha.A + refToNew.translation().squaredNorm() * npts); + + //printf("AE = %f * %f + %f\n", alphaW, EAlpha.A, refToNew.translation().squaredNorm() * npts); + + + // compute alpha opt. + float alphaOpt; + if(alphaEnergy > alphaK*npts) + { + alphaOpt = 0; + alphaEnergy = alphaK*npts; + } + else + { + alphaOpt = alphaW; + } + + + acc9SC.initialize(); + for(int i=0;iisGood_new) + continue; + + point->lastHessian_new = JbBuffer_new[i][9]; + + JbBuffer_new[i][8] += alphaOpt*(point->idepth_new - 1); + JbBuffer_new[i][9] += alphaOpt; + + if(alphaOpt==0) + { + JbBuffer_new[i][8] += couplingWeight*(point->idepth_new - point->iR); + JbBuffer_new[i][9] += couplingWeight; + } + + JbBuffer_new[i][9] = 1/(1+JbBuffer_new[i][9]); + acc9SC.updateSingleWeighted( + (float)JbBuffer_new[i][0],(float)JbBuffer_new[i][1],(float)JbBuffer_new[i][2],(float)JbBuffer_new[i][3], + (float)JbBuffer_new[i][4],(float)JbBuffer_new[i][5],(float)JbBuffer_new[i][6],(float)JbBuffer_new[i][7], + (float)JbBuffer_new[i][8],(float)JbBuffer_new[i][9]); + } + acc9SC.finish(); + + + //printf("nelements in H: %d, in E: %d, in Hsc: %d / 9!\n", (int)acc9.num, (int)E.num, (int)acc9SC.num*9); + H_out = acc9.H.topLeftCorner<8,8>();// / acc9.num; + b_out = acc9.H.topRightCorner<8,1>();// / acc9.num; + H_out_sc = acc9SC.H.topLeftCorner<8,8>();// / acc9.num; + b_out_sc = acc9SC.H.topRightCorner<8,1>();// / acc9.num; + + + + H_out(0,0) += alphaOpt*npts; + H_out(1,1) += alphaOpt*npts; + H_out(2,2) += alphaOpt*npts; + + Vec3f tlog = refToNew.log().head<3>().cast(); + b_out[0] += tlog[0]*alphaOpt*npts; + b_out[1] += tlog[1]*alphaOpt*npts; + b_out[2] += tlog[2]*alphaOpt*npts; + + + // Add zero prior to translation. + // setting_weightZeroPriorDSOInitY is the squared weight of the prior residual. + H_out(1, 1) += setting_weightZeroPriorDSOInitY; + b_out(1) += setting_weightZeroPriorDSOInitY * refToNew.translation().y(); + + H_out(0, 0) += setting_weightZeroPriorDSOInitX; + b_out(0) += setting_weightZeroPriorDSOInitX * refToNew.translation().x(); + + return Vec3f(E.A, alphaEnergy ,E.num); +} + +float CoarseInitializer::rescale() +{ + float factor = 20*thisToNext.translation().norm(); +// float factori = 1.0f/factor; +// float factori2 = factori*factori; +// +// for(int lvl=0;lvl E; + E.initialize(); + int npts = numPoints[lvl]; + for(int i=0;iisGood_new) continue; + float rOld = (point->idepth-point->iR); + float rNew = (point->idepth_new-point->iR); + E.updateNoWeight(Vec2f(rOld*rOld,rNew*rNew)); + + //printf("%f %f %f!\n", point->idepth, point->idepth_new, point->iR); + } + E.finish(); + + //printf("ER: %f %f %f!\n", couplingWeight*E.A1m[0], couplingWeight*E.A1m[1], (float)E.num.numIn1m); + return Vec3f(couplingWeight*E.A1m[0], couplingWeight*E.A1m[1], E.num); +} +void CoarseInitializer::optReg(int lvl) +{ + int npts = numPoints[lvl]; + Pnt* ptsl = points[lvl]; + if(!snapped) + { + return; + } + + + for(int i=0;iisGood) continue; + + float idnn[10]; + int nnn=0; + for(int j=0;j<10;j++) + { + if(point->neighbours[j] == -1) continue; + Pnt* other = ptsl+point->neighbours[j]; + if(!other->isGood) continue; + idnn[nnn] = other->iR; + nnn++; + } + + if(nnn > 2) + { + std::nth_element(idnn,idnn+nnn/2,idnn+nnn); + point->iR = (1-regWeight)*point->idepth + regWeight*idnn[nnn/2]; + } + } + +} + + + +void CoarseInitializer::propagateUp(int srcLvl) +{ + assert(srcLvl+1iR=0; + parent->iRSumNum=0; + } + + for(int i=0;iisGood) continue; + + Pnt* parent = ptst + point->parent; + parent->iR += point->iR * point->lastHessian; + parent->iRSumNum += point->lastHessian; + } + + for(int i=0;iiRSumNum > 0) + { + parent->idepth = parent->iR = (parent->iR / parent->iRSumNum); + parent->isGood = true; + } + } + + optReg(srcLvl+1); +} + +void CoarseInitializer::propagateDown(int srcLvl) +{ + assert(srcLvl>0); + // set idepth of target + + int nptst= numPoints[srcLvl-1]; + Pnt* ptss = points[srcLvl]; + Pnt* ptst = points[srcLvl-1]; + + for(int i=0;iparent; + + if(!parent->isGood || parent->lastHessian < 0.1) continue; + if(!point->isGood) + { + point->iR = point->idepth = point->idepth_new = parent->iR; + point->isGood=true; + point->lastHessian=0; + } + else + { + float newiR = (point->iR*point->lastHessian*2 + parent->iR*parent->lastHessian) / (point->lastHessian*2+parent->lastHessian); + point->iR = point->idepth = point->idepth_new = newiR; + } + } + optReg(srcLvl-1); +} + + +void CoarseInitializer::makeGradients(Eigen::Vector3f** data) +{ + for(int lvl=1; lvldIp[lvl], statusMapB, w[lvl], h[lvl], densities[lvl]*w[0]*h[0]); + + + + if(points[lvl] != 0) delete[] points[lvl]; + points[lvl] = new Pnt[npts]; + + // set idepth map to initially 1 everywhere. + int wl = w[lvl], hl = h[lvl]; + Pnt* pl = points[lvl]; + int nl = 0; + for(int y=patternPadding+1;ydIp[lvl] + x + y*w[lvl]; + float sumGrad2=0; + for(int idx=0;idx().squaredNorm(); + sumGrad2 += absgrad; + } + +// float gth = setting_outlierTH * (sqrtf(sumGrad2)+setting_outlierTHSumComponent); +// pl[nl].outlierTH = patternNum*gth*gth; +// + + pl[nl].outlierTH = patternNum*setting_outlierTH; + + + + nl++; + assert(nl <= npts); + } + } + + + numPoints[lvl]=nl; + } + delete[] statusMap; + delete[] statusMapB; + + makeNN(); + + thisToNext=SE3(); + snapped = false; + frameID = snappedAt = 0; + + for(int i=0;i 0) + { + pts[i].isGood=true; + pts[i].iR = pts[i].idepth = pts[i].idepth_new = snd/sn; + } + } + } +} +void CoarseInitializer::doStep(int lvl, float lambda, Vec8f inc) +{ + + const float maxPixelStep = 0.25; + const float idMaxStep = 1e10; + Pnt* pts = points[lvl]; + int npts = numPoints[lvl]; + for(int i=0;i().dot(inc); + float step = - b * JbBuffer[i][9] / (1+lambda); + + + float maxstep = maxPixelStep*pts[i].maxstep; + if(maxstep > idMaxStep) maxstep=idMaxStep; + + if(step > maxstep) step = maxstep; + if(step < -maxstep) step = -maxstep; + + float newIdepth = pts[i].idepth + step; + if(newIdepth < 1e-3 ) newIdepth = 1e-3; + if(newIdepth > 50) newIdepth = 50; + pts[i].idepth_new = newIdepth; + } + +} +void CoarseInitializer::applyStep(int lvl) +{ + Pnt* pts = points[lvl]; + int npts = numPoints[lvl]; + for(int i=0;i(JbBuffer, JbBuffer_new); +} + +void CoarseInitializer::makeK(CalibHessian* HCalib) +{ + w[0] = wG[0]; + h[0] = hG[0]; + + fx[0] = HCalib->fxl(); + fy[0] = HCalib->fyl(); + cx[0] = HCalib->cxl(); + cy[0] = HCalib->cyl(); + + for (int level = 1; level < pyrLevelsUsed; ++ level) + { + w[level] = w[0] >> level; + h[level] = h[0] >> level; + fx[level] = fx[level-1] * 0.5; + fy[level] = fy[level-1] * 0.5; + cx[level] = (cx[0] + 0.5) / ((int)1< , + FLANNPointcloud,2> KDTree; + + // build indices + FLANNPointcloud pcs[PYR_LEVELS]; + KDTree* indexes[PYR_LEVELS]; + for(int i=0;ibuildIndex(); + } + + const int nn=10; + + // find NN & parents + for(int lvl=0;lvl resultSet(nn); + nanoflann::KNNResultSet resultSet1(1); + + for(int i=0;ifindNeighbors(resultSet, (float*)&pt, nanoflann::SearchParams()); + int myidx=0; + float sumDF = 0; + for(int k=0;k=0 && ret_index[k] < npts); + myidx++; + } + for(int k=0;kfindNeighbors(resultSet1, (float*)&pt, nanoflann::SearchParams()); + + pts[i].parent = ret_index[0]; + pts[i].parentDist = expf(-ret_dist[0]*NNDistFactor); + + assert(ret_index[0]>=0 && ret_index[0] < numPoints[lvl+1]); + } + else + { + pts[i].parent = -1; + pts[i].parentDist = -1; + } + } + } + + + + // done. + + for(int i=0;i +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "util/NumType.h" +#include "OptimizationBackend/MatrixAccumulators.h" +#include "IOWrapper/Output3DWrapper.h" +#include "util/settings.h" +#include "vector" +#include +#include "IMU/IMUIntegration.hpp" + + +namespace dso +{ +struct CalibHessian; +struct FrameHessian; + + +struct Pnt +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + // index in jacobian. never changes (actually, there is no reason why). + float u,v; + + // idepth / isgood / energy during optimization. + float idepth; + bool isGood; + Vec2f energy; // (UenergyPhotometric, energyRegularizer) + bool isGood_new; + float idepth_new; + Vec2f energy_new; + + float iR; + float iRSumNum; + + float lastHessian; + float lastHessian_new; + + // max stepsize for idepth (corresponding to max. movement in pixel-space). + float maxstep; + + // idx (x+y*w) of closest point one pyramid level above. + int parent; + float parentDist; + + // idx (x+y*w) of up to 10 nearest points in pixel space. + int neighbours[10]; + float neighboursDist[10]; + + float my_type; + float outlierTH; +}; + +class CoarseInitializer { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + CoarseInitializer(int ww, int hh); + ~CoarseInitializer(); + + + void setFirst( CalibHessian* HCalib, FrameHessian* newFrameHessian); + bool trackFrame(FrameHessian* newFrameHessian, std::vector &wraps); + + int frameID; + bool fixAffine; + bool printDebug; + + Pnt* points[PYR_LEVELS]; + int numPoints[PYR_LEVELS]; + AffLight thisToNext_aff; + SE3 thisToNext; + + + FrameHessian* firstFrame; + FrameHessian* newFrame; +private: + + Mat33 K[PYR_LEVELS]; + Mat33 Ki[PYR_LEVELS]; + double fx[PYR_LEVELS]; + double fy[PYR_LEVELS]; + double fxi[PYR_LEVELS]; + double fyi[PYR_LEVELS]; + double cx[PYR_LEVELS]; + double cy[PYR_LEVELS]; + double cxi[PYR_LEVELS]; + double cyi[PYR_LEVELS]; + int w[PYR_LEVELS]; + int h[PYR_LEVELS]; + void makeK(CalibHessian* HCalib); + + bool snapped; + int snappedAt; + + // pyramid images & levels on all levels + Eigen::Vector3f* dINew[PYR_LEVELS]; + Eigen::Vector3f* dIFist[PYR_LEVELS]; + + Eigen::DiagonalMatrix wM; + + // temporary buffers for H and b. + Vec10f* JbBuffer; // 0-7: sum(dd * dp). 8: sum(res*dd). 9: 1/(1+sum(dd*dd))=inverse hessian entry. + Vec10f* JbBuffer_new; + + Accumulator9 acc9; + Accumulator9 acc9SC; + + + Vec3f dGrads[PYR_LEVELS]; + + float alphaK; + float alphaW; + float regWeight; + float couplingWeight; + + Vec3f calcResAndGS( + int lvl, + Mat88f &H_out, Vec8f &b_out, + Mat88f &H_out_sc, Vec8f &b_out_sc, + const SE3 &refToNew, AffLight refToNew_aff, + bool plot); + Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS. + void optReg(int lvl); + + void propagateUp(int srcLvl); + void propagateDown(int srcLvl); + float rescale(); + + void resetPoints(int lvl); + void doStep(int lvl, float lambda, Vec8f inc); + void applyStep(int lvl); + + void makeGradients(Eigen::Vector3f** data); + + void debugPlot(int lvl, std::vector &wraps); + void makeNN(); +}; + + + + +struct FLANNPointcloud +{ + inline FLANNPointcloud() {num=0; points=0;} + inline FLANNPointcloud(int n, Pnt* p) : num(n), points(p) {} + int num; + Pnt* points; + inline size_t kdtree_get_point_count() const { return num; } + inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const + { + const float d0=p1[0]-points[idx_p2].u; + const float d1=p1[1]-points[idx_p2].v; + return d0*d0+d1*d1; + } + + inline float kdtree_get_pt(const size_t idx, int dim) const + { + if (dim==0) return points[idx].u; + else return points[idx].v; + } + template + bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } +}; + +} + + diff --git a/src/dso/FullSystem/CoarseTracker.cpp b/src/dso/FullSystem/CoarseTracker.cpp new file mode 100644 index 0000000..0e6d3ae --- /dev/null +++ b/src/dso/FullSystem/CoarseTracker.cpp @@ -0,0 +1,1117 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include "FullSystem/CoarseTracker.h" +#include "FullSystem/FullSystem.h" +#include "FullSystem/HessianBlocks.h" +#include "FullSystem/Residuals.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" +#include "IOWrapper/ImageRW.h" +#include +#include "util/TimeMeasurement.h" + +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#include "SSE2NEON.h" +#endif + +namespace dso +{ + + +template +T* allocAligned(int size, std::vector &rawPtrVec) +{ + const int padT = 1 + ((1 << b)/sizeof(T)); + T* ptr = new T[size + padT]; + rawPtrVec.push_back(ptr); + T* alignedPtr = (T*)(( ((uintptr_t)(ptr+padT)) >> b) << b); + return alignedPtr; +} + + +CoarseTracker::CoarseTracker(int ww, int hh, dmvio::IMUIntegration &imuIntegration) : lastRef_aff_g2l(0, 0), imuIntegration(imuIntegration) +{ + // make coarse tracking templates. + for(int lvl=0; lvl>lvl; + int hl = hh>>lvl; + + idepth[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + weightSums[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + weightSums_bak[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + + pc_u[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + pc_v[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + pc_idepth[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + pc_color[lvl] = allocAligned<4,float>(wl*hl, ptrToDelete); + + } + + // warped buffers + buf_warped_idepth = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_u = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_v = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_dx = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_dy = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_residual = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_weight = allocAligned<4,float>(ww*hh, ptrToDelete); + buf_warped_refColor = allocAligned<4,float>(ww*hh, ptrToDelete); + + + newFrame = 0; + lastRef = 0; + debugPlot = debugPrint = true; + w[0]=h[0]=0; + refFrameID=-1; +} +CoarseTracker::~CoarseTracker() +{ + for(float* ptr : ptrToDelete) + delete[] ptr; + ptrToDelete.clear(); +} + +void CoarseTracker::makeK(CalibHessian* HCalib) +{ + w[0] = wG[0]; + h[0] = hG[0]; + + fx[0] = HCalib->fxl(); + fy[0] = HCalib->fyl(); + cx[0] = HCalib->cxl(); + cy[0] = HCalib->cyl(); + + for (int level = 1; level < pyrLevelsUsed; ++ level) + { + w[level] = w[0] >> level; + h[level] = h[0] >> level; + fx[level] = fx[level-1] * 0.5; + fy[level] = fy[level-1] * 0.5; + cx[level] = (cx[0] + 0.5) / ((int)1< frameHessians) +{ + // make coarse tracking templates for latstRef. + memset(idepth[0], 0, sizeof(float)*w[0]*h[0]); + memset(weightSums[0], 0, sizeof(float)*w[0]*h[0]); + + for(FrameHessian* fh : frameHessians) + { + for(PointHessian* ph : fh->pointHessians) + { + if(ph->lastResiduals[0].first != 0 && ph->lastResiduals[0].second == ResState::IN) + { + PointFrameResidual* r = ph->lastResiduals[0].first; + assert(r->efResidual->isActive() && r->target == lastRef); + int u = r->centerProjectedTo[0] + 0.5f; + int v = r->centerProjectedTo[1] + 0.5f; + float new_idepth = r->centerProjectedTo[2]; + float weight = sqrtf(1e-3 / (ph->efPoint->HdiF+1e-12)); + + idepth[0][u+w[0]*v] += new_idepth *weight; + weightSums[0][u+w[0]*v] += weight; + } + } + } + + + for(int lvl=1; lvl0, and write ones with weightSumsl<=0. + for(int i=w[lvl]+1;i 0) { sum += idepthl[i+1+wl]; num+=weightSumsl_bak[i+1+wl]; numn++;} + if(weightSumsl_bak[i-1-wl] > 0) { sum += idepthl[i-1-wl]; num+=weightSumsl_bak[i-1-wl]; numn++;} + if(weightSumsl_bak[i+wl-1] > 0) { sum += idepthl[i+wl-1]; num+=weightSumsl_bak[i+wl-1]; numn++;} + if(weightSumsl_bak[i-wl+1] > 0) { sum += idepthl[i-wl+1]; num+=weightSumsl_bak[i-wl+1]; numn++;} + if(numn>0) {idepthl[i] = sum/numn; weightSumsl[i] = num/numn;} + } + } + } + } + + + // dilate idepth by 1 (2 on lower levels). + for(int lvl=2; lvl0, and write ones with weightSumsl<=0. + for(int i=w[lvl]+1;i 0) { sum += idepthl[i+1]; num+=weightSumsl_bak[i+1]; numn++;} + if(weightSumsl_bak[i-1] > 0) { sum += idepthl[i-1]; num+=weightSumsl_bak[i-1]; numn++;} + if(weightSumsl_bak[i+wl] > 0) { sum += idepthl[i+wl]; num+=weightSumsl_bak[i+wl]; numn++;} + if(weightSumsl_bak[i-wl] > 0) { sum += idepthl[i-wl]; num+=weightSumsl_bak[i-wl]; numn++;} + if(numn>0) {idepthl[i] = sum/numn; weightSumsl[i] = num/numn;} + } + } + } + + + // normalize idepths and weights. + for(int lvl=0; lvldIp[lvl]; + + int wl = w[lvl], hl = h[lvl]; + + int lpc_n=0; + float* lpc_u = pc_u[lvl]; + float* lpc_v = pc_v[lvl]; + float* lpc_idepth = pc_idepth[lvl]; + float* lpc_color = pc_color[lvl]; + + + for(int y=2;y 0) + { + idepthl[i] /= weightSumsl[i]; + lpc_u[lpc_n] = x; + lpc_v[lpc_n] = y; + lpc_idepth[lpc_n] = idepthl[i]; + lpc_color[lpc_n] = dIRefl[i][0]; + + + + if(!std::isfinite(lpc_color[lpc_n]) || !(idepthl[i]>0)) + { + idepthl[i] = -1; + continue; // just skip if something is wrong. + } + lpc_n++; + } + else + idepthl[i] = -1; + + weightSumsl[i] = 1; + } + + pc_n[lvl] = lpc_n; + } + +} + + + +void CoarseTracker::calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l) +{ + acc.initialize(); + + __m128 fxl = _mm_set1_ps(fx[lvl]); + __m128 fyl = _mm_set1_ps(fy[lvl]); + __m128 b0 = _mm_set1_ps(lastRef_aff_g2l.b); + __m128 a = _mm_set1_ps((float)(AffLight::fromToVecExposure(lastRef->ab_exposure, newFrame->ab_exposure, lastRef_aff_g2l, aff_g2l)[0])); + + __m128 one = _mm_set1_ps(1); + __m128 minusOne = _mm_set1_ps(-1); + __m128 zero = _mm_set1_ps(0); + + int n = buf_warped_n; + assert(n%4==0); + for(int i=0;i().cast() * (1.0f/n); + b_out = acc.H.topRightCorner<8,1>().cast() * (1.0f/n); + + H_out.block<8,3>(0,0) *= SCALE_XI_ROT; + H_out.block<8,3>(0,3) *= SCALE_XI_TRANS; + H_out.block<8,1>(0,6) *= SCALE_A; + H_out.block<8,1>(0,7) *= SCALE_B; + H_out.block<3,8>(0,0) *= SCALE_XI_ROT; + H_out.block<3,8>(3,0) *= SCALE_XI_TRANS; + H_out.block<1,8>(6,0) *= SCALE_A; + H_out.block<1,8>(7,0) *= SCALE_B; + b_out.segment<3>(0) *= SCALE_XI_ROT; + b_out.segment<3>(3) *= SCALE_XI_TRANS; + b_out.segment<1>(6) *= SCALE_A; + b_out.segment<1>(7) *= SCALE_B; +} + + + + +Vec6 CoarseTracker::calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH) +{ + float E = 0; + int numTermsInE = 0; + int numTermsInWarped = 0; + int numSaturated=0; + + int wl = w[lvl]; + int hl = h[lvl]; + Eigen::Vector3f* dINewl = newFrame->dIp[lvl]; + float fxl = fx[lvl]; + float fyl = fy[lvl]; + float cxl = cx[lvl]; + float cyl = cy[lvl]; + + + Mat33f RKi = (refToNew.rotationMatrix().cast() * Ki[lvl]); + Vec3f t = (refToNew.translation()).cast(); + Vec2f affLL = AffLight::fromToVecExposure(lastRef->ab_exposure, newFrame->ab_exposure, lastRef_aff_g2l, aff_g2l).cast(); + + + float sumSquaredShiftT=0; + float sumSquaredShiftRT=0; + float sumSquaredShiftNum=0; + + float maxEnergy = 2*setting_huberTH*cutoffTH-setting_huberTH*setting_huberTH; // energy for r=setting_coarseCutoffTH. + + + MinimalImageB3* resImage = 0; + if(debugPlot) + { + resImage = new MinimalImageB3(wl,hl); + resImage->setConst(Vec3b(255,255,255)); + } + + int nl = pc_n[lvl]; + float* lpc_u = pc_u[lvl]; + float* lpc_v = pc_v[lvl]; + float* lpc_idepth = pc_idepth[lvl]; + float* lpc_color = pc_color[lvl]; + + + for(int i=0;i 2 && Kv > 2 && Ku < wl-3 && Kv < hl-3 && new_idepth > 0)) continue; + + + + float refColor = lpc_color[i]; + Vec3f hitColor = getInterpolatedElement33(dINewl, Ku, Kv, wl); + if(!std::isfinite((float)hitColor[0])) continue; + float residual = hitColor[0] - (float)(affLL[0] * refColor + affLL[1]); + float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual); + + + if(fabs(residual) > cutoffTH) + { + if(debugPlot) resImage->setPixel4(lpc_u[i], lpc_v[i], Vec3b(0,0,255)); + E += maxEnergy; + numTermsInE++; + numSaturated++; + } + else + { + if(debugPlot) resImage->setPixel4(lpc_u[i], lpc_v[i], Vec3b(residual+128,residual+128,residual+128)); + + E += hw *residual*residual*(2-hw); + numTermsInE++; + + buf_warped_idepth[numTermsInWarped] = new_idepth; + buf_warped_u[numTermsInWarped] = u; + buf_warped_v[numTermsInWarped] = v; + buf_warped_dx[numTermsInWarped] = hitColor[1]; + buf_warped_dy[numTermsInWarped] = hitColor[2]; + buf_warped_residual[numTermsInWarped] = residual; + buf_warped_weight[numTermsInWarped] = hw; + buf_warped_refColor[numTermsInWarped] = lpc_color[i]; + numTermsInWarped++; + } + } + + while(numTermsInWarped%4!=0) + { + buf_warped_idepth[numTermsInWarped] = 0; + buf_warped_u[numTermsInWarped] = 0; + buf_warped_v[numTermsInWarped] = 0; + buf_warped_dx[numTermsInWarped] = 0; + buf_warped_dy[numTermsInWarped] = 0; + buf_warped_residual[numTermsInWarped] = 0; + buf_warped_weight[numTermsInWarped] = 0; + buf_warped_refColor[numTermsInWarped] = 0; + numTermsInWarped++; + } + buf_warped_n = numTermsInWarped; + + + if(debugPlot) + { + IOWrap::displayImage("RES", resImage, false); + IOWrap::waitKey(0); + delete resImage; + } + + Vec6 rs; + rs[0] = E; + rs[1] = numTermsInE; + rs[2] = sumSquaredShiftT/(sumSquaredShiftNum+0.1); + rs[3] = 0; + rs[4] = sumSquaredShiftRT/(sumSquaredShiftNum+0.1); + rs[5] = numSaturated / (float)numTermsInE; + + return rs; +} + + + + + + +void CoarseTracker::setCoarseTrackingRef( + std::vector frameHessians) +{ + assert(frameHessians.size()>0); + lastRef = frameHessians.back(); + makeCoarseDepthL0(frameHessians); + + + + refFrameID = lastRef->shell->id; + lastRef_aff_g2l = lastRef->aff_g2l(); + + firstCoarseRMSE=-1; + +} +bool CoarseTracker::trackNewestCoarse( + FrameHessian* newFrameHessian, + SE3 &lastToNew_out, AffLight &aff_g2l_out, + int coarsestLvl, + Vec5 minResForAbort, + IOWrap::Output3DWrapper* wrap) +{ + debugPlot = setting_render_displayCoarseTrackingFull; + debugPrint = !setting_debugout_runquiet; + + assert(coarsestLvl < 5 && coarsestLvl < pyrLevelsUsed); + + lastResiduals.setConstant(NAN); + lastFlowIndicators.setConstant(1000); + + + newFrame = newFrameHessian; + int maxIterations[] = {10,20,50,50,50}; + float lambdaExtrapolationLimit = 0.001; + + SE3 refToNew_current = lastToNew_out; + AffLight aff_g2l_current = aff_g2l_out; + + bool haveRepeated = false; + + + Mat88 H; Vec8 b; + int lastLvl = -1; + for(int lvl=coarsestLvl; lvl>=0; lvl--) + { + float levelCutoffRepeat=1; + Vec6 resOld = calcRes(lvl, refToNew_current, aff_g2l_current, setting_coarseCutoffTH*levelCutoffRepeat); + while(resOld[5] > 0.6 && (levelCutoffRepeat < 50 || resOld[5] > 0.99) ) + { + levelCutoffRepeat*=2; + resOld = calcRes(lvl, refToNew_current, aff_g2l_current, setting_coarseCutoffTH*levelCutoffRepeat); + + if(!setting_debugout_runquiet) + printf("INCREASING cutoff to %f (ratio is %f)!\n", setting_coarseCutoffTH*levelCutoffRepeat, resOld[5]); + } + + calcGSSSE(lvl, H, b, refToNew_current, aff_g2l_current); + + float lambda = 0.01; + + if(debugPrint) + { + Vec2f relAff = AffLight::fromToVecExposure(lastRef->ab_exposure, newFrame->ab_exposure, lastRef_aff_g2l, aff_g2l_current).cast(); + printf("lvl%d, it %d (l=%f / %f) %s: %.3f->%.3f (%d -> %d) (|inc| = %f)! \t", + lvl, -1, lambda, 1.0f, + "INITIA", + 0.0f, + resOld[0] / resOld[1], + 0,(int)resOld[1], + 0.0f); + std::cout << refToNew_current.log().transpose() << " AFF " << aff_g2l_current.vec().transpose() <<" (rel " << relAff.transpose() << ")\n"; + } + + + for(int iteration=0; iteration < maxIterations[lvl]; iteration++) + { + dmvio::TimeMeasurement timeMeasurement("coarseTrackingIteration"); + Mat88 Hl = H; + for(int i=0;i<8;i++) Hl(i,i) *= (1+lambda); + + + float extrapFac = 1; + if(lambda < lambdaExtrapolationLimit) extrapFac = sqrt(sqrt(lambdaExtrapolationLimit / lambda)); + + + SE3 refToNew_new; + AffLight aff_g2l_new = aff_g2l_current; + double incNorm; + if(dso::setting_useIMU && imuIntegration.isCoarseInitialized()) + { + // The idea of the integration of the IMU (and GTSAM) into the coarse tracking is to replace the line + // Vec8 inc = Hl.ldlt().solve(-b); + // with a call to computeCoarseUpdate, which will add GTSAM factors before calculating the update. + + double incA, incB; + // Note that we pass H instead of Hl as the lambda multiplication is done inside... + refToNew_new = imuIntegration.computeCoarseUpdate(H, b, extrapFac, lambda, incA, incB, incNorm); + + SE3 oldVal = refToNew_current; + SE3 newVal = refToNew_new; + dso::Vec6 increment = (newVal * oldVal.inverse()).log(); + + dso::Vec8 totalIncrement; + totalIncrement.segment(0, 6) = increment; + + totalIncrement(6) = incA; + totalIncrement(7) = incB; + + incA *= SCALE_A; + incB *= SCALE_B; + + aff_g2l_new.a += incA; + aff_g2l_new.b += incB; + }else + { + Vec8 inc = Hl.ldlt().solve(-b); + + if(setting_affineOptModeA < 0 && setting_affineOptModeB < 0) // fix a, b + { + inc.head<6>() = Hl.topLeftCorner<6,6>().ldlt().solve(-b.head<6>()); + inc.tail<2>().setZero(); + } + if(!(setting_affineOptModeA < 0) && setting_affineOptModeB < 0) // fix b + { + inc.head<7>() = Hl.topLeftCorner<7,7>().ldlt().solve(-b.head<7>()); + inc.tail<1>().setZero(); + } + if(setting_affineOptModeA < 0 && !(setting_affineOptModeB < 0)) // fix a + { + Mat88 HlStitch = Hl; + Vec8 bStitch = b; + HlStitch.col(6) = HlStitch.col(7); + HlStitch.row(6) = HlStitch.row(7); + bStitch[6] = bStitch[7]; + Vec7 incStitch = HlStitch.topLeftCorner<7,7>().ldlt().solve(-bStitch.head<7>()); + inc.setZero(); + inc.head<6>() = incStitch.head<6>(); + inc[6] = 0; + inc[7] = incStitch[6]; + } + + inc *= extrapFac; + + Vec8 incScaled = inc; + incScaled.segment<3>(0) *= SCALE_XI_ROT; + incScaled.segment<3>(3) *= SCALE_XI_TRANS; + incScaled.segment<1>(6) *= SCALE_A; + incScaled.segment<1>(7) *= SCALE_B; + + if(!std::isfinite(incScaled.sum())) incScaled.setZero(); + + // exp: first three: translational part, last three: rotational part. + // Note: gtsam::Pose3 contains first rotational and then translational part! + refToNew_new = SE3::exp((Vec6) (incScaled.head<6>())) * refToNew_current; + aff_g2l_new = aff_g2l_current; + aff_g2l_new.a += incScaled[6]; + aff_g2l_new.b += incScaled[7]; + + incNorm = inc.norm(); + } + + Vec6 resNew = calcRes(lvl, refToNew_new, aff_g2l_new, setting_coarseCutoffTH*levelCutoffRepeat); + + bool accept = (resNew[0] / resNew[1]) < (resOld[0] / resOld[1]); + + if(debugPrint) + { + Vec2f relAff = AffLight::fromToVecExposure(lastRef->ab_exposure, newFrame->ab_exposure, lastRef_aff_g2l, aff_g2l_new).cast(); + printf("lvl %d, it %d (l=%f / %f) %s: %.3f->%.3f (%d -> %d) (|inc| = %f)! \t", + lvl, iteration, lambda, + extrapFac, + (accept ? "ACCEPT" : "REJECT"), + resOld[0] / resOld[1], + resNew[0] / resNew[1], + (int)resOld[1], (int)resNew[1], + incNorm); + std::cout << refToNew_new.log().transpose() << " AFF " << aff_g2l_new.vec().transpose() <<" (rel " << relAff.transpose() << ")\n"; + } + if(accept) + { + calcGSSSE(lvl, H, b, refToNew_new, aff_g2l_new); + resOld = resNew; + aff_g2l_current = aff_g2l_new; + refToNew_current = refToNew_new; + if(dso::setting_useIMU) + imuIntegration.acceptCoarseUpdate(); + lambda *= 0.5; + } + else + { + lambda *= 4; + if(lambda < lambdaExtrapolationLimit) lambda = lambdaExtrapolationLimit; + } + + lastLvl = lvl; + + if(!(incNorm > 1e-3)) + { + if(debugPrint) + printf("inc too small, break!\n"); + break; + } + } + + // set last residual for that level, as well as flow indicators. + lastResiduals[lvl] = sqrtf((float)(resOld[0] / resOld[1])); + lastFlowIndicators = resOld.segment<3>(2); + if(std::isnan(lastResiduals[lvl])) return false; + if(lastResiduals[lvl] > 1.5*minResForAbort[lvl]) return false; + + + if(levelCutoffRepeat > 1 && !haveRepeated) + { + lvl++; + haveRepeated=true; + printf("REPEAT LEVEL!\n"); + } + + } + + // set! + lastToNew_out = refToNew_current; + aff_g2l_out = aff_g2l_current; + + bool trackingGood = true; + + if((setting_affineOptModeA != 0 && (fabsf(aff_g2l_out.a) > 1.2)) + || (setting_affineOptModeB != 0 && (fabsf(aff_g2l_out.b) > 200))) + trackingGood = false; + + Vec2f relAff = AffLight::fromToVecExposure(lastRef->ab_exposure, newFrame->ab_exposure, lastRef_aff_g2l, aff_g2l_out).cast(); + + if((setting_affineOptModeA == 0 && (fabsf(logf((float)relAff[0])) > 1.5)) + || (setting_affineOptModeB == 0 && (fabsf((float)relAff[1]) > 200))) + trackingGood = false; + + if(setting_affineOptModeA < 0) aff_g2l_out.a=0; + if(setting_affineOptModeB < 0) aff_g2l_out.b=0; + + if(lastLvl == 0) + { + if(dso::setting_useIMU) + imuIntegration.addVisualToCoarseGraph(H, b, trackingGood); + } + + return trackingGood; +} + + + +void CoarseTracker::debugPlotIDepthMap(float* minID_pt, float* maxID_pt, std::vector &wraps) const +{ + dmvio::TimeMeasurement timeMeasurement("debugPlotIDepthMap"); + if(wraps.empty() && !debugSaveImages) + { + return; + } + if(w[1] == 0) return; + + + int lvl = 0; + + { + std::vector allID; + for(int i=0;i 0) + allID.push_back(idepth[lvl][i]); + } + std::sort(allID.begin(), allID.end()); + int n = allID.size()-1; + if(n <= 0) + { + return; + } + + float minID_new = allID[(int)(n*0.05)]; + float maxID_new = allID[(int)(n*0.95)]; + + float minID, maxID; + minID = minID_new; + maxID = maxID_new; + if(minID_pt!=0 && maxID_pt!=0) + { + if(*minID_pt < 0 || *maxID_pt < 0) + { + *maxID_pt = maxID; + *minID_pt = minID; + } + else + { + + // slowly adapt: change by maximum 10% of old span. + float maxChange = 0.3*(*maxID_pt - *minID_pt); + + if(minID < *minID_pt - maxChange) + minID = *minID_pt - maxChange; + if(minID > *minID_pt + maxChange) + minID = *minID_pt + maxChange; + + + if(maxID < *maxID_pt - maxChange) + maxID = *maxID_pt - maxChange; + if(maxID > *maxID_pt + maxChange) + maxID = *maxID_pt + maxChange; + + *maxID_pt = maxID; + *minID_pt = minID; + } + } + + + MinimalImageB3 mf(w[lvl], h[lvl]); + mf.setBlack(); + for(int i=0;idIp[lvl][i][0]*0.9f; + if(c>255) c=255; + mf.at(i) = Vec3b(c,c,c); + } + int wl = w[lvl]; + for(int y=3;y 0) {sid+=bp[0]; nid++;} + if(bp[1] > 0) {sid+=bp[1]; nid++;} + if(bp[-1] > 0) {sid+=bp[-1]; nid++;} + if(bp[wl] > 0) {sid+=bp[wl]; nid++;} + if(bp[-wl] > 0) {sid+=bp[-wl]; nid++;} + + if(bp[0] > 0 || nid >= 3) + { + float id = ((sid / nid)-minID) / ((maxID-minID)); + mf.setPixelCirc(x,y,makeJet3B(id)); + //mf.at(idx) = makeJet3B(id); + } + } + //IOWrap::displayImage("coarseDepth LVL0", &mf, false); + + + for(IOWrap::Output3DWrapper* ow : wraps) + ow->pushDepthImage(&mf); + + if(debugSaveImages) + { + char buf[1000]; + snprintf(buf, 1000, "images_out/predicted_%05d_%05d.png", lastRef->shell->id, refFrameID); + IOWrap::writeImage(buf,&mf); + } + + } +} + + + +void CoarseTracker::debugPlotIDepthMapFloat(std::vector &wraps) +{ + dmvio::TimeMeasurement timeMeasurement("debugPlotIDepthMapFloat"); + if(w[1] == 0) return; + int lvl = 0; + MinimalImageF mim(w[lvl], h[lvl], idepth[lvl]); + for(IOWrap::Output3DWrapper* ow : wraps) + ow->pushDepthImageFloat(&mim, lastRef); +} + + + + + + + + + + + +CoarseDistanceMap::CoarseDistanceMap(int ww, int hh) +{ + fwdWarpedIDDistFinal = new float[ww*hh/4]; + + bfsList1 = new Eigen::Vector2i[ww*hh/4]; + bfsList2 = new Eigen::Vector2i[ww*hh/4]; + + int fac = 1 << (pyrLevelsUsed-1); + + + coarseProjectionGrid = new PointFrameResidual*[2048*(ww*hh/(fac*fac))]; + coarseProjectionGridNum = new int[ww*hh/(fac*fac)]; + + w[0]=h[0]=0; +} +CoarseDistanceMap::~CoarseDistanceMap() +{ + delete[] fwdWarpedIDDistFinal; + delete[] bfsList1; + delete[] bfsList2; + delete[] coarseProjectionGrid; + delete[] coarseProjectionGridNum; +} + + + + + +void CoarseDistanceMap::makeDistanceMap( + std::vector frameHessians, + FrameHessian* frame) +{ + int w1 = w[1]; + int h1 = h[1]; + int wh1 = w1*h1; + for(int i=0;iPRE_worldToCam * fh->PRE_camToWorld; + Mat33f KRKi = (K[1] * fhToNew.rotationMatrix().cast() * Ki[0]); + Vec3f Kt = (K[1] * fhToNew.translation().cast()); + + for(PointHessian* ph : fh->pointHessians) + { + assert(ph->status == PointHessian::ACTIVE); + Vec3f ptp = KRKi * Vec3f(ph->u, ph->v, 1) + Kt*ph->idepth_scaled; + int u = ptp[0] / ptp[2] + 0.5f; + int v = ptp[1] / ptp[2] + 0.5f; + if(!(u > 0 && v > 0 && u < w[1] && v < h[1])) continue; + fwdWarpedIDDistFinal[u+w1*v]=0; + bfsList1[numItems] = Eigen::Vector2i(u,v); + numItems++; + } + } + + growDistBFS(numItems); +} + + + + +void CoarseDistanceMap::makeInlierVotes(std::vector frameHessians) +{ + +} + + + +void CoarseDistanceMap::growDistBFS(int bfsNum) +{ + assert(w[0] != 0); + int w1 = w[1], h1 = h[1]; + for(int k=1;k<40;k++) + { + int bfsNum2 = bfsNum; + std::swap(bfsList1,bfsList2); + bfsNum=0; + + if(k%2==0) + { + for(int i=0;i k) + { + fwdWarpedIDDistFinal[idx+1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x+1,y); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx-1] > k) + { + fwdWarpedIDDistFinal[idx-1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x-1,y); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx+w1] > k) + { + fwdWarpedIDDistFinal[idx+w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x,y+1); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx-w1] > k) + { + fwdWarpedIDDistFinal[idx-w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x,y-1); bfsNum++; + } + } + } + else + { + for(int i=0;i k) + { + fwdWarpedIDDistFinal[idx+1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x+1,y); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx-1] > k) + { + fwdWarpedIDDistFinal[idx-1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x-1,y); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx+w1] > k) + { + fwdWarpedIDDistFinal[idx+w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x,y+1); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx-w1] > k) + { + fwdWarpedIDDistFinal[idx-w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x,y-1); bfsNum++; + } + + if(fwdWarpedIDDistFinal[idx+1+w1] > k) + { + fwdWarpedIDDistFinal[idx+1+w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x+1,y+1); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx-1+w1] > k) + { + fwdWarpedIDDistFinal[idx-1+w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x-1,y+1); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx-1-w1] > k) + { + fwdWarpedIDDistFinal[idx-1-w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x-1,y-1); bfsNum++; + } + if(fwdWarpedIDDistFinal[idx+1-w1] > k) + { + fwdWarpedIDDistFinal[idx+1-w1] = k; + bfsList1[bfsNum] = Eigen::Vector2i(x+1,y-1); bfsNum++; + } + } + } + } +} + + +void CoarseDistanceMap::addIntoDistFinal(int u, int v) +{ + if(w[0] == 0) return; + bfsList1[0] = Eigen::Vector2i(u,v); + fwdWarpedIDDistFinal[u+w[1]*v] = 0; + growDistBFS(1); +} + + + +void CoarseDistanceMap::makeK(CalibHessian* HCalib) +{ + w[0] = wG[0]; + h[0] = hG[0]; + + fx[0] = HCalib->fxl(); + fy[0] = HCalib->fyl(); + cx[0] = HCalib->cxl(); + cy[0] = HCalib->cyl(); + + for (int level = 1; level < pyrLevelsUsed; ++ level) + { + w[level] = w[0] >> level; + h[level] = h[0] >> level; + fx[level] = fx[level-1] * 0.5; + fy[level] = fy[level-1] * 0.5; + cx[level] = (cx[0] + 0.5) / ((int)1< +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" +#include "vector" +#include +#include "util/settings.h" +#include "OptimizationBackend/MatrixAccumulators.h" +#include "IOWrapper/Output3DWrapper.h" + +#include "IMU/IMUIntegration.hpp" + + +namespace dso +{ +struct CalibHessian; +struct FrameHessian; +struct PointFrameResidual; + +class CoarseTracker { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + CoarseTracker(int w, int h, dmvio::IMUIntegration &imuIntegration); + ~CoarseTracker(); + + bool trackNewestCoarse( + FrameHessian* newFrameHessian, + SE3 &lastToNew_out, AffLight &aff_g2l_out, + int coarsestLvl, Vec5 minResForAbort, + IOWrap::Output3DWrapper* wrap=0); + + void setCoarseTrackingRef( + std::vector frameHessians); + + void makeK( + CalibHessian* HCalib); + + bool debugPrint, debugPlot; + + Mat33f K[PYR_LEVELS]; + Mat33f Ki[PYR_LEVELS]; + float fx[PYR_LEVELS]; + float fy[PYR_LEVELS]; + float fxi[PYR_LEVELS]; + float fyi[PYR_LEVELS]; + float cx[PYR_LEVELS]; + float cy[PYR_LEVELS]; + float cxi[PYR_LEVELS]; + float cyi[PYR_LEVELS]; + int w[PYR_LEVELS]; + int h[PYR_LEVELS]; + + void debugPlotIDepthMap(float* minID, float* maxID, std::vector &wraps) const; + void debugPlotIDepthMapFloat(std::vector &wraps); + + FrameHessian* lastRef; + AffLight lastRef_aff_g2l; + FrameHessian* newFrame; + int refFrameID; + + // act as pure ouptut + Vec5 lastResiduals; + Vec3 lastFlowIndicators; + double firstCoarseRMSE; +private: + + + void makeCoarseDepthL0(std::vector frameHessians); + float* idepth[PYR_LEVELS]; + float* weightSums[PYR_LEVELS]; + float* weightSums_bak[PYR_LEVELS]; + + + Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); + Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); + void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); + void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); + + // pc buffers + float* pc_u[PYR_LEVELS]; + float* pc_v[PYR_LEVELS]; + float* pc_idepth[PYR_LEVELS]; + float* pc_color[PYR_LEVELS]; + int pc_n[PYR_LEVELS]; + + // warped buffers + float* buf_warped_idepth; + float* buf_warped_u; + float* buf_warped_v; + float* buf_warped_dx; + float* buf_warped_dy; + float* buf_warped_residual; + float* buf_warped_weight; + float* buf_warped_refColor; + int buf_warped_n; + + std::vector ptrToDelete; + Accumulator9 acc; + + dmvio::IMUIntegration &imuIntegration; + +}; + + +class CoarseDistanceMap { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + CoarseDistanceMap(int w, int h); + ~CoarseDistanceMap(); + + void makeDistanceMap( + std::vector frameHessians, + FrameHessian* frame); + + void makeInlierVotes( + std::vector frameHessians); + + void makeK( CalibHessian* HCalib); + + + float* fwdWarpedIDDistFinal; + + Mat33f K[PYR_LEVELS]; + Mat33f Ki[PYR_LEVELS]; + float fx[PYR_LEVELS]; + float fy[PYR_LEVELS]; + float fxi[PYR_LEVELS]; + float fyi[PYR_LEVELS]; + float cx[PYR_LEVELS]; + float cy[PYR_LEVELS]; + float cxi[PYR_LEVELS]; + float cyi[PYR_LEVELS]; + int w[PYR_LEVELS]; + int h[PYR_LEVELS]; + + void addIntoDistFinal(int u, int v); + + +private: + + PointFrameResidual** coarseProjectionGrid; + int* coarseProjectionGridNum; + Eigen::Vector2i* bfsList1; + Eigen::Vector2i* bfsList2; + + void growDistBFS(int bfsNum); +}; + +} + diff --git a/src/dso/FullSystem/FullSystem.cpp b/src/dso/FullSystem/FullSystem.cpp new file mode 100644 index 0000000..68f5b81 --- /dev/null +++ b/src/dso/FullSystem/FullSystem.cpp @@ -0,0 +1,1838 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include "FullSystem/FullSystem.h" + +#include "stdio.h" +#include "util/globalFuncs.h" +#include +#include +#include "IOWrapper/ImageDisplay.h" +#include "util/globalCalib.h" +#include +#include +#include "FullSystem/PixelSelector.h" +#include "FullSystem/PixelSelector2.h" +#include "FullSystem/ResidualProjections.h" +#include "FullSystem/ImmaturePoint.h" + +#include "FullSystem/CoarseTracker.h" +#include "FullSystem/CoarseInitializer.h" + +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +#include "IOWrapper/Output3DWrapper.h" +#include "util/ImageAndExposure.h" +#include + +#include "util/TimeMeasurement.h" +#include "GTSAMIntegration/ExtUtils.h" + +using dmvio::GravityInitializer; + +namespace dso +{ +int FrameHessian::instanceCounter=0; +int PointHessian::instanceCounter=0; +int CalibHessian::instanceCounter=0; + + +boost::mutex FrameShell::shellPoseMutex{}; + +FullSystem::FullSystem(bool linearizeOperationPassed, const dmvio::IMUCalibration& imuCalibration, + dmvio::IMUSettings& imuSettings) + : linearizeOperation(linearizeOperationPassed), imuIntegration(&Hcalib, imuCalibration, imuSettings, + linearizeOperation), + secondKeyframeDone(false), gravityInit(imuSettings.numMeasurementsGravityInit, imuCalibration), + shellPoseMutex(FrameShell::shellPoseMutex) +{ + setting_useGTSAMIntegration = setting_useIMU; + baIntegration = imuIntegration.getBAGTSAMIntegration().get(); + + int retstat =0; + if(setting_logStuff) + { + + retstat += system("rm -rf logs"); + retstat += system("mkdir logs"); + + retstat += system("rm -rf mats"); + retstat += system("mkdir mats"); + + calibLog = new std::ofstream(); + calibLog->open("logs/calibLog.txt", std::ios::trunc | std::ios::out); + calibLog->precision(12); + + numsLog = new std::ofstream(); + numsLog->open("logs/numsLog.txt", std::ios::trunc | std::ios::out); + numsLog->precision(10); + + coarseTrackingLog = new std::ofstream(); + coarseTrackingLog->open("logs/coarseTrackingLog.txt", std::ios::trunc | std::ios::out); + coarseTrackingLog->precision(10); + + eigenAllLog = new std::ofstream(); + eigenAllLog->open("logs/eigenAllLog.txt", std::ios::trunc | std::ios::out); + eigenAllLog->precision(10); + + eigenPLog = new std::ofstream(); + eigenPLog->open("logs/eigenPLog.txt", std::ios::trunc | std::ios::out); + eigenPLog->precision(10); + + eigenALog = new std::ofstream(); + eigenALog->open("logs/eigenALog.txt", std::ios::trunc | std::ios::out); + eigenALog->precision(10); + + DiagonalLog = new std::ofstream(); + DiagonalLog->open("logs/diagonal.txt", std::ios::trunc | std::ios::out); + DiagonalLog->precision(10); + + variancesLog = new std::ofstream(); + variancesLog->open("logs/variancesLog.txt", std::ios::trunc | std::ios::out); + variancesLog->precision(10); + + + nullspacesLog = new std::ofstream(); + nullspacesLog->open("logs/nullspacesLog.txt", std::ios::trunc | std::ios::out); + nullspacesLog->precision(10); + } + else + { + nullspacesLog=0; + variancesLog=0; + DiagonalLog=0; + eigenALog=0; + eigenPLog=0; + eigenAllLog=0; + numsLog=0; + calibLog=0; + } + + assert(retstat!=293847); + + + + selectionMap = new float[wG[0]*hG[0]]; + + coarseDistanceMap = new CoarseDistanceMap(wG[0], hG[0]); + coarseTracker = new CoarseTracker(wG[0], hG[0], imuIntegration); + coarseTracker_forNewKF = new CoarseTracker(wG[0], hG[0], imuIntegration); + coarseInitializer = new CoarseInitializer(wG[0], hG[0]); + pixelSelector = new PixelSelector(wG[0], hG[0]); + + statistics_lastNumOptIts=0; + statistics_numDroppedPoints=0; + statistics_numActivatedPoints=0; + statistics_numCreatedPoints=0; + statistics_numForceDroppedResBwd = 0; + statistics_numForceDroppedResFwd = 0; + statistics_numMargResFwd = 0; + statistics_numMargResBwd = 0; + + lastCoarseRMSE.setConstant(100); + + currentMinActDist=2; + initialized=false; + + + ef = new EnergyFunctional(*baIntegration); + ef->red = &this->treadReduce; + + isLost=false; + initFailed=false; + + + needNewKFAfter = -1; + runMapping=true; + mappingThread = boost::thread(&FullSystem::mappingLoop, this); + lastRefStopID=0; + + + + minIdJetVisDebug = -1; + maxIdJetVisDebug = -1; + minIdJetVisTracker = -1; + maxIdJetVisTracker = -1; +} + +FullSystem::~FullSystem() +{ + blockUntilMappingIsFinished(); + + if(setting_logStuff) + { + calibLog->close(); delete calibLog; + numsLog->close(); delete numsLog; + coarseTrackingLog->close(); delete coarseTrackingLog; + //errorsLog->close(); delete errorsLog; + eigenAllLog->close(); delete eigenAllLog; + eigenPLog->close(); delete eigenPLog; + eigenALog->close(); delete eigenALog; + DiagonalLog->close(); delete DiagonalLog; + variancesLog->close(); delete variancesLog; + nullspacesLog->close(); delete nullspacesLog; + } + + delete[] selectionMap; + + for(FrameShell* s : allFrameHistory) + delete s; + for(FrameHessian* fh : unmappedTrackedFrames) + delete fh; + + delete coarseDistanceMap; + delete coarseTracker; + delete coarseTracker_forNewKF; + delete coarseInitializer; + delete pixelSelector; + delete ef; +} + +void FullSystem::setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH) +{ + +} + +void FullSystem::setGammaFunction(float* BInv) +{ + if(BInv==0) return; + + // copy BInv. + memcpy(Hcalib.Binv, BInv, sizeof(float)*256); + + + // invert. + for(int i=1;i<255;i++) + { + // find val, such that Binv[val] = i. + // I dont care about speed for this, so do it the stupid way. + + for(int s=1;s<255;s++) + { + if(BInv[s] <= i && BInv[s+1] >= i) + { + Hcalib.B[i] = s+(i - BInv[s]) / (BInv[s+1]-BInv[s]); + break; + } + } + } + Hcalib.B[0] = 0; + Hcalib.B[255] = 255; +} + + + +void FullSystem::printResult(std::string file, bool onlyLogKFPoses, bool saveMetricPoses, bool useCamToTrackingRef) +{ + boost::unique_lock lock(trackMutex); + boost::unique_lock crlock(shellPoseMutex); + + std::ofstream myfile; + myfile.open (file.c_str()); + myfile << std::setprecision(15); + + for(FrameShell* s : allFrameHistory) + { + if(!s->poseValid) continue; + + if(onlyLogKFPoses && s->marginalizedAt == s->id) continue; + + // firstPose is transformFirstToWorld. We actually want camToFirst here -> + Sophus::SE3 camToWorld = s->camToWorld; + + // Use camToTrackingReference for nonKFs and the updated camToWorld for KFs. + if(useCamToTrackingRef && s->keyframeId == -1) + { + camToWorld = s->trackingRef->camToWorld * s->camToTrackingRef; + } + Sophus::SE3 camToFirst = firstPose.inverse() * camToWorld; + + if(saveMetricPoses) + { + // Transform pose to IMU frame. + // not actually camToFirst any more... + camToFirst = Sophus::SE3d(imuIntegration.getTransformDSOToIMU().transformPose(camToWorld.inverse().matrix())); + } + + myfile << s->timestamp << + " " << camToFirst.translation().transpose()<< + " " << camToFirst.so3().unit_quaternion().x()<< + " " << camToFirst.so3().unit_quaternion().y()<< + " " << camToFirst.so3().unit_quaternion().z()<< + " " << camToFirst.unit_quaternion().w() << "\n"; + } + myfile.close(); +} + +std::pair FullSystem::trackNewCoarse(FrameHessian* fh, Sophus::SE3 *referenceToFrameHint) +{ + dmvio::TimeMeasurement timeMeasurement(referenceToFrameHint ? "FullSystem::trackNewCoarse" : "FullSystem::trackNewCoarseNoIMU"); + assert(allFrameHistory.size() > 0); + // set pose initialization. + + for(IOWrap::Output3DWrapper* ow : outputWrapper) + ow->pushLiveFrame(fh); + + + + FrameHessian* lastF = coarseTracker->lastRef; + + AffLight aff_last_2_l = AffLight(0,0); + + // Seems to contain poses reference_to_newframe. + std::vector> lastF_2_fh_tries; + + if(referenceToFrameHint) + { + // We got a hint (typically from IMU) where our pose is, so we don't need the random initializations below. + lastF_2_fh_tries.push_back(*referenceToFrameHint); + { + // lock on global pose consistency (probably we don't need this for AffineLight, but just to make sure). + boost::unique_lock crlock(shellPoseMutex); + // Set Affine light to last frame, where tracking was good!: + for(int i = allFrameHistory.size() - 2; i >= 0; i--) + { + FrameShell* slast = allFrameHistory[i]; + if(slast->trackingWasGood) + { + aff_last_2_l = slast->aff_g2l; + break; + } + if(slast->trackingRef != lastF->shell) + { + std::cout << "WARNING: No well tracked frame with the same tracking ref available!" << std::endl; + aff_last_2_l = lastF->aff_g2l(); + break; + } + } + } + } + + if(!referenceToFrameHint) + { + if(allFrameHistory.size() == 2) + for(unsigned int i=0;i crlock(shellPoseMutex); + slast_2_sprelast = sprelast->camToWorld.inverse() * slast->camToWorld; + lastF_2_slast = slast->camToWorld.inverse() * lastF->shell->camToWorld; + aff_last_2_l = slast->aff_g2l; + } + SE3 fh_2_slast = slast_2_sprelast;// assumed to be the same as fh_2_slast. + + + // get last delta-movement. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * fh_2_slast.inverse() * lastF_2_slast); // assume double motion (frame skipped) + lastF_2_fh_tries.push_back(SE3::exp(fh_2_slast.log()*0.5).inverse() * lastF_2_slast); // assume half motion. + lastF_2_fh_tries.push_back(lastF_2_slast); // assume zero motion. + lastF_2_fh_tries.push_back(SE3()); // assume zero motion FROM KF. + + + // just try a TON of different initializations (all rotations). In the end, + // if they don't work they will only be tried on the coarsest level, which is super fast anyway. + // also, if tracking rails here we loose, so we really, really want to avoid that. + for(float rotDelta=0.02; rotDelta < 0.05; rotDelta++) + { + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,0,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + } + + if(!slast->poseValid || !sprelast->poseValid || !lastF->shell->poseValid) + { + lastF_2_fh_tries.clear(); + lastF_2_fh_tries.push_back(SE3()); + } + } + } + + Vec3 flowVecs = Vec3(100,100,100); + SE3 lastF_2_fh = SE3(); + AffLight aff_g2l = AffLight(0,0); + + + // as long as maxResForImmediateAccept is not reached, I'll continue through the options. + // I'll keep track of the so-far best achieved residual for each level in achievedRes. + // If on a coarse level, tracking is WORSE than achievedRes, we will not continue to save time. + + bool trackingGoodRet = false; + + Vec5 achievedRes = Vec5::Constant(NAN); + bool haveOneGood = false; + int tryIterations=0; + for(unsigned int i=0;itrackNewestCoarse( + fh, lastF_2_fh_this, aff_g2l_this, + pyrLevelsUsed-1, + achievedRes); // in each level has to be at least as good as the last try. + tryIterations++; + + if(trackingIsGood) + { + trackingGoodRet = true; + } + if(!trackingIsGood && setting_useIMU) + { + std::cout << "WARNING: Coarse tracker thinks that tracking was not good!" << std::endl; + // In IMU mode we can still estimate the pose sufficiently, even if vision is bad. + trackingIsGood = true; + } + + if(i != 0) + { + printf("RE-TRACK ATTEMPT %d with initOption %d and start-lvl %d (ab %f %f): %f %f %f %f %f -> %f %f %f %f %f \n", + i, + i, pyrLevelsUsed-1, + aff_g2l_this.a,aff_g2l_this.b, + achievedRes[0], + achievedRes[1], + achievedRes[2], + achievedRes[3], + achievedRes[4], + coarseTracker->lastResiduals[0], + coarseTracker->lastResiduals[1], + coarseTracker->lastResiduals[2], + coarseTracker->lastResiduals[3], + coarseTracker->lastResiduals[4]); + } + + + // do we have a new winner? + if(trackingIsGood && std::isfinite((float)coarseTracker->lastResiduals[0]) && !(coarseTracker->lastResiduals[0] >= achievedRes[0])) + { + flowVecs = coarseTracker->lastFlowIndicators; + aff_g2l = aff_g2l_this; + lastF_2_fh = lastF_2_fh_this; + haveOneGood = true; + } + + // take over achieved res (always). + if(haveOneGood) + { + for(int i=0;i<5;i++) + { + if(!std::isfinite((float)achievedRes[i]) || achievedRes[i] > coarseTracker->lastResiduals[i]) // take over if achievedRes is either bigger or NAN. + achievedRes[i] = coarseTracker->lastResiduals[i]; + } + } + + + if(haveOneGood && achievedRes[0] < lastCoarseRMSE[0]*setting_reTrackThreshold) + break; + + } + + if(!haveOneGood) + { + printf("BIG ERROR! tracking failed entirely. Take predicted pose and hope we may somehow recover.\n"); + flowVecs = Vec3(0,0,0); + aff_g2l = aff_last_2_l; + lastF_2_fh = lastF_2_fh_tries[0]; + std::cout << "Predicted pose:\n" << lastF_2_fh.matrix() << std::endl; + if(lastF_2_fh.translation().norm() > 100000 || lastF_2_fh.matrix().hasNaN()) + { + std::cout << "TRACKING FAILED ENTIRELY, NO HOPE TO RECOVER" << std::endl; + std::cerr << "TRACKING FAILED ENTIRELY, NO HOPE TO RECOVER" << std::endl; + exit(1); + } + } + + lastCoarseRMSE = achievedRes; + + // no lock required, as fh is not used anywhere yet. + fh->shell->camToTrackingRef = lastF_2_fh.inverse(); + fh->shell->trackingRef = lastF->shell; + fh->shell->aff_g2l = aff_g2l; + fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef; + fh->shell->trackingWasGood = trackingGoodRet; + + + if(coarseTracker->firstCoarseRMSE < 0) + coarseTracker->firstCoarseRMSE = achievedRes[0]; + + if(!setting_debugout_runquiet) + printf("Coarse Tracker tracked ab = %f %f (exp %f). Res %f!\n", aff_g2l.a, aff_g2l.b, fh->ab_exposure, achievedRes[0]); + + + + if(setting_logStuff) + { + (*coarseTrackingLog) << std::setprecision(16) + << fh->shell->id << " " + << fh->shell->timestamp << " " + << fh->ab_exposure << " " + << fh->shell->camToWorld.log().transpose() << " " + << aff_g2l.a << " " + << aff_g2l.b << " " + << achievedRes[0] << " " + << tryIterations << "\n"; + } + + + return std::make_pair(Vec4(achievedRes[0], flowVecs[0], flowVecs[1], flowVecs[2]), trackingGoodRet); +} + +void FullSystem::traceNewCoarse(FrameHessian* fh) +{ + dmvio::TimeMeasurement timeMeasurement("traceNewCoarse"); + boost::unique_lock lock(mapMutex); + + int trace_total=0, trace_good=0, trace_oob=0, trace_out=0, trace_skip=0, trace_badcondition=0, trace_uninitialized=0; + + Mat33f K = Mat33f::Identity(); + K(0,0) = Hcalib.fxl(); + K(1,1) = Hcalib.fyl(); + K(0,2) = Hcalib.cxl(); + K(1,2) = Hcalib.cyl(); + + for(FrameHessian* host : frameHessians) // go through all active frames + { + + SE3 hostToNew = fh->PRE_worldToCam * host->PRE_camToWorld; + Mat33f KRKi = K * hostToNew.rotationMatrix().cast() * K.inverse(); + Vec3f Kt = K * hostToNew.translation().cast(); + + Vec2f aff = AffLight::fromToVecExposure(host->ab_exposure, fh->ab_exposure, host->aff_g2l(), fh->aff_g2l()).cast(); + + for(ImmaturePoint* ph : host->immaturePoints) + { + ph->traceOn(fh, KRKi, Kt, aff, &Hcalib, false ); + + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD) trace_good++; + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION) trace_badcondition++; + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OOB) trace_oob++; + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OUTLIER) trace_out++; + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED) trace_skip++; + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED) trace_uninitialized++; + trace_total++; + } + } +// printf("ADD: TRACE: %'d points. %'d (%.0f%%) good. %'d (%.0f%%) skip. %'d (%.0f%%) badcond. %'d (%.0f%%) oob. %'d (%.0f%%) out. %'d (%.0f%%) uninit.\n", +// trace_total, +// trace_good, 100*trace_good/(float)trace_total, +// trace_skip, 100*trace_skip/(float)trace_total, +// trace_badcondition, 100*trace_badcondition/(float)trace_total, +// trace_oob, 100*trace_oob/(float)trace_total, +// trace_out, 100*trace_out/(float)trace_total, +// trace_uninitialized, 100*trace_uninitialized/(float)trace_total); +} + + + + +void FullSystem::activatePointsMT_Reductor( + std::vector* optimized, + std::vector* toOptimize, + int min, int max, Vec10* stats, int tid) +{ + ImmaturePointTemporaryResidual* tr = new ImmaturePointTemporaryResidual[frameHessians.size()]; + for(int k=min;knPoints < setting_desiredPointDensity*0.66) + currentMinActDist -= 0.8; + if(ef->nPoints < setting_desiredPointDensity*0.8) + currentMinActDist -= 0.5; + else if(ef->nPoints < setting_desiredPointDensity*0.9) + currentMinActDist -= 0.2; + else if(ef->nPoints < setting_desiredPointDensity) + currentMinActDist -= 0.1; + + if(ef->nPoints > setting_desiredPointDensity*1.5) + currentMinActDist += 0.8; + if(ef->nPoints > setting_desiredPointDensity*1.3) + currentMinActDist += 0.5; + if(ef->nPoints > setting_desiredPointDensity*1.15) + currentMinActDist += 0.2; + if(ef->nPoints > setting_desiredPointDensity) + currentMinActDist += 0.1; + + if(currentMinActDist < 0) currentMinActDist = 0; + if(currentMinActDist > 4) currentMinActDist = 4; + + if(!setting_debugout_runquiet) + printf("SPARSITY: MinActDist %f (need %d points, have %d points)!\n", + currentMinActDist, (int)(setting_desiredPointDensity), ef->nPoints); + + + + FrameHessian* newestHs = frameHessians.back(); + + // make dist map. + coarseDistanceMap->makeK(&Hcalib); + coarseDistanceMap->makeDistanceMap(frameHessians, newestHs); + + //coarseTracker->debugPlotDistMap("distMap"); + + std::vector toOptimize; toOptimize.reserve(20000); + + + for(FrameHessian* host : frameHessians) // go through all active frames + { + if(host == newestHs) continue; + + SE3 fhToNew = newestHs->PRE_worldToCam * host->PRE_camToWorld; + Mat33f KRKi = (coarseDistanceMap->K[1] * fhToNew.rotationMatrix().cast() * coarseDistanceMap->Ki[0]); + Vec3f Kt = (coarseDistanceMap->K[1] * fhToNew.translation().cast()); + + + for(unsigned int i=0;iimmaturePoints.size();i+=1) + { + ImmaturePoint* ph = host->immaturePoints[i]; + ph->idxInImmaturePoints = i; + + // delete points that have never been traced successfully, or that are outlier on the last trace. + if(!std::isfinite(ph->idepth_max) || ph->lastTraceStatus == IPS_OUTLIER) + { +// immature_invalid_deleted++; + // remove point. + delete ph; + host->immaturePoints[i]=0; + continue; + } + + // can activate only if this is true. + bool canActivate = (ph->lastTraceStatus == IPS_GOOD + || ph->lastTraceStatus == IPS_SKIPPED + || ph->lastTraceStatus == IPS_BADCONDITION + || ph->lastTraceStatus == IPS_OOB ) + && ph->lastTracePixelInterval < 8 + && ph->quality > setting_minTraceQuality + && (ph->idepth_max+ph->idepth_min) > 0; + + + // if I cannot activate the point, skip it. Maybe also delete it. + if(!canActivate) + { + // if point will be out afterwards, delete it instead. + if(ph->host->flaggedForMarginalization || ph->lastTraceStatus == IPS_OOB) + { +// immature_notReady_deleted++; + delete ph; + host->immaturePoints[i]=0; + } +// immature_notReady_skipped++; + continue; + } + + + // see if we need to activate point due to distance map. + Vec3f ptp = KRKi * Vec3f(ph->u, ph->v, 1) + Kt*(0.5f*(ph->idepth_max+ph->idepth_min)); + int u = ptp[0] / ptp[2] + 0.5f; + int v = ptp[1] / ptp[2] + 0.5f; + + if((u > 0 && v > 0 && u < wG[1] && v < hG[1])) + { + + float dist = coarseDistanceMap->fwdWarpedIDDistFinal[u+wG[1]*v] + (ptp[0]-floorf((float)(ptp[0]))); + + if(dist>=currentMinActDist* ph->my_type) + { + coarseDistanceMap->addIntoDistFinal(u,v); + toOptimize.push_back(ph); + } + } + else + { + delete ph; + host->immaturePoints[i]=0; + } + } + } + + +// printf("ACTIVATE: %d. (del %d, notReady %d, marg %d, good %d, marg-skip %d)\n", +// (int)toOptimize.size(), immature_deleted, immature_notReady, immature_needMarg, immature_want, immature_margskip); + + std::vector optimized; optimized.resize(toOptimize.size()); + + if(multiThreading) + treadReduce.reduce(boost::bind(&FullSystem::activatePointsMT_Reductor, this, &optimized, &toOptimize, _1, _2, _3, _4), 0, toOptimize.size(), 50); + + else + activatePointsMT_Reductor(&optimized, &toOptimize, 0, toOptimize.size(), 0, 0); + + + for(unsigned k=0;khost->immaturePoints[ph->idxInImmaturePoints]=0; + newpoint->host->pointHessians.push_back(newpoint); + ef->insertPoint(newpoint); + for(PointFrameResidual* r : newpoint->residuals) + ef->insertResidual(r); + assert(newpoint->efPoint != 0); + delete ph; + } + else if(newpoint == (PointHessian*)((long)(-1)) || ph->lastTraceStatus==IPS_OOB) + { + ph->host->immaturePoints[ph->idxInImmaturePoints]=0; + delete ph; + } + else + { + assert(newpoint == 0 || newpoint == (PointHessian*)((long)(-1))); + } + } + + + for(FrameHessian* host : frameHessians) + { + for(int i=0;i<(int)host->immaturePoints.size();i++) + { + if(host->immaturePoints[i]==0) + { + host->immaturePoints[i] = host->immaturePoints.back(); + host->immaturePoints.pop_back(); + i--; + } + } + } + + +} + + + + + + +void FullSystem::activatePointsOldFirst() +{ + assert(false); +} + +void FullSystem::flagPointsForRemoval() +{ + assert(EFIndicesValid); + + std::vector fhsToKeepPoints; + std::vector fhsToMargPoints; + + //if(setting_margPointVisWindow>0) + { + for(int i=((int)frameHessians.size())-1;i>=0 && i >= ((int)frameHessians.size());i--) + if(!frameHessians[i]->flaggedForMarginalization) fhsToKeepPoints.push_back(frameHessians[i]); + + for(int i=0; i< (int)frameHessians.size();i++) + if(frameHessians[i]->flaggedForMarginalization) fhsToMargPoints.push_back(frameHessians[i]); + } + + + + //ef->setAdjointsF(); + //ef->setDeltaF(&Hcalib); + int flag_oob=0, flag_in=0, flag_inin=0, flag_nores=0; + + for(FrameHessian* host : frameHessians) // go through all active frames + { + for(unsigned int i=0;ipointHessians.size();i++) + { + PointHessian* ph = host->pointHessians[i]; + if(ph==0) continue; + + if(ph->idepth_scaled < setting_minIdepth || ph->residuals.size()==0) + { + host->pointHessiansOut.push_back(ph); + ph->efPoint->stateFlag = EFPointStatus::PS_DROP; + host->pointHessians[i]=0; + flag_nores++; + } + else if(ph->isOOB(fhsToKeepPoints, fhsToMargPoints) || host->flaggedForMarginalization) + { + flag_oob++; + if(ph->isInlierNew()) + { + flag_in++; + int ngoodRes=0; + for(PointFrameResidual* r : ph->residuals) + { + r->resetOOB(); + r->linearize(&Hcalib); + r->efResidual->isLinearized = false; + r->applyRes(true); + if(r->efResidual->isActive()) + { + r->efResidual->fixLinearizationF(ef); + ngoodRes++; + } + } + if(ph->idepth_hessian > setting_minIdepthH_marg) + { + flag_inin++; + ph->efPoint->stateFlag = EFPointStatus::PS_MARGINALIZE; + host->pointHessiansMarginalized.push_back(ph); + } + else + { + ph->efPoint->stateFlag = EFPointStatus::PS_DROP; + host->pointHessiansOut.push_back(ph); + } + + + } + else + { + host->pointHessiansOut.push_back(ph); + ph->efPoint->stateFlag = EFPointStatus::PS_DROP; + + + //printf("drop point in frame %d (%d goodRes, %d activeRes)\n", ph->host->idx, ph->numGoodResiduals, (int)ph->residuals.size()); + } + + host->pointHessians[i]=0; + } + } + + + for(int i=0;i<(int)host->pointHessians.size();i++) + { + if(host->pointHessians[i]==0) + { + host->pointHessians[i] = host->pointHessians.back(); + host->pointHessians.pop_back(); + i--; + } + } + } + +} + +// The function is passed the IMU-data from the previous frame until the current frame. +void FullSystem::addActiveFrame(ImageAndExposure* image, int id, dmvio::IMUData* imuData, dmvio::GTData* gtData) +{ + // Measure Time of the time measurement. + dmvio::TimeMeasurement timeMeasurementMeasurement("timeMeasurement"); + dmvio::TimeMeasurement timeMeasurementZero("zero"); + timeMeasurementZero.end(); + timeMeasurementMeasurement.end(); + + dmvio::TimeMeasurement timeMeasurement("addActiveFrame"); + boost::unique_lock lock(trackMutex); + + + dmvio::TimeMeasurement measureInit("initObjectsAndMakeImage"); + // =========================== add into allFrameHistory ========================= + FrameHessian* fh = new FrameHessian(); + FrameShell* shell = new FrameShell(); + shell->camToWorld = SE3(); // no lock required, as fh is not used anywhere yet. + shell->aff_g2l = AffLight(0,0); + shell->marginalizedAt = shell->id = allFrameHistory.size(); + shell->timestamp = image->timestamp; + shell->incoming_id = id; + fh->shell = shell; + allFrameHistory.push_back(shell); + + + // =========================== make Images / derivatives etc. ========================= + fh->ab_exposure = image->exposure_time; + fh->makeImages(image->image, &Hcalib); + + measureInit.end(); + + if(!initialized) + { + // use initializer! + if(coarseInitializer->frameID<0) // first frame set. fh is kept by coarseInitializer. + { + // Only in this case no IMU-data is accumulated for the BA as this is the first frame. + dmvio::TimeMeasurement initMeasure("InitializerFirstFrame"); + coarseInitializer->setFirst(&Hcalib, fh); + if(setting_useIMU) + { + gravityInit.addMeasure(*imuData, Sophus::SE3d()); + } + }else + { + dmvio::TimeMeasurement initMeasure("InitializerOtherFrames"); + bool initDone = coarseInitializer->trackFrame(fh, outputWrapper); + if(setting_useIMU) + { + imuIntegration.addIMUDataToBA(*imuData); + Sophus::SE3 imuToWorld = gravityInit.addMeasure(*imuData, Sophus::SE3d()); + if(initDone) + { + firstPose = imuToWorld * imuIntegration.TS_cam_imu.inverse(); + } + } + if (initDone) // if SNAPPED + { + initializeFromInitializer(fh); + if(setting_useIMU && linearizeOperation) + { + imuIntegration.setGTData(gtData, fh->shell->id); + } + lock.unlock(); + initMeasure.end(); + deliverTrackedFrame(fh, true); + } else + { + // if still initializing + + // Maybe change first frame. + double timeBetweenFrames = fh->shell->timestamp - coarseInitializer->firstFrame->shell->timestamp; + std::cout << "InitTimeBetweenFrames: " << timeBetweenFrames << std::endl; + if(timeBetweenFrames > imuIntegration.getImuSettings().maxTimeBetweenInitFrames) + { + // Make this the first frame. + // delete old first frame + coarseInitializer->firstFrame->shell->poseValid = false; + delete coarseInitializer->firstFrame; + + // Clear frame history + allFrameHistory.pop_back(); // Pop the current shell as we do not want to delete it. + for(auto&& shell : allFrameHistory) + { + delete shell; + } + allFrameHistory.clear(); + + // Set this to the first id again. + shell->marginalizedAt = shell->id = allFrameHistory.size(); + allFrameHistory.push_back(shell); + + if(setting_useIMU) imuIntegration.resetBAPreintegration(); + + coarseInitializer->setFirst(&Hcalib, fh); + }else + { + fh->shell->poseValid = false; + delete fh; + } + } + } + return; + } + else // do front-end operation. + { + // -------------------------- Coarse tracking (after visual initializer succeeded). -------------------------- + dmvio::TimeMeasurement coarseTrackingTime("fullCoarseTracking"); + int lastFrameId = -1; + + // =========================== SWAP tracking reference?. ========================= + bool trackingRefChanged = false; + if(coarseTracker_forNewKF->refFrameID > coarseTracker->refFrameID) + { + dmvio::TimeMeasurement referenceSwapTime("swapTrackingRef"); + boost::unique_lock crlock(coarseTrackerSwapMutex); + CoarseTracker* tmp = coarseTracker; coarseTracker=coarseTracker_forNewKF; coarseTracker_forNewKF=tmp; + + if(dso::setting_useIMU) + { + // BA for new keyframe has finished and we have a new tracking reference. + std::cout << "New ref frame id: " << coarseTracker->refFrameID << " prepared keyframe id: " << imuIntegration.getPreparedKeyframe() << std::endl; + + lastFrameId = coarseTracker->refFrameID; + + assert(coarseTracker->refFrameID == imuIntegration.getPreparedKeyframe()); + SE3 lastRefToNewRef = imuIntegration.initCoarseGraph(); + + trackingRefChanged = true; + } + } + + SE3 *referenceToFramePassed = 0; + SE3 referenceToFrame; + if(dso::setting_useIMU) + { + SE3 referenceToFrame = imuIntegration.addIMUData(*imuData, fh->shell->id, + fh->shell->timestamp, trackingRefChanged, lastFrameId); + // If initialized we use the prediction from IMU data as initialization for the coarse tracking. + referenceToFramePassed = &referenceToFrame; + if(!imuIntegration.isCoarseInitialized()) + { + referenceToFramePassed = nullptr; + } + imuIntegration.addIMUDataToBA(*imuData); + } + + std::pair pair = trackNewCoarse(fh, referenceToFramePassed); + dso::Vec4 tres = std::move(pair.first); + bool forceNoKF = !pair.second; // If coarse tracking was bad don't make KF. + bool forceKF = false; + if(!std::isfinite((double)tres[0]) || !std::isfinite((double)tres[1]) || !std::isfinite((double)tres[2]) || !std::isfinite((double)tres[3])) + { + if(setting_useIMU) + { + // If completely Nan, don't force noKF! + forceNoKF = false; + forceKF = true; // actually we force a KF in that situation as there are no points to track. + }else + { + printf("Initial Tracking failed: LOST!\n"); + isLost=true; + return; + } + } + + double timeSinceLastKeyframe = fh->shell->timestamp - allKeyFramesHistory.back()->timestamp; + bool needToMakeKF = false; + if(setting_keyframesPerSecond > 0) + { + needToMakeKF = allFrameHistory.size()== 1 || + (fh->shell->timestamp - allKeyFramesHistory.back()->timestamp) > 0.95f/setting_keyframesPerSecond; + } + else + { + Vec2 refToFh=AffLight::fromToVecExposure(coarseTracker->lastRef->ab_exposure, fh->ab_exposure, + coarseTracker->lastRef_aff_g2l, fh->shell->aff_g2l); + + // BRIGHTNESS CHECK + needToMakeKF = allFrameHistory.size()== 1 || + setting_kfGlobalWeight*setting_maxShiftWeightT * sqrtf((double)tres[1]) / (wG[0]+hG[0]) + + setting_kfGlobalWeight*setting_maxShiftWeightR * sqrtf((double)tres[2]) / (wG[0]+hG[0]) + + setting_kfGlobalWeight*setting_maxShiftWeightRT * sqrtf((double)tres[3]) / (wG[0]+hG[0]) + + setting_kfGlobalWeight*setting_maxAffineWeight * fabs(logf((float)refToFh[0])) > 1 || + 2*coarseTracker->firstCoarseRMSE < tres[0] || + (setting_maxTimeBetweenKeyframes > 0 && timeSinceLastKeyframe > setting_maxTimeBetweenKeyframes) || + forceKF; + + if(needToMakeKF) + { + std::cout << "Time since last keyframe: " << timeSinceLastKeyframe << std::endl; + } + + } + double transNorm = fh->shell->camToTrackingRef.translation().norm() * imuIntegration.getCoarseScale(); + if(imuIntegration.isCoarseInitialized() && transNorm < setting_forceNoKFTranslationThresh) + { + forceNoKF = true; + } + if(forceNoKF) + { + std::cout << "Forcing NO KF!" << std::endl; + needToMakeKF = false; + } + + if(needToMakeKF) + { + int prevKFId = fh->shell->trackingRef->id; + // In non-RT mode this will always be accurate, but in RT mode the printout in makeKeyframe is correct (because some of these KFs do not end up getting created). + int framesBetweenKFs = fh->shell->id - prevKFId - 1; + + // Enforce setting_minFramesBetweenKeyframes. + if(framesBetweenKFs < (int) setting_minFramesBetweenKeyframes) // if integer value is smaller we just skip. + { + std::cout << "Skipping KF because of minFramesBetweenKeyframes." << std::endl; + needToMakeKF = false; + }else if(framesBetweenKFs < setting_minFramesBetweenKeyframes) // Enforce it for non-integer values. + { + double fractionalPart = setting_minFramesBetweenKeyframes - (int) setting_minFramesBetweenKeyframes; + framesBetweenKFsRest += fractionalPart; + if(framesBetweenKFsRest >= 1.0) + { + std::cout << "Skipping KF because of minFramesBetweenKeyframes." << std::endl; + needToMakeKF = false; + framesBetweenKFsRest--; + } + } + + } + + if(setting_useIMU) + { + imuIntegration.finishCoarseTracking(*(fh->shell), needToMakeKF); + } + + if(needToMakeKF && setting_useIMU && linearizeOperation) + { + imuIntegration.setGTData(gtData, fh->shell->id); + } + + dmvio::TimeMeasurement timeLastStuff("afterCoarseTracking"); + + for(IOWrap::Output3DWrapper* ow : outputWrapper) + ow->publishCamPose(fh->shell, &Hcalib); + + lock.unlock(); + timeLastStuff.end(); + coarseTrackingTime.end(); + deliverTrackedFrame(fh, needToMakeKF); + return; + } +} +void FullSystem::deliverTrackedFrame(FrameHessian* fh, bool needKF) +{ + dmvio::TimeMeasurement timeMeasurement("deliverTrackedFrame"); + // There seems to be exactly one instance where needKF is false but the mapper creates a keyframe nevertheless: if it is the second tracked frame (so it will become the third keyframe in total) + // There are also some cases where needKF is true but the mapper does not create a keyframe. + + bool alreadyPreparedKF = setting_useIMU && imuIntegration.getPreparedKeyframe() != -1 && !linearizeOperation; + + std::cout << "Frame history size: " << allFrameHistory.size() << std::endl; + if((needKF || (!secondKeyframeDone && !linearizeOperation)) && setting_useIMU && !alreadyPreparedKF) + { + // prepareKeyframe tells the IMU-Integration that this frame will become a keyframe. -> don' marginalize it during addIMUData. + // Also resets the IMU preintegration for the BA. + std::cout << "Preparing keyframe: " << fh->shell->id << std::endl; + imuIntegration.prepareKeyframe(fh->shell->id); + + if(!needKF) + { + secondKeyframeDone = true; + } + }else + { + std::cout << "Creating a non-keyframe: " << fh->shell->id << std::endl; + } + + if(linearizeOperation) + { + if(goStepByStep && lastRefStopID != coarseTracker->refFrameID) + { + MinimalImageF3 img(wG[0], hG[0], fh->dI); + IOWrap::displayImage("frameToTrack", &img); + while(true) + { + char k=IOWrap::waitKey(0); + if(k==' ') break; + handleKey( k ); + } + lastRefStopID = coarseTracker->refFrameID; + } + else handleKey( IOWrap::waitKey(1) ); + + + + if(needKF) + { + if(setting_useIMU) + { + imuIntegration.keyframeCreated(fh->shell->id); + } + makeKeyFrame(fh); + } + else makeNonKeyFrame(fh); + } + else + { + boost::unique_lock lock(trackMapSyncMutex); + unmappedTrackedFrames.push_back(fh); + + // If the prepared KF is still in the queue right now the current frame will become a KF instead. + if(alreadyPreparedKF && !imuIntegration.isPreparedKFCreated()) + { + imuIntegration.prepareKeyframe(fh->shell->id); + needKF = true; + } + + if(setting_useIMU) + { + if(needKF) needNewKFAfter=imuIntegration.getPreparedKeyframe(); + }else + { + if(needKF) needNewKFAfter=fh->shell->trackingRef->id; + } + trackedFrameSignal.notify_all(); + + while(coarseTracker_forNewKF->refFrameID == -1 && coarseTracker->refFrameID == -1 ) + { + mappedFrameSignal.wait(lock); + } + + lock.unlock(); + } +} + +void FullSystem::mappingLoop() +{ + boost::unique_lock lock(trackMapSyncMutex); + + while(runMapping) + { + while(unmappedTrackedFrames.size()==0) + { + trackedFrameSignal.wait(lock); + if(!runMapping) return; + } + + FrameHessian* fh = unmappedTrackedFrames.front(); + unmappedTrackedFrames.pop_front(); + + std::cout << "Current mapping id: " << fh->shell->id << " create KF after: " << needNewKFAfter << std::endl; + + // guaranteed to make a KF for the very first two tracked frames. + if(allKeyFramesHistory.size() <= 2) + { + if(setting_useIMU) + { + imuIntegration.keyframeCreated(fh->shell->id); + } + lock.unlock(); + makeKeyFrame(fh); + lock.lock(); + mappedFrameSignal.notify_all(); + continue; + } + + if(unmappedTrackedFrames.size() > 3) + needToKetchupMapping=true; + + + if(unmappedTrackedFrames.size() > 0) // if there are other frames to track, do that first. + { + + if(setting_useIMU && needNewKFAfter == fh->shell->id) + { + std::cout << "WARNING: Prepared keyframe got skipped!" << std::endl; + imuIntegration.skipPreparedKeyframe(); + assert(false); + } + + lock.unlock(); + makeNonKeyFrame(fh); + lock.lock(); + + if(needToKetchupMapping && unmappedTrackedFrames.size() > 0) + { + FrameHessian* fh = unmappedTrackedFrames.front(); + unmappedTrackedFrames.pop_front(); + { + boost::unique_lock crlock(shellPoseMutex); + assert(fh->shell->trackingRef != 0); + fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef; + fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l); + } + delete fh; + } + + } + else + { + bool createKF = setting_useIMU ? needNewKFAfter==fh->shell->id : needNewKFAfter >= frameHessians.back()->shell->id; + if(setting_realTimeMaxKF || createKF) + { + if(setting_useIMU) + { + imuIntegration.keyframeCreated(fh->shell->id); + } + lock.unlock(); + makeKeyFrame(fh); + needToKetchupMapping=false; + lock.lock(); + } + else + { + lock.unlock(); + makeNonKeyFrame(fh); + lock.lock(); + } + } + mappedFrameSignal.notify_all(); + } + printf("MAPPING FINISHED!\n"); +} + +void FullSystem::blockUntilMappingIsFinished() +{ + boost::unique_lock lock(trackMapSyncMutex); + runMapping = false; + trackedFrameSignal.notify_all(); + lock.unlock(); + + mappingThread.join(); + +} + +void FullSystem::makeNonKeyFrame( FrameHessian* fh) +{ + dmvio::TimeMeasurement timeMeasurement("makeNonKeyframe"); + // needs to be set by mapping thread. no lock required since we are in mapping thread. + { + boost::unique_lock crlock(shellPoseMutex); + assert(fh->shell->trackingRef != 0); + fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef; + fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l); + } + + traceNewCoarse(fh); + delete fh; +} + +void FullSystem::makeKeyFrame( FrameHessian* fh) +{ + dmvio::TimeMeasurement timeMeasurement("makeKeyframe"); + // needs to be set by mapping thread + { + boost::unique_lock crlock(shellPoseMutex); + assert(fh->shell->trackingRef != 0); + fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef; + fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l); + int prevKFId = fh->shell->trackingRef->id; + int framesBetweenKFs = fh->shell->id - prevKFId - 1; + std::cout << "Frames between KFs: " << framesBetweenKFs << std::endl; + } + + traceNewCoarse(fh); + + boost::unique_lock lock(mapMutex); + + // =========================== Flag Frames to be Marginalized. ========================= + flagFramesForMarginalization(fh); + + + // =========================== add New Frame to Hessian Struct. ========================= + dmvio::TimeMeasurement timeMeasurementAddFrame("newFrameAndNewResidualsForOldPoints"); + fh->idx = frameHessians.size(); + frameHessians.push_back(fh); + fh->frameID = allKeyFramesHistory.size(); + fh->shell->keyframeId = fh->frameID; + allKeyFramesHistory.push_back(fh->shell); + ef->insertFrame(fh, &Hcalib); + + setPrecalcValues(); + + + + // =========================== add new residuals for old points ========================= + int numFwdResAdde=0; + for(FrameHessian* fh1 : frameHessians) // go through all active frames + { + if(fh1 == fh) continue; + for(PointHessian* ph : fh1->pointHessians) + { + PointFrameResidual* r = new PointFrameResidual(ph, fh1, fh); + r->setState(ResState::IN); + ph->residuals.push_back(r); + ef->insertResidual(r); + ph->lastResiduals[1] = ph->lastResiduals[0]; + ph->lastResiduals[0] = std::pair(r, ResState::IN); + numFwdResAdde+=1; + } + } + + timeMeasurementAddFrame.end(); + + + // =========================== Activate Points (& flag for marginalization). ========================= + activatePointsMT(); + ef->makeIDX(); + + + + + if(setting_useGTSAMIntegration) + { + // Adds new keyframe to the BA graph, together with matching factors (e.g. IMUFactors). + baIntegration->addKeyframeToBA(fh->shell->id, fh->shell->camToWorld, ef->frames); + } + + // =========================== OPTIMIZE ALL ========================= + + fh->frameEnergyTH = frameHessians.back()->frameEnergyTH; + float rmse = optimize(setting_maxOptIterations); + + + + + // =========================== Figure Out if INITIALIZATION FAILED ========================= + if(allKeyFramesHistory.size() <= 4) + { + if(allKeyFramesHistory.size()==2 && rmse > 20*benchmark_initializerSlackFactor) + { + printf("I THINK INITIALIZATINO FAILED! Resetting.\n"); + initFailed=true; + } + if(allKeyFramesHistory.size()==3 && rmse > 13*benchmark_initializerSlackFactor) + { + printf("I THINK INITIALIZATINO FAILED! Resetting.\n"); + initFailed=true; + } + if(allKeyFramesHistory.size()==4 && rmse > 9*benchmark_initializerSlackFactor) + { + printf("I THINK INITIALIZATINO FAILED! Resetting.\n"); + initFailed=true; + } + } + + + // =========================== REMOVE OUTLIER ========================= + removeOutliers(); + + + + if(setting_useIMU) + { + imuIntegration.postOptimization(fh->shell->id); + } + + { + dmvio::TimeMeasurement timeMeasurement("makeKeyframeChangeTrackingRef"); + boost::unique_lock crlock(coarseTrackerSwapMutex); + + if(setting_useIMU) + { + imuIntegration.finishKeyframeOptimization(fh->shell->id); + } + + coarseTracker_forNewKF->makeK(&Hcalib); + coarseTracker_forNewKF->setCoarseTrackingRef(frameHessians); + + + coarseTracker_forNewKF->debugPlotIDepthMap(&minIdJetVisTracker, &maxIdJetVisTracker, outputWrapper); + coarseTracker_forNewKF->debugPlotIDepthMapFloat(outputWrapper); + } + + + debugPlot("post Optimize"); + + + for(auto* ow : outputWrapper) + { + ow->publishTransformDSOToIMU(imuIntegration.getTransformDSOToIMU()); + } + + + + // =========================== (Activate-)Marginalize Points ========================= + dmvio::TimeMeasurement timeMeasurementMarginalizePoints("marginalizeAndRemovePoints"); + flagPointsForRemoval(); + ef->dropPointsF(); + getNullspaces( + ef->lastNullspaces_pose, + ef->lastNullspaces_scale, + ef->lastNullspaces_affA, + ef->lastNullspaces_affB); + ef->marginalizePointsF(); + timeMeasurementMarginalizePoints.end(); + + + // =========================== add new Immature points & new residuals ========================= + makeNewTraces(fh, 0); + + + + dmvio::TimeMeasurement timeMeasurementPublish("publishInMakeKeyframe"); + for(IOWrap::Output3DWrapper* ow : outputWrapper) + { + ow->publishGraph(ef->connectivityMap); + ow->publishKeyframes(frameHessians, false, &Hcalib); + } + timeMeasurementPublish.end(); + + + + // =========================== Marginalize Frames ========================= + + dmvio::TimeMeasurement timeMeasurementMargFrames("marginalizeFrames"); + for(unsigned int i=0;iflaggedForMarginalization) + { + marginalizeFrame(frameHessians[i]); + i=0; + if(setting_useGTSAMIntegration) + { + baIntegration->updateBAOrdering(ef->frames); + } + } + timeMeasurementMargFrames.end(); + + + + printLogLine(); + printEigenValLine(); + + if(setting_useGTSAMIntegration) + { + baIntegration->updateBAValues(ef->frames); + } + + if(setting_useIMU) + { + imuIntegration.finishKeyframeOperations(fh->shell->id); + } +} + + +void FullSystem::initializeFromInitializer(FrameHessian* newFrame) +{ + boost::unique_lock lock(mapMutex); + + // add firstframe. + FrameHessian* firstFrame = coarseInitializer->firstFrame; + firstFrame->idx = frameHessians.size(); + frameHessians.push_back(firstFrame); + firstFrame->frameID = allKeyFramesHistory.size(); + allKeyFramesHistory.push_back(firstFrame->shell); + ef->insertFrame(firstFrame, &Hcalib); + setPrecalcValues(); + + baIntegration->addFirstBAFrame(firstFrame->shell->id); + + firstFrame->pointHessians.reserve(wG[0]*hG[0]*0.2f); + firstFrame->pointHessiansMarginalized.reserve(wG[0]*hG[0]*0.2f); + firstFrame->pointHessiansOut.reserve(wG[0]*hG[0]*0.2f); + + + float sumID=1e-5, numID=1e-5; + + double sumFirst = 0.0; + double sumSecond = 0.0; + int num = 0; + for(int i=0;inumPoints[0];i++) + { + sumID += coarseInitializer->points[0][i].iR; + numID++; + } + + sumFirst /= num; + sumSecond /= num; + + float rescaleFactor = 1; + + rescaleFactor = 1 / (sumID / numID); + + SE3 firstToNew = coarseInitializer->thisToNext; + std::cout << "Scaling with rescaleFactor: " << rescaleFactor << std::endl; + firstToNew.translation() /= rescaleFactor; + + // randomly sub-select the points I need. + float keepPercentage = setting_desiredPointDensity / coarseInitializer->numPoints[0]; + + if(!setting_debugout_runquiet) + printf("Initialization: keep %.1f%% (need %d, have %d)!\n", 100*keepPercentage, + (int)(setting_desiredPointDensity), coarseInitializer->numPoints[0] ); + + for(int i=0;inumPoints[0];i++) + { + if(rand()/(float)RAND_MAX > keepPercentage) continue; + + Pnt* point = coarseInitializer->points[0]+i; + ImmaturePoint* pt = new ImmaturePoint(point->u+0.5f,point->v+0.5f,firstFrame,point->my_type, &Hcalib); + + if(!std::isfinite(pt->energyTH)) { delete pt; continue; } + + + pt->idepth_max=pt->idepth_min=1; + PointHessian* ph = new PointHessian(pt, &Hcalib); + delete pt; + if(!std::isfinite(ph->energyTH)) {delete ph; continue;} + + ph->setIdepthScaled(point->iR * rescaleFactor); + ph->setIdepthZero(ph->idepth); + ph->hasDepthPrior=true; + ph->setPointStatus(PointHessian::ACTIVE); + + firstFrame->pointHessians.push_back(ph); + ef->insertPoint(ph); + } + + + // really no lock required, as we are initializing. + { + boost::unique_lock crlock(shellPoseMutex); + firstFrame->shell->camToWorld = firstPose; + firstFrame->shell->aff_g2l = AffLight(0,0); + firstFrame->setEvalPT_scaled(firstFrame->shell->camToWorld.inverse(),firstFrame->shell->aff_g2l); + firstFrame->shell->trackingRef=0; + firstFrame->shell->camToTrackingRef = SE3(); + firstFrame->shell->keyframeId = 0; + + newFrame->shell->camToWorld = firstPose * firstToNew.inverse(); + newFrame->shell->aff_g2l = AffLight(0,0); + newFrame->setEvalPT_scaled(newFrame->shell->camToWorld.inverse(),newFrame->shell->aff_g2l); + newFrame->shell->trackingRef = firstFrame->shell; + newFrame->shell->camToTrackingRef = firstToNew.inverse(); + + } + imuIntegration.finishCoarseTracking(*(newFrame->shell), true); + + initialized=true; + printf("INITIALIZE FROM INITIALIZER (%d pts)!\n", (int)firstFrame->pointHessians.size()); +} + +void FullSystem::makeNewTraces(FrameHessian* newFrame, float* gtDepth) +{ + dmvio::TimeMeasurement timeMeasurement("makeNewTraces"); + pixelSelector->allowFast = true; + //int numPointsTotal = makePixelStatus(newFrame->dI, selectionMap, wG[0], hG[0], setting_desiredDensity); + int numPointsTotal = pixelSelector->makeMaps(newFrame, selectionMap,setting_desiredImmatureDensity); + + newFrame->pointHessians.reserve(numPointsTotal*1.2f); + //fh->pointHessiansInactive.reserve(numPointsTotal*1.2f); + newFrame->pointHessiansMarginalized.reserve(numPointsTotal*1.2f); + newFrame->pointHessiansOut.reserve(numPointsTotal*1.2f); + + + for(int y=patternPadding+1;yenergyTH)) delete impt; + else newFrame->immaturePoints.push_back(impt); + + } + //printf("MADE %d IMMATURE POINTS!\n", (int)newFrame->immaturePoints.size()); + +} + + + +void FullSystem::setPrecalcValues() +{ + for(FrameHessian* fh : frameHessians) + { + fh->targetPrecalc.resize(frameHessians.size()); + for(unsigned int i=0;itargetPrecalc[i].set(fh, frameHessians[i], &Hcalib); + } + + ef->setDeltaF(&Hcalib); +} + + +void FullSystem::printLogLine() +{ + dmvio::TimeMeasurement timeMeasurementMargFrames("printLogLine"); + if(frameHessians.size()==0) return; + + if(!setting_debugout_runquiet) + printf("LOG %d: %.3f fine. Res: %d A, %d L, %d M; (%'d / %'d) forceDrop. a=%f, b=%f. Window %d (%d)\n", + allKeyFramesHistory.back()->id, + statistics_lastFineTrackRMSE, + ef->resInA, + ef->resInL, + ef->resInM, + (int)statistics_numForceDroppedResFwd, + (int)statistics_numForceDroppedResBwd, + allKeyFramesHistory.back()->aff_g2l.a, + allKeyFramesHistory.back()->aff_g2l.b, + frameHessians.back()->shell->id - frameHessians.front()->shell->id, + (int)frameHessians.size()); + + + if(!setting_logStuff) return; + + if(numsLog != 0) + { + (*numsLog) << allKeyFramesHistory.back()->id << " " << + statistics_lastFineTrackRMSE << " " << + (int)statistics_numCreatedPoints << " " << + (int)statistics_numActivatedPoints << " " << + (int)statistics_numDroppedPoints << " " << + (int)statistics_lastNumOptIts << " " << + ef->resInA << " " << + ef->resInL << " " << + ef->resInM << " " << + statistics_numMargResFwd << " " << + statistics_numMargResBwd << " " << + statistics_numForceDroppedResFwd << " " << + statistics_numForceDroppedResBwd << " " << + frameHessians.back()->aff_g2l().a << " " << + frameHessians.back()->aff_g2l().b << " " << + frameHessians.back()->shell->id - frameHessians.front()->shell->id << " " << + (int)frameHessians.size() << " " << "\n"; + numsLog->flush(); + } + + +} + + + +void FullSystem::printEigenValLine() +{ + dmvio::TimeMeasurement timeMeasurementMargFrames("printEigenValLine"); + if(!setting_logStuff) return; + if(ef->lastHS.rows() < 12) return; + + + MatXX Hp = ef->lastHS.bottomRightCorner(ef->lastHS.cols()-CPARS,ef->lastHS.cols()-CPARS); + MatXX Ha = ef->lastHS.bottomRightCorner(ef->lastHS.cols()-CPARS,ef->lastHS.cols()-CPARS); + int n = Hp.cols()/8; + assert(Hp.cols()%8==0); + + // sub-select + for(int i=0;ilastHS.eigenvalues().real(); + VecX eigenP = Hp.topLeftCorner(n*6,n*6).eigenvalues().real(); + VecX eigenA = Ha.topLeftCorner(n*2,n*2).eigenvalues().real(); + VecX diagonal = ef->lastHS.diagonal(); + + std::sort(eigenvaluesAll.data(), eigenvaluesAll.data()+eigenvaluesAll.size()); + std::sort(eigenP.data(), eigenP.data()+eigenP.size()); + std::sort(eigenA.data(), eigenA.data()+eigenA.size()); + + int nz = std::max(100,setting_maxFrames*10); + + if(eigenAllLog != 0) + { + VecX ea = VecX::Zero(nz); ea.head(eigenvaluesAll.size()) = eigenvaluesAll; + (*eigenAllLog) << allKeyFramesHistory.back()->id << " " << ea.transpose() << "\n"; + eigenAllLog->flush(); + } + if(eigenALog != 0) + { + VecX ea = VecX::Zero(nz); ea.head(eigenA.size()) = eigenA; + (*eigenALog) << allKeyFramesHistory.back()->id << " " << ea.transpose() << "\n"; + eigenALog->flush(); + } + if(eigenPLog != 0) + { + VecX ea = VecX::Zero(nz); ea.head(eigenP.size()) = eigenP; + (*eigenPLog) << allKeyFramesHistory.back()->id << " " << ea.transpose() << "\n"; + eigenPLog->flush(); + } + + if(DiagonalLog != 0) + { + VecX ea = VecX::Zero(nz); ea.head(diagonal.size()) = diagonal; + (*DiagonalLog) << allKeyFramesHistory.back()->id << " " << ea.transpose() << "\n"; + DiagonalLog->flush(); + } + + if(variancesLog != 0) + { + VecX ea = VecX::Zero(nz); ea.head(diagonal.size()) = ef->lastHS.inverse().diagonal(); + (*variancesLog) << allKeyFramesHistory.back()->id << " " << ea.transpose() << "\n"; + variancesLog->flush(); + } + + std::vector &nsp = ef->lastNullspaces_forLogging; + (*nullspacesLog) << allKeyFramesHistory.back()->id << " "; + for(unsigned int i=0;ilastHS * nsp[i]) << " " << nsp[i].dot(ef->lastbS) << " " ; + (*nullspacesLog) << "\n"; + nullspacesLog->flush(); + +} + +void FullSystem::printFrameLifetimes() +{ + if(!setting_logStuff) return; + + + boost::unique_lock lock(trackMutex); + + std::ofstream* lg = new std::ofstream(); + lg->open("logs/lifetimeLog.txt", std::ios::trunc | std::ios::out); + lg->precision(15); + + for(FrameShell* s : allFrameHistory) + { + (*lg) << s->id + << " " << s->marginalizedAt + << " " << s->statistics_goodResOnThis + << " " << s->statistics_outlierResOnThis + << " " << s->movedByOpt; + + + + (*lg) << "\n"; + } + + + + + + lg->close(); + delete lg; + +} + + +void FullSystem::printEvalLine() +{ + return; +} + + + +} diff --git a/src/dso/FullSystem/FullSystem.h b/src/dso/FullSystem/FullSystem.h new file mode 100644 index 0000000..6c9dd85 --- /dev/null +++ b/src/dso/FullSystem/FullSystem.h @@ -0,0 +1,341 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#define MAX_ACTIVE_FRAMES 100 + +#include +#include "util/NumType.h" +#include "util/globalCalib.h" +#include "vector" + +#include +#include +#include "util/NumType.h" +#include "FullSystem/Residuals.h" +#include "FullSystem/HessianBlocks.h" +#include "util/FrameShell.h" +#include "util/IndexThreadReduce.h" +#include "OptimizationBackend/EnergyFunctional.h" +#include "FullSystem/PixelSelector2.h" +#include "IMU/IMUIntegration.hpp" +#include "util/GTData.hpp" + +#include +#include "IMUInitialization/GravityInitializer.h" + +namespace dso +{ +namespace IOWrap +{ +class Output3DWrapper; +} + +class PixelSelector; +class PCSyntheticPoint; +class CoarseTracker; +struct FrameHessian; +struct PointHessian; +class CoarseInitializer; +struct ImmaturePointTemporaryResidual; +class ImageAndExposure; +class CoarseDistanceMap; + +class EnergyFunctional; + +template inline void deleteOut(std::vector &v, const int i) +{ + delete v[i]; + v[i] = v.back(); + v.pop_back(); +} +template inline void deleteOutPt(std::vector &v, const T* i) +{ + delete i; + + for(unsigned int k=0;k inline void deleteOutOrder(std::vector &v, const int i) +{ + delete v[i]; + for(unsigned int k=i+1; k inline void deleteOutOrder(std::vector &v, const T* element) +{ + int i=-1; + for(unsigned int k=0; k outputWrapper; + + bool isLost; + bool initFailed; + bool initialized; + bool linearizeOperation; + + + void setGammaFunction(float* BInv); + void setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH); + +private: + + dmvio::IMUIntegration imuIntegration; + dmvio::BAGTSAMIntegration* baIntegration = nullptr; +public: + dmvio::IMUIntegration &getImuIntegration(); + + Sophus::SE3 firstPose; // contains transform from first to world. + +private: + CalibHessian Hcalib; + + dmvio::GravityInitializer gravityInit; + + double framesBetweenKFsRest = 0.0; + + + + // opt single point + int optimizePoint(PointHessian* point, int minObs, bool flagOOB); + PointHessian* optimizeImmaturePoint(ImmaturePoint* point, int minObs, ImmaturePointTemporaryResidual* residuals); + + double linAllPointSinle(PointHessian* point, float outlierTHSlack, bool plot); + + // mainPipelineFunctions + std::pair trackNewCoarse(FrameHessian* fh, Sophus::SE3 *referenceToFrameHint = 0); + void traceNewCoarse(FrameHessian* fh); + void activatePoints(); + void activatePointsMT(); + void activatePointsOldFirst(); + void flagPointsForRemoval(); + void makeNewTraces(FrameHessian* newFrame, float* gtDepth); + void initializeFromInitializer(FrameHessian* newFrame); + void flagFramesForMarginalization(FrameHessian* newFH); + + + void removeOutliers(); + + + // set precalc values. + void setPrecalcValues(); + + + // solce. eventually migrate to ef. + void solveSystem(int iteration, double lambda); + Vec3 linearizeAll(bool fixLinearization); + bool doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD); + void backupState(bool backupLastStep); + void loadSateBackup(); + double calcLEnergy(); + double calcMEnergy(bool useNewValues); + void linearizeAll_Reductor(bool fixLinearization, std::vector* toRemove, int min, int max, Vec10* stats, int tid); + void activatePointsMT_Reductor(std::vector* optimized,std::vector* toOptimize,int min, int max, Vec10* stats, int tid); + void applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid); + + void printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b); + + void debugPlotTracking(); + + std::vector getNullspaces( + std::vector &nullspaces_pose, + std::vector &nullspaces_scale, + std::vector &nullspaces_affA, + std::vector &nullspaces_affB); + + void setNewFrameEnergyTH(); + + + void printLogLine(); + void printEvalLine(); + void printEigenValLine(); + std::ofstream* calibLog; + std::ofstream* numsLog; + std::ofstream* errorsLog; + std::ofstream* eigenAllLog; + std::ofstream* eigenPLog; + std::ofstream* eigenALog; + std::ofstream* DiagonalLog; + std::ofstream* variancesLog; + std::ofstream* nullspacesLog; + + std::ofstream* coarseTrackingLog; + + // statistics + long int statistics_lastNumOptIts; + long int statistics_numDroppedPoints; + long int statistics_numActivatedPoints; + long int statistics_numCreatedPoints; + long int statistics_numForceDroppedResBwd; + long int statistics_numForceDroppedResFwd; + long int statistics_numMargResFwd; + long int statistics_numMargResBwd; + float statistics_lastFineTrackRMSE; + + + + + + + + // =================== changed by tracker-thread. protected by trackMutex ============ + boost::mutex trackMutex; + std::vector allFrameHistory; + std::vector gtPoses; + CoarseInitializer* coarseInitializer; + Vec5 lastCoarseRMSE; + + + // ================== changed by mapper-thread. protected by mapMutex =============== + boost::mutex mapMutex; + std::vector allKeyFramesHistory; + + EnergyFunctional* ef; + IndexThreadReduce treadReduce; + + float* selectionMap; + PixelSelector* pixelSelector; + CoarseDistanceMap* coarseDistanceMap; + + std::vector frameHessians; // ONLY changed in marginalizeFrame and addFrame. + std::vector activeResiduals; + float currentMinActDist; + + + std::vector allResVec; + + + + // mutex etc. for tracker exchange. + boost::mutex coarseTrackerSwapMutex; // if tracker sees that there is a new reference, tracker locks [coarseTrackerSwapMutex] and swaps the two. + CoarseTracker* coarseTracker_forNewKF; // set as as reference. protected by [coarseTrackerSwapMutex]. + CoarseTracker* coarseTracker; // always used to track new frames. protected by [trackMutex]. + float minIdJetVisTracker, maxIdJetVisTracker; + float minIdJetVisDebug, maxIdJetVisDebug; + + + + + + // mutex for camToWorl's in shells (these are always in a good configuration). + boost::mutex& shellPoseMutex; + + + +/* + * tracking always uses the newest KF as reference. + * + */ + + void makeKeyFrame( FrameHessian* fh); + void makeNonKeyFrame( FrameHessian* fh); + void deliverTrackedFrame(FrameHessian* fh, bool needKF); + void mappingLoop(); + + // tracking / mapping synchronization. All protected by [trackMapSyncMutex]. + boost::mutex trackMapSyncMutex; + boost::condition_variable trackedFrameSignal; + boost::condition_variable mappedFrameSignal; + std::deque unmappedTrackedFrames; + int needNewKFAfter; // Otherwise, a new KF is *needed that has ID bigger than [needNewKFAfter]*. + boost::thread mappingThread; + bool runMapping; + bool needToKetchupMapping; + + int lastRefStopID; + + + bool secondKeyframeDone; +}; +} + diff --git a/src/dso/FullSystem/FullSystemDebugStuff.cpp b/src/dso/FullSystem/FullSystemDebugStuff.cpp new file mode 100644 index 0000000..1760eb2 --- /dev/null +++ b/src/dso/FullSystem/FullSystemDebugStuff.cpp @@ -0,0 +1,369 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include "FullSystem/FullSystem.h" + +#include "stdio.h" +#include "util/globalFuncs.h" +#include +#include +#include "IOWrapper/ImageDisplay.h" +#include "IOWrapper/ImageRW.h" +#include "util/globalCalib.h" +#include +#include +#include + +#include "FullSystem/ImmaturePoint.h" +#include "util/TimeMeasurement.h" + + +namespace dso +{ + + + void FullSystem::debugPlotTracking() + { + if(disableAllDisplay) return; + if(!setting_render_plotTrackingFull) return; + int wh = hG[0]*wG[0]; + + int idx=0; + for(FrameHessian* f : frameHessians) + { + std::vector images; + + // make images for all frames. will be deleted by the FrameHessian's destructor. + for(FrameHessian* f2 : frameHessians) + if(f2->debugImage == 0) f2->debugImage = new MinimalImageB3(wG[0], hG[0]); + + for(FrameHessian* f2 : frameHessians) + { + MinimalImageB3* debugImage=f2->debugImage; + images.push_back(debugImage); + + Eigen::Vector3f* fd = f2->dI; + + Vec2 affL = AffLight::fromToVecExposure(f2->ab_exposure, f->ab_exposure, f2->aff_g2l(), f->aff_g2l()); + + for(int i=0;i255) colL =255; + debugImage->at(i) = Vec3b(colL, colL, colL); + } + } + + + for(PointHessian* ph : f->pointHessians) + { + assert(ph->status == PointHessian::ACTIVE); + if(ph->status == PointHessian::ACTIVE || ph->status == PointHessian::MARGINALIZED) + { + for(PointFrameResidual* r : ph->residuals) + r->debugPlot(); + f->debugImage->setPixel9(ph->u+0.5, ph->v+0.5, makeRainbow3B(ph->idepth_scaled)); + } + } + + + char buf[100]; + snprintf(buf, 100, "IMG %d", idx); + IOWrap::displayImageStitch(buf, images); + idx++; + } + + IOWrap::waitKey(0); + + } + + + void FullSystem::debugPlot(std::string name) + { + dmvio::TimeMeasurement timeMeasurement("debugPlot"); + if(disableAllDisplay) return; + if(!setting_render_renderWindowFrames) return; + std::vector images; + + + + + float minID=0, maxID=0; + if((int)(freeDebugParam5+0.5f) == 7 || (debugSaveImages&&false)) + { + std::vector allID; + for(unsigned int f=0;fpointHessians) + if(ph!=0) allID.push_back(ph->idepth_scaled); + + for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized) + if(ph!=0) allID.push_back(ph->idepth_scaled); + + for(PointHessian* ph : frameHessians[f]->pointHessiansOut) + if(ph!=0) allID.push_back(ph->idepth_scaled); + } + std::sort(allID.begin(), allID.end()); + int n = allID.size()-1; + minID = allID[(int)(n*0.05)]; + maxID = allID[(int)(n*0.95)]; + + + // slowly adapt: change by maximum 10% of old span. + float maxChange = 0.1*(maxIdJetVisDebug - minIdJetVisDebug); + if(maxIdJetVisDebug < 0 || minIdJetVisDebug < 0 ) maxChange = 1e5; + + + if(minID < minIdJetVisDebug - maxChange) + minID = minIdJetVisDebug - maxChange; + if(minID > minIdJetVisDebug + maxChange) + minID = minIdJetVisDebug + maxChange; + + + if(maxID < maxIdJetVisDebug - maxChange) + maxID = maxIdJetVisDebug - maxChange; + if(maxID > maxIdJetVisDebug + maxChange) + maxID = maxIdJetVisDebug + maxChange; + + maxIdJetVisDebug = maxID; + minIdJetVisDebug = minID; + + } + + + + + + + + + + + + + int wh = hG[0]*wG[0]; + for(unsigned int f=0;fI; + Eigen::Vector3f* fd = frameHessians[f]->dI; + + + for(int i=0;i255) c=255; + img->at(i) = Vec3b(c,c,c); + } + + if((int)(freeDebugParam5+0.5f) == 0) + { + for(PointHessian* ph : frameHessians[f]->pointHessians) + { + if(ph==0) continue; + + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B(ph->idepth_scaled)); + } + for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized) + { + if(ph==0) continue; + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B(ph->idepth_scaled)); + } + for(PointHessian* ph : frameHessians[f]->pointHessiansOut) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,255)); + } + else if((int)(freeDebugParam5+0.5f) == 1) + { + for(PointHessian* ph : frameHessians[f]->pointHessians) + { + if(ph==0) continue; + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B(ph->idepth_scaled)); + } + + for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0)); + + for(PointHessian* ph : frameHessians[f]->pointHessiansOut) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,255)); + } + else if((int)(freeDebugParam5+0.5f) == 2) + { + + } + else if((int)(freeDebugParam5+0.5f) == 3) + { + for(ImmaturePoint* ph : frameHessians[f]->immaturePoints) + { + if(ph==0) continue; + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD || + ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED || + ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION) + { + if(!std::isfinite(ph->idepth_max)) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0)); + else + { + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B((ph->idepth_min + ph->idepth_max)*0.5f)); + } + } + } + } + else if((int)(freeDebugParam5+0.5f) == 4) + { + for(ImmaturePoint* ph : frameHessians[f]->immaturePoints) + { + if(ph==0) continue; + + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,255,0)); + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OOB) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,0)); + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OUTLIER) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,255)); + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,0)); + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,255)); + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0)); + } + } + else if((int)(freeDebugParam5+0.5f) == 5) + { + for(ImmaturePoint* ph : frameHessians[f]->immaturePoints) + { + if(ph==0) continue; + + if(ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED) continue; + float d = freeDebugParam1 * (sqrtf(ph->quality)-1); + if(d<0) d=0; + if(d>1) d=1; + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,d*255,(1-d)*255)); + } + + } + else if((int)(freeDebugParam5+0.5f) == 6) + { + for(PointHessian* ph : frameHessians[f]->pointHessians) + { + if(ph==0) continue; + if(ph->my_type==0) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,255)); + if(ph->my_type==1) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,0)); + if(ph->my_type==2) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,255)); + if(ph->my_type==3) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,255,255)); + } + for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized) + { + if(ph==0) continue; + if(ph->my_type==0) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,255)); + if(ph->my_type==1) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,0)); + if(ph->my_type==2) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,255)); + if(ph->my_type==3) + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,255,255)); + } + + } + if((int)(freeDebugParam5+0.5f) == 7) + { + for(PointHessian* ph : frameHessians[f]->pointHessians) + { + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeJet3B((ph->idepth_scaled-minID) / ((maxID-minID)))); + } + for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized) + { + if(ph==0) continue; + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0)); + } + } + } + IOWrap::displayImageStitch(name.c_str(), images); + IOWrap::waitKey(5); + + for(unsigned int i=0;idI; + + for(int i=0;i255) c=255; + img->at(i) = Vec3b(c,c,c); + } + + for(PointHessian* ph : frameHessians[f]->pointHessians) + { + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeJet3B((ph->idepth_scaled-minID) / ((maxID-minID)))); + } + for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized) + { + if(ph==0) continue; + img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0)); + } + + char buf[1000]; + snprintf(buf, 1000, "images_out/kf_%05d_%05d_%02d.png", + frameHessians.back()->shell->id, frameHessians.back()->frameID, f); + IOWrap::writeImage(buf,img); + + delete img; + } + } + + + + + } + + + + + + +} diff --git a/src/dso/FullSystem/FullSystemMarginalize.cpp b/src/dso/FullSystem/FullSystemMarginalize.cpp new file mode 100644 index 0000000..8db04d0 --- /dev/null +++ b/src/dso/FullSystem/FullSystemMarginalize.cpp @@ -0,0 +1,244 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include +#include "FullSystem/FullSystem.h" + +#include "stdio.h" +#include "util/globalFuncs.h" +#include +#include +#include "IOWrapper/ImageDisplay.h" +#include "util/globalCalib.h" + +#include +#include +#include "FullSystem/ResidualProjections.h" +#include "FullSystem/ImmaturePoint.h" + +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +#include "IOWrapper/Output3DWrapper.h" + +#include "FullSystem/CoarseTracker.h" + +namespace dso +{ + + + +void FullSystem::flagFramesForMarginalization(FrameHessian* newFH) +{ + dmvio::TimeMeasurement timeMeasurement("flagFramesForMarginalization"); + if(setting_minFrameAge > setting_maxFrames) + { + for(int i=setting_maxFrames;i<(int)frameHessians.size();i++) + { + FrameHessian* fh = frameHessians[i-setting_maxFrames]; + fh->flaggedForMarginalization = true; + } + return; + } + + + int flagged = 0; + // marginalize all frames that have not enough points. + for(int i=0;i<(int)frameHessians.size();i++) + { + FrameHessian* fh = frameHessians[i]; + int in = fh->pointHessians.size() + fh->immaturePoints.size(); + int out = fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size(); + + + Vec2 refToFh=AffLight::fromToVecExposure(frameHessians.back()->ab_exposure, fh->ab_exposure, + frameHessians.back()->aff_g2l(), fh->aff_g2l()); + + + if( (in < setting_minPointsRemaining *(in+out) || fabs(logf((float)refToFh[0])) > setting_maxLogAffFacInWindow) + && ((int)frameHessians.size())-flagged > setting_minFrames) + { +// printf("MARGINALIZE frame %d, as only %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n", +// fh->frameID, in, in+out, +// (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), +// (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(), +// visInLast, outInLast, +// fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame); + fh->flaggedForMarginalization = true; + flagged++; + } + else + { +// printf("May Keep frame %d, as %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n", +// fh->frameID, in, in+out, +// (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), +// (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(), +// visInLast, outInLast, +// fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame); + } + } + + // marginalize one. + if((int)frameHessians.size()-flagged >= setting_maxFrames) + { + double smallestScore = 1; + FrameHessian* toMarginalize=0; + FrameHessian* latest = frameHessians.back(); + + + for(FrameHessian* fh : frameHessians) + { + if(fh->frameID > latest->frameID-setting_minFrameAge || fh->frameID == 0) continue; + //if(fh==frameHessians.front() == 0) continue; + + double distScore = 0; + for(FrameFramePrecalc &ffh : fh->targetPrecalc) + { + if(ffh.target->frameID > latest->frameID-setting_minFrameAge+1 || ffh.target == ffh.host) continue; + distScore += 1/(1e-5+ffh.distanceLL); + + } + distScore *= -sqrtf(fh->targetPrecalc.back().distanceLL); + + + if(distScore < smallestScore) + { + smallestScore = distScore; + toMarginalize = fh; + } + } + +// printf("MARGINALIZE frame %d, as it is the closest (score %.2f)!\n", +// toMarginalize->frameID, smallestScore); + toMarginalize->flaggedForMarginalization = true; + flagged++; + } + +// printf("FRAMES LEFT: "); +// for(FrameHessian* fh : frameHessians) +// printf("%d ", fh->frameID); +// printf("\n"); +} + + + + +void FullSystem::marginalizeFrame(FrameHessian* frame) +{ + dmvio::TimeMeasurement timeMeasurement("marginalizeFrame"); + // marginalize or remove all this frames points. + + assert((int)frame->pointHessians.size()==0); + + + ef->marginalizeFrame(frame->efFrame); + + dmvio::TimeMeasurement timeMeasurementEnd("marginalizeFrameOverhead"); + + // drop all observations of existing points in that frame. + + for(FrameHessian* fh : frameHessians) + { + if(fh==frame) continue; + + for(PointHessian* ph : fh->pointHessians) + { + for(unsigned int i=0;iresiduals.size();i++) + { + PointFrameResidual* r = ph->residuals[i]; + if(r->target == frame) + { + if(ph->lastResiduals[0].first == r) + ph->lastResiduals[0].first=0; + else if(ph->lastResiduals[1].first == r) + ph->lastResiduals[1].first=0; + + + if(r->host->frameID < r->target->frameID) + statistics_numForceDroppedResFwd++; + else + statistics_numForceDroppedResBwd++; + + ef->dropResidual(r->efResidual); + deleteOut(ph->residuals,i); + break; + } + } + } + } + + // ef->marginalizeFrame does not delete efFrame anymore, because it's field frameID was needed in dropResidual. + delete frame->efFrame; + frame->efFrame = nullptr; + + { + std::vector v; + v.push_back(frame); + for(IOWrap::Output3DWrapper* ow : outputWrapper) + ow->publishKeyframes(v, true, &Hcalib); + } + + + frame->shell->marginalizedAt = frameHessians.back()->shell->id; + frame->shell->movedByOpt = frame->w2c_leftEps().norm(); + + auto frameID = frame->frameID; + + deleteOutOrder(frameHessians, frame); + for(unsigned int i=0;iidx = i; + + + int numDel = 0; + for(auto it = ef->connectivityMap.begin(); it != ef->connectivityMap.end();) + { + int host = (int)(it->first >> 32); + int target = (int)(it->first & (uint64_t)0xFFFFFFFF); + if(host == frameID || target == frameID) + { + numDel++; + it = ef->connectivityMap.erase(it); + }else + { + it++; + } + } + + setPrecalcValues(); + ef->setAdjointsF(&Hcalib); +} + + + + +} diff --git a/src/dso/FullSystem/FullSystemOptPoint.cpp b/src/dso/FullSystem/FullSystemOptPoint.cpp new file mode 100644 index 0000000..7588221 --- /dev/null +++ b/src/dso/FullSystem/FullSystemOptPoint.cpp @@ -0,0 +1,209 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include "FullSystem/FullSystem.h" + +#include "stdio.h" +#include "util/globalFuncs.h" +#include +#include +#include "IOWrapper/ImageDisplay.h" +#include "util/globalCalib.h" + +#include +#include +#include "FullSystem/ImmaturePoint.h" +#include "math.h" + +namespace dso +{ + + + +PointHessian* FullSystem::optimizeImmaturePoint( + ImmaturePoint* point, int minObs, + ImmaturePointTemporaryResidual* residuals) +{ + int nres = 0; + for(FrameHessian* fh : frameHessians) + { + if(fh != point->host) + { + residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0; + residuals[nres].state_NewState = ResState::OUTLIER; + residuals[nres].state_state = ResState::IN; + residuals[nres].target = fh; + nres++; + } + } + assert(nres == ((int)frameHessians.size())-1); + + bool print = false;//rand()%50==0; + + float lastEnergy = 0; + float lastHdd=0; + float lastbd=0; + float currentIdepth=(point->idepth_max+point->idepth_min)*0.5f; + + + + + + + for(int i=0;ilinearizeResidual(&Hcalib, 1000, residuals+i,lastHdd, lastbd, currentIdepth); + residuals[i].state_state = residuals[i].state_NewState; + residuals[i].state_energy = residuals[i].state_NewEnergy; + } + + if(!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act) + { + if(print) + printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", + nres, lastHdd, lastEnergy); + return 0; + } + + if(print) printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" , + nres, lastHdd,lastEnergy,currentIdepth); + + float lambda = 0.1; + for(int iteration=0;iterationlinearizeResidual(&Hcalib, 1, residuals+i,newHdd, newbd, newIdepth); + + if(!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act) + { + if(print) printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", + nres, + newHdd, + lastEnergy); + return 0; + } + + if(print) printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n", + (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT", + iteration, + log10(lambda), + "", + lastEnergy, newEnergy, newIdepth); + + if(newEnergy < lastEnergy) + { + currentIdepth = newIdepth; + lastHdd = newHdd; + lastbd = newbd; + lastEnergy = newEnergy; + for(int i=0;ienergyTH)) {delete p; return (PointHessian*)((long)(-1));} + + p->lastResiduals[0].first = 0; + p->lastResiduals[0].second = ResState::OOB; + p->lastResiduals[1].first = 0; + p->lastResiduals[1].second = ResState::OOB; + p->setIdepthZero(currentIdepth); + p->setIdepth(currentIdepth); + p->setPointStatus(PointHessian::ACTIVE); + + for(int i=0;ihost, residuals[i].target); + r->state_NewEnergy = r->state_energy = 0; + r->state_NewState = ResState::OUTLIER; + r->setState(ResState::IN); + p->residuals.push_back(r); + + if(r->target == frameHessians.back()) + { + p->lastResiduals[0].first = r; + p->lastResiduals[0].second = ResState::IN; + } + else if(r->target == (frameHessians.size()<2 ? 0 : frameHessians[frameHessians.size()-2])) + { + p->lastResiduals[1].first = r; + p->lastResiduals[1].second = ResState::IN; + } + } + + if(print) printf("point activated!\n"); + + statistics_numActivatedPoints++; + return p; +} + + + +} diff --git a/src/dso/FullSystem/FullSystemOptimize.cpp b/src/dso/FullSystem/FullSystemOptimize.cpp new file mode 100644 index 0000000..22faa5b --- /dev/null +++ b/src/dso/FullSystem/FullSystemOptimize.cpp @@ -0,0 +1,758 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "FullSystem/FullSystem.h" + +#include "stdio.h" +#include "util/globalFuncs.h" +#include +#include +#include "IOWrapper/ImageDisplay.h" +#include "util/globalCalib.h" +#include +#include +#include "FullSystem/ResidualProjections.h" + +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" +#include "util/TimeMeasurement.h" + +#include + +#include + +namespace dso +{ + + + + + +void FullSystem::linearizeAll_Reductor(bool fixLinearization, std::vector* toRemove, int min, int max, Vec10* stats, int tid) +{ + for(int k=min;klinearize(&Hcalib); + + if(fixLinearization) + { + r->applyRes(true); + + if(r->efResidual->isActive()) + { + if(r->isNew) + { + PointHessian* p = r->point; + Vec3f ptp_inf = r->host->targetPrecalc[r->target->idx].PRE_KRKiTll * Vec3f(p->u,p->v, 1); // projected point assuming infinite depth. + Vec3f ptp = ptp_inf + r->host->targetPrecalc[r->target->idx].PRE_KtTll*p->idepth_scaled; // projected point with real depth. + float relBS = 0.01*((ptp_inf.head<2>() / ptp_inf[2])-(ptp.head<2>() / ptp[2])).norm(); // 0.01 = one pixel. + + + if(relBS > p->maxRelBaseline) + p->maxRelBaseline = relBS; + + p->numGoodResiduals++; + } + } + else + { + toRemove[tid].push_back(activeResiduals[k]); + } + } + } +} + + +void FullSystem::applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid) +{ + for(int k=min;kapplyRes(true); +} +void FullSystem::setNewFrameEnergyTH() +{ + + // collect all residuals and make decision on TH. + allResVec.clear(); + allResVec.reserve(activeResiduals.size()*2); + FrameHessian* newFrame = frameHessians.back(); + + for(PointFrameResidual* r : activeResiduals) + if(r->state_NewEnergyWithOutlier >= 0 && r->target == newFrame) + { + allResVec.push_back(r->state_NewEnergyWithOutlier); + + } + + if(allResVec.size()==0) + { + newFrame->frameEnergyTH = 12*12*patternNum; + return; // should never happen, but lets make sure. + } + + + int nthIdx = setting_frameEnergyTHN*allResVec.size(); + + assert(nthIdx < (int)allResVec.size()); + assert(setting_frameEnergyTHN < 1); + + std::nth_element(allResVec.begin(), allResVec.begin()+nthIdx, allResVec.end()); + float nthElement = sqrtf(allResVec[nthIdx]); + + + + + + + newFrame->frameEnergyTH = nthElement*setting_frameEnergyTHFacMedian; + newFrame->frameEnergyTH = 26.0f*setting_frameEnergyTHConstWeight + newFrame->frameEnergyTH*(1-setting_frameEnergyTHConstWeight); + newFrame->frameEnergyTH = newFrame->frameEnergyTH*newFrame->frameEnergyTH; + newFrame->frameEnergyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight; + + if(setting_useIMU) + { + // Used to enforce a maximum energy threshold. + imuIntegration.newFrameEnergyTH(newFrame->frameEnergyTH); + } + + +// +// int good=0,bad=0; +// for(float f : allResVec) if(fframeEnergyTH) good++; else bad++; +// printf("EnergyTH: mean %f, median %f, result %f (in %d, out %d)! \n", +// meanElement, nthElement, sqrtf(newFrame->frameEnergyTH), +// good, bad); +} +Vec3 FullSystem::linearizeAll(bool fixLinearization) +{ + double lastEnergyP = 0; + double lastEnergyR = 0; + double num = 0; + + + std::vector toRemove[NUM_THREADS]; + for(int i=0;ipoint; + if(ph->lastResiduals[0].first == r) + ph->lastResiduals[0].second = r->state_state; + else if(ph->lastResiduals[1].first == r) + ph->lastResiduals[1].second = r->state_state; + + + + } + + int nResRemoved=0; + for(int i=0;ipoint; + + if(ph->lastResiduals[0].first == r) + ph->lastResiduals[0].first=0; + else if(ph->lastResiduals[1].first == r) + ph->lastResiduals[1].first=0; + + for(unsigned int k=0; kresiduals.size();k++) + if(ph->residuals[k] == r) + { + ef->dropResidual(r->efResidual); + deleteOut(ph->residuals,k); + nResRemoved++; + break; + } + } + } + //printf("FINAL LINEARIZATION: removed %d / %d residuals!\n", nResRemoved, (int)activeResiduals.size()); + + } + + return Vec3(lastEnergyP, lastEnergyR, num); +} + + + + +// applies step to linearization point. +bool FullSystem::doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD) +{ +// float meanStepC=0,meanStepP=0,meanStepD=0; +// meanStepC += Hcalib.step.norm(); + + Vec10 pstepfac; + pstepfac.segment<3>(0).setConstant(stepfacT); + pstepfac.segment<3>(3).setConstant(stepfacR); + pstepfac.segment<4>(6).setConstant(stepfacA); + + + float sumA=0, sumB=0, sumT=0, sumR=0, sumID=0, numID=0; + + float sumNID=0; + + if(setting_solverMode & SOLVER_MOMENTUM) + { + Hcalib.setValue(Hcalib.value_backup + Hcalib.step); + for(FrameHessian* fh : frameHessians) + { + Vec10 step = fh->step; + step.head<6>() += 0.5f*(fh->step_backup.head<6>()); + + fh->setState(fh->state_backup + step); + sumA += step[6]*step[6]; + sumB += step[7]*step[7]; + sumT += step.segment<3>(0).squaredNorm(); + sumR += step.segment<3>(3).squaredNorm(); + + for(PointHessian* ph : fh->pointHessians) + { + float step = ph->step+0.5f*(ph->step_backup); + ph->setIdepth(ph->idepth_backup + step); + sumID += step*step; + sumNID += fabsf(ph->idepth_backup); + numID++; + + ph->setIdepthZero(ph->idepth_backup + step); + } + } + } + else + { + Hcalib.setValue(Hcalib.value_backup + stepfacC*Hcalib.step); + for(FrameHessian* fh : frameHessians) + { + fh->setState(fh->state_backup + pstepfac.cwiseProduct(fh->step)); + sumA += fh->step[6]*fh->step[6]; + sumB += fh->step[7]*fh->step[7]; + sumT += fh->step.segment<3>(0).squaredNorm(); + sumR += fh->step.segment<3>(3).squaredNorm(); + + for(PointHessian* ph : fh->pointHessians) + { + ph->setIdepth(ph->idepth_backup + stepfacD*ph->step); + sumID += ph->step*ph->step; + sumNID += fabsf(ph->idepth_backup); + numID++; + + ph->setIdepthZero(ph->idepth_backup + stepfacD*ph->step); + } + } + } + + sumA /= frameHessians.size(); + sumB /= frameHessians.size(); + sumR /= frameHessians.size(); + sumT /= frameHessians.size(); + sumID /= numID; + sumNID /= numID; + + + + if(!setting_debugout_runquiet) + printf("STEPS: A %.1f; B %.1f; R %.1f; T %.1f. \t", + sqrtf(sumA) / (0.0005*setting_thOptIterations), + sqrtf(sumB) / (0.00005*setting_thOptIterations), + sqrtf(sumR) / (0.00005*setting_thOptIterations), + sqrtf(sumT)*sumNID / (0.00005*setting_thOptIterations)); + + + EFDeltaValid=false; + setPrecalcValues(); + + + + return sqrtf(sumA) < 0.0005*setting_thOptIterations && + sqrtf(sumB) < 0.00005*setting_thOptIterations && + sqrtf(sumR) < 0.00005*setting_thOptIterations && + sqrtf(sumT)*sumNID < 0.00005*setting_thOptIterations; +// +// printf("mean steps: %f %f %f!\n", +// meanStepC, meanStepP, meanStepD); +} + + + +// sets linearization point. +void FullSystem::backupState(bool backupLastStep) +{ + if(setting_solverMode & SOLVER_MOMENTUM) + { + if(backupLastStep) + { + Hcalib.step_backup = Hcalib.step; + Hcalib.value_backup = Hcalib.value; + for(FrameHessian* fh : frameHessians) + { + fh->step_backup = fh->step; + fh->state_backup = fh->get_state(); + for(PointHessian* ph : fh->pointHessians) + { + ph->idepth_backup = ph->idepth; + ph->step_backup = ph->step; + } + } + } + else + { + Hcalib.step_backup.setZero(); + Hcalib.value_backup = Hcalib.value; + for(FrameHessian* fh : frameHessians) + { + fh->step_backup.setZero(); + fh->state_backup = fh->get_state(); + for(PointHessian* ph : fh->pointHessians) + { + ph->idepth_backup = ph->idepth; + ph->step_backup=0; + } + } + } + } + else + { + Hcalib.value_backup = Hcalib.value; + for(FrameHessian* fh : frameHessians) + { + fh->state_backup = fh->get_state(); + for(PointHessian* ph : fh->pointHessians) + ph->idepth_backup = ph->idepth; + } + } +} + +// sets linearization point. +void FullSystem::loadSateBackup() +{ + Hcalib.setValue(Hcalib.value_backup); + for(FrameHessian* fh : frameHessians) + { + fh->setState(fh->state_backup); + for(PointHessian* ph : fh->pointHessians) + { + ph->setIdepth(ph->idepth_backup); + + ph->setIdepthZero(ph->idepth_backup); + } + + } + + + EFDeltaValid=false; + setPrecalcValues(); +} + + +double FullSystem::calcMEnergy(bool useNewValues) +{ + if(setting_forceAceptStep) return 0; + // calculate (x-x0)^T * [2b + H * (x-x0)] for everything saved in L. + //ef->makeIDX(); + //ef->setDeltaF(&Hcalib); + + return ef->calcMEnergyF(useNewValues); + +} + + +void FullSystem::printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b) +{ + printf("A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n", + res[0], + sqrtf((float)(res[0] / (patternNum*ef->resInA))), + ef->resInA, + ef->resInM, + a, + b + ); + +} + + +float FullSystem::optimize(int mnumOptIts) +{ + dmvio::TimeMeasurement timeMeasurement("FullSystemOptimize"); + if(frameHessians.size() < 2) return 0; + if(frameHessians.size() < 3) mnumOptIts = 20; + if(frameHessians.size() < 4) mnumOptIts = 15; + + + + + + + // get statistics and active residuals. + + activeResiduals.clear(); + int numPoints = 0; + int numLRes = 0; + for(FrameHessian* fh : frameHessians) + for(PointHessian* ph : fh->pointHessians) + { + for(PointFrameResidual* r : ph->residuals) + { + if(!r->efResidual->isLinearized) + { + activeResiduals.push_back(r); + r->resetOOB(); + } + else + numLRes++; + } + numPoints++; + } + + if(!setting_debugout_runquiet) + printf("OPTIMIZE %d pts, %d active res, %d lin res!\n",ef->nPoints,(int)activeResiduals.size(), numLRes); + + + Vec3 lastEnergy = linearizeAll(false); + double lastEnergyL = calcLEnergy(); + double lastEnergyM = calcMEnergy(false); + + + + + + if(multiThreading) + treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50); + else + applyRes_Reductor(true,0,activeResiduals.size(),0,0); + + + if(!setting_debugout_runquiet) + { + printf("Initial Error \t"); + printOptRes(lastEnergy, lastEnergyL, lastEnergyM, 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b); + } + + debugPlotTracking(); + + double dynamicGTSAMWeight = 1.0; + + double minLambda = 1e-5; + +// double lambda = 1e-1; + double lambda = minLambda; + float stepsize=1; + VecX previousX = VecX::Constant(CPARS+ 8*frameHessians.size(), NAN); + int numIterations = 0; + for(int iteration=0;iterationupdateDynamicWeight(lastEnergy[0], sqrtf((float) (lastEnergy[0] / + (patternNum * + ef->resInA))), + frameHessians.back()->shell->trackingWasGood); + std::cout << "Dynamic weight: " << dynamicGTSAMWeight << std::endl; + } + + solveSystem(iteration, lambda); + double incDirChange = (1e-20 + previousX.dot(ef->lastX)) / (1e-20 + previousX.norm() * ef->lastX.norm()); + previousX = ef->lastX; + + + if(std::isfinite(incDirChange) && (setting_solverMode & SOLVER_STEPMOMENTUM)) + { + float newStepsize = exp(incDirChange*1.4); + if(incDirChange<0 && stepsize>1) stepsize=1; + + stepsize = sqrtf(sqrtf(newStepsize*stepsize*stepsize*stepsize)); + if(stepsize > 2) stepsize=2; + if(stepsize <0.25) stepsize=0.25; + } + + bool canbreak = doStepFromBackup(stepsize,stepsize,stepsize,stepsize,stepsize); + + + canbreak = canbreak && baIntegration->canBreak(); + + + + + // eval new energy! + Vec3 newEnergy = linearizeAll(false); + double newEnergyL = calcLEnergy(); + double newEnergyM = calcMEnergy(true); + + + if(imuIntegration.getImuSettings().updateDynamicWeightDuringOptimization) + { + // Update dynamic weight before deciding whether to accept the step. + dynamicGTSAMWeight = baIntegration->updateDynamicWeight(lastEnergy[0], sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))), frameHessians.back()->shell->trackingWasGood); + } + + + if(!setting_debugout_runquiet) + { + printf("%s %d (L %.2f, dir %.2f, ss %.1f): \t", + (newEnergy[0] + newEnergy[1] + newEnergyL + newEnergyM / dynamicGTSAMWeight < + lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM / dynamicGTSAMWeight) ? "ACCEPT" : "REJECT", + iteration, + log10(lambda), + incDirChange, + stepsize); + printOptRes(newEnergy, newEnergyL, newEnergyM , 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b); + } + + if(setting_forceAceptStep || (newEnergy[0] + newEnergy[1] + newEnergyL + newEnergyM / dynamicGTSAMWeight < + lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM / dynamicGTSAMWeight)) + { + + if(multiThreading) + treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50); + else + applyRes_Reductor(true,0,activeResiduals.size(),0,0); + + lastEnergy = newEnergy; + lastEnergyL = newEnergyL; + lastEnergyM = newEnergyM; + + lambda *= 0.25; + lambda = std::max(lambda, minLambda); + + if(setting_useGTSAMIntegration) + { + baIntegration->acceptBAUpdate(lastEnergy[0]); + } + } + else + { + loadSateBackup(); + lastEnergy = linearizeAll(false); + lastEnergyL = calcLEnergy(); + lastEnergyM = calcMEnergy(false); + lambda *= 1e2; + } + numIterations++; + + + if(canbreak && iteration >= setting_minOptIterations) break; + } + + std::cout << "Num BA Iterations done: " << numIterations << "\n"; + + // Update again! + baIntegration->updateDynamicWeight(lastEnergy[0], sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))), frameHessians.back()->shell->trackingWasGood); + + Vec10 newStateZero = Vec10::Zero(); + newStateZero.segment<2>(6) = frameHessians.back()->get_state().segment<2>(6); + + frameHessians.back()->setEvalPT(frameHessians.back()->PRE_worldToCam, + newStateZero); + EFDeltaValid=false; + EFAdjointsValid=false; + ef->setAdjointsF(&Hcalib); + setPrecalcValues(); + + + + + lastEnergy = linearizeAll(true); + + + + if(!std::isfinite((double)lastEnergy[0]) || !std::isfinite((double)lastEnergy[1]) || !std::isfinite((double)lastEnergy[2])) + { + std::cout << "Tracking lost after bundle adjustment!" << std::endl; + isLost = true; + } + + + statistics_lastFineTrackRMSE = sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))); + + if(calibLog != 0) + { + (*calibLog) << Hcalib.value_scaled.transpose() << + " " << frameHessians.back()->get_state_scaled().transpose() << + " " << sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))) << + " " << ef->resInM << "\n"; + calibLog->flush(); + } + + { + boost::unique_lock crlock(shellPoseMutex); + for(FrameHessian* fh : frameHessians) + { + fh->shell->camToWorld = fh->PRE_camToWorld; + fh->shell->aff_g2l = fh->aff_g2l(); + } + } + + + baIntegration->postOptimization(ef->frames); + + debugPlotTracking(); + + return statistics_lastFineTrackRMSE; + +} + + + + + +void FullSystem::solveSystem(int iteration, double lambda) +{ + ef->lastNullspaces_forLogging = getNullspaces( + ef->lastNullspaces_pose, + ef->lastNullspaces_scale, + ef->lastNullspaces_affA, + ef->lastNullspaces_affB); + + ef->solveSystemF(iteration, lambda,&Hcalib); +} + + + +double FullSystem::calcLEnergy() +{ + if(setting_forceAceptStep) return 0; + + double Ef = ef->calcLEnergyF_MT(); + return Ef; + +} + + +void FullSystem::removeOutliers() +{ + dmvio::TimeMeasurement timeMeasurement("removeOutliers"); + int numPointsDropped=0; + for(FrameHessian* fh : frameHessians) + { + for(unsigned int i=0;ipointHessians.size();i++) + { + PointHessian* ph = fh->pointHessians[i]; + if(ph==0) continue; + + if(ph->residuals.size() == 0) + { + fh->pointHessiansOut.push_back(ph); + ph->efPoint->stateFlag = EFPointStatus::PS_DROP; + fh->pointHessians[i] = fh->pointHessians.back(); + fh->pointHessians.pop_back(); + i--; + numPointsDropped++; + } + } + } + ef->dropPointsF(); +} + + + + +std::vector FullSystem::getNullspaces( + std::vector &nullspaces_pose, + std::vector &nullspaces_scale, + std::vector &nullspaces_affA, + std::vector &nullspaces_affB) +{ + nullspaces_pose.clear(); + nullspaces_scale.clear(); + nullspaces_affA.clear(); + nullspaces_affB.clear(); + + + int n=CPARS+frameHessians.size()*8; + std::vector nullspaces_x0_pre; + for(int i=0;i<6;i++) + { + VecX nullspace_x0(n); + nullspace_x0.setZero(); + for(FrameHessian* fh : frameHessians) + { + nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_pose.col(i); + nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE; + nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE; + } + nullspaces_x0_pre.push_back(nullspace_x0); + nullspaces_pose.push_back(nullspace_x0); + } + for(int i=0;i<2;i++) + { + VecX nullspace_x0(n); + nullspace_x0.setZero(); + for(FrameHessian* fh : frameHessians) + { + nullspace_x0.segment<2>(CPARS+fh->idx*8+6) = fh->nullspaces_affine.col(i).head<2>(); + nullspace_x0[CPARS+fh->idx*8+6] *= SCALE_A_INVERSE; + nullspace_x0[CPARS+fh->idx*8+7] *= SCALE_B_INVERSE; + } + nullspaces_x0_pre.push_back(nullspace_x0); + if(i==0) nullspaces_affA.push_back(nullspace_x0); + if(i==1) nullspaces_affB.push_back(nullspace_x0); + } + + VecX nullspace_x0(n); + nullspace_x0.setZero(); + for(FrameHessian* fh : frameHessians) + { + nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_scale; + nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE; + nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE; + } + nullspaces_x0_pre.push_back(nullspace_x0); + nullspaces_scale.push_back(nullspace_x0); + + return nullspaces_x0_pre; +} + + dmvio::IMUIntegration &FullSystem::getImuIntegration() { + return imuIntegration; + } + +} diff --git a/src/dso/FullSystem/HessianBlocks.cpp b/src/dso/FullSystem/HessianBlocks.cpp new file mode 100644 index 0000000..ef801b7 --- /dev/null +++ b/src/dso/FullSystem/HessianBlocks.cpp @@ -0,0 +1,226 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "FullSystem/HessianBlocks.h" +#include "util/FrameShell.h" +#include "FullSystem/ImmaturePoint.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +namespace dso +{ + + +PointHessian::PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib) +{ + instanceCounter++; + host = rawPoint->host; + hasDepthPrior=false; + + idepth_hessian=0; + maxRelBaseline=0; + numGoodResiduals=0; + + // set static values & initialization. + u = rawPoint->u; + v = rawPoint->v; + assert(std::isfinite(rawPoint->idepth_max)); + //idepth_init = rawPoint->idepth_GT; + + my_type = rawPoint->my_type; + + setIdepthScaled((rawPoint->idepth_max + rawPoint->idepth_min)*0.5); + setPointStatus(PointHessian::INACTIVE); + + int n = patternNum; + memcpy(color, rawPoint->color, sizeof(float)*n); + memcpy(weights, rawPoint->weights, sizeof(float)*n); + energyTH = rawPoint->energyTH; + + efPoint=0; + + +} + + +void PointHessian::release() +{ + for(unsigned int i=0;i().squaredNorm() < 1e-20); + + this->state_zero = state_zero; + + + for(int i=0;i<6;i++) + { + Vec6 eps; eps.setZero(); eps[i] = 1e-3; + SE3 EepsP = Sophus::SE3::exp(eps); + SE3 EepsM = Sophus::SE3::exp(-eps); + SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse(); + SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse(); + nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); + } + //nullspaces_pose.topRows<3>() *= SCALE_XI_TRANS_INVERSE; + //nullspaces_pose.bottomRows<3>() *= SCALE_XI_ROT_INVERSE; + + // scale change + SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT()); + w2c_leftEps_P_x0.translation() *= 1.00001; + w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse(); + SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT()); + w2c_leftEps_M_x0.translation() /= 1.00001; + w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse(); + nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); + + + nullspaces_affine.setZero(); + nullspaces_affine.topLeftCorner<2,1>() = Vec2(1,0); + assert(ab_exposure > 0); + nullspaces_affine.topRightCorner<2,1>() = Vec2(0, expf(aff_g2l_0().a)*ab_exposure); +}; + + + +void FrameHessian::release() +{ + // DELETE POINT + // DELETE RESIDUAL + for(unsigned int i=0;i0) + { + int lvlm1 = lvl-1; + int wlm1 = wG[lvlm1]; + Eigen::Vector3f* dI_lm = dIp[lvlm1]; + + + + for(int y=0;ygetBGradOnly((float)(dI_l[idx][0])); + dabs_l[idx] *= gw*gw; // convert to gradient of original color space (before removing response). + } + } + } +} + +void FrameFramePrecalc::set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib ) +{ + this->host = host; + this->target = target; + + SE3 leftToLeft_0 = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse(); + PRE_RTll_0 = (leftToLeft_0.rotationMatrix()).cast(); + PRE_tTll_0 = (leftToLeft_0.translation()).cast(); + + + + SE3 leftToLeft = target->PRE_worldToCam * host->PRE_camToWorld; + PRE_RTll = (leftToLeft.rotationMatrix()).cast(); + PRE_tTll = (leftToLeft.translation()).cast(); + distanceLL = leftToLeft.translation().norm(); + + + Mat33f K = Mat33f::Zero(); + K(0,0) = HCalib->fxl(); + K(1,1) = HCalib->fyl(); + K(0,2) = HCalib->cxl(); + K(1,2) = HCalib->cyl(); + K(2,2) = 1; + PRE_KRKiTll = K * PRE_RTll * K.inverse(); + PRE_RKiTll = PRE_RTll * K.inverse(); + PRE_KtTll = K * PRE_tTll; + + + PRE_aff_mode = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l(), target->aff_g2l()).cast(); + PRE_b0_mode = host->aff_g2l_0().b; +} + +} + diff --git a/src/dso/FullSystem/HessianBlocks.h b/src/dso/FullSystem/HessianBlocks.h new file mode 100644 index 0000000..d9db8cc --- /dev/null +++ b/src/dso/FullSystem/HessianBlocks.h @@ -0,0 +1,515 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#define MAX_ACTIVE_FRAMES 100 + + +#include "util/globalCalib.h" +#include "vector" + +#include +#include +#include "util/NumType.h" +#include "FullSystem/Residuals.h" +#include "util/ImageAndExposure.h" + + +namespace dso +{ + + +inline Vec2 affFromTo(const Vec2 &from, const Vec2 &to) // contains affine parameters as XtoWorld. +{ + return Vec2(from[0] / to[0], (from[1] - to[1]) / to[0]); +} + + +struct FrameHessian; +struct PointHessian; + +class ImmaturePoint; +class FrameShell; + +class EFFrame; +class EFPoint; + +#define SCALE_IDEPTH 1.0f // scales internal value to idepth. +#define SCALE_XI_ROT 1.0f +// #define SCALE_XI_TRANS 0.5f +#define SCALE_XI_TRANS 1.0f +#define SCALE_F 50.0f +#define SCALE_C 50.0f +#define SCALE_W 1.0f +#define SCALE_A 10.0f +#define SCALE_B 1000.0f + +#define SCALE_IDEPTH_INVERSE (1.0f / SCALE_IDEPTH) +#define SCALE_XI_ROT_INVERSE (1.0f / SCALE_XI_ROT) +#define SCALE_XI_TRANS_INVERSE (1.0f / SCALE_XI_TRANS) +#define SCALE_F_INVERSE (1.0f / SCALE_F) +#define SCALE_C_INVERSE (1.0f / SCALE_C) +#define SCALE_W_INVERSE (1.0f / SCALE_W) +#define SCALE_A_INVERSE (1.0f / SCALE_A) +#define SCALE_B_INVERSE (1.0f / SCALE_B) + + +struct FrameFramePrecalc +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + // static values + static int instanceCounter; + FrameHessian* host; // defines row + FrameHessian* target; // defines column + + // precalc values + Mat33f PRE_RTll; + Mat33f PRE_KRKiTll; + Mat33f PRE_RKiTll; + Mat33f PRE_RTll_0; + + Vec2f PRE_aff_mode; + float PRE_b0_mode; + + Vec3f PRE_tTll; + Vec3f PRE_KtTll; + Vec3f PRE_tTll_0; + + float distanceLL; + + + inline ~FrameFramePrecalc() {} + inline FrameFramePrecalc() {host=target=0;} + void set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib); +}; + + + + + +struct FrameHessian +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EFFrame* efFrame; + + // constant info & pre-calculated values + //DepthImageWrap* frame; + FrameShell* shell; + + Eigen::Vector3f* dI; // trace, fine tracking. Used for direction select (not for gradient histograms etc.) + Eigen::Vector3f* dIp[PYR_LEVELS]; // coarse tracking / coarse initializer. NAN in [0] only. + float* absSquaredGrad[PYR_LEVELS]; // only used for pixel select (histograms etc.). no NAN. + + bool addCamPrior; + + int frameID; // incremental ID for keyframes only! + static int instanceCounter; + int idx; + + // Photometric Calibration Stuff + float frameEnergyTH; // set dynamically depending on tracking residual + float ab_exposure; + + bool flaggedForMarginalization; + + std::vector pointHessians; // contains all ACTIVE points. + std::vector pointHessiansMarginalized; // contains all MARGINALIZED points (= fully marginalized, usually because point went OOB.) + std::vector pointHessiansOut; // contains all OUTLIER points (= discarded.). + std::vector immaturePoints; // contains all OUTLIER points (= discarded.). + + + Mat66 nullspaces_pose; + Mat42 nullspaces_affine; + Vec6 nullspaces_scale; + + // variable info. + SE3 worldToCam_evalPT; + Vec10 state_zero; + Vec10 state_scaled; + Vec10 state; // [0-5: worldToCam-leftEps. 6-7: a,b] + Vec10 step; + Vec10 step_backup; + Vec10 state_backup; + + + EIGEN_STRONG_INLINE const SE3 &get_worldToCam_evalPT() const {return worldToCam_evalPT;} + EIGEN_STRONG_INLINE const Vec10 &get_state_zero() const {return state_zero;} // the first 6 parameters of state_zero seem to be always 0 (as this part ist represented by the worldToCam_evalPT. The last two parameters on the other hand are not zero. + EIGEN_STRONG_INLINE const Vec10 &get_state() const {return state;} + EIGEN_STRONG_INLINE const Vec10 &get_state_scaled() const {return state_scaled;} + EIGEN_STRONG_INLINE const Vec10 get_state_minus_stateZero() const {return get_state() - get_state_zero();} + + + // precalc values + SE3 PRE_worldToCam; + SE3 PRE_camToWorld; + std::vector> targetPrecalc; + MinimalImageB3* debugImage; + + + inline Vec6 w2c_leftEps() const {return get_state_scaled().head<6>();} + inline AffLight aff_g2l() const {return AffLight(get_state_scaled()[6], get_state_scaled()[7]);} + inline AffLight aff_g2l_0() const {return AffLight(get_state_zero()[6]*SCALE_A, get_state_zero()[7]*SCALE_B);} + + + + void setStateZero(const Vec10 &state_zero); + inline void setState(const Vec10 &state) + { + + this->state = state; + state_scaled.segment<3>(0) = SCALE_XI_TRANS * state.segment<3>(0); + state_scaled.segment<3>(3) = SCALE_XI_ROT * state.segment<3>(3); + state_scaled[6] = SCALE_A * state[6]; + state_scaled[7] = SCALE_B * state[7]; + state_scaled[8] = SCALE_A * state[8]; + state_scaled[9] = SCALE_B * state[9]; + + PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT(); + PRE_camToWorld = PRE_worldToCam.inverse(); + //setCurrentNullspace(); + }; + inline void setStateScaled(const Vec10 &state_scaled) + { + + this->state_scaled = state_scaled; + state.segment<3>(0) = SCALE_XI_TRANS_INVERSE * state_scaled.segment<3>(0); + state.segment<3>(3) = SCALE_XI_ROT_INVERSE * state_scaled.segment<3>(3); + state[6] = SCALE_A_INVERSE * state_scaled[6]; + state[7] = SCALE_B_INVERSE * state_scaled[7]; + state[8] = SCALE_A_INVERSE * state_scaled[8]; + state[9] = SCALE_B_INVERSE * state_scaled[9]; + + PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT(); + PRE_camToWorld = PRE_worldToCam.inverse(); + //setCurrentNullspace(); + }; + inline void setEvalPT(const SE3 &worldToCam_evalPT, const Vec10 &state) + { + + this->worldToCam_evalPT = worldToCam_evalPT; + setState(state); + setStateZero(state); + }; + + + + inline void setEvalPT_scaled(const SE3 &worldToCam_evalPT, const AffLight &aff_g2l) + { + Vec10 initial_state = Vec10::Zero(); + initial_state[6] = aff_g2l.a; + initial_state[7] = aff_g2l.b; + this->worldToCam_evalPT = worldToCam_evalPT; + setStateScaled(initial_state); + setStateZero(this->get_state()); + }; + + void release(); + + inline ~FrameHessian() + { + assert(efFrame==0); + release(); instanceCounter--; + for(int i=0;i() = Vec3::Constant(setting_initialTransPrior); + p.segment<3>(3) = Vec3::Constant(setting_initialRotPrior); + if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) p.head<6>().setZero(); + + p[6] = setting_initialAffAPrior; + p[7] = setting_initialAffBPrior; + } + else + { + if(setting_affineOptModeA < 0) + p[6] = setting_initialAffAPrior; + else + p[6] = setting_affineOptModeA; + + if(setting_affineOptModeB < 0) + p[7] = setting_initialAffBPrior; + else + p[7] = setting_affineOptModeB; + } + p[8] = setting_initialAffAPrior; + p[9] = setting_initialAffBPrior; + + if(addCamPrior) + { + p.head<3>() = Vec3::Constant(setting_initialTransPrior); + p.segment<3>(3) = Vec3::Constant(setting_initialRotPrior); + if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) p.head<6>().setZero(); + } + + return p; + } + + + inline Vec10 getPriorZero() + { + return Vec10::Zero(); + } + +}; + +struct CalibHessian +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + static int instanceCounter; + + VecC value_zero; + VecC value_scaled; + VecCf value_scaledf; + VecCf value_scaledi; + VecC value; + VecC step; + VecC step_backup; + VecC value_backup; + VecC value_minus_value_zero; + + inline ~CalibHessian() {instanceCounter--;} + inline CalibHessian() + { + + VecC initial_value = VecC::Zero(); + initial_value[0] = fxG[0]; + initial_value[1] = fyG[0]; + initial_value[2] = cxG[0]; + initial_value[3] = cyG[0]; + + setValueScaled(initial_value); + value_zero = value; + value_minus_value_zero.setZero(); + + instanceCounter++; + for(int i=0;i<256;i++) + Binv[i] = B[i] = i; // set gamma function to identity + }; + + + // normal mode: use the optimized parameters everywhere! + inline float& fxl() {return value_scaledf[0];} + inline float& fyl() {return value_scaledf[1];} + inline float& cxl() {return value_scaledf[2];} + inline float& cyl() {return value_scaledf[3];} + inline float& fxli() {return value_scaledi[0];} + inline float& fyli() {return value_scaledi[1];} + inline float& cxli() {return value_scaledi[2];} + inline float& cyli() {return value_scaledi[3];} + + + + inline void setValue(const VecC &value) + { + // [0-3: Kl, 4-7: Kr, 8-12: l2r] + this->value = value; + value_scaled[0] = SCALE_F * value[0]; + value_scaled[1] = SCALE_F * value[1]; + value_scaled[2] = SCALE_C * value[2]; + value_scaled[3] = SCALE_C * value[3]; + + this->value_scaledf = this->value_scaled.cast(); + this->value_scaledi[0] = 1.0f / this->value_scaledf[0]; + this->value_scaledi[1] = 1.0f / this->value_scaledf[1]; + this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0]; + this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1]; + this->value_minus_value_zero = this->value - this->value_zero; + }; + + inline void setValueScaled(const VecC &value_scaled) + { + this->value_scaled = value_scaled; + this->value_scaledf = this->value_scaled.cast(); + value[0] = SCALE_F_INVERSE * value_scaled[0]; + value[1] = SCALE_F_INVERSE * value_scaled[1]; + value[2] = SCALE_C_INVERSE * value_scaled[2]; + value[3] = SCALE_C_INVERSE * value_scaled[3]; + + this->value_minus_value_zero = this->value - this->value_zero; + this->value_scaledi[0] = 1.0f / this->value_scaledf[0]; + this->value_scaledi[1] = 1.0f / this->value_scaledf[1]; + this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0]; + this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1]; + }; + + + float Binv[256]; + float B[256]; + + + EIGEN_STRONG_INLINE float getBGradOnly(float color) + { + int c = color+0.5f; + if(c<5) c=5; + if(c>250) c=250; + return B[c+1]-B[c]; + } + + EIGEN_STRONG_INLINE float getBInvGradOnly(float color) + { + int c = color+0.5f; + if(c<5) c=5; + if(c>250) c=250; + return Binv[c+1]-Binv[c]; + } +}; + + +// hessian component associated with one point. +struct PointHessian +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + static int instanceCounter; + EFPoint* efPoint; + + // static values + float color[MAX_RES_PER_POINT]; // colors in host frame + float weights[MAX_RES_PER_POINT]; // host-weights for respective residuals. + + + + float u,v; + int idx; + float energyTH; + FrameHessian* host; + bool hasDepthPrior; + + float my_type; + + float idepth_scaled; + float idepth_zero_scaled; + float idepth_zero; + float idepth; + float step; + float step_backup; + float idepth_backup; + + float nullspaces_scale; + float idepth_hessian; + float maxRelBaseline; + int numGoodResiduals; + + enum PtStatus {ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED}; + PtStatus status; + + inline void setPointStatus(PtStatus s) {status=s;} + + + inline void setIdepth(float idepth) { + this->idepth = idepth; + this->idepth_scaled = SCALE_IDEPTH * idepth; + } + inline void setIdepthScaled(float idepth_scaled) { + this->idepth = SCALE_IDEPTH_INVERSE * idepth_scaled; + this->idepth_scaled = idepth_scaled; + } + inline void setIdepthZero(float idepth) { + idepth_zero = idepth; + idepth_zero_scaled = SCALE_IDEPTH * idepth; + nullspaces_scale = -(idepth*1.001 - idepth/1.001)*500; + } + + + std::vector residuals; // only contains good residuals (not OOB and not OUTLIER). Arbitrary order. + std::pair lastResiduals[2]; // contains information about residuals to the last two (!) frames. ([0] = latest, [1] = the one before). + + + void release(); + PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib); + inline ~PointHessian() {assert(efPoint==0); release(); instanceCounter--;} + + + inline bool isOOB(const std::vector& toKeep, const std::vector& toMarg) const + { + + int visInToMarg = 0; + for(PointFrameResidual* r : residuals) + { + if(r->state_state != ResState::IN) continue; + for(FrameHessian* k : toMarg) + if(r->target == k) visInToMarg++; + } + if((int)residuals.size() >= setting_minGoodActiveResForMarg && + numGoodResiduals > setting_minGoodResForMarg+10 && + (int)residuals.size()-visInToMarg < setting_minGoodActiveResForMarg) + return true; + + + + + + if(lastResiduals[0].second == ResState::OOB) return true; + if(residuals.size() < 2) return false; + if(lastResiduals[0].second == ResState::OUTLIER && lastResiduals[1].second == ResState::OUTLIER) return true; + return false; + } + + + inline bool isInlierNew() + { + return (int)residuals.size() >= setting_minGoodActiveResForMarg + && numGoodResiduals >= setting_minGoodResForMarg; + } + +}; + + + + + +} + diff --git a/src/dso/FullSystem/ImmaturePoint.cpp b/src/dso/FullSystem/ImmaturePoint.cpp new file mode 100644 index 0000000..4d905ea --- /dev/null +++ b/src/dso/FullSystem/ImmaturePoint.cpp @@ -0,0 +1,569 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "FullSystem/ImmaturePoint.h" +#include "util/FrameShell.h" +#include "FullSystem/ResidualProjections.h" + +namespace dso +{ +ImmaturePoint::ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib) +: u(u_), v(v_), host(host_), my_type(type), idepth_min(0), idepth_max(NAN), lastTraceStatus(IPS_UNINITIALIZED) +{ + + gradH.setZero(); + + for(int idx=0;idxdI, u+dx, v+dy,wG[0]); + + + + color[idx] = ptc[0]; + if(!std::isfinite(color[idx])) {energyTH=NAN; return;} + + + gradH += ptc.tail<2>() * ptc.tail<2>().transpose(); + + weights[idx] = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + ptc.tail<2>().squaredNorm())); + } + + energyTH = patternNum*setting_outlierTH; + energyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight; + + idepth_GT=0; + quality=10000; +} + +ImmaturePoint::~ImmaturePoint() +{ +} + + + +/* + * returns + * * OOB -> point is optimized and marginalized + * * UPDATED -> point has been updated. + * * SKIP -> point has not been updated. + */ +ImmaturePointStatus ImmaturePoint::traceOn(FrameHessian* frame,const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f& hostToFrame_affine, CalibHessian* HCalib, bool debugPrint) +{ + if(lastTraceStatus == ImmaturePointStatus::IPS_OOB) return lastTraceStatus; + + + debugPrint = false;//rand()%100==0; + float maxPixSearch = (wG[0]+hG[0])*setting_maxPixSearch; + + if(debugPrint) + printf("trace pt (%.1f %.1f) from frame %d to %d. Range %f -> %f. t %f %f %f!\n", + u,v, + host->shell->id, frame->shell->id, + idepth_min, idepth_max, + hostToFrame_Kt[0],hostToFrame_Kt[1],hostToFrame_Kt[2]); + +// const float stepsize = 1.0; // stepsize for initial discrete search. +// const int GNIterations = 3; // max # GN iterations +// const float GNThreshold = 0.1; // GN stop after this stepsize. +// const float extraSlackOnTH = 1.2; // for energy-based outlier check, be slightly more relaxed by this factor. +// const float slackInterval = 0.8; // if pixel-interval is smaller than this, leave it be. +// const float minImprovementFactor = 2; // if pixel-interval is smaller than this, leave it be. + // ============== project min and max. return if one of them is OOB =================== + Vec3f pr = hostToFrame_KRKi * Vec3f(u,v, 1); + Vec3f ptpMin = pr + hostToFrame_Kt*idepth_min; + float uMin = ptpMin[0] / ptpMin[2]; + float vMin = ptpMin[1] / ptpMin[2]; + + Mat22f Rplane = hostToFrame_KRKi.topLeftCorner<2,2>(); + int maxRotPatX = 0; + int maxRotPatY = 0; + Vec2f rotatetPattern[MAX_RES_PER_POINT]; + for(int idx=0;idx boundU && vMin > boundV && uMin < wG[0]-boundU-1 && vMin < hG[0]-boundV-1)) + { + if(debugPrint) printf("OOB uMin %f %f - %f %f %f (id %f-%f)!\n", + u,v,uMin, vMin, ptpMin[2], idepth_min, idepth_max); + lastTraceUV = Vec2f(-1,-1); + lastTracePixelInterval=0; + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + } + + float dist; + float uMax; + float vMax; + Vec3f ptpMax; + if(std::isfinite(idepth_max)) + { + ptpMax = pr + hostToFrame_Kt*idepth_max; + uMax = ptpMax[0] / ptpMax[2]; + vMax = ptpMax[1] / ptpMax[2]; + + + if(!(uMax > boundU && vMax > boundV && uMax < wG[0]-boundU-1 && vMax < hG[0]-boundV-1)) + { + if(debugPrint) printf("OOB uMax %f %f - %f %f!\n",u,v, uMax, vMax); + lastTraceUV = Vec2f(-1,-1); + lastTracePixelInterval=0; + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + } + + + + // ============== check their distance. everything below 2px is OK (-> skip). =================== + dist = (uMin-uMax)*(uMin-uMax) + (vMin-vMax)*(vMin-vMax); + dist = sqrtf(dist); + if(dist < setting_trace_slackInterval) + { + if(debugPrint) + printf("TOO CERTAIN ALREADY (dist %f)!\n", dist); + + lastTraceUV = Vec2f(uMax+uMin, vMax+vMin)*0.5; + lastTracePixelInterval=dist; + return lastTraceStatus = ImmaturePointStatus::IPS_SKIPPED; + } + assert(dist>0); + } + else + { + dist = maxPixSearch; + + // project to arbitrary depth to get direction. + ptpMax = pr + hostToFrame_Kt*0.01; + uMax = ptpMax[0] / ptpMax[2]; + vMax = ptpMax[1] / ptpMax[2]; + + // direction. + float dx = uMax-uMin; + float dy = vMax-vMin; + float d = 1.0f / sqrtf(dx*dx+dy*dy); + + // set to [setting_maxPixSearch]. + uMax = uMin + dist*dx*d; + vMax = vMin + dist*dy*d; + + // may still be out! + if(!(uMax > boundU && vMax > boundV && uMax < wG[0]-boundU-1 && vMax < hG[0]-boundV-1)) + { + if(debugPrint) printf("OOB uMax-coarse %f %f %f!\n", uMax, vMax, ptpMax[2]); + lastTraceUV = Vec2f(-1,-1); + lastTracePixelInterval=0; + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + } + assert(dist>0); + } + + + // set OOB if scale change too big. + if(!(idepth_min<0 || (ptpMin[2]>0.75 && ptpMin[2]<1.5))) + { + if(debugPrint) printf("OOB SCALE %f %f %f!\n", uMax, vMax, ptpMin[2]); + lastTraceUV = Vec2f(-1,-1); + lastTracePixelInterval=0; + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + } + + + // ============== compute error-bounds on result in pixel. if the new interval is not at least 1/2 of the old, SKIP =================== + float dx = setting_trace_stepsize*(uMax-uMin); + float dy = setting_trace_stepsize*(vMax-vMin); + + float a = (Vec2f(dx,dy).transpose() * gradH * Vec2f(dx,dy)); + float b = (Vec2f(dy,-dx).transpose() * gradH * Vec2f(dy,-dx)); + float errorInPixel = 0.2f + 0.2f * (a+b) / a; + + if(errorInPixel*setting_trace_minImprovementFactor > dist && std::isfinite(idepth_max)) + { + if(debugPrint) + printf("NO SIGNIFICANT IMPROVMENT (%f)!\n", errorInPixel); + lastTraceUV = Vec2f(uMax+uMin, vMax+vMin)*0.5; + lastTracePixelInterval=dist; + return lastTraceStatus = ImmaturePointStatus::IPS_BADCONDITION; + } + + if(errorInPixel >10) errorInPixel=10; + + + // ============== do the discrete search =================== + dx /= dist; + dy /= dist; + + if(debugPrint) + printf("trace pt (%.1f %.1f) from frame %d to %d. Range %f (%.1f %.1f) -> %f (%.1f %.1f)! ErrorInPixel %.1f!\n", + u,v, + host->shell->id, frame->shell->id, + idepth_min, uMin, vMin, + idepth_max, uMax, vMax, + errorInPixel + ); + + + if(dist>maxPixSearch) + { + uMax = uMin + maxPixSearch*dx; + vMax = vMin + maxPixSearch*dy; + dist = maxPixSearch; + } + + int numSteps = 1.9999f + dist / setting_trace_stepsize; + + float randShift = uMin*1000-floorf(uMin*1000); + float ptx = uMin-randShift*dx; + float pty = vMin-randShift*dy; + + + + + if(!std::isfinite(dx) || !std::isfinite(dy)) + { + //printf("COUGHT INF / NAN dxdy (%f %f)!\n", dx, dx); + + lastTracePixelInterval=0; + lastTraceUV = Vec2f(-1,-1); + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + } + + + + float errors[100]; + float bestU=0, bestV=0, bestEnergy=1e10; + int bestIdx=-1; + if(numSteps >= 100) numSteps = 99; + + for(int i=0;idI, + (float)(ptx+rotatetPattern[idx][0]), + (float)(pty+rotatetPattern[idx][1]), + wG[0]); + + if(!std::isfinite(hitColor)) {energy+=1e5; continue;} + float residual = hitColor - (float)(hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]); + float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual); + energy += hw *residual*residual*(2-hw); + } + + if(debugPrint) + printf("step %.1f %.1f (id %f): energy = %f!\n", + ptx, pty, 0.0f, energy); + + + errors[i] = energy; + if(energy < bestEnergy) + { + bestU = ptx; bestV = pty; bestEnergy = energy; bestIdx = i; + } + + ptx+=dx; + pty+=dy; + } + + + // find best score outside a +-2px radius. + float secondBest=1e10; + for(int i=0;i bestIdx+setting_minTraceTestRadius) && errors[i] < secondBest) + secondBest = errors[i]; + } + float newQuality = secondBest / bestEnergy; + if(newQuality < quality || numSteps > 10) quality = newQuality; + + + // ============== do GN optimization =================== + float uBak=bestU, vBak=bestV, gnstepsize=1, stepBack=0; + if(setting_trace_GNIterations>0) bestEnergy = 1e5; + int gnStepsGood=0, gnStepsBad=0; + for(int it=0;it= wG[0] - 1 || posV >= hG[0] - 1) + { + if(debugPrint) printf("OOB uMax %f %f - %f %f!\n", posU, posV, uMax, vMax); + lastTraceUV = Vec2f(-1,-1); + lastTracePixelInterval=0; + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + } + + Vec3f hitColor = getInterpolatedElement33(frame->dI, posU, posV, wG[0]); + + if(!std::isfinite((float)hitColor[0])) {energy+=1e5; continue;} + float residual = hitColor[0] - (hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]); + float dResdDist = dx*hitColor[1] + dy*hitColor[2]; + float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual); + + H += hw*dResdDist*dResdDist; + b += hw*residual*dResdDist; + energy += weights[idx]*weights[idx]*hw *residual*residual*(2-hw); + } + + + if(energy > bestEnergy) + { + gnStepsBad++; + + // do a smaller step from old point. + stepBack*=0.5; + bestU = uBak + stepBack*dx; + bestV = vBak + stepBack*dy; + if(debugPrint) + printf("GN BACK %d: E %f, H %f, b %f. id-step %f. UV %f %f -> %f %f.\n", + it, energy, H, b, stepBack, + uBak, vBak, bestU, bestV); + } + else + { + gnStepsGood++; + + float step = -gnstepsize*b/H; + if(step < -0.5) step = -0.5; + else if(step > 0.5) step=0.5; + + if(!std::isfinite(step)) step=0; + + uBak=bestU; + vBak=bestV; + stepBack=step; + + bestU += step*dx; + bestV += step*dy; + bestEnergy = energy; + + if(debugPrint) + printf("GN step %d: E %f, H %f, b %f. id-step %f. UV %f %f -> %f %f.\n", + it, energy, H, b, step, + uBak, vBak, bestU, bestV); + } + + if(fabsf(stepBack) < setting_trace_GNThreshold) break; + } + + + // ============== detect energy-based outlier. =================== +// float absGrad0 = getInterpolatedElement(frame->absSquaredGrad[0],bestU, bestV, wG[0]); +// float absGrad1 = getInterpolatedElement(frame->absSquaredGrad[1],bestU*0.5-0.25, bestV*0.5-0.25, wG[1]); +// float absGrad2 = getInterpolatedElement(frame->absSquaredGrad[2],bestU*0.25-0.375, bestV*0.25-0.375, wG[2]); + if(!(bestEnergy < energyTH*setting_trace_extraSlackOnTH)) +// || (absGrad0*areaGradientSlackFactor < host->frameGradTH +// && absGrad1*areaGradientSlackFactor < host->frameGradTH*0.75f +// && absGrad2*areaGradientSlackFactor < host->frameGradTH*0.50f)) + { + if(debugPrint) + printf("OUTLIER!\n"); + + lastTracePixelInterval=0; + lastTraceUV = Vec2f(-1,-1); + if(lastTraceStatus == ImmaturePointStatus::IPS_OUTLIER) + return lastTraceStatus = ImmaturePointStatus::IPS_OOB; + else + return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER; + } + + + // ============== set new interval =================== + if(dx*dx>dy*dy) + { + idepth_min = (pr[2]*(bestU-errorInPixel*dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2]*(bestU-errorInPixel*dx)); + idepth_max = (pr[2]*(bestU+errorInPixel*dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2]*(bestU+errorInPixel*dx)); + } + else + { + idepth_min = (pr[2]*(bestV-errorInPixel*dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2]*(bestV-errorInPixel*dy)); + idepth_max = (pr[2]*(bestV+errorInPixel*dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2]*(bestV+errorInPixel*dy)); + } + if(idepth_min > idepth_max) std::swap(idepth_min, idepth_max); + + + if(!std::isfinite(idepth_min) || !std::isfinite(idepth_max) || (idepth_max<0)) + { + //printf("COUGHT INF / NAN minmax depth (%f %f)!\n", idepth_min, idepth_max); + + lastTracePixelInterval=0; + lastTraceUV = Vec2f(-1,-1); + return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER; + } + + lastTracePixelInterval=2*errorInPixel; + lastTraceUV = Vec2f(bestU, bestV); + return lastTraceStatus = ImmaturePointStatus::IPS_GOOD; +} + + +float ImmaturePoint::getdPixdd( + CalibHessian * HCalib, + ImmaturePointTemporaryResidual* tmpRes, + float idepth) +{ + FrameFramePrecalc* precalc = &(host->targetPrecalc[tmpRes->target->idx]); + const Vec3f &PRE_tTll = precalc->PRE_tTll; + float drescale, u=0, v=0, new_idepth; + float Ku, Kv; + Vec3f KliP; + + projectPoint(this->u,this->v, idepth, 0, 0,HCalib, + precalc->PRE_RTll,PRE_tTll, drescale, u, v, Ku, Kv, KliP, new_idepth); + + float dxdd = (PRE_tTll[0]-PRE_tTll[2]*u)*HCalib->fxl(); + float dydd = (PRE_tTll[1]-PRE_tTll[2]*v)*HCalib->fyl(); + return drescale*sqrtf(dxdd*dxdd + dydd*dydd); +} + + +float ImmaturePoint::calcResidual( + CalibHessian * HCalib, const float outlierTHSlack, + ImmaturePointTemporaryResidual* tmpRes, + float idepth) +{ + FrameFramePrecalc* precalc = &(host->targetPrecalc[tmpRes->target->idx]); + + float energyLeft=0; + const Eigen::Vector3f* dIl = tmpRes->target->dI; + const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll; + const Vec3f &PRE_KtTll = precalc->PRE_KtTll; + Vec2f affLL = precalc->PRE_aff_mode; + + for(int idx=0;idxu+patternP[idx][0], this->v+patternP[idx][1], idepth, PRE_KRKiTll, PRE_KtTll, Ku, Kv)) + {return 1e10;} + + Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0])); + if(!std::isfinite((float)hitColor[0])) {return 1e10;} + //if(benchmarkSpecialOption==5) hitColor = (getInterpolatedElement13BiCub(tmpRes->target->I, Ku, Kv, wG[0])); + + float residual = hitColor[0] - (affLL[0] * color[idx] + affLL[1]); + + float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual); + energyLeft += weights[idx]*weights[idx]*hw *residual*residual*(2-hw); + } + + if(energyLeft > energyTH*outlierTHSlack) + { + energyLeft = energyTH*outlierTHSlack; + } + return energyLeft; +} + + + + +double ImmaturePoint::linearizeResidual( + CalibHessian * HCalib, const float outlierTHSlack, + ImmaturePointTemporaryResidual* tmpRes, + float &Hdd, float &bd, + float idepth) +{ + if(tmpRes->state_state == ResState::OOB) + { tmpRes->state_NewState = ResState::OOB; return tmpRes->state_energy; } + + FrameFramePrecalc* precalc = &(host->targetPrecalc[tmpRes->target->idx]); + + // check OOB due to scale angle change. + + float energyLeft=0; + const Eigen::Vector3f* dIl = tmpRes->target->dI; + const Mat33f &PRE_RTll = precalc->PRE_RTll; + const Vec3f &PRE_tTll = precalc->PRE_tTll; + //const float * const Il = tmpRes->target->I; + + Vec2f affLL = precalc->PRE_aff_mode; + + for(int idx=0;idxu,this->v, idepth, dx, dy,HCalib, + PRE_RTll,PRE_tTll, drescale, u, v, Ku, Kv, KliP, new_idepth)) + {tmpRes->state_NewState = ResState::OOB; return tmpRes->state_energy;} + + + Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0])); + + if(!std::isfinite((float)hitColor[0])) {tmpRes->state_NewState = ResState::OOB; return tmpRes->state_energy;} + float residual = hitColor[0] - (affLL[0] * color[idx] + affLL[1]); + + float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual); + energyLeft += weights[idx]*weights[idx]*hw *residual*residual*(2-hw); + + // depth derivatives. + float dxInterp = hitColor[1]*HCalib->fxl(); + float dyInterp = hitColor[2]*HCalib->fyl(); + float d_idepth = derive_idepth(PRE_tTll, u, v, dx, dy, dxInterp, dyInterp, drescale); + + hw *= weights[idx]*weights[idx]; + + Hdd += (hw*d_idepth)*d_idepth; + bd += (hw*residual)*d_idepth; + } + + + if(energyLeft > energyTH*outlierTHSlack) + { + energyLeft = energyTH*outlierTHSlack; + tmpRes->state_NewState = ResState::OUTLIER; + } + else + { + tmpRes->state_NewState = ResState::IN; + } + + tmpRes->state_NewEnergy = energyLeft; + return energyLeft; +} + + + +} diff --git a/src/dso/FullSystem/ImmaturePoint.h b/src/dso/FullSystem/ImmaturePoint.h new file mode 100644 index 0000000..24a11d4 --- /dev/null +++ b/src/dso/FullSystem/ImmaturePoint.h @@ -0,0 +1,111 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" + +#include "FullSystem/HessianBlocks.h" +namespace dso +{ + + +struct ImmaturePointTemporaryResidual +{ +public: + ResState state_state; + double state_energy; + ResState state_NewState; + double state_NewEnergy; + FrameHessian* target; +}; + + +enum ImmaturePointStatus { + IPS_GOOD=0, // traced well and good + IPS_OOB, // OOB: end tracking & marginalize! + IPS_OUTLIER, // energy too high: if happens again: outlier! + IPS_SKIPPED, // traced well and good (but not actually traced). + IPS_BADCONDITION, // not traced because of bad condition. + IPS_UNINITIALIZED}; // not even traced once. + + +class ImmaturePoint +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + // static values + float color[MAX_RES_PER_POINT]; + float weights[MAX_RES_PER_POINT]; + + + + + + Mat22f gradH; + Vec2f gradH_ev; + Mat22f gradH_eig; + float energyTH; + float u,v; + FrameHessian* host; + int idxInImmaturePoints; + + float quality; + + float my_type; + + float idepth_min; + float idepth_max; + ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib); + ~ImmaturePoint(); + + ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false); + + ImmaturePointStatus lastTraceStatus; + Vec2f lastTraceUV; + float lastTracePixelInterval; + + float idepth_GT; + + double linearizeResidual( + CalibHessian * HCalib, const float outlierTHSlack, + ImmaturePointTemporaryResidual* tmpRes, + float &Hdd, float &bd, + float idepth); + float getdPixdd( + CalibHessian * HCalib, + ImmaturePointTemporaryResidual* tmpRes, + float idepth); + + float calcResidual( + CalibHessian * HCalib, const float outlierTHSlack, + ImmaturePointTemporaryResidual* tmpRes, + float idepth); + +private: +}; + +} + diff --git a/src/dso/FullSystem/PixelSelector.h b/src/dso/FullSystem/PixelSelector.h new file mode 100644 index 0000000..bb92ace --- /dev/null +++ b/src/dso/FullSystem/PixelSelector.h @@ -0,0 +1,256 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" + + + + +namespace dso +{ + + +const float minUseGrad_pixsel = 10; + + +template +inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac) +{ + + memset(map_out, 0, sizeof(bool)*w*h); + + int numGood = 0; + for(int y=1;y().squaredNorm(); + float TH = THFac*minUseGrad_pixsel * (0.75f); + + if(sqgd > TH*TH) + { + float agx = fabs((float)g[1]); + if(agx > bestXX) {bestXX=agx; bestXXID=idx;} + + float agy = fabs((float)g[2]); + if(agy > bestYY) {bestYY=agy; bestYYID=idx;} + + float gxpy = fabs((float)(g[1]-g[2])); + if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;} + + float gxmy = fabs((float)(g[1]+g[2])); + if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;} + } + } + + bool* map0 = map_out+x+y*w; + + if(bestXXID>=0) + { + if(!map0[bestXXID]) + numGood++; + map0[bestXXID] = true; + + } + if(bestYYID>=0) + { + if(!map0[bestYYID]) + numGood++; + map0[bestYYID] = true; + + } + if(bestXYID>=0) + { + if(!map0[bestXYID]) + numGood++; + map0[bestXYID] = true; + + } + if(bestYXID>=0) + { + if(!map0[bestYXID]) + numGood++; + map0[bestYXID] = true; + + } + } + } + + return numGood; +} + + +inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac) +{ + + memset(map_out, 0, sizeof(bool)*w*h); + + int numGood = 0; + for(int y=1;y().squaredNorm(); + float TH = THFac*minUseGrad_pixsel * (0.75f); + + if(sqgd > TH*TH) + { + float agx = fabs((float)g[1]); + if(agx > bestXX) {bestXX=agx; bestXXID=idx;} + + float agy = fabs((float)g[2]); + if(agy > bestYY) {bestYY=agy; bestYYID=idx;} + + float gxpy = fabs((float)(g[1]-g[2])); + if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;} + + float gxmy = fabs((float)(g[1]+g[2])); + if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;} + } + } + + bool* map0 = map_out+x+y*w; + + if(bestXXID>=0) + { + if(!map0[bestXXID]) + numGood++; + map0[bestXXID] = true; + + } + if(bestYYID>=0) + { + if(!map0[bestYYID]) + numGood++; + map0[bestYYID] = true; + + } + if(bestXYID>=0) + { + if(!map0[bestXYID]) + numGood++; + map0[bestXYID] = true; + + } + if(bestYXID>=0) + { + if(!map0[bestYXID]) + numGood++; + map0[bestYXID] = true; + + } + } + } + + return numGood; +} + + +inline int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1) +{ + if(sparsityFactor < 1) sparsityFactor = 1; + + int numGoodPoints; + + + if(sparsityFactor==1) numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac); + else if(sparsityFactor==2) numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac); + else if(sparsityFactor==3) numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac); + else if(sparsityFactor==4) numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac); + else if(sparsityFactor==5) numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac); + else if(sparsityFactor==6) numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac); + else if(sparsityFactor==7) numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac); + else if(sparsityFactor==8) numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac); + else if(sparsityFactor==9) numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac); + else if(sparsityFactor==10) numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac); + else if(sparsityFactor==11) numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac); + else numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac); + + + /* + * #points is approximately proportional to sparsityFactor^2. + */ + + float quotia = numGoodPoints / (float)(desiredDensity); + + int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f; + + + if(newSparsity < 1) newSparsity=1; + + + float oldTHFac = THFac; + if(newSparsity==1 && sparsityFactor==1) THFac = 0.5; + + + if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) || + ( quotia > 0.8 && 1.0f / quotia > 0.8) || + recsLeft == 0) + { + +// printf(" \n"); + //all good + sparsityFactor = newSparsity; + return numGoodPoints; + } + else + { +// printf(" -> re-evaluate! \n"); + // re-evaluate. + sparsityFactor = newSparsity; + return makePixelStatus(grads, map, w,h, desiredDensity, recsLeft-1, THFac); + } +} + +} + diff --git a/src/dso/FullSystem/PixelSelector2.cpp b/src/dso/FullSystem/PixelSelector2.cpp new file mode 100644 index 0000000..d4be0ff --- /dev/null +++ b/src/dso/FullSystem/PixelSelector2.cpp @@ -0,0 +1,458 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#include "FullSystem/PixelSelector2.h" + +// + + + +#include "util/NumType.h" +#include "IOWrapper/ImageDisplay.h" +#include "util/globalCalib.h" +#include "FullSystem/HessianBlocks.h" +#include "util/globalFuncs.h" + +namespace dso +{ + + +PixelSelector::PixelSelector(int w, int h) +{ + randomPattern = new unsigned char[w*h]; + std::srand(3141592); // want to be deterministic. + for(int i=0;iabsSquaredGrad[0]; + + int w = wG[0]; + int h = hG[0]; + + int w32 = nbW; + int h32 = nbH; + thsStep = w32; + + for(int y=0;yw-2 || jt>h-2 || it<1 || jt<1) continue; + int g = sqrtf(map0[i+j*w]); + if(g>48) g=48; + hist0[g+1]++; + hist0[0]++; + } + + ths[x+y*w32] = computeHistQuantil(hist0,setting_minGradHistCut) + setting_minGradHistAdd; + } + + for(int y=0;y0) + { + if(y>0) {num++; sum+=ths[x-1+(y-1)*w32];} + if(y0) {num++; sum+=ths[x+1+(y-1)*w32];} + if(y0) {num++; sum+=ths[x+(y-1)*w32];} + if(y0 && allowFast) +// { +// memset(map_out, 0, sizeof(float)*wG[0]*hG[0]); +// std::vector pts; +// cv::Mat img8u(hG[0],wG[0],CV_8U); +// for(int i=0;idI[i][0]*0.8; +// img8u.at(i) = (!std::isfinite(v) || v>255) ? 255 : v; +// } +// cv::FAST(img8u, pts, setting_pixelSelectionUseFast, true); +// for(unsigned int i=0;iselect(fh, map_out,currentPotential, thFactor); + + // sub-select! + numHave = n[0]+n[1]+n[2]; + quotia = numWant / numHave; + + // by default we want to over-sample by 40% just to be sure. + float K = numHave * (currentPotential+1) * (currentPotential+1); + idealPotential = sqrtf(K/numWant)-1; // round down. + if(idealPotential<1) idealPotential=1; + + if( recursionsLeft>0 && quotia > 1.25 && currentPotential>1) + { + //re-sample to get more points! + // potential needs to be smaller + if(idealPotential>=currentPotential) + idealPotential = currentPotential-1; + + // printf("PixelSelector: have %.2f%%, need %.2f%%. RESAMPLE with pot %d -> %d.\n", + // 100*numHave/(float)(wG[0]*hG[0]), + // 100*numWant/(float)(wG[0]*hG[0]), + // currentPotential, + // idealPotential); + currentPotential = idealPotential; + return makeMaps(fh,map_out, density, recursionsLeft-1, plot,thFactor); + } + else if(recursionsLeft>0 && quotia < 0.25) + { + // re-sample to get less points! + + if(idealPotential<=currentPotential) + idealPotential = currentPotential+1; + + // printf("PixelSelector: have %.2f%%, need %.2f%%. RESAMPLE with pot %d -> %d.\n", + // 100*numHave/(float)(wG[0]*hG[0]), + // 100*numWant/(float)(wG[0]*hG[0]), + // currentPotential, + // idealPotential); + currentPotential = idealPotential; + return makeMaps(fh,map_out, density, recursionsLeft-1, plot,thFactor); + + } + } + + int numHaveSub = numHave; + if(quotia < 0.95) + { + int wh=wG[0]*hG[0]; + int rn=0; + unsigned char charTH = 255*quotia; + for(int i=0;i charTH ) + { + map_out[i]=0; + numHaveSub--; + } + rn++; + } + } + } + +// printf("PixelSelector: have %.2f%%, need %.2f%%. KEEPCURR with pot %d -> %d. Subsampled to %.2f%%\n", +// 100*numHave/(float)(wG[0]*hG[0]), +// 100*numWant/(float)(wG[0]*hG[0]), +// currentPotential, +// idealPotential, +// 100*numHaveSub/(float)(wG[0]*hG[0])); + currentPotential = idealPotential; + + + if(plot) + { + int w = wG[0]; + int h = hG[0]; + + + MinimalImageB3 img(w,h); + + for(int i=0;idI[i][0]*0.7; + if(c>255) c=255; + img.at(i) = Vec3b(c,c,c); + } + IOWrap::displayImage("Selector Image", &img); + + for(int y=0; ydI; + + float * mapmax0 = fh->absSquaredGrad[0]; + float * mapmax1 = fh->absSquaredGrad[1]; + float * mapmax2 = fh->absSquaredGrad[2]; + + + int w = wG[0]; + int w1 = wG[1]; + int w2 = wG[2]; + int h = hG[0]; + + + const Vec2f directions[16] = { + Vec2f(0, 1.0000), + Vec2f(0.3827, 0.9239), + Vec2f(0.1951, 0.9808), + Vec2f(0.9239, 0.3827), + Vec2f(0.7071, 0.7071), + Vec2f(0.3827, -0.9239), + Vec2f(0.8315, 0.5556), + Vec2f(0.8315, -0.5556), + Vec2f(0.5556, -0.8315), + Vec2f(0.9808, 0.1951), + Vec2f(0.9239, -0.3827), + Vec2f(0.7071, -0.7071), + Vec2f(0.5556, 0.8315), + Vec2f(0.9808, -0.1951), + Vec2f(1.0000, 0.0000), + Vec2f(0.1951, -0.9808)}; + + memset(map_out,0,w*h*sizeof(PixelSelectorStatus)); + + + + float dw1 = setting_gradDownweightPerLevel; + float dw2 = dw1*dw1; + + + int n3=0, n2=0, n4=0; + for(int y4=0;y4=w-5 || yf<4 || yf>h-4) continue; + + + float pixelTH0 = thsSmoothed[xf / bW + (yf / bH) * thsStep]; + float pixelTH1 = pixelTH0*dw1; + float pixelTH2 = pixelTH1*dw2; + + + float ag0 = mapmax0[idx]; + if(ag0 > pixelTH0*thFactor) + { + Vec2f ag0d = map0[idx].tail<2>(); + float dirNorm = fabsf((float)(ag0d.dot(dir2))); + if(!setting_selectDirectionDistribution) dirNorm = ag0; + + if(dirNorm > bestVal2) + { bestVal2 = dirNorm; bestIdx2 = idx; bestIdx3 = -2; bestIdx4 = -2;} + } + if(bestIdx3==-2) continue; + + float ag1 = mapmax1[(int)(xf*0.5f+0.25f) + (int)(yf*0.5f+0.25f)*w1]; + if(ag1 > pixelTH1*thFactor) + { + Vec2f ag0d = map0[idx].tail<2>(); + float dirNorm = fabsf((float)(ag0d.dot(dir3))); + if(!setting_selectDirectionDistribution) dirNorm = ag1; + + if(dirNorm > bestVal3) + { bestVal3 = dirNorm; bestIdx3 = idx; bestIdx4 = -2;} + } + if(bestIdx4==-2) continue; + + float ag2 = mapmax2[(int)(xf*0.25f+0.125) + (int)(yf*0.25f+0.125)*w2]; + if(ag2 > pixelTH2*thFactor) + { + Vec2f ag0d = map0[idx].tail<2>(); + float dirNorm = fabsf((float)(ag0d.dot(dir4))); + if(!setting_selectDirectionDistribution) dirNorm = ag2; + + if(dirNorm > bestVal4) + { bestVal4 = dirNorm; bestIdx4 = idx; } + } + } + + if(bestIdx2>0) + { + map_out[bestIdx2] = 1; + bestVal3 = 1e10; + n2++; + } + } + + if(bestIdx3>0) + { + map_out[bestIdx3] = 2; + bestVal4 = 1e10; + n3++; + } + } + + if(bestIdx4>0) + { + map_out[bestIdx4] = 4; + n4++; + } + } + + + return Eigen::Vector3i(n2,n3,n4); +} + + +} + diff --git a/src/dso/FullSystem/PixelSelector2.h b/src/dso/FullSystem/PixelSelector2.h new file mode 100644 index 0000000..fc06d96 --- /dev/null +++ b/src/dso/FullSystem/PixelSelector2.h @@ -0,0 +1,79 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "util/NumType.h" + +namespace dso +{ + +enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; + + +class FrameHessian; + +class PixelSelector +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + int makeMaps( + const FrameHessian* const fh, + float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1); + + PixelSelector(int w, int h); + ~PixelSelector(); + int currentPotential; + + + bool allowFast; + void makeHists(const FrameHessian* const fh); +private: + + Eigen::Vector3i select(const FrameHessian* const fh, + float* map_out, int pot, float thFactor=1); + + + unsigned char* randomPattern; + + + int* gradHist; + float* ths; + float* thsSmoothed; + int thsStep; + const FrameHessian* gradHistFrame; + + // block width, and block height. + int bW, bH; + // number of blocks in x and y dimension. + int nbW, nbH; +}; + + + + +} + diff --git a/src/dso/FullSystem/ResidualProjections.h b/src/dso/FullSystem/ResidualProjections.h new file mode 100644 index 0000000..ae95454 --- /dev/null +++ b/src/dso/FullSystem/ResidualProjections.h @@ -0,0 +1,93 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "util/NumType.h" +#include "FullSystem/FullSystem.h" +#include "FullSystem/HessianBlocks.h" +#include "util/settings.h" + +namespace dso +{ + + +EIGEN_STRONG_INLINE float derive_idepth( + const Vec3f &t, const float &u, const float &v, + const int &dx, const int &dy, const float &dxInterp, + const float &dyInterp, const float &drescale) +{ + return (dxInterp*drescale * (t[0]-t[2]*u) + + dyInterp*drescale * (t[1]-t[2]*v))*SCALE_IDEPTH; +} + + + +EIGEN_STRONG_INLINE bool projectPoint( + const float &u_pt,const float &v_pt, + const float &idepth, + const Mat33f &KRKi, const Vec3f &Kt, + float &Ku, float &Kv) +{ + Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth; + Ku = ptp[0] / ptp[2]; + Kv = ptp[1] / ptp[2]; + return Ku>1.1f && Kv>1.1f && Kucxl())*HCalib->fxli(), + (v_pt+dy-HCalib->cyl())*HCalib->fyli(), + 1); + + Vec3f ptp = R * KliP + t*idepth; + drescale = 1.0f/ptp[2]; + new_idepth = idepth*drescale; + + if(!(drescale>0)) return false; + + u = ptp[0] * drescale; + v = ptp[1] * drescale; + Ku = u*HCalib->fxl() + HCalib->cxl(); + Kv = v*HCalib->fyl() + HCalib->cyl(); + + return Ku>1.1f && Kv>1.1f && Ku, +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/* + * KFBuffer.cpp + * + * Created on: Jan 7, 2014 + * Author: engelj + */ + +#include "FullSystem/FullSystem.h" + +#include "stdio.h" +#include "util/globalFuncs.h" +#include +#include +#include "IOWrapper/ImageDisplay.h" +#include "util/globalCalib.h" +#include +#include + +#include "FullSystem/ResidualProjections.h" +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +#include "FullSystem/HessianBlocks.h" + +namespace dso +{ +int PointFrameResidual::instanceCounter = 0; + + +long runningResID=0; + + +PointFrameResidual::PointFrameResidual(){assert(false); instanceCounter++;} + +PointFrameResidual::~PointFrameResidual(){assert(efResidual==0); instanceCounter--; delete J;} + +PointFrameResidual::PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_) : + point(point_), + host(host_), + target(target_) +{ + efResidual=0; + instanceCounter++; + resetOOB(); + J = new RawResidualJacobian(); + assert(((long)J)%16==0); + + isNew=true; +} + + + + +double PointFrameResidual::linearize(CalibHessian* HCalib) +{ + state_NewEnergyWithOutlier=-1; + + if(state_state == ResState::OOB) + { state_NewState = ResState::OOB; return state_energy; } + + FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]); + float energyLeft=0; + const Eigen::Vector3f* dIl = target->dI; + //const float* const Il = target->I; + const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll; + const Vec3f &PRE_KtTll = precalc->PRE_KtTll; + const Mat33f &PRE_RTll_0 = precalc->PRE_RTll_0; + const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0; + const float * const color = point->color; + const float * const weights = point->weights; + + Vec2f affLL = precalc->PRE_aff_mode; + float b0 = precalc->PRE_b0_mode; + + + Vec6f d_xi_x, d_xi_y; + Vec4f d_C_x, d_C_y; + float d_d_x, d_d_y; + { + float drescale, u, v, new_idepth; + float Ku, Kv; + Vec3f KliP; + + if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0,HCalib, + PRE_RTll_0,PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth)) + { state_NewState = ResState::OOB; return state_energy; } + + centerProjectedTo = Vec3f(Ku, Kv, new_idepth); + + + // diff d_idepth + d_d_x = drescale * (PRE_tTll_0[0]-PRE_tTll_0[2]*u)*SCALE_IDEPTH*HCalib->fxl(); + d_d_y = drescale * (PRE_tTll_0[1]-PRE_tTll_0[2]*v)*SCALE_IDEPTH*HCalib->fyl(); + + + + + // diff calib + d_C_x[2] = drescale*(PRE_RTll_0(2,0)*u-PRE_RTll_0(0,0)); + d_C_x[3] = HCalib->fxl() * drescale*(PRE_RTll_0(2,1)*u-PRE_RTll_0(0,1)) * HCalib->fyli(); + d_C_x[0] = KliP[0]*d_C_x[2]; + d_C_x[1] = KliP[1]*d_C_x[3]; + + d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0(2,0)*v-PRE_RTll_0(1,0)) * HCalib->fxli(); + d_C_y[3] = drescale*(PRE_RTll_0(2,1)*v-PRE_RTll_0(1,1)); + d_C_y[0] = KliP[0]*d_C_y[2]; + d_C_y[1] = KliP[1]*d_C_y[3]; + + d_C_x[0] = (d_C_x[0]+u)*SCALE_F; + d_C_x[1] *= SCALE_F; + d_C_x[2] = (d_C_x[2]+1)*SCALE_C; + d_C_x[3] *= SCALE_C; + + d_C_y[0] *= SCALE_F; + d_C_y[1] = (d_C_y[1]+v)*SCALE_F; + d_C_y[2] *= SCALE_C; + d_C_y[3] = (d_C_y[3]+1)*SCALE_C; + + + d_xi_x[0] = new_idepth*HCalib->fxl(); + d_xi_x[1] = 0; + d_xi_x[2] = -new_idepth*u*HCalib->fxl(); + d_xi_x[3] = -u*v*HCalib->fxl(); + d_xi_x[4] = (1+u*u)*HCalib->fxl(); + d_xi_x[5] = -v*HCalib->fxl(); + + d_xi_y[0] = 0; + d_xi_y[1] = new_idepth*HCalib->fyl(); + d_xi_y[2] = -new_idepth*v*HCalib->fyl(); + d_xi_y[3] = -(1+v*v)*HCalib->fyl(); + d_xi_y[4] = u*v*HCalib->fyl(); + d_xi_y[5] = u*HCalib->fyl(); + } + + + { + J->Jpdxi[0] = d_xi_x; + J->Jpdxi[1] = d_xi_y; + + J->Jpdc[0] = d_C_x; + J->Jpdc[1] = d_C_y; + + J->Jpdd[0] = d_d_x; + J->Jpdd[1] = d_d_y; + + } + + + + + + + float JIdxJIdx_00=0, JIdxJIdx_11=0, JIdxJIdx_10=0; + float JabJIdx_00=0, JabJIdx_01=0, JabJIdx_10=0, JabJIdx_11=0; + float JabJab_00=0, JabJab_01=0, JabJab_11=0; + + float wJI2_sum = 0; + + for(int idx=0;idxu+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv)) + { state_NewState = ResState::OOB; return state_energy; } + + projectedTo[idx][0] = Ku; + projectedTo[idx][1] = Kv; + + + Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0])); + float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]); + + + + float drdA = (color[idx]-b0); + if(!std::isfinite((float)hitColor[0])) + { state_NewState = ResState::OOB; return state_energy; } + + + float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm())); + w = 0.5f*(w + weights[idx]); + + + + float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual); + energyLeft += w*w*hw *residual*residual*(2-hw); + + { + if(hw < 1) hw = sqrtf(hw); + hw = hw*w; + + hitColor[1]*=hw; + hitColor[2]*=hw; + + J->resF[idx] = residual*hw; + + J->JIdx[0][idx] = hitColor[1]; + J->JIdx[1][idx] = hitColor[2]; + J->JabF[0][idx] = drdA*hw; + J->JabF[1][idx] = hw; + + JIdxJIdx_00+=hitColor[1]*hitColor[1]; + JIdxJIdx_11+=hitColor[2]*hitColor[2]; + JIdxJIdx_10+=hitColor[1]*hitColor[2]; + + JabJIdx_00+= drdA*hw * hitColor[1]; + JabJIdx_01+= drdA*hw * hitColor[2]; + JabJIdx_10+= hw * hitColor[1]; + JabJIdx_11+= hw * hitColor[2]; + + JabJab_00+= drdA*drdA*hw*hw; + JabJab_01+= drdA*hw*hw; + JabJab_11+= hw*hw; + + + wJI2_sum += hw*hw*(hitColor[1]*hitColor[1]+hitColor[2]*hitColor[2]); + + if(setting_affineOptModeA < 0) J->JabF[0][idx]=0; + if(setting_affineOptModeB < 0) J->JabF[1][idx]=0; + + } + } + + J->JIdx2(0,0) = JIdxJIdx_00; + J->JIdx2(0,1) = JIdxJIdx_10; + J->JIdx2(1,0) = JIdxJIdx_10; + J->JIdx2(1,1) = JIdxJIdx_11; + J->JabJIdx(0,0) = JabJIdx_00; + J->JabJIdx(0,1) = JabJIdx_01; + J->JabJIdx(1,0) = JabJIdx_10; + J->JabJIdx(1,1) = JabJIdx_11; + J->Jab2(0,0) = JabJab_00; + J->Jab2(0,1) = JabJab_01; + J->Jab2(1,0) = JabJab_01; + J->Jab2(1,1) = JabJab_11; + + state_NewEnergyWithOutlier = energyLeft; + + if(energyLeft > std::max(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2) + { + energyLeft = std::max(host->frameEnergyTH, target->frameEnergyTH); + state_NewState = ResState::OUTLIER; + } + else + { + state_NewState = ResState::IN; + } + + state_NewEnergy = energyLeft; + return energyLeft; +} + + + +void PointFrameResidual::debugPlot() +{ + if(state_state==ResState::OOB) return; + Vec3b cT = Vec3b(0,0,0); + + if(freeDebugParam5==0) + { + float rT = 20*sqrt(state_energy/9); + if(rT<0) rT=0; if(rT>255)rT=255; + cT = Vec3b(0,255-rT,rT); + } + else + { + if(state_state == ResState::IN) cT = Vec3b(255,0,0); + else if(state_state == ResState::OOB) cT = Vec3b(255,255,0); + else if(state_state == ResState::OUTLIER) cT = Vec3b(0,0,255); + else cT = Vec3b(255,255,255); + } + + for(int i=0;i 2 && projectedTo[i][1] > 2 && projectedTo[i][0] < wG[0]-3 && projectedTo[i][1] < hG[0]-3 )) + target->debugImage->setPixel1((float)projectedTo[i][0], (float)projectedTo[i][1],cT); + } +} + + + +void PointFrameResidual::applyRes(bool copyJacobians) +{ + if(copyJacobians) + { + if(state_state == ResState::OOB) + { + assert(!efResidual->isActiveAndIsGoodNEW); + return; // can never go back from OOB + } + if(state_NewState == ResState::IN)// && ) + { + efResidual->isActiveAndIsGoodNEW=true; + efResidual->takeDataF(); + } + else + { + efResidual->isActiveAndIsGoodNEW=false; + } + } + + setState(state_NewState); + state_energy = state_NewEnergy; +} +} diff --git a/src/dso/FullSystem/Residuals.h b/src/dso/FullSystem/Residuals.h new file mode 100644 index 0000000..73e277a --- /dev/null +++ b/src/dso/FullSystem/Residuals.h @@ -0,0 +1,106 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/globalCalib.h" +#include "vector" + +#include "util/NumType.h" +#include +#include +#include "util/globalFuncs.h" +#include "OptimizationBackend/RawResidualJacobian.h" + +namespace dso +{ +class PointHessian; +class FrameHessian; +class CalibHessian; + +class EFResidual; + + +enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE}; +enum ResState {IN=0, OOB, OUTLIER}; + +struct FullJacRowT +{ + Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; +}; + +class PointFrameResidual +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EFResidual* efResidual; + + static int instanceCounter; + + + ResState state_state; + double state_energy; + ResState state_NewState; + double state_NewEnergy; + double state_NewEnergyWithOutlier; + + + void setState(ResState s) {state_state = s;} + + + PointHessian* point; + FrameHessian* host; + FrameHessian* target; + RawResidualJacobian* J; + + + bool isNew; + + + Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; + Vec3f centerProjectedTo; + + ~PointFrameResidual(); + PointFrameResidual(); + PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_); + double linearize(CalibHessian* HCalib); + + + void resetOOB() + { + state_NewEnergy = state_energy = 0; + state_NewState = ResState::OUTLIER; + + setState(ResState::IN); + }; + void applyRes( bool copyJacobians); + + void debugPlot(); + + void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, int res); +}; +} + diff --git a/src/dso/IOWrapper/ImageDisplay.h b/src/dso/IOWrapper/ImageDisplay.h new file mode 100644 index 0000000..974324d --- /dev/null +++ b/src/dso/IOWrapper/ImageDisplay.h @@ -0,0 +1,55 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#pragma once +#include +#include "util/NumType.h" +#include "util/MinimalImage.h" + + +namespace dso +{ + +namespace IOWrap +{ + +void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize = false); +void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize = false); +void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize = false); +void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize = false); +void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize = false); + + +void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); +void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); +void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); +void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); + +int waitKey(int milliseconds); +void closeAllWindows(); + +} + +} diff --git a/src/dso/IOWrapper/ImageDisplay_dummy.cpp b/src/dso/IOWrapper/ImageDisplay_dummy.cpp new file mode 100644 index 0000000..64820d0 --- /dev/null +++ b/src/dso/IOWrapper/ImageDisplay_dummy.cpp @@ -0,0 +1,50 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "IOWrapper/ImageDisplay.h" + +namespace dso +{ + + +namespace IOWrap +{ +void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) {}; +void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) {}; +void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) {}; +void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) {}; +void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) {}; + + +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; + +int waitKey(int milliseconds) {return 0;}; +void closeAllWindows() {}; +} + +} diff --git a/src/dso/IOWrapper/ImageRW.h b/src/dso/IOWrapper/ImageRW.h new file mode 100644 index 0000000..ee1c33b --- /dev/null +++ b/src/dso/IOWrapper/ImageRW.h @@ -0,0 +1,48 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#pragma once +#include "util/NumType.h" +#include "util/MinimalImage.h" + +namespace dso +{ +namespace IOWrap +{ + +MinimalImageB* readImageBW_8U(std::string filename); +MinimalImageB3* readImageRGB_8U(std::string filename); +MinimalImage* readImageBW_16U(std::string filename); + + +MinimalImageB* readStreamBW_8U(char* data, int numBytes); + +void writeImage(std::string filename, MinimalImageB* img); +void writeImage(std::string filename, MinimalImageB3* img); +void writeImage(std::string filename, MinimalImageF* img); +void writeImage(std::string filename, MinimalImageF3* img); + +} +} diff --git a/src/dso/IOWrapper/ImageRW_dummy.cpp b/src/dso/IOWrapper/ImageRW_dummy.cpp new file mode 100644 index 0000000..a554733 --- /dev/null +++ b/src/dso/IOWrapper/ImageRW_dummy.cpp @@ -0,0 +1,46 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "IOWrapper/ImageRW.h" + +namespace dso +{ + + +namespace IOWrap +{ + +MinimalImageB* readImageBW_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; +MinimalImageB3* readImageRGB_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; +MinimalImage* readImageBW_16U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; +MinimalImageB* readStreamBW_8U(char* data, int numBytes) {printf("not implemented. bye!\n"); return 0;}; +void writeImage(std::string filename, MinimalImageB* img) {}; +void writeImage(std::string filename, MinimalImageB3* img) {}; +void writeImage(std::string filename, MinimalImageF* img) {}; +void writeImage(std::string filename, MinimalImageF3* img) {}; + +} + +} diff --git a/src/dso/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp b/src/dso/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp new file mode 100644 index 0000000..61a78a4 --- /dev/null +++ b/src/dso/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp @@ -0,0 +1,197 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "IOWrapper/ImageDisplay.h" + +#include + +#include +#include + +#include + +#include "util/settings.h" + +namespace dso +{ + + +namespace IOWrap +{ + +std::unordered_set openWindows; +boost::mutex openCVdisplayMutex; + + + +void displayImage(const char* windowName, const cv::Mat& image, bool autoSize) +{ + if(disableAllDisplay) return; + + boost::unique_lock lock(openCVdisplayMutex); + if(!autoSize) + { + if(openWindows.find(windowName) == openWindows.end()) + { + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, image.cols, image.rows); + openWindows.insert(windowName); + } + } + cv::imshow(windowName, image); +} + + +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) +{ + if(disableAllDisplay) return; + if(images.size() == 0) return; + + // get dimensions. + int w = images[0]->cols; + int h = images[0]->rows; + + int num = std::max((int)setting_maxFrames, (int)images.size()); + + // get optimal dimensions. + int bestCC = 0; + float bestLoss = 1e10; + for(int cc=1;cc<10;cc++) + { + int ww = w * cc; + int hh = h * ((num+cc-1)/cc); + + + float wLoss = ww/16.0f; + float hLoss = hh/10.0f; + float loss = std::max(wLoss, hLoss); + + if(loss < bestLoss) + { + bestLoss = loss; + bestCC = cc; + } + } + + int bestRC = ((num+bestCC-1)/bestCC); + if(cc != 0) + { + bestCC = cc; + bestRC= rc; + } + cv::Mat stitch = cv::Mat(bestRC*h, bestCC*w, images[0]->type()); + stitch.setTo(0); + for(int i=0;i<(int)images.size() && i < bestCC*bestRC;i++) + { + int c = i%bestCC; + int r = i/bestCC; + + cv::Mat roi = stitch(cv::Rect(c*w, r*h, w,h)); + images[i]->copyTo(roi); + } + displayImage(windowName, stitch, false); +} + + + +void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) +{ + displayImage(windowName, cv::Mat(img->h, img->w, CV_8U, img->data), autoSize); +} +void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) +{ + displayImage(windowName, cv::Mat(img->h, img->w, CV_8UC3, img->data), autoSize); +} +void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) +{ + displayImage(windowName, cv::Mat(img->h, img->w, CV_32F, img->data)*(1/254.0f), autoSize); +} +void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) +{ + displayImage(windowName, cv::Mat(img->h, img->w, CV_32FC3, img->data)*(1/254.0f), autoSize); +} +void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) +{ + displayImage(windowName, cv::Mat(img->h, img->w, CV_16U, img->data), autoSize); +} + + +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) +{ + std::vector imagesCV; + for(size_t i=0; i < images.size();i++) + imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8U, images[i]->data)); + displayImageStitch(windowName, imagesCV, cc, rc); + for(size_t i=0; i < images.size();i++) + delete imagesCV[i]; +} +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) +{ + std::vector imagesCV; + for(size_t i=0; i < images.size();i++) + imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8UC3, images[i]->data)); + displayImageStitch(windowName, imagesCV, cc, rc); + for(size_t i=0; i < images.size();i++) + delete imagesCV[i]; +} +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) +{ + std::vector imagesCV; + for(size_t i=0; i < images.size();i++) + imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32F, images[i]->data)); + displayImageStitch(windowName, imagesCV, cc, rc); + for(size_t i=0; i < images.size();i++) + delete imagesCV[i]; +} +void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) +{ + std::vector imagesCV; + for(size_t i=0; i < images.size();i++) + imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32FC3, images[i]->data)); + displayImageStitch(windowName, imagesCV, cc, rc); + for(size_t i=0; i < images.size();i++) + delete imagesCV[i]; +} + + + +int waitKey(int milliseconds) +{ + if(disableAllDisplay) return 0; + + boost::unique_lock lock(openCVdisplayMutex); + return cv::waitKey(milliseconds); +} + +void closeAllWindows() +{ + if(disableAllDisplay) return; + boost::unique_lock lock(openCVdisplayMutex); + cv::destroyAllWindows(); + openWindows.clear(); +} +} + +} diff --git a/src/dso/IOWrapper/OpenCV/ImageRW_OpenCV.cpp b/src/dso/IOWrapper/OpenCV/ImageRW_OpenCV.cpp new file mode 100644 index 0000000..ea26e37 --- /dev/null +++ b/src/dso/IOWrapper/OpenCV/ImageRW_OpenCV.cpp @@ -0,0 +1,130 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "IOWrapper/ImageRW.h" +#include + + +namespace dso +{ + +namespace IOWrap +{ +MinimalImageB* readImageBW_8U(std::string filename) +{ + cv::Mat m = cv::imread(filename, cv::IMREAD_GRAYSCALE); + if(m.rows*m.cols==0) + { + printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); + return 0; + } + if(m.type() != CV_8U) + { + printf("cv::imread did something strange! this may segfault. \n"); + return 0; + } + MinimalImageB* img = new MinimalImageB(m.cols, m.rows); + memcpy(img->data, m.data, m.rows*m.cols); + return img; +} + +MinimalImageB3* readImageRGB_8U(std::string filename) +{ + cv::Mat m = cv::imread(filename, cv::IMREAD_COLOR); + if(m.rows*m.cols==0) + { + printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); + return 0; + } + if(m.type() != CV_8UC3) + { + printf("cv::imread did something strange! this may segfault. \n"); + return 0; + } + MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows); + memcpy(img->data, m.data, 3*m.rows*m.cols); + return img; +} + +MinimalImage* readImageBW_16U(std::string filename) +{ + cv::Mat m = cv::imread(filename, cv::IMREAD_UNCHANGED); + if(m.rows*m.cols==0) + { + printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); + return 0; + } + if(m.type() != CV_16U) + { + printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); + return 0; + } + MinimalImage* img = new MinimalImage(m.cols, m.rows); + memcpy(img->data, m.data, 2*m.rows*m.cols); + return img; +} + +MinimalImageB* readStreamBW_8U(char* data, int numBytes) +{ + cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), cv::IMREAD_GRAYSCALE); + if(m.rows*m.cols==0) + { + printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes); + return 0; + } + if(m.type() != CV_8U) + { + printf("cv::imdecode did something strange! this may segfault. \n"); + return 0; + } + MinimalImageB* img = new MinimalImageB(m.cols, m.rows); + memcpy(img->data, m.data, m.rows*m.cols); + return img; +} + + + +void writeImage(std::string filename, MinimalImageB* img) +{ + cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data)); +} +void writeImage(std::string filename, MinimalImageB3* img) +{ + cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data)); +} +void writeImage(std::string filename, MinimalImageF* img) +{ + cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data)); +} +void writeImage(std::string filename, MinimalImageF3* img) +{ + cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data)); +} + +} + +} diff --git a/src/dso/IOWrapper/Output3DWrapper.h b/src/dso/IOWrapper/Output3DWrapper.h new file mode 100644 index 0000000..32f1366 --- /dev/null +++ b/src/dso/IOWrapper/Output3DWrapper.h @@ -0,0 +1,238 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + + +#pragma once +#include +#include + +#include "util/NumType.h" +#include "util/MinimalImage.h" +#include "map" + +namespace cv { + class Mat; +} + +namespace dmvio +{ +class TransformDSOToIMU; +} + +namespace dso +{ + +class FrameHessian; +class CalibHessian; +class FrameShell; + +namespace IOWrap +{ +/* ======================= Additional information for DM-VIO: =============== + * + * All poses published are still in DSO frame, that means they have an arbitrary scale instead of the metric scale. + * If you are interested in metrically scaled poses you should additionally implement [publishTransformDSOToIMU] + * It returns an object of type TransformDSOToIMU which can be used to transform poses from DSO frame to IMU frame + * (with the method transformPose). + * Note that you need to pass the method transformPose DSO poses in worldToCam, not camToWorld! + * + */ + +/* ======================= Some typical usecases: =============== + * + * (1) always get the pose of the most recent frame: + * -> Implement [publishCamPose]. + * + * (2) always get the depthmap of the most recent keyframe + * -> Implement [pushDepthImageFloat] (use inverse depth in [image], and pose / frame information from [KF]). + * + * (3) accumulate final model + * -> Implement [publishKeyframes] (skip for final!=false), and accumulate frames. + * + * (4) get evolving model in real-time + * -> Implement [publishKeyframes] (update all frames for final==false). + * + * + * + * + * ==================== How to use the structs: =================== + * [FrameShell]: minimal struct kept for each frame ever tracked. + * ->camToWorld = camera to world transformation + * ->poseValid = false if [camToWorld] is invalid (only happens for frames during initialization). + * ->trackingRef = Shell of the frame this frame was tracked on. + * ->id = ID of that frame, starting with 0 for the very first frame. + * + * ->incoming_id = ID passed into [addActiveFrame( ImageAndExposure* image, int id )]. + * ->timestamp = timestamp passed into [addActiveFrame( ImageAndExposure* image, int id )] as image.timestamp. + * + * [FrameHessian] + * ->immaturePoints: contains points that have not been "activated" (they do however have a depth initialization). + * ->pointHessians: contains active points. + * ->pointHessiansMarginalized: contains marginalized points. + * ->pointHessiansOut: contains outlier points. + * + * ->frameID: incremental ID for keyframes only. + * ->shell: corresponding [FrameShell] struct. + * + * + * [CalibHessian] + * ->fxl(), fyl(), cxl(), cyl(): get optimized, most recent (pinhole) camera intrinsics. + * + * + * [PointHessian] + * ->u,v: pixel-coordinates of point. + * ->idepth_scaled: inverse depth of point. + * DO NOT USE [idepth], since it may be scaled with [SCALE_IDEPTH] ... however that is currently set to 1 so never mind. + * ->host: pointer to host-frame of point. + * ->status: current status of point (ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED) + * ->numGoodResiduals: number of non-outlier residuals supporting this point (approximate). + * ->maxRelBaseline: value roughly proportional to the relative baseline this point was observed with (0 = no baseline). + * points for which this value is low are badly contrained. + * ->idepth_hessian: hessian value (inverse variance) of inverse depth. + * + * [ImmaturePoint] + * ->u,v: pixel-coordinates of point. + * ->idepth_min, idepth_max: the initialization sais that the inverse depth of this point is very likely + * between these two thresholds (their mean being the best guess) + * ->host: pointer to host-frame of point. + */ + + + + + + + +class Output3DWrapper +{ +public: + Output3DWrapper() {} + virtual ~Output3DWrapper() {} + + + /* + * Usage: + * Called once after each keyframe is optimized and passes the new transformation from DSO frame (worldToCam in + * DSO scale) to metric frame (imuToWorld in metric scale). + * Use transformPose of the passed object to transform poses between the frames. + * Note that the object should not be used any more after the method returns. + * The caller can create copy however , preferable with the following constructor (as otherwise the shared_ptrs will be kept). + * TransformDSOToIMU(TransformDSOToIMU& other, std::shared_ptr optScale, std::shared_ptr optGravity, std::shared_ptr optT_cam_imu); + */ + virtual void publishTransformDSOToIMU(const dmvio::TransformDSOToIMU& transformDSOToIMU) {} + + + /* Usage: + * Called once after each new Keyframe is inserted & optimized. + * [connectivity] contains for each frame-frame pair the number of [0] active residuals in between them, + * and [1] the number of marginalized reisduals between them. + * frame-frame pairs are encoded as HASH_IDX = [(int)hostFrameKFID << 32 + (int)targetFrameKFID]. + * the [***frameKFID] used for hashing correspond to the [FrameHessian]->frameID. + * + * Calling: + * Always called, no overhead if not used. + */ + virtual void publishGraph(const std::map, Eigen::aligned_allocator > > &connectivity) {} + + + + + + /* Usage: + * Called after each new Keyframe is inserted & optimized, with all keyframes that were part of the active window during + * that optimization in [frames] (with final=false). Use to access the new frame pose and points. + * Also called just before a frame is marginalized (with final=true) with only that frame in [frames]; at that point, + * no further updates will ever occur to it's optimized values (pose & idepth values of it's points). + * + * If you want always all most recent values for all frames, just use the [final=false] calls. + * If you only want to get the final model, but don't care about it being delay-free, only use the + * [final=true] calls, to save compute. + * + * Calling: + * Always called, negligible overhead if not used. + */ + virtual void publishKeyframes(std::vector &frames, bool final, CalibHessian* HCalib) {} + + + + + + /* Usage: + * Called once for each tracked frame, with the real-time, low-delay frame pose. + * + * Calling: + * Always called, no overhead if not used. + */ + virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) {} + + + + + + /* Usage: + * Called once for each new frame, before it is tracked (i.e., it doesn't have a pose yet). + * + * Calling: + * Always called, no overhead if not used. + */ + virtual void pushLiveFrame(FrameHessian* image) {} + + + + + /* called once after a new keyframe is created, with the color-coded, forward-warped inverse depthmap for that keyframe, + * which is used for initial alignment of future frames. Meant for visualization. + * + * Calling: + * Needs to prepare the depth image, so it is only called if [needPushDepthImage()] returned true. + */ + virtual void pushDepthImage(MinimalImageB3* image) {} + virtual bool needPushDepthImage() {return false;} + + + + /* Usage: + * called once after a new keyframe is created, with the forward-warped inverse depthmap for that keyframe. + * (<= 0 for pixels without inv. depth value, >0 for pixels with inv. depth value) + * + * Calling: + * Always called, almost no overhead if not used. + */ + virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) {} + + + + /* call on finish */ + virtual void join() {} + + /* call on reset */ + virtual void reset() {} + +}; + +} +} diff --git a/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h b/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h new file mode 100644 index 0000000..d7d1b1a --- /dev/null +++ b/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h @@ -0,0 +1,158 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#include "boost/thread.hpp" +#include "util/MinimalImage.h" +#include "IOWrapper/Output3DWrapper.h" + + + +#include "FullSystem/HessianBlocks.h" +#include "util/FrameShell.h" + +namespace dso +{ + +class FrameHessian; +class CalibHessian; +class FrameShell; + + +namespace IOWrap +{ + +class SampleOutputWrapper : public Output3DWrapper +{ +public: + inline SampleOutputWrapper() + { + printf("OUT: Created SampleOutputWrapper\n"); + } + + virtual ~SampleOutputWrapper() + { + printf("OUT: Destroyed SampleOutputWrapper\n"); + } + + virtual void publishGraph(const std::map, Eigen::aligned_allocator>> &connectivity) override + { + printf("OUT: got graph with %d edges\n", (int)connectivity.size()); + + int maxWrite = 5; + + for(const std::pair &p : connectivity) + { + int idHost = p.first>>32; + int idTarget = p.first & ((uint64_t)0xFFFFFFFF); + printf("OUT: Example Edge %d -> %d has %d active and %d marg residuals\n", idHost, idTarget, p.second[0], p.second[1]); + maxWrite--; + if(maxWrite==0) break; + } + } + + + + virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override + { + for(FrameHessian* f : frames) + { + printf("OUT: KF %d (%s) (id %d, tme %f): %d active, %d marginalized, %d immature points. CameraToWorld:\n", + f->frameID, + final ? "final" : "non-final", + f->shell->incoming_id, + f->shell->timestamp, + (int)f->pointHessians.size(), (int)f->pointHessiansMarginalized.size(), (int)f->immaturePoints.size()); + std::cout << f->shell->camToWorld.matrix3x4() << "\n"; + + + int maxWrite = 5; + for(PointHessian* p : f->pointHessians) + { + printf("OUT: Example Point x=%.1f, y=%.1f, idepth=%f, idepth std.dev. %f, %d inlier-residuals\n", + p->u, p->v, p->idepth_scaled, sqrt(1.0f / p->idepth_hessian), p->numGoodResiduals ); + maxWrite--; + if(maxWrite==0) break; + } + } + } + + virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override + { + printf("OUT: Current Frame %d (time %f, internal ID %d). CameraToWorld:\n", + frame->incoming_id, + frame->timestamp, + frame->id); + std::cout << frame->camToWorld.matrix3x4() << "\n"; + } + + + virtual void pushLiveFrame(FrameHessian* image) override + { + // can be used to get the raw image / intensity pyramid. + } + + virtual void pushDepthImage(MinimalImageB3* image) override + { + // can be used to get the raw image with depth overlay. + } + virtual bool needPushDepthImage() override + { + return false; + } + + virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) override + { + printf("OUT: Predicted depth for KF %d (id %d, time %f, internal frame-ID %d). CameraToWorld:\n", + KF->frameID, + KF->shell->incoming_id, + KF->shell->timestamp, + KF->shell->id); + std::cout << KF->shell->camToWorld.matrix3x4() << "\n"; + + int maxWrite = 5; + for(int y=0;yh;y++) + { + for(int x=0;xw;x++) + { + if(image->at(x,y) <= 0) continue; + + printf("OUT: Example Idepth at pixel (%d,%d): %f.\n", x,y,image->at(x,y)); + maxWrite--; + if(maxWrite==0) break; + } + if(maxWrite==0) break; + } + } + + +}; + + + +} + + + +} diff --git a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp new file mode 100644 index 0000000..0890dc5 --- /dev/null +++ b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp @@ -0,0 +1,422 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include +#include "util/settings.h" + +//#include +//#include +//#include + +#include +#include "KeyFrameDisplay.h" +#include "FullSystem/HessianBlocks.h" +#include "FullSystem/ImmaturePoint.h" +#include "util/FrameShell.h" + + + +namespace dso +{ +namespace IOWrap +{ + + +KeyFrameDisplay::KeyFrameDisplay() +{ + originalInputSparse = 0; + numSparseBufferSize=0; + numSparsePoints=0; + + id = 0; + active= true; + camToWorld = SE3(); + + needRefresh=true; + + my_scaledTH =1e10; + my_absTH = 1e10; + my_displayMode = 1; + my_minRelBS = 0; + my_sparsifyFactor = 1; + + numGLBufferPoints=0; + bufferValid = false; +} +void KeyFrameDisplay::setFromF(FrameShell* frame, CalibHessian* HCalib) +{ + id = frame->id; + fx = HCalib->fxl(); + fy = HCalib->fyl(); + cx = HCalib->cxl(); + cy = HCalib->cyl(); + width = wG[0]; + height = hG[0]; + fxi = 1/fx; + fyi = 1/fy; + cxi = -cx / fx; + cyi = -cy / fy; + camToWorld = frame->camToWorld; + needRefresh=true; +} + +void KeyFrameDisplay::setFromPose(const Sophus::SE3& pose, CalibHessian *HCalib) +{ + id = 0; + fx = HCalib->fxl(); + fy = HCalib->fyl(); + cx = HCalib->cxl(); + cy = HCalib->cyl(); + width = wG[0]; + height = hG[0]; + fxi = 1/fx; + fyi = 1/fy; + cxi = -cx / fx; + cyi = -cy / fy; + camToWorld = pose; + needRefresh=true; +} + +void KeyFrameDisplay::setFromKF(FrameHessian* fh, CalibHessian* HCalib) +{ + setFromF(fh->shell, HCalib); + + // add all traces, inlier and outlier points. + int npoints = fh->immaturePoints.size() + + fh->pointHessians.size() + + fh->pointHessiansMarginalized.size() + + fh->pointHessiansOut.size(); + + if(numSparseBufferSize < npoints) + { + if(originalInputSparse != 0) delete originalInputSparse; + numSparseBufferSize = npoints+100; + originalInputSparse = new InputPointSparse[numSparseBufferSize]; + } + + InputPointSparse* pc = originalInputSparse; + numSparsePoints=0; + for(ImmaturePoint* p : fh->immaturePoints) + { + for(int i=0;icolor[i]; + + pc[numSparsePoints].u = p->u; + pc[numSparsePoints].v = p->v; + pc[numSparsePoints].idpeth = (p->idepth_max+p->idepth_min)*0.5f; + pc[numSparsePoints].idepth_hessian = 1000; + pc[numSparsePoints].relObsBaseline = 0; + pc[numSparsePoints].numGoodRes = 1; + pc[numSparsePoints].status = 0; + numSparsePoints++; + } + + for(PointHessian* p : fh->pointHessians) + { + for(int i=0;icolor[i]; + pc[numSparsePoints].u = p->u; + pc[numSparsePoints].v = p->v; + pc[numSparsePoints].idpeth = p->idepth_scaled; + pc[numSparsePoints].relObsBaseline = p->maxRelBaseline; + pc[numSparsePoints].idepth_hessian = p->idepth_hessian; + pc[numSparsePoints].numGoodRes = 0; + pc[numSparsePoints].status=1; + + numSparsePoints++; + } + + for(PointHessian* p : fh->pointHessiansMarginalized) + { + for(int i=0;icolor[i]; + pc[numSparsePoints].u = p->u; + pc[numSparsePoints].v = p->v; + pc[numSparsePoints].idpeth = p->idepth_scaled; + pc[numSparsePoints].relObsBaseline = p->maxRelBaseline; + pc[numSparsePoints].idepth_hessian = p->idepth_hessian; + pc[numSparsePoints].numGoodRes = 0; + pc[numSparsePoints].status=2; + numSparsePoints++; + } + + for(PointHessian* p : fh->pointHessiansOut) + { + for(int i=0;icolor[i]; + pc[numSparsePoints].u = p->u; + pc[numSparsePoints].v = p->v; + pc[numSparsePoints].idpeth = p->idepth_scaled; + pc[numSparsePoints].relObsBaseline = p->maxRelBaseline; + pc[numSparsePoints].idepth_hessian = p->idepth_hessian; + pc[numSparsePoints].numGoodRes = 0; + pc[numSparsePoints].status=3; + numSparsePoints++; + } + assert(numSparsePoints <= npoints); + + camToWorld = fh->PRE_camToWorld; + needRefresh=true; +} + + +KeyFrameDisplay::~KeyFrameDisplay() +{ + if(originalInputSparse != 0) + delete[] originalInputSparse; +} + +bool KeyFrameDisplay::refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity) +{ + if(canRefresh) + { + needRefresh = needRefresh || + my_scaledTH != scaledTH || + my_absTH != absTH || + my_displayMode != mode || + my_minRelBS != minBS || + my_sparsifyFactor != sparsity; + } + + if(!needRefresh) return false; + needRefresh=false; + + my_scaledTH = scaledTH; + my_absTH = absTH; + my_displayMode = mode; + my_minRelBS = minBS; + my_sparsifyFactor = sparsity; + + + // if there are no vertices, done! + if(numSparsePoints == 0) + return false; + + // make data + Vec3f* tmpVertexBuffer = new Vec3f[numSparsePoints*patternNum]; + Vec3b* tmpColorBuffer = new Vec3b[numSparsePoints*patternNum]; + int vertexBufferNumPoints=0; + + for(int i=0;i2) continue; + + if(originalInputSparse[i].idpeth < 0) continue; + + + float depth = (1.0f / originalInputSparse[i].idpeth); + float depth4 = depth*depth; depth4*= depth4; + float var = (1.0f / (originalInputSparse[i].idepth_hessian+0.01)); + + if(var * depth4 > my_scaledTH) + continue; + + if(var > my_absTH) + continue; + + if(originalInputSparse[i].relObsBaseline < my_minRelBS) + continue; + + + for(int pnt=0;pnt 1 && rand()%my_sparsifyFactor != 0) continue; + int dx = patternP[pnt][0]; + int dy = patternP[pnt][1]; + + tmpVertexBuffer[vertexBufferNumPoints][0] = ((originalInputSparse[i].u+dx)*fxi + cxi) * depth; + tmpVertexBuffer[vertexBufferNumPoints][1] = ((originalInputSparse[i].v+dy)*fyi + cyi) * depth; + tmpVertexBuffer[vertexBufferNumPoints][2] = depth*(1 + 2*fxi * (rand()/(float)RAND_MAX-0.5f)); + + + + if(my_displayMode==0) + { + if(originalInputSparse[i].status==0) + { + tmpColorBuffer[vertexBufferNumPoints][0] = 0; + tmpColorBuffer[vertexBufferNumPoints][1] = 255; + tmpColorBuffer[vertexBufferNumPoints][2] = 255; + } + else if(originalInputSparse[i].status==1) + { + tmpColorBuffer[vertexBufferNumPoints][0] = 0; + tmpColorBuffer[vertexBufferNumPoints][1] = 255; + tmpColorBuffer[vertexBufferNumPoints][2] = 0; + } + else if(originalInputSparse[i].status==2) + { + tmpColorBuffer[vertexBufferNumPoints][0] = 0; + tmpColorBuffer[vertexBufferNumPoints][1] = 0; + tmpColorBuffer[vertexBufferNumPoints][2] = 255; + } + else if(originalInputSparse[i].status==3) + { + tmpColorBuffer[vertexBufferNumPoints][0] = 255; + tmpColorBuffer[vertexBufferNumPoints][1] = 0; + tmpColorBuffer[vertexBufferNumPoints][2] = 0; + } + else + { + tmpColorBuffer[vertexBufferNumPoints][0] = 255; + tmpColorBuffer[vertexBufferNumPoints][1] = 255; + tmpColorBuffer[vertexBufferNumPoints][2] = 255; + } + + } + else + { + tmpColorBuffer[vertexBufferNumPoints][0] = originalInputSparse[i].color[pnt]; + tmpColorBuffer[vertexBufferNumPoints][1] = originalInputSparse[i].color[pnt]; + tmpColorBuffer[vertexBufferNumPoints][2] = originalInputSparse[i].color[pnt]; + } + vertexBufferNumPoints++; + + + assert(vertexBufferNumPoints <= numSparsePoints*patternNum); + } + } + + if(vertexBufferNumPoints==0) + { + delete[] tmpColorBuffer; + delete[] tmpVertexBuffer; + return true; + } + + numGLBufferGoodPoints = vertexBufferNumPoints; + if(numGLBufferGoodPoints > numGLBufferPoints) + { + numGLBufferPoints = vertexBufferNumPoints*1.3; + vertexBuffer.Reinitialise(pangolin::GlArrayBuffer, numGLBufferPoints, GL_FLOAT, 3, GL_DYNAMIC_DRAW ); + colorBuffer.Reinitialise(pangolin::GlArrayBuffer, numGLBufferPoints, GL_UNSIGNED_BYTE, 3, GL_DYNAMIC_DRAW ); + } + vertexBuffer.Upload(tmpVertexBuffer, sizeof(float)*3*numGLBufferGoodPoints, 0); + colorBuffer.Upload(tmpColorBuffer, sizeof(unsigned char)*3*numGLBufferGoodPoints, 0); + bufferValid=true; + delete[] tmpColorBuffer; + delete[] tmpVertexBuffer; + + + return true; +} + + + +void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor) +{ + if(width == 0) + return; + + float sz=sizeFactor; + + glPushMatrix(); + + Sophus::Matrix4f m = camToWorld.matrix().cast(); + glMultMatrixf((GLfloat*)m.data()); + + if(color == 0) + { + glColor3f(1,0,0); + } + else + glColor3f(color[0],color[1],color[2]); + + glLineWidth(lineWidth); + glBegin(GL_LINES); + glVertex3f(0,0,0); + glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz); + glVertex3f(0,0,0); + glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz); + glVertex3f(0,0,0); + glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz); + glVertex3f(0,0,0); + glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz); + + glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz); + glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz); + + glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz); + glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz); + + glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz); + glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz); + + glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz); + glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz); + + glEnd(); + glPopMatrix(); +} + + +void KeyFrameDisplay::drawPC(float pointSize) +{ + + if(!bufferValid || numGLBufferGoodPoints==0) + return; + + + glDisable(GL_LIGHTING); + + glPushMatrix(); + + Sophus::Matrix4f m = camToWorld.matrix().cast(); + glMultMatrixf((GLfloat*)m.data()); + + glPointSize(pointSize); + + + colorBuffer.Bind(); + glColorPointer(colorBuffer.count_per_element, colorBuffer.datatype, 0, 0); + glEnableClientState(GL_COLOR_ARRAY); + + vertexBuffer.Bind(); + glVertexPointer(vertexBuffer.count_per_element, vertexBuffer.datatype, 0, 0); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, numGLBufferGoodPoints); + glDisableClientState(GL_VERTEX_ARRAY); + vertexBuffer.Unbind(); + + glDisableClientState(GL_COLOR_ARRAY); + colorBuffer.Unbind(); + + glPopMatrix(); +} + +} +} diff --git a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h new file mode 100644 index 0000000..cadf66e --- /dev/null +++ b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h @@ -0,0 +1,127 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#undef Success +#include +#include "util/NumType.h" +#include + +#include +#include + +namespace dso +{ +class CalibHessian; +class FrameHessian; +class FrameShell; + +namespace IOWrap +{ + +template +struct InputPointSparse +{ + float u; + float v; + float idpeth; + float idepth_hessian; + float relObsBaseline; + int numGoodRes; + unsigned char color[ppp]; + unsigned char status; +}; + +struct MyVertex +{ + float point[3]; + unsigned char color[4]; +}; + +// stores a pointcloud associated to a Keyframe. +class KeyFrameDisplay +{ + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + KeyFrameDisplay(); + ~KeyFrameDisplay(); + + // copies points from KF over to internal buffer, + // keeping some additional information so we can render it differently. + void setFromKF(FrameHessian* fh, CalibHessian* HCalib); + + // copies points from KF over to internal buffer, + // keeping some additional information so we can render it differently. + void setFromF(FrameShell* fs, CalibHessian* HCalib); + + void setFromPose(const Sophus::SE3& pose, CalibHessian* HCalib); + + // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. + bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); + + // renders cam & pointcloud. + void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1); + void drawPC(float pointSize); + + int id; + bool active; + SE3 camToWorld; + + inline bool operator < (const KeyFrameDisplay& other) const + { + return (id < other.id); + } + + +private: + float fx,fy,cx,cy; + float fxi,fyi,cxi,cyi; + int width, height; + + float my_scaledTH, my_absTH, my_scale; + int my_sparsifyFactor; + int my_displayMode; + float my_minRelBS; + bool needRefresh; + + + int numSparsePoints; + int numSparseBufferSize; + InputPointSparse* originalInputSparse; + + + bool bufferValid; + int numGLBufferPoints; + int numGLBufferGoodPoints; + pangolin::GlBuffer vertexBuffer; + pangolin::GlBuffer colorBuffer; +}; + +} +} + diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp new file mode 100644 index 0000000..cce378f --- /dev/null +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp @@ -0,0 +1,631 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "PangolinDSOViewer.h" +#include "KeyFrameDisplay.h" + +#include "util/settings.h" +#include "util/globalCalib.h" +#include "FullSystem/HessianBlocks.h" +#include "FullSystem/FullSystem.h" +#include "FullSystem/ImmaturePoint.h" + +namespace dso +{ +namespace IOWrap +{ + + + +PangolinDSOViewer::PangolinDSOViewer(int w, int h, bool startRunThread, std::shared_ptr settingsUtilPassed) + : HCalib(0), settingsUtil(std::move(settingsUtilPassed)) +{ + this->w = w; + this->h = h; + running=true; + + + { + boost::unique_lock lk(openImagesMutex); + internalVideoImg = new MinimalImageB3(w,h); + internalKFImg = new MinimalImageB3(w,h); + internalResImg = new MinimalImageB3(w,h); + videoImgChanged=kfImgChanged=resImgChanged=true; + + internalVideoImg->setBlack(); + internalKFImg->setBlack(); + internalResImg->setBlack(); + } + + + { + currentCam = new KeyFrameDisplay(); + currentGTCam = new KeyFrameDisplay(); + } + + needReset = false; + + + if(startRunThread) + runThread = boost::thread(&PangolinDSOViewer::run, this); + +} + + +PangolinDSOViewer::~PangolinDSOViewer() +{ + close(); + if(runThread.joinable()) + runThread.join(); +} + + +void PangolinDSOViewer::run() +{ + printf("START PANGOLIN!\n"); + + pangolin::CreateWindowAndBind("Main",2*w,2*h); + const int UI_WIDTH = 180; + + glEnable(GL_DEPTH_TEST); + + // 3D visualization + pangolin::OpenGlRenderState Visualization3D_camera( + pangolin::ProjectionMatrix(w,h,400,400,w/2,h/2,0.1,1000), + pangolin::ModelViewLookAt(-0,-5,-10, 0,0,0, pangolin::AxisNegY) + ); + + pangolin::View& Visualization3D_display = pangolin::CreateDisplay() + .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -w/(float)h) + .SetHandler(new pangolin::Handler3D(Visualization3D_camera)); + + + // 3 images + pangolin::View& d_kfDepth = pangolin::Display("imgKFDepth") + .SetAspect(w/(float)h); + + pangolin::View& d_video = pangolin::Display("imgVideo") + .SetAspect(w/(float)h); + + pangolin::View& d_residual = pangolin::Display("imgResidual") + .SetAspect(w/(float)h); + + pangolin::GlTexture texKFDepth(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE); + pangolin::GlTexture texVideo(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE); + pangolin::GlTexture texResidual(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE); + + + pangolin::CreateDisplay() + .SetBounds(0.0, 0.3, pangolin::Attach::Pix(UI_WIDTH), 1.0) + .SetLayout(pangolin::LayoutEqual) + .AddDisplay(d_kfDepth) + .AddDisplay(d_video) + .AddDisplay(d_residual); + + // parameter reconfigure gui + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH)); + + pangolin::Var settings_pointCloudMode("ui.PC_mode",1,1,4,false); + + pangolin::Var settings_showKFCameras("ui.KFCam",false,true); + pangolin::Var settings_showCurrentCamera("ui.CurrCam",true,true); + pangolin::Var settings_showTrajectory("ui.Trajectory",false,true); + pangolin::Var settings_showFullTrajectory("ui.FullTrajectory",true,true); + pangolin::Var settings_showActiveConstraints("ui.ActiveConst",true,true); + pangolin::Var settings_showAllConstraints("ui.AllConst",false,true); + + + pangolin::Var settings_show3D("ui.show3D",true,true); + pangolin::Var settings_showLiveDepth("ui.showDepth",true,true); + pangolin::Var settings_showLiveVideo("ui.showVideo",true,true); + pangolin::Var settings_showLiveResidual("ui.showResidual",false,true); + + pangolin::Var settings_showFramesWindow("ui.showFramesWindow",false,true); + pangolin::Var settings_showFullTracking("ui.showFullTracking",false,true); + pangolin::Var settings_showCoarseTracking("ui.showCoarseTracking",false,true); + + + pangolin::Var settings_sparsity("ui.sparsity",1,1,20,false); + pangolin::Var settings_scaledVarTH("ui.relVarTH",0.001,1e-10,1e10, true); + pangolin::Var settings_absVarTH("ui.absVarTH",0.001,1e-10,1e10, true); + pangolin::Var settings_minRelBS("ui.minRelativeBS",0.1,0,1, false); + + + pangolin::Var settings_resetButton("ui.Reset",false,false); + + + pangolin::Var settings_nPts("ui.activePoints",setting_desiredPointDensity, 50,5000, false); + pangolin::Var settings_nCandidates("ui.pointCandidates",setting_desiredImmatureDensity, 50,5000, false); + pangolin::Var settings_nMaxFrames("ui.maxFrames",setting_maxFrames, 4,10, false); + pangolin::Var settings_kfFrequency("ui.kfFrequency",setting_kfGlobalWeight,0.1,3, false); + pangolin::Var settings_gradHistAdd("ui.minGradAdd",setting_minGradHistAdd,0,15, false); + + pangolin::Var settings_trackFps("ui.Track fps",0,0,0,false); + pangolin::Var settings_mapFps("ui.KF fps",0,0,0,false); + + + + if(settingsUtil) + { + settingsUtil->createPangolinSettings(); + } + + + // Default hooks for exiting (Esc) and fullscreen (tab). + while(running) + { + // Clear entire screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if(setting_render_display3D) + { + // Activate efficiently by object + Visualization3D_display.Activate(Visualization3D_camera); + boost::unique_lock lk3d(model3DMutex); + //pangolin::glDrawColouredCube(); + int refreshed=0; + for(KeyFrameDisplay* fh : keyframes) + { + float blue[3] = {0,0,1}; + if(this->settings_showKFCameras) fh->drawCam(1,blue,0.1); + + + refreshed += (int)(fh->refreshPC(refreshed < 10, this->settings_scaledVarTH, this->settings_absVarTH, + this->settings_pointCloudMode, this->settings_minRelBS, this->settings_sparsity)); + fh->drawPC(1); + } + if(this->settings_showCurrentCamera) currentCam->drawCam(2,0,0.2); + + float green[3] = {0,1,0}; + currentGTCam->drawCam(2, green, 0.2); + + drawConstraints(); + lk3d.unlock(); + } + + + + openImagesMutex.lock(); + if(videoImgChanged) texVideo.Upload(internalVideoImg->data,GL_BGR,GL_UNSIGNED_BYTE); + if(kfImgChanged) texKFDepth.Upload(internalKFImg->data,GL_BGR,GL_UNSIGNED_BYTE); + if(resImgChanged) texResidual.Upload(internalResImg->data,GL_BGR,GL_UNSIGNED_BYTE); + videoImgChanged=kfImgChanged=resImgChanged=false; + openImagesMutex.unlock(); + + + + + // update fps counters + { + openImagesMutex.lock(); + float sd=0; + for(float d : lastNMappingMs) sd+=d; + settings_mapFps=lastNMappingMs.size()*1000.0f / sd; + openImagesMutex.unlock(); + } + { + model3DMutex.lock(); + float sd=0; + for(float d : lastNTrackingMs) sd+=d; + settings_trackFps = lastNTrackingMs.size()*1000.0f / sd; + model3DMutex.unlock(); + } + + + if(setting_render_displayVideo) + { + d_video.Activate(); + glColor4f(1.0f,1.0f,1.0f,1.0f); + texVideo.RenderToViewportFlipY(); + } + + if(setting_render_displayDepth) + { + d_kfDepth.Activate(); + glColor4f(1.0f,1.0f,1.0f,1.0f); + texKFDepth.RenderToViewportFlipY(); + } + + if(setting_render_displayResidual) + { + d_residual.Activate(); + glColor4f(1.0f,1.0f,1.0f,1.0f); + texResidual.RenderToViewportFlipY(); + } + + + // update parameters + this->settings_pointCloudMode = settings_pointCloudMode.Get(); + + this->settings_showActiveConstraints = settings_showActiveConstraints.Get(); + this->settings_showAllConstraints = settings_showAllConstraints.Get(); + this->settings_showCurrentCamera = settings_showCurrentCamera.Get(); + this->settings_showKFCameras = settings_showKFCameras.Get(); + this->settings_showTrajectory = settings_showTrajectory.Get(); + this->settings_showFullTrajectory = settings_showFullTrajectory.Get(); + + setting_render_display3D = settings_show3D.Get(); + setting_render_displayDepth = settings_showLiveDepth.Get(); + setting_render_displayVideo = settings_showLiveVideo.Get(); + setting_render_displayResidual = settings_showLiveResidual.Get(); + + setting_render_renderWindowFrames = settings_showFramesWindow.Get(); + setting_render_plotTrackingFull = settings_showFullTracking.Get(); + setting_render_displayCoarseTrackingFull = settings_showCoarseTracking.Get(); + + + this->settings_absVarTH = settings_absVarTH.Get(); + this->settings_scaledVarTH = settings_scaledVarTH.Get(); + this->settings_minRelBS = settings_minRelBS.Get(); + this->settings_sparsity = settings_sparsity.Get(); + + setting_desiredPointDensity = settings_nPts.Get(); + setting_desiredImmatureDensity = settings_nCandidates.Get(); + setting_maxFrames = settings_nMaxFrames.Get(); + setting_kfGlobalWeight = settings_kfFrequency.Get(); + setting_minGradHistAdd = settings_gradHistAdd.Get(); + + if(settingsUtil) + { + settingsUtil->updatePangolinSettings(); + } + + + if(settings_resetButton.Get()) + { + printf("RESET!\n"); + settings_resetButton.Reset(); + setting_fullResetRequested = true; + } + + // Swap frames and Process Events + pangolin::FinishFrame(); + + + if(needReset) reset_internal(); + + if(pangolin::ShouldQuit()) + { + shouldQuitVar = true; + } + } + + + printf("QUIT Pangolin thread!\n"); + printf("So Long, and Thanks for All the Fish!\n"); +} + + +void PangolinDSOViewer::close() +{ + running = false; +} + +void PangolinDSOViewer::join() +{ + close(); + if(runThread.joinable()) + runThread.join(); + printf("JOINED Pangolin thread!\n"); +} + +void PangolinDSOViewer::reset() +{ + needReset = true; +} + +void PangolinDSOViewer::reset_internal() +{ + model3DMutex.lock(); + for(size_t i=0; isetBlack(); + internalKFImg->setBlack(); + internalResImg->setBlack(); + videoImgChanged= kfImgChanged= resImgChanged=true; + openImagesMutex.unlock(); + + needReset = false; +} + + +void PangolinDSOViewer::drawConstraints() +{ + if(settings_showAllConstraints) + { + // draw constraints + glLineWidth(1); + glBegin(GL_LINES); + + glColor3f(0,1,0); + glBegin(GL_LINES); + for(unsigned int i=0;i0 ) + { + Sophus::Vector3f t = connections[i].from->camToWorld.translation().cast(); + glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]); + t = connections[i].to->camToWorld.translation().cast(); + glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]); + } + } + glEnd(); + } + + if(settings_showActiveConstraints) + { + glLineWidth(3); + glColor3f(0,0,1); + glBegin(GL_LINES); + for(unsigned int i=0;i0) + { + Sophus::Vector3f t = connections[i].from->camToWorld.translation().cast(); + glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]); + t = connections[i].to->camToWorld.translation().cast(); + glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]); + } + } + glEnd(); + } + + if(settings_showTrajectory) + { + float colorGreen[3] = {0,1,0}; + glColor3f(colorGreen[0],colorGreen[1],colorGreen[2]); + glLineWidth(3); + + glBegin(GL_LINE_STRIP); + for(unsigned int i=0;icamToWorld.translation()[0], + (float)keyframes[i]->camToWorld.translation()[1], + (float)keyframes[i]->camToWorld.translation()[2]); + } + glEnd(); + } + + if(settings_showFullTrajectory) + { + float colorRed[3] = {1,0,0}; + glColor3f(colorRed[0],colorRed[1],colorRed[2]); + glLineWidth(3); + + glBegin(GL_LINE_STRIP); + for(unsigned int i=0;i, Eigen::aligned_allocator>> &connectivity) +{ + if(!setting_render_display3D) return; + if(disableAllDisplay) return; + + model3DMutex.lock(); + connections.resize(connectivity.size()); + int runningID=0; + int totalActFwd=0, totalActBwd=0, totalMargFwd=0, totalMargBwd=0; + for(std::pair p : connectivity) + { + int host = (int)(p.first >> 32); + int target = (int)(p.first & (uint64_t)0xFFFFFFFF); + + assert(host >= 0 && target >= 0); + if(host == target) + { + assert(p.second[0] == 0 && p.second[1] == 0); + continue; + } + + if(host > target) continue; + + connections[runningID].from = keyframesByKFID.count(host) == 0 ? 0 : keyframesByKFID[host]; + connections[runningID].to = keyframesByKFID.count(target) == 0 ? 0 : keyframesByKFID[target]; + connections[runningID].fwdAct = p.second[0]; + connections[runningID].fwdMarg = p.second[1]; + totalActFwd += p.second[0]; + totalMargFwd += p.second[1]; + + uint64_t inverseKey = (((uint64_t)target) << 32) + ((uint64_t)host); + Eigen::Vector2i st = connectivity.at(inverseKey); + connections[runningID].bwdAct = st[0]; + connections[runningID].bwdMarg = st[1]; + + totalActBwd += st[0]; + totalMargBwd += st[1]; + + runningID++; + } + + + model3DMutex.unlock(); +} +void PangolinDSOViewer::publishKeyframes( + std::vector &frames, + bool final, + CalibHessian* HCalib) +{ + if(!setting_render_display3D) return; + if(disableAllDisplay) return; + + boost::unique_lock lk(model3DMutex); + for(FrameHessian* fh : frames) + { + if(keyframesByKFID.find(fh->frameID) == keyframesByKFID.end()) + { + KeyFrameDisplay* kfd = new KeyFrameDisplay(); + keyframesByKFID[fh->frameID] = kfd; + keyframes.push_back(kfd); + } + keyframesByKFID[fh->frameID]->setFromKF(fh, HCalib); + } +} + +void PangolinDSOViewer::publishCamPose(FrameShell* frame, + CalibHessian* HCalib) +{ + if(!setting_render_display3D) return; + if(disableAllDisplay) return; + + boost::unique_lock lk(model3DMutex); + struct timeval time_now; + gettimeofday(&time_now, NULL); + lastNTrackingMs.push_back(((time_now.tv_sec-last_track.tv_sec)*1000.0f + (time_now.tv_usec-last_track.tv_usec)/1000.0f)); + if(lastNTrackingMs.size() > 10) lastNTrackingMs.pop_front(); + last_track = time_now; + + if(!setting_render_display3D) return; + + this->HCalib = HCalib; + + currentCam->setFromF(frame, HCalib); + allFramePoses.push_back(frame->camToWorld.translation().cast()); +} + + +void PangolinDSOViewer::pushLiveFrame(FrameHessian* image) +{ + if(!setting_render_displayVideo) return; + if(disableAllDisplay) return; + + boost::unique_lock lk(openImagesMutex); + + for(int i=0;idata[i][0] = + internalVideoImg->data[i][1] = + internalVideoImg->data[i][2] = + image->dI[i][0]*0.8 > 255.0f ? 255.0 : image->dI[i][0]*0.8; + + videoImgChanged=true; +} + +bool PangolinDSOViewer::needPushDepthImage() +{ + return setting_render_displayDepth; +} +void PangolinDSOViewer::pushDepthImage(MinimalImageB3* image) +{ + + if(!setting_render_displayDepth) return; + if(disableAllDisplay) return; + + boost::unique_lock lk(openImagesMutex); + + struct timeval time_now; + gettimeofday(&time_now, NULL); + lastNMappingMs.push_back(((time_now.tv_sec-last_map.tv_sec)*1000.0f + (time_now.tv_usec-last_map.tv_usec)/1000.0f)); + if(lastNMappingMs.size() > 10) lastNMappingMs.pop_front(); + last_map = time_now; + + memcpy(internalKFImg->data, image->data, w*h*3); + kfImgChanged=true; +} + +void PangolinDSOViewer::publishTransformDSOToIMU(const dmvio::TransformDSOToIMU& transformDSOToIMUPassed) +{ + if(!setting_render_display3D) return; + if(disableAllDisplay) return; + + boost::unique_lock lk(model3DMutex); + transformDSOToIMU = std::make_unique(transformDSOToIMUPassed, std::make_shared(false), + std::make_shared(false), std::make_shared(false)); +} + +void PangolinDSOViewer::addGTCamPose(const Sophus::SE3& gtPose) +{ + boost::unique_lock lk(model3DMutex); + + if(!setting_render_display3D || !HCalib) return; + + std::cout << "GTPose: " << gtPose.translation().transpose() << std::endl; + + if(!gtCamPoseSet) + { + firstGTCamPoseMetric = gtPose; + firstCamPoseDSO = currentCam->camToWorld; // Needed to make sure the first pose is the same for both. + } + + gtCamPoseMetric = gtPose; + gtCamPoseSet = true; + updateDisplayedCamPose(); + +} + +// Caller should aquire model lock for us. +void PangolinDSOViewer::updateDisplayedCamPose() +{ + if(!gtCamPoseSet || !transformDSOToIMU) return; + if(!setting_render_display3D || !HCalib) return; + + // The visualizer shows cam to world in dso scale. The groundtruth pose is imu to world in metric scale. + // This transforms to worldToCam + SE3 worldToCam(transformDSOToIMU->transformPoseInverse(gtCamPoseMetric.matrix())); + + SE3 firstGTWorldToCam(transformDSOToIMU->transformPoseInverse(firstGTCamPoseMetric.matrix())); + + // We want the first pose to stay the same: + // firstPose = offset * gtFirstPose; + // --> offset = firstPose * gtFirstPose^(-1) + SE3 offset = firstCamPoseDSO * firstGTWorldToCam; + SE3 gtPoseTransformed = offset * worldToCam.inverse(); + + currentGTCam->setFromPose(gtPoseTransformed, HCalib); + +} + +bool PangolinDSOViewer::shouldQuit() +{ + return shouldQuitVar; +} + +} +} diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h new file mode 100644 index 0000000..0b31e77 --- /dev/null +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h @@ -0,0 +1,168 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#include +#include "boost/thread.hpp" +#include "util/MinimalImage.h" +#include "IOWrapper/Output3DWrapper.h" +#include +#include +#include "util/SettingsUtil.h" + + +namespace dmvio +{ +class TransformDSOToIMU; +} + +namespace dso +{ + +class FrameHessian; +class CalibHessian; +class FrameShell; + + +namespace IOWrap +{ + +class KeyFrameDisplay; + +struct GraphConnection +{ + KeyFrameDisplay* from; + KeyFrameDisplay* to; + int fwdMarg, bwdMarg, fwdAct, bwdAct; +}; + + +class PangolinDSOViewer : public Output3DWrapper +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + PangolinDSOViewer(int w, int h, bool startRunThread=true, std::shared_ptr settingsUtil = nullptr); + virtual ~PangolinDSOViewer(); + + void run(); + void close(); + + void addImageToDisplay(std::string name, MinimalImageB3* image); + void clearAllImagesToDisplay(); + + + // ==================== Output3DWrapper Functionality ====================== + virtual void publishTransformDSOToIMU(const dmvio::TransformDSOToIMU& transformDSOToIMU) override; + virtual void publishGraph(const std::map, Eigen::aligned_allocator>> &connectivity) override; + virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override; + virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override; + + void addGTCamPose(const Sophus::SE3& gtPose); + + virtual void pushLiveFrame(FrameHessian* image) override; + virtual void pushDepthImage(MinimalImageB3* image) override; + virtual bool needPushDepthImage() override; + + bool shouldQuit(); + + virtual void join() override; + + virtual void reset() override; +private: + + bool needReset; + void reset_internal(); + void drawConstraints(); + + boost::thread runThread; + bool running; + bool shouldQuitVar{false}; + int w,h; + + + + // images rendering + boost::mutex openImagesMutex; + MinimalImageB3* internalVideoImg; + MinimalImageB3* internalKFImg; + MinimalImageB3* internalResImg; + bool videoImgChanged, kfImgChanged, resImgChanged; + + + CalibHessian *HCalib; + + // 3D model rendering + boost::mutex model3DMutex; + KeyFrameDisplay* currentCam, *currentGTCam; + std::vector keyframes; + std::vector> allFramePoses; + std::map keyframesByKFID; + std::vector> connections; + + + + // render settings + bool settings_showKFCameras; + bool settings_showCurrentCamera; + bool settings_showTrajectory; + bool settings_showFullTrajectory; + bool settings_showActiveConstraints; + bool settings_showAllConstraints; + + float settings_scaledVarTH; + float settings_absVarTH; + int settings_pointCloudMode; + float settings_minRelBS; + int settings_sparsity; + + + // timings + struct timeval last_track; + struct timeval last_map; + + + std::deque lastNTrackingMs; + std::deque lastNMappingMs; + + // GT cam poses + SE3 gtCamPoseMetric; // in metric frame, but will be displayed in DSO frame using. + SE3 firstCamPoseDSO; // DSO pose corresponding to the first groundtruth pose. + SE3 firstGTCamPoseMetric; + bool gtCamPoseSet = false; + std::unique_ptr transformDSOToIMU; + void updateDisplayedCamPose(); + + + std::shared_ptr settingsUtil; +}; + + + +} + + + +} diff --git a/src/dso/OptimizationBackend/AccumulatedSCHessian.cpp b/src/dso/OptimizationBackend/AccumulatedSCHessian.cpp new file mode 100644 index 0000000..e72bf41 --- /dev/null +++ b/src/dso/OptimizationBackend/AccumulatedSCHessian.cpp @@ -0,0 +1,221 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#include "OptimizationBackend/AccumulatedSCHessian.h" +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" + +#include "FullSystem/HessianBlocks.h" + +namespace dso +{ + +void AccumulatedSCHessianSSE::addPoint(EFPoint* p, bool shiftPriorToZero, int tid) +{ + int ngoodres = 0; + for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++; + if(ngoodres==0) + { + p->HdiF=0; + p->bdSumF=0; + p->data->idepth_hessian=0; + p->data->maxRelBaseline=0; + return; + } + + float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF; + if(H < 1e-10) H = 1e-10; + + p->data->idepth_hessian=H; + + p->HdiF = 1.0 / H; + p->bdSumF = p->bd_accAF + p->bd_accLF; + if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF; + VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF; + accHcc[tid].update(Hcd,Hcd,p->HdiF); + accbc[tid].update(Hcd, p->bdSumF * p->HdiF); + + assert(std::isfinite((float)(p->HdiF))); + + int nFrames2 = nframes[tid]*nframes[tid]; + for(EFResidual* r1 : p->residualsAll) + { + if(!r1->isActive()) continue; + int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid]; + + for(EFResidual* r2 : p->residualsAll) + { + if(!r2->isActive()) continue; + + accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF); + } + + accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF); + accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF); + } +} +void AccumulatedSCHessianSSE::stitchDoubleInternal( + MatXX* H, VecX* b, EnergyFunctional const * const EF, + int min, int max, Vec10* stats, int tid) +{ + int toAggregate = NUM_THREADS; + if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. + if(min==max) return; + + + int nf = nframes[0]; + int nframes2 = nf*nf; + + for(int k=min;k(); + bp += accEB[tid2][ijIdx].A1m.cast(); + } + + H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc; + H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc; + b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp; + b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp; + + + + for(int k=0;k(); + } + + H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); + H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); + H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); + H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); + } + } + + if(min==0) + { + for(int tid2=0;tid2 < toAggregate;tid2++) + { + accHcc[tid2].finish(); + accbc[tid2].finish(); + H[tid].topLeftCorner() += accHcc[tid2].A1m.cast(); + b[tid].head() += accbc[tid2].A1m.cast(); + } + } + + +// // ----- new: copy transposed parts for calibration only. +// for(int h=0;h(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose(); +// } +} + +void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid) +{ + + int nf = nframes[0]; + int nframes2 = nf*nf; + + H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS); + b = VecX::Zero(nf*8+CPARS); + + + for(int i=0;i(); + Vec8 accEBV = accEB[tid][ijIdx].A1m.cast(); + + H.block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * accEM; + H.block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * accEM; + + b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV; + b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV; + + for(int k=0;k(); + + H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); + + H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); + + H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); + + H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); + } + } + + accHcc[tid].finish(); + accbc[tid].finish(); + H.topLeftCorner() = accHcc[tid].A1m.cast(); + b.head() = accbc[tid].A1m.cast(); + + // ----- new: copy transposed parts for calibration only. + for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); + } +} + +} diff --git a/src/dso/OptimizationBackend/AccumulatedSCHessian.h b/src/dso/OptimizationBackend/AccumulatedSCHessian.h new file mode 100644 index 0000000..d9901d7 --- /dev/null +++ b/src/dso/OptimizationBackend/AccumulatedSCHessian.h @@ -0,0 +1,159 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" +#include "util/IndexThreadReduce.h" +#include "OptimizationBackend/MatrixAccumulators.h" +#include "vector" +#include + +namespace dso +{ + +class EFPoint; +class EnergyFunctional; + + +class AccumulatedSCHessianSSE +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + inline AccumulatedSCHessianSSE() + { + for(int i=0;i[n*n]; + accEB[tid] = new AccumulatorX<8>[n*n]; + accD[tid] = new AccumulatorXX<8,8>[n*n*n]; + } + accbc[tid].initialize(); + accHcc[tid].initialize(); + + for(int i=0;i* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT) + { + // sum up, splitting by bock in square. + if(MT) + { + MatXX Hs[NUM_THREADS]; + VecX bs[NUM_THREADS]; + for(int i=0;ireduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal, + this,Hs, bs, EF, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); + + // sum up results + H = Hs[0]; + b = bs[0]; + + for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); + } + } + + + AccumulatorXX<8,CPARS>* accE[NUM_THREADS]; + AccumulatorX<8>* accEB[NUM_THREADS]; + AccumulatorXX<8,8>* accD[NUM_THREADS]; + AccumulatorXX accHcc[NUM_THREADS]; + AccumulatorX accbc[NUM_THREADS]; + int nframes[NUM_THREADS]; + + + void addPointsInternal( + std::vector* points, bool shiftPriorToZero, + int min=0, int max=1, Vec10* stats=0, int tid=0) + { + for(int i=min;i, +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#include "OptimizationBackend/AccumulatedTopHessian.h" +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" +#include + +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#include "SSE2NEON.h" +#endif + +namespace dso +{ + + + +template +void AccumulatedTopHessianSSE::addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid) // 0 = active, 1 = linearized, 2=marginalize +{ + + + assert(mode==0 || mode==1 || mode==2); + + VecCf dc = ef->cDeltaF; + float dd = p->deltaF; + + float bd_acc=0; + float Hdd_acc=0; + VecCf Hcd_acc = VecCf::Zero(); + + for(EFResidual* r : p->residualsAll) + { + if(mode==0) + { + if(r->isLinearized || !r->isActive()) continue; + } + if(mode==1) + { + if(!r->isLinearized || !r->isActive()) continue; + } + if(mode==2) + { + if(!r->isActive()) continue; + assert(r->isLinearized); + } + + + RawResidualJacobian* rJ = r->J; + int htIDX = r->hostIDX + r->targetIDX*nframes[tid]; + Mat18f dp = ef->adHTdeltaF[htIDX]; + + + + VecNRf resApprox; + if(mode==0) + resApprox = rJ->resF; + if(mode==2) + resApprox = r->res_toZeroF; + if(mode==1) + { + // compute Jp*delta + __m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd); + __m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd); + __m128 delta_a = _mm_set1_ps((float)(dp[6])); + __m128 delta_b = _mm_set1_ps((float)(dp[7])); + + for(int i=0;ires_toZeroF)+i); + rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x)); + rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y)); + rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a)); + rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b)); + _mm_store_ps(((float*)&resApprox)+i, rtz); + } + } + + // need to compute JI^T * r, and Jab^T * r. (both are 2-vectors). + Vec2f JI_r(0,0); + Vec2f Jab_r(0,0); + float rr=0; + for(int i=0;iJIdx[0][i]; + JI_r[1] += resApprox[i] *rJ->JIdx[1][i]; + Jab_r[0] += resApprox[i] *rJ->JabF[0][i]; + Jab_r[1] += resApprox[i] *rJ->JabF[1][i]; + rr += resApprox[i]*resApprox[i]; + } + + + acc[tid][htIDX].update( + rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(), + rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(), + rJ->JIdx2(0,0),rJ->JIdx2(0,1),rJ->JIdx2(1,1)); + + acc[tid][htIDX].updateBotRight( + rJ->Jab2(0,0), rJ->Jab2(0,1), Jab_r[0], + rJ->Jab2(1,1), Jab_r[1],rr); + + acc[tid][htIDX].updateTopRight( + rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(), + rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(), + rJ->JabJIdx(0,0), rJ->JabJIdx(0,1), + rJ->JabJIdx(1,0), rJ->JabJIdx(1,1), + JI_r[0], JI_r[1]); + + + Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd; + bd_acc += JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1]; + Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd); + Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1]; + + nres[tid]++; + } + + if(mode==0) + { + p->Hdd_accAF = Hdd_acc; + p->bd_accAF = bd_acc; + p->Hcd_accAF = Hcd_acc; + } + if(mode==1 || mode==2) + { + p->Hdd_accLF = Hdd_acc; + p->bd_accLF = bd_acc; + p->Hcd_accLF = Hcd_acc; + } + if(mode==2) + { + p->Hcd_accAF.setZero(); + p->Hdd_accAF = 0; + p->bd_accAF = 0; + } + +} +template void AccumulatedTopHessianSSE::addPoint<0>(EFPoint* p, EnergyFunctional const * const ef, int tid); +template void AccumulatedTopHessianSSE::addPoint<1>(EFPoint* p, EnergyFunctional const * const ef, int tid); +template void AccumulatedTopHessianSSE::addPoint<2>(EFPoint* p, EnergyFunctional const * const ef, int tid); + + + + + + + + +void AccumulatedTopHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool useDelta, int tid) +{ + H = MatXX::Zero(nframes[tid]*8+CPARS, nframes[tid]*8+CPARS); + b = VecX::Zero(nframes[tid]*8+CPARS); + + + for(int h=0;h(); + + + H.block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose(); + + H.block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); + + H.block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); + + H.block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0); + + H.block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0); + + H.topLeftCorner().noalias() += accH.block(0,0); + + b.segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,8+CPARS); + + b.segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,8+CPARS); + + b.head().noalias() += accH.block(0,8+CPARS); + } + + + // ----- new: copy transposed parts. + for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); + + for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); + H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); + } + } + + + if(usePrior) + { + assert(useDelta); + H.diagonal().head() += EF->cPrior; + b.head() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast()); + for(int h=0;h(CPARS+h*8) += EF->frames[h]->prior; + b.segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior); + } + } +} + + +void AccumulatedTopHessianSSE::stitchDoubleInternal( + MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, + int min, int max, Vec10* stats, int tid) +{ + int toAggregate = NUM_THREADS; + if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. + if(min==max) return; + + + for(int k=min;k(); + } + + H[tid].block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose(); + + H[tid].block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); + + H[tid].block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); + + H[tid].block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0); + + H[tid].block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0); + + H[tid].topLeftCorner().noalias() += accH.block(0,0); + + b[tid].segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,CPARS+8); + + b[tid].segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,CPARS+8); + + b[tid].head().noalias() += accH.block(0,CPARS+8); + + } + + + // only do this on one thread. + if(min==0 && usePrior) + { + H[tid].diagonal().head() += EF->cPrior; + b[tid].head() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast()); + for(int h=0;h(CPARS+h*8) += EF->frames[h]->prior; + b[tid].segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior); + + } + } +} + + + +} + + diff --git a/src/dso/OptimizationBackend/AccumulatedTopHessian.h b/src/dso/OptimizationBackend/AccumulatedTopHessian.h new file mode 100644 index 0000000..63ee616 --- /dev/null +++ b/src/dso/OptimizationBackend/AccumulatedTopHessian.h @@ -0,0 +1,168 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" +#include "OptimizationBackend/MatrixAccumulators.h" +#include "vector" +#include +#include "util/IndexThreadReduce.h" + + +namespace dso +{ + +class EFPoint; +class EnergyFunctional; + + + +class AccumulatedTopHessianSSE +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + inline AccumulatedTopHessianSSE() + { + for(int tid=0;tid < NUM_THREADS; tid++) + { + nres[tid]=0; + acc[tid]=0; + nframes[tid]=0; + } + + }; + inline ~AccumulatedTopHessianSSE() + { + for(int tid=0;tid < NUM_THREADS; tid++) + { + if(acc[tid] != 0) delete[] acc[tid]; + } + }; + + inline void setZero(int nFrames, int min=0, int max=1, Vec10* stats=0, int tid=0) + { + + if(nFrames != nframes[tid]) + { + if(acc[tid] != 0) delete[] acc[tid]; +#if USE_XI_MODEL + acc[tid] = new Accumulator14[nFrames*nFrames]; +#else + acc[tid] = new AccumulatorApprox[nFrames*nFrames]; +#endif + } + + for(int i=0;i void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0); + + + + void stitchDoubleMT(IndexThreadReduce* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT) + { + // sum up, splitting by bock in square. + if(MT) + { + MatXX Hs[NUM_THREADS]; + VecX bs[NUM_THREADS]; + for(int i=0;ireduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal, + this,Hs, bs, EF, usePrior, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); + + // sum up results + H = Hs[0]; + b = bs[0]; + + for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); + + for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); + H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); + } + } + } + + + + + int nframes[NUM_THREADS]; + + EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS]; + + + int nres[NUM_THREADS]; + + + template void addPointsInternal( + std::vector* points, EnergyFunctional const * const ef, + int min=0, int max=1, Vec10* stats=0, int tid=0) + { + for(int i=min;i((*points)[i],ef,tid); + } + + + +private: + + void stitchDoubleInternal( + MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, + int min, int max, Vec10* stats, int tid); +}; +} + diff --git a/src/dso/OptimizationBackend/EnergyFunctional.cpp b/src/dso/OptimizationBackend/EnergyFunctional.cpp new file mode 100644 index 0000000..6c18918 --- /dev/null +++ b/src/dso/OptimizationBackend/EnergyFunctional.cpp @@ -0,0 +1,1029 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#include +#include "OptimizationBackend/EnergyFunctional.h" +#include "OptimizationBackend/EnergyFunctionalStructs.h" +#include "FullSystem/FullSystem.h" +#include "FullSystem/HessianBlocks.h" +#include "FullSystem/Residuals.h" +#include "OptimizationBackend/AccumulatedSCHessian.h" +#include "OptimizationBackend/AccumulatedTopHessian.h" + +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#include "SSE2NEON.h" +#endif + +namespace dso +{ + + +bool EFAdjointsValid = false; +bool EFIndicesValid = false; +bool EFDeltaValid = false; + +void EnergyFunctional::setAdjointsF(CalibHessian* Hcalib) +{ + + if(adHost != 0) delete[] adHost; + if(adTarget != 0) delete[] adTarget; + adHost = new Mat88[nFrames*nFrames]; + adTarget = new Mat88[nFrames*nFrames]; + + for(int h=0;hdata; + FrameHessian* target = frames[t]->data; + + SE3 hostToTarget = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse(); + + Mat88 AH = Mat88::Identity(); + Mat88 AT = Mat88::Identity(); + + AH.topLeftCorner<6,6>() = -hostToTarget.Adj().transpose(); + AT.topLeftCorner<6,6>() = Mat66::Identity(); + + + Vec2f affLL = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l_0(), target->aff_g2l_0()).cast(); + AT(6,6) = -affLL[0]; + AH(6,6) = affLL[0]; + AT(7,7) = -1; + AH(7,7) = affLL[0]; + + AH.block<3,8>(0,0) *= SCALE_XI_TRANS; + AH.block<3,8>(3,0) *= SCALE_XI_ROT; + AH.block<1,8>(6,0) *= SCALE_A; + AH.block<1,8>(7,0) *= SCALE_B; + AT.block<3,8>(0,0) *= SCALE_XI_TRANS; + AT.block<3,8>(3,0) *= SCALE_XI_ROT; + AT.block<1,8>(6,0) *= SCALE_A; + AT.block<1,8>(7,0) *= SCALE_B; + + adHost[h+t*nFrames] = AH; + adTarget[h+t*nFrames] = AT; + } + cPrior = VecC::Constant(setting_initialCalibHessian); + + + if(adHostF != 0) delete[] adHostF; + if(adTargetF != 0) delete[] adTargetF; + adHostF = new Mat88f[nFrames*nFrames]; + adTargetF = new Mat88f[nFrames*nFrames]; + + for(int h=0;h(); + adTargetF[h+t*nFrames] = adTarget[h+t*nFrames].cast(); + } + + cPriorF = cPrior.cast(); + + + EFAdjointsValid = true; +} + + + +EnergyFunctional::EnergyFunctional(dmvio::BAGTSAMIntegration >samIntegration) : gtsamIntegration(gtsamIntegration) +{ + adHost=0; + adTarget=0; + + + red=0; + + adHostF=0; + adTargetF=0; + adHTdeltaF=0; + + nFrames = nResiduals = nPoints = 0; + + HM = MatXX::Zero(CPARS,CPARS); + HMForGTSAM = MatXX::Zero(CPARS, CPARS); + bM = VecX::Zero(CPARS); + bMForGTSAM = VecX::Zero(CPARS); + + + accSSE_top_L = new AccumulatedTopHessianSSE(); + accSSE_top_A = new AccumulatedTopHessianSSE(); + accSSE_bot = new AccumulatedSCHessianSSE(); + + resInA = resInL = resInM = 0; + currentLambda=0; +} +EnergyFunctional::~EnergyFunctional() +{ + for(EFFrame* f : frames) + { + for(EFPoint* p : f->points) + { + for(EFResidual* r : p->residualsAll) + { + r->data->efResidual=0; + delete r; + } + p->data->efPoint=0; + delete p; + } + f->data->efFrame=0; + delete f; + } + + if(adHost != 0) delete[] adHost; + if(adTarget != 0) delete[] adTarget; + + + if(adHostF != 0) delete[] adHostF; + if(adTargetF != 0) delete[] adTargetF; + if(adHTdeltaF != 0) delete[] adHTdeltaF; + + + + delete accSSE_top_L; + delete accSSE_top_A; + delete accSSE_bot; +} + + + + +void EnergyFunctional::setDeltaF(CalibHessian* HCalib) +{ + if(adHTdeltaF != 0) delete[] adHTdeltaF; + adHTdeltaF = new Mat18f[nFrames*nFrames]; + for(int h=0;hdata->get_state_minus_stateZero().head<8>().cast().transpose() * adHostF[idx] + +frames[t]->data->get_state_minus_stateZero().head<8>().cast().transpose() * adTargetF[idx]; + } + + cDeltaF = HCalib->value_minus_value_zero.cast(); + for(EFFrame* f : frames) + { + f->delta = f->data->get_state_minus_stateZero().head<8>(); + f->delta_prior = (f->data->get_state() - f->data->getPriorZero()).head<8>(); + + for(EFPoint* p : f->points) + p->deltaF = p->data->idepth-p->data->idepth_zero; + } + + EFDeltaValid = true; +} + +// accumulates & shifts L. +void EnergyFunctional::accumulateAF_MT(MatXX &H, VecX &b, bool MT) +{ + if(MT) + { + red->reduce(boost::bind(&AccumulatedTopHessianSSE::setZero, accSSE_top_A, nFrames, _1, _2, _3, _4), 0, 0, 0); + red->reduce(boost::bind(&AccumulatedTopHessianSSE::addPointsInternal<0>, + accSSE_top_A, &allPoints, this, _1, _2, _3, _4), 0, allPoints.size(), 50); + accSSE_top_A->stitchDoubleMT(red,H,b,this,false,true); + resInA = accSSE_top_A->nres[0]; + } + else + { + accSSE_top_A->setZero(nFrames); + for(EFFrame* f : frames) + for(EFPoint* p : f->points) + accSSE_top_A->addPoint<0>(p,this); + accSSE_top_A->stitchDoubleMT(red,H,b,this,false,false); + resInA = accSSE_top_A->nres[0]; + } +} + +// accumulates & shifts L. +void EnergyFunctional::accumulateLF_MT(MatXX &H, VecX &b, bool MT) +{ + if(MT) + { + red->reduce(boost::bind(&AccumulatedTopHessianSSE::setZero, accSSE_top_L, nFrames, _1, _2, _3, _4), 0, 0, 0); + red->reduce(boost::bind(&AccumulatedTopHessianSSE::addPointsInternal<1>, + accSSE_top_L, &allPoints, this, _1, _2, _3, _4), 0, allPoints.size(), 50); + accSSE_top_L->stitchDoubleMT(red,H,b,this,true,true); + resInL = accSSE_top_L->nres[0]; + } + else + { + accSSE_top_L->setZero(nFrames); + for(EFFrame* f : frames) + for(EFPoint* p : f->points) + accSSE_top_L->addPoint<1>(p,this); + accSSE_top_L->stitchDoubleMT(red,H,b,this,true,false); + resInL = accSSE_top_L->nres[0]; + } +} + + + + + +void EnergyFunctional::accumulateSCF_MT(MatXX &H, VecX &b, bool MT) +{ + if(MT) + { + red->reduce(boost::bind(&AccumulatedSCHessianSSE::setZero, accSSE_bot, nFrames, _1, _2, _3, _4), 0, 0, 0); + red->reduce(boost::bind(&AccumulatedSCHessianSSE::addPointsInternal, + accSSE_bot, &allPoints, true, _1, _2, _3, _4), 0, allPoints.size(), 50); + accSSE_bot->stitchDoubleMT(red,H,b,this,true); + } + else + { + accSSE_bot->setZero(nFrames); + for(EFFrame* f : frames) + for(EFPoint* p : f->points) + accSSE_bot->addPoint(p, true); + accSSE_bot->stitchDoubleMT(red, H, b,this,false); + } +} + +void EnergyFunctional::resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT) +{ + assert(x.size() == CPARS+nFrames*8); + + VecXf xF = x.cast(); + HCalib->step = - x.head(); + + Mat18f* xAd = new Mat18f[nFrames*nFrames]; + VecCf cstep = xF.head(); + for(EFFrame* h : frames) + { + h->data->step.head<8>() = - x.segment<8>(CPARS+8*h->idx); + h->data->step.tail<2>().setZero(); + + for(EFFrame* t : frames) + xAd[nFrames*h->idx + t->idx] = xF.segment<8>(CPARS+8*h->idx).transpose() * adHostF[h->idx+nFrames*t->idx] + + xF.segment<8>(CPARS+8*t->idx).transpose() * adTargetF[h->idx+nFrames*t->idx]; + } + + if(MT) + red->reduce(boost::bind(&EnergyFunctional::resubstituteFPt, + this, cstep, xAd, _1, _2, _3, _4), 0, allPoints.size(), 50); + else + resubstituteFPt(cstep, xAd, 0, allPoints.size(), 0,0); + + delete[] xAd; +} + +void EnergyFunctional::resubstituteFPt( + const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid) +{ + for(int k=min;kresidualsAll) if(r->isActive()) ngoodres++; + if(ngoodres==0) + { + p->data->step = 0; + continue; + } + float b = p->bdSumF; + b -= xc.dot(p->Hcd_accAF + p->Hcd_accLF); + + for(EFResidual* r : p->residualsAll) + { + if(!r->isActive()) continue; + b -= xAd[r->hostIDX*nFrames + r->targetIDX] * r->JpJdF; + } + + p->data->step = - b*p->HdiF; + assert(std::isfinite(p->data->step)); + } +} + + +double EnergyFunctional::calcMEnergyF(bool useNewValues) +{ + + assert(EFDeltaValid); + assert(EFAdjointsValid); + assert(EFIndicesValid); + + VecX delta = getStitchedDeltaF(); + + double firstVal = delta.dot(2*bM + HM*delta); + + if(setting_useGTSAMIntegration) + { + if(!useNewValues) + { + gtsamIntegration.updateBAValues(frames); + } + double secondVal = gtsamIntegration.getBAEnergy(useNewValues) + delta.dot(2 * bMForGTSAM + HMForGTSAM * delta); + return secondVal; + } + + return firstVal; +} + + +void EnergyFunctional::calcLEnergyPt(int min, int max, Vec10* stats, int tid) +{ + + Accumulator11 E; + E.initialize(); + VecCf dc = cDeltaF; + + for(int i=min;ideltaF; + + for(EFResidual* r : p->residualsAll) + { + if(!r->isLinearized || !r->isActive()) continue; + + Mat18f dp = adHTdeltaF[r->hostIDX+nFrames*r->targetIDX]; + RawResidualJacobian* rJ = r->J; + + + + // compute Jp*delta + float Jp_delta_x_1 = rJ->Jpdxi[0].dot(dp.head<6>()) + +rJ->Jpdc[0].dot(dc) + +rJ->Jpdd[0]*dd; + + float Jp_delta_y_1 = rJ->Jpdxi[1].dot(dp.head<6>()) + +rJ->Jpdc[1].dot(dc) + +rJ->Jpdd[1]*dd; + + __m128 Jp_delta_x = _mm_set1_ps(Jp_delta_x_1); + __m128 Jp_delta_y = _mm_set1_ps(Jp_delta_y_1); + __m128 delta_a = _mm_set1_ps((float)(dp[6])); + __m128 delta_b = _mm_set1_ps((float)(dp[7])); + + for(int i=0;i+3JIdx))+i),Jp_delta_x); + Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y)); + Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a)); + Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b)); + + __m128 r0 = _mm_load_ps(((float*)&r->res_toZeroF)+i); + r0 = _mm_add_ps(r0,r0); + r0 = _mm_add_ps(r0,Jdelta); + Jdelta = _mm_mul_ps(Jdelta,r0); + E.updateSSENoShift(Jdelta); + } + for(int i=((patternNum>>2)<<2); i < patternNum; i++) + { + float Jdelta = rJ->JIdx[0][i]*Jp_delta_x_1 + rJ->JIdx[1][i]*Jp_delta_y_1 + + rJ->JabF[0][i]*dp[6] + rJ->JabF[1][i]*dp[7]; + E.updateSingleNoShift((float)(Jdelta * (Jdelta + 2*r->res_toZeroF[i]))); + } + } + E.updateSingle(p->deltaF*p->deltaF*p->priorF); + } + E.finish(); + (*stats)[0] += E.A; +} + + + + +double EnergyFunctional::calcLEnergyF_MT() +{ + assert(EFDeltaValid); + assert(EFAdjointsValid); + assert(EFIndicesValid); + + double E = 0; + for(EFFrame* f : frames) + { + E += f->delta_prior.cwiseProduct(f->prior).dot(f->delta_prior); + } + E += cDeltaF.cwiseProduct(cPriorF).dot(cDeltaF); + + red->reduce(boost::bind(&EnergyFunctional::calcLEnergyPt, + this, _1, _2, _3, _4), 0, allPoints.size(), 50); + + return E+red->stats[0]; +} + + + +EFResidual* EnergyFunctional::insertResidual(PointFrameResidual* r) +{ + EFResidual* efr = new EFResidual(r, r->point->efPoint, r->host->efFrame, r->target->efFrame); + efr->idxInAll = r->point->efPoint->residualsAll.size(); + r->point->efPoint->residualsAll.push_back(efr); + + connectivityMap[(((uint64_t)efr->host->frameID) << 32) + ((uint64_t)efr->target->frameID)][0]++; + + nResiduals++; + r->efResidual = efr; + return efr; +} +EFFrame* EnergyFunctional::insertFrame(FrameHessian* fh, CalibHessian* Hcalib) +{ + EFFrame* eff = new EFFrame(fh); + eff->idx = frames.size(); + frames.push_back(eff); + + nFrames++; + fh->efFrame = eff; + + assert(HM.cols() == 8*nFrames+CPARS-8); + bM.conservativeResize(8*nFrames+CPARS); + bMForGTSAM.conservativeResize(8 * nFrames + CPARS); + HM.conservativeResize(8*nFrames+CPARS,8*nFrames+CPARS); + HMForGTSAM.conservativeResize(8 * nFrames + CPARS, 8 * nFrames + CPARS); + bM.tail<8>().setZero(); + bMForGTSAM.tail<8>().setZero(); + HM.rightCols<8>().setZero(); + HM.bottomRows<8>().setZero(); + HMForGTSAM.rightCols<8>().setZero(); + HMForGTSAM.bottomRows<8>().setZero(); + + EFIndicesValid = false; + EFAdjointsValid=false; + EFDeltaValid=false; + + setAdjointsF(Hcalib); + makeIDX(); + + + for(EFFrame* fh2 : frames) + { + connectivityMap[(((uint64_t)eff->frameID) << 32) + ((uint64_t)fh2->frameID)] = Eigen::Vector2i(0,0); + if(fh2 != eff) + connectivityMap[(((uint64_t)fh2->frameID) << 32) + ((uint64_t)eff->frameID)] = Eigen::Vector2i(0,0); + } + + return eff; +} +EFPoint* EnergyFunctional::insertPoint(PointHessian* ph) +{ + EFPoint* efp = new EFPoint(ph, ph->host->efFrame); + efp->idxInPoints = ph->host->efFrame->points.size(); + ph->host->efFrame->points.push_back(efp); + + nPoints++; + ph->efPoint = efp; + + EFIndicesValid = false; + + return efp; +} + + +void EnergyFunctional::dropResidual(EFResidual* r) +{ + EFPoint* p = r->point; + assert(r == p->residualsAll[r->idxInAll]); + + p->residualsAll[r->idxInAll] = p->residualsAll.back(); + p->residualsAll[r->idxInAll]->idxInAll = r->idxInAll; + p->residualsAll.pop_back(); + + + if(r->isActive()) + r->host->data->shell->statistics_goodResOnThis++; + else + r->host->data->shell->statistics_outlierResOnThis++; + + + connectivityMap[(((uint64_t)r->host->frameID) << 32) + ((uint64_t)r->target->frameID)][0]--; + nResiduals--; + r->data->efResidual=0; + delete r; +} + +void EnergyFunctional::marginalizeFrame(EFFrame* fh) +{ + dmvio::TimeMeasurement timeMeasurement("EF-marginalizeFrame"); + assert(EFDeltaValid); + assert(EFAdjointsValid); + assert(EFIndicesValid); + + assert((int)fh->points.size()==0); + int ndim = nFrames*8+CPARS-8;// new dimension + int odim = nFrames*8+CPARS;// old dimension + + +// VecX eigenvaluesPre = HM.eigenvalues().real(); +// std::sort(eigenvaluesPre.data(), eigenvaluesPre.data()+eigenvaluesPre.size()); +// + + if(setting_useGTSAMIntegration) + { + // When adding additional factors with GTSAM they need to be accounted for during keyframe marginalization. + // Hence we move the whole keyframe marginalization to the GTSAMIntegration. + dmvio::TimeMeasurement innerMeas("MainMarginalization"); + assert(odim == (int)HM.rows()); + assert(odim == (int)HM.cols()); + assert(odim == (int)bM.size()); + + // Adds H and b from the last points to the graph. Needs the current evaluation point for each frames. + gtsamIntegration.addMarginalizedPointsBA(HMForGTSAM, bMForGTSAM, frames); + + Vec8 priorH; + Vec8 priorB; + priorH = fh->prior; + priorB = fh->prior.cwiseProduct(fh->delta_prior); + + gtsamIntegration.addPriorBA(fh, priorH, priorB); + + // Marginalizes out the frame. Adds the symbols of this frame and then calls marginalize out. + gtsamIntegration.marginalizeBAFrame(fh); + + HMForGTSAM.resize(ndim, ndim); + bMForGTSAM.resize(ndim); + + HMForGTSAM.setZero(); + bMForGTSAM.setZero(); + + } + +// if(!setting_useGTSAMIntegration) // enable to remove the redundant visual only marginalization. + if(true) + { + dmvio::TimeMeasurement measVis("VisualMarginalization"); + if((int)fh->idx != (int)frames.size()-1) + { + int io = fh->idx*8+CPARS; // index of frame to move to end + int ntail = 8*(nFrames-fh->idx-1); + assert((io+8+ntail) == nFrames*8+CPARS); + + Vec8 bTmp = bM.segment<8>(io); + VecX tailTMP = bM.tail(ntail); + bM.segment(io,ntail) = tailTMP; + bM.tail<8>() = bTmp; + + MatXX HtmpCol = HM.block(0,io,odim,8); + MatXX rightColsTmp = HM.rightCols(ntail); + HM.block(0,io,odim,ntail) = rightColsTmp; + HM.rightCols(8) = HtmpCol; + + MatXX HtmpRow = HM.block(io,0,8,odim); + MatXX botRowsTmp = HM.bottomRows(ntail); + HM.block(io,0,ntail,odim) = botRowsTmp; + HM.bottomRows(8) = HtmpRow; + } + + + HM.bottomRightCorner<8,8>().diagonal() += fh->prior; + bM.tail<8>() += fh->prior.cwiseProduct(fh->delta_prior); + + + + // std::cout << std::setprecision(16) << "HMPre:\n" << HM << "\n\n"; + + + VecX SVec = (HM.diagonal().cwiseAbs()+VecX::Constant(HM.cols(), 10)).cwiseSqrt(); + VecX SVecI = SVec.cwiseInverse(); + + + // std::cout << std::setprecision(16) << "SVec: " << SVec.transpose() << "\n\n"; + // std::cout << std::setprecision(16) << "SVecI: " << SVecI.transpose() << "\n\n"; + + // scale! + MatXX HMScaled = SVecI.asDiagonal() * HM * SVecI.asDiagonal(); + VecX bMScaled = SVecI.asDiagonal() * bM; + + // invert bottom part! + Mat88 hpi = HMScaled.bottomRightCorner<8,8>(); + hpi = 0.5f*(hpi+hpi); + hpi = hpi.inverse(); + hpi = 0.5f*(hpi+hpi); + + // schur-complement! + MatXX bli = HMScaled.bottomLeftCorner(8,ndim).transpose() * hpi; + HMScaled.topLeftCorner(ndim,ndim).noalias() -= bli * HMScaled.bottomLeftCorner(8,ndim); + bMScaled.head(ndim).noalias() -= bli*bMScaled.tail<8>(); + + //unscale! + HMScaled = SVec.asDiagonal() * HMScaled * SVec.asDiagonal(); + bMScaled = SVec.asDiagonal() * bMScaled; + + // set. + HM = 0.5*(HMScaled.topLeftCorner(ndim,ndim) + HMScaled.topLeftCorner(ndim,ndim).transpose()); + bM = bMScaled.head(ndim); + + // With the imu-integration this cannot be used, because there are other variables that have to be considered. + }else + { + // Just update the dimensions without actually marginalizing, as these are not used in practice. + HM = HM.topLeftCorner(ndim, ndim); + bM = bM.head(ndim); + } + + + // remove from vector, without changing the order! + for(unsigned int i=fh->idx; i+1idx = i; + } + frames.pop_back(); + nFrames--; + fh->data->efFrame=0; + + assert((int)frames.size()*8+CPARS == (int)HM.rows()); + assert((int)frames.size()*8+CPARS == (int)HM.cols()); + assert((int)frames.size()*8+CPARS == (int)bM.size()); + assert((int)frames.size() == (int)nFrames); + + + + +// VecX eigenvaluesPost = HM.eigenvalues().real(); +// std::sort(eigenvaluesPost.data(), eigenvaluesPost.data()+eigenvaluesPost.size()); + +// std::cout << std::setprecision(16) << "HMPost:\n" << HM << "\n\n"; + +// std::cout << "EigPre:: " << eigenvaluesPre.transpose() << "\n"; +// std::cout << "EigPost: " << eigenvaluesPost.transpose() << "\n"; + + EFIndicesValid = false; + EFAdjointsValid=false; + EFDeltaValid=false; + + makeIDX(); +} + + + + +void EnergyFunctional::marginalizePointsF() +{ + assert(EFDeltaValid); + assert(EFAdjointsValid); + assert(EFIndicesValid); + + + allPointsToMarg.clear(); + for(EFFrame* f : frames) + { + for(int i=0;i<(int)f->points.size();i++) + { + EFPoint* p = f->points[i]; + if(p->stateFlag == EFPointStatus::PS_MARGINALIZE) + { + p->priorF *= setting_idepthFixPriorMargFac; + for(EFResidual* r : p->residualsAll) + if(r->isActive()) + connectivityMap[(((uint64_t)r->host->frameID) << 32) + ((uint64_t)r->target->frameID)][1]++; + allPointsToMarg.push_back(p); + } + } + } + + accSSE_bot->setZero(nFrames); + accSSE_top_A->setZero(nFrames); + for(EFPoint* p : allPointsToMarg) + { + accSSE_top_A->addPoint<2>(p,this); + accSSE_bot->addPoint(p,false); + removePoint(p); + } + MatXX M, Msc; + VecX Mb, Mbsc; + accSSE_top_A->stitchDouble(M,Mb,this,false,false); + accSSE_bot->stitchDouble(Msc,Mbsc,this); + + resInM+= accSSE_top_A->nres[0]; + + MatXX H = M-Msc; + VecX b = Mb-Mbsc; + + if(setting_solverMode & SOLVER_ORTHOGONALIZE_POINTMARG) + { + // have a look if prior is there. + bool haveFirstFrame = false; + for(EFFrame* f : frames) if(f->frameID==0) haveFirstFrame=true; + + if(!haveFirstFrame) + orthogonalize(&b, &H); + + } + + HM += setting_margWeightFac*H; + bM += setting_margWeightFac*b; + + HMForGTSAM += setting_margWeightFac * H; + bMForGTSAM += setting_margWeightFac * b; + + if(setting_solverMode & SOLVER_ORTHOGONALIZE_FULL) + orthogonalize(&bM, &HM); + + EFIndicesValid = false; + makeIDX(); +} + +void EnergyFunctional::dropPointsF() +{ + + + for(EFFrame* f : frames) + { + for(int i=0;i<(int)f->points.size();i++) + { + EFPoint* p = f->points[i]; + if(p->stateFlag == EFPointStatus::PS_DROP) + { + removePoint(p); + i--; + } + } + } + + EFIndicesValid = false; + makeIDX(); +} + + +void EnergyFunctional::removePoint(EFPoint* p) +{ + for(EFResidual* r : p->residualsAll) + dropResidual(r); + + EFFrame* h = p->host; + h->points[p->idxInPoints] = h->points.back(); + h->points[p->idxInPoints]->idxInPoints = p->idxInPoints; + h->points.pop_back(); + + nPoints--; + p->data->efPoint = 0; + + EFIndicesValid = false; + + delete p; +} + +void EnergyFunctional::orthogonalize(VecX* b, MatXX* H) +{ +// VecX eigenvaluesPre = H.eigenvalues().real(); +// std::sort(eigenvaluesPre.data(), eigenvaluesPre.data()+eigenvaluesPre.size()); +// std::cout << "EigPre:: " << eigenvaluesPre.transpose() << "\n"; + + + // decide to which nullspaces to orthogonalize. + std::vector ns; + ns.insert(ns.end(), lastNullspaces_pose.begin(), lastNullspaces_pose.end()); + ns.insert(ns.end(), lastNullspaces_scale.begin(), lastNullspaces_scale.end()); +// if(setting_affineOptModeA <= 0) +// ns.insert(ns.end(), lastNullspaces_affA.begin(), lastNullspaces_affA.end()); +// if(setting_affineOptModeB <= 0) +// ns.insert(ns.end(), lastNullspaces_affB.begin(), lastNullspaces_affB.end()); + + + + + + // make Nullspaces matrix + MatXX N(ns[0].rows(), ns.size()); + for(unsigned int i=0;i svdNN(N, Eigen::ComputeThinU | Eigen::ComputeThinV); + + VecX SNN = svdNN.singularValues(); + double minSv = 1e10, maxSv = 0; + for(int i=0;i maxSv) maxSv = SNN[i]; + } + for(int i=0;i setting_solverModeDelta*maxSv) SNN[i] = 1.0 / SNN[i]; else SNN[i] = 0; } + + MatXX Npi = svdNN.matrixU() * SNN.asDiagonal() * svdNN.matrixV().transpose(); // [dim] x 9. + MatXX NNpiT = N*Npi.transpose(); // [dim] x [dim]. + MatXX NNpiTS = 0.5*(NNpiT + NNpiT.transpose()); // = N * (N' * N)^-1 * N'. + + if(b!=0) *b -= NNpiTS * *b; + if(H!=0) *H -= NNpiTS * *H * NNpiTS; + + +// std::cout << std::setprecision(16) << "Orth SV: " << SNN.reverse().transpose() << "\n"; + +// VecX eigenvaluesPost = H.eigenvalues().real(); +// std::sort(eigenvaluesPost.data(), eigenvaluesPost.data()+eigenvaluesPost.size()); +// std::cout << "EigPost:: " << eigenvaluesPost.transpose() << "\n"; + +} + + +void EnergyFunctional::solveSystemF(int iteration, double lambda, CalibHessian* HCalib) +{ + if(setting_solverMode & SOLVER_USE_GN) lambda=0; + if(setting_solverMode & SOLVER_FIX_LAMBDA) lambda = 1e-5; + + assert(EFDeltaValid); + assert(EFAdjointsValid); + assert(EFIndicesValid); + + MatXX HL_top, HA_top, H_sc; + VecX bL_top, bA_top, bM_top, b_sc; + + accumulateAF_MT(HA_top, bA_top,multiThreading); + + + accumulateLF_MT(HL_top, bL_top,multiThreading); + + + + accumulateSCF_MT(H_sc, b_sc,multiThreading); + + + + bM_top = (bM+ HM * getStitchedDeltaF()); + VecX bMGTSAM_top = (bMForGTSAM + HMForGTSAM * getStitchedDeltaF()); + + + + + + + + MatXX HFinal_top; + VecX bFinal_top; + + if(setting_solverMode & SOLVER_ORTHOGONALIZE_SYSTEM) + { + // have a look if prior is there. + bool haveFirstFrame = false; + for(EFFrame* f : frames) if(f->frameID==0) haveFirstFrame=true; + + + + + MatXX HT_act = HL_top + HA_top - H_sc; + VecX bT_act = bL_top + bA_top - b_sc; + + + if(!haveFirstFrame) + orthogonalize(&bT_act, &HT_act); + + HFinal_top = HT_act + HM; + bFinal_top = bT_act + bM_top; + + + + + + lastHS = HFinal_top; + lastbS = bFinal_top; + + for(int i=0;i<8*nFrames+CPARS;i++) HFinal_top(i,i) *= (1+lambda); + + } + else + { + + + HFinal_top = HL_top + HM + HA_top; + bFinal_top = bL_top + bM_top + bA_top - b_sc; + + lastHS = HFinal_top - H_sc; + lastbS = bFinal_top; + + for(int i=0;i<8*nFrames+CPARS;i++) HFinal_top(i,i) *= (1+lambda); + HFinal_top -= H_sc * (1.0f/(1+lambda)); + } + + + + + + + VecX x; + if(setting_solverMode & SOLVER_SVD) + { + VecX SVecI = HFinal_top.diagonal().cwiseSqrt().cwiseInverse(); + MatXX HFinalScaled = SVecI.asDiagonal() * HFinal_top * SVecI.asDiagonal(); + VecX bFinalScaled = SVecI.asDiagonal() * bFinal_top; + Eigen::JacobiSVD svd(HFinalScaled, Eigen::ComputeThinU | Eigen::ComputeThinV); + + VecX S = svd.singularValues(); + double minSv = 1e10, maxSv = 0; + for(int i=0;i maxSv) maxSv = S[i]; + } + + VecX Ub = svd.matrixU().transpose()*bFinalScaled; + int setZero=0; + for(int i=0;i= Ub.size()-7)) + { Ub[i] = 0; setZero++; } + + else Ub[i] /= S[i]; + } + x = SVecI.asDiagonal() * svd.matrixV() * Ub; + + } + else + { + VecX myX; + if(setting_useGTSAMIntegration) + { + // Instead of directly solving the system we instead pass it to the GTSAMIntegration which will add more + // factors and then solve it for us. This is mathematically correct as long as the new residuals are + // independent of the DSO residuals (which usually they are) and as long as they don't depend on the + // points (as otherwise the Schur-complement trick doesn't work like this anymore). + MatXX HPassed = HL_top + HMForGTSAM + HA_top; + for(int i=0;i<8*nFrames+CPARS;i++) HPassed(i,i) *= (1+lambda); + HPassed -= H_sc * (1.0f/(1+lambda)); + x = gtsamIntegration.computeBAUpdate(HPassed, bL_top + bMGTSAM_top + bA_top - b_sc, lambda, + frames, HL_top + HMForGTSAM + HA_top - H_sc); + }else + { + VecX SVecI = (HFinal_top.diagonal()+VecX::Constant(HFinal_top.cols(), 10)).cwiseSqrt().cwiseInverse(); + MatXX HFinalScaled = SVecI.asDiagonal() * HFinal_top * SVecI.asDiagonal(); + x = SVecI.asDiagonal() * HFinalScaled.ldlt().solve(SVecI.asDiagonal() * bFinal_top); + } + // Important: x is -step ! + } + + + + if((setting_solverMode & SOLVER_ORTHOGONALIZE_X) || (iteration >= 2 && (setting_solverMode & SOLVER_ORTHOGONALIZE_X_LATER))) + { + VecX xOld = x; + orthogonalize(&x, 0); + } + + + lastX = x; + + + //resubstituteF(x, HCalib); + currentLambda= lambda; + resubstituteF_MT(x, HCalib,multiThreading); + currentLambda=0; + + +} +void EnergyFunctional::makeIDX() +{ + for(unsigned int idx=0;idxidx = idx; + + allPoints.clear(); + + for(EFFrame* f : frames) + for(EFPoint* p : f->points) + { + allPoints.push_back(p); + for(EFResidual* r : p->residualsAll) + { + r->hostIDX = r->host->idx; + r->targetIDX = r->target->idx; + } + } + + + EFIndicesValid=true; +} + + +VecX EnergyFunctional::getStitchedDeltaF() const +{ + VecX d = VecX(CPARS+nFrames*8); d.head() = cDeltaF.cast(); + for(int h=0;h(CPARS+8*h) = frames[h]->delta; + return d; +} + + + +} diff --git a/src/dso/OptimizationBackend/EnergyFunctional.h b/src/dso/OptimizationBackend/EnergyFunctional.h new file mode 100644 index 0000000..b8d9b9b --- /dev/null +++ b/src/dso/OptimizationBackend/EnergyFunctional.h @@ -0,0 +1,173 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" +#include "util/IndexThreadReduce.h" +#include "vector" +#include +#include "map" + + +namespace dmvio +{ +class BAGTSAMIntegration; +} + +namespace dso +{ + +class PointFrameResidual; +class CalibHessian; +class FrameHessian; +class PointHessian; + + +class EFResidual; +class EFPoint; +class EFFrame; +class EnergyFunctional; +class AccumulatedTopHessian; +class AccumulatedTopHessianSSE; +class AccumulatedSCHessian; +class AccumulatedSCHessianSSE; + + +extern bool EFAdjointsValid; +extern bool EFIndicesValid; +extern bool EFDeltaValid; + + + +class EnergyFunctional { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + friend class EFFrame; + friend class EFPoint; + friend class EFResidual; + friend class AccumulatedTopHessian; + friend class AccumulatedTopHessianSSE; + friend class AccumulatedSCHessian; + friend class AccumulatedSCHessianSSE; + + EnergyFunctional(dmvio::BAGTSAMIntegration >samIntegration); + ~EnergyFunctional(); + + + EFResidual* insertResidual(PointFrameResidual* r); + EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib); + EFPoint* insertPoint(PointHessian* ph); + + void dropResidual(EFResidual* r); + void marginalizeFrame(EFFrame* fh); + void removePoint(EFPoint* ph); + + + + void marginalizePointsF(); + void dropPointsF(); + void solveSystemF(int iteration, double lambda, CalibHessian* HCalib); + double calcMEnergyF(bool useNewValues); + double calcLEnergyF_MT(); + + + void makeIDX(); + + void setDeltaF(CalibHessian* HCalib); + + void setAdjointsF(CalibHessian* Hcalib); + + std::vector frames; + int nPoints, nFrames, nResiduals; + + // HMForGTSAM, bMForGTSAM only contain marginalized points until the next time a keyframe is marginalized. + // With each keyframe marginalization the information in them is transferred to the GTSAMIntegration. + MatXX HM, HMForGTSAM; + VecX bM, bMForGTSAM; + + int resInA, resInL, resInM; + MatXX lastHS; + VecX lastbS; + VecX lastX; + std::vector lastNullspaces_forLogging; + std::vector lastNullspaces_pose; + std::vector lastNullspaces_scale; + std::vector lastNullspaces_affA; + std::vector lastNullspaces_affB; + + IndexThreadReduce* red; + + + std::map, + Eigen::aligned_allocator> + > connectivityMap; + +private: + + VecX getStitchedDeltaF() const; + + void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT); + void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid); + + void accumulateAF_MT(MatXX &H, VecX &b, bool MT); + void accumulateLF_MT(MatXX &H, VecX &b, bool MT); + void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); + + void calcLEnergyPt(int min, int max, Vec10* stats, int tid); + + void orthogonalize(VecX* b, MatXX* H); + Mat18f* adHTdeltaF; + + Mat88* adHost; + Mat88* adTarget; + + Mat88f* adHostF; + Mat88f* adTargetF; + + + VecC cPrior; + VecCf cDeltaF; + VecCf cPriorF; + + AccumulatedTopHessianSSE* accSSE_top_L; + AccumulatedTopHessianSSE* accSSE_top_A; + + + AccumulatedSCHessianSSE* accSSE_bot; + + std::vector allPoints; + std::vector allPointsToMarg; + + float currentLambda; + + dmvio::BAGTSAMIntegration >samIntegration; +}; +} + diff --git a/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp b/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp new file mode 100644 index 0000000..31dc873 --- /dev/null +++ b/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp @@ -0,0 +1,116 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#include "OptimizationBackend/EnergyFunctionalStructs.h" +#include "OptimizationBackend/EnergyFunctional.h" +#include "FullSystem/FullSystem.h" +#include "FullSystem/HessianBlocks.h" +#include "FullSystem/Residuals.h" + +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#include "SSE2NEON.h" +#endif + +namespace dso +{ + + +void EFResidual::takeDataF() +{ + std::swap(J, data->J); + + Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd; + + for(int i=0;i<6;i++) + JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; + + JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd; +} + + +void EFFrame::takeData() +{ + prior = data->getPrior().head<8>(); + delta = data->get_state_minus_stateZero().head<8>(); + delta_prior = (data->get_state() - data->getPriorZero()).head<8>(); + + + +// Vec10 state_zero = data->get_state_zero(); +// state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0); +// state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3); +// state_zero[6] = SCALE_A * state_zero[6]; +// state_zero[7] = SCALE_B * state_zero[7]; +// state_zero[8] = SCALE_A * state_zero[8]; +// state_zero[9] = SCALE_B * state_zero[9]; +// +// std::cout << "state_zero: " << state_zero.transpose() << "\n"; + + + assert(data->frameID != -1); + + frameID = data->frameID; +} + + + + +void EFPoint::takeData() +{ + priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0; + if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0; + + deltaF = data->idepth-data->idepth_zero; +} + + +void EFResidual::fixLinearizationF(EnergyFunctional* ef) +{ + Vec8f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX]; + + // compute Jp*delta + __m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>()) + +J->Jpdc[0].dot(ef->cDeltaF) + +J->Jpdd[0]*point->deltaF); + __m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>()) + +J->Jpdc[1].dot(ef->cDeltaF) + +J->Jpdd[1]*point->deltaF); + __m128 delta_a = _mm_set1_ps((float)(dp[6])); + __m128 delta_b = _mm_set1_ps((float)(dp[7])); + + for(int i=0;iresF)+i); + rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x)); + rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y)); + rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a)); + rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b)); + _mm_store_ps(((float*)&res_toZeroF)+i, rtz); + } + + isLinearized = true; +} + +} diff --git a/src/dso/OptimizationBackend/EnergyFunctionalStructs.h b/src/dso/OptimizationBackend/EnergyFunctionalStructs.h new file mode 100644 index 0000000..149aba6 --- /dev/null +++ b/src/dso/OptimizationBackend/EnergyFunctionalStructs.h @@ -0,0 +1,168 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" +#include "vector" +#include +#include "OptimizationBackend/RawResidualJacobian.h" + +namespace dso +{ + +class PointFrameResidual; +class CalibHessian; +class FrameHessian; +class PointHessian; + +class EFResidual; +class EFPoint; +class EFFrame; +class EnergyFunctional; + + + + + + +class EFResidual +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) : + data(org), point(point_), host(host_), target(target_) + { + isLinearized=false; + isActiveAndIsGoodNEW=false; + J = new RawResidualJacobian(); + assert(((long)this)%16==0); + assert(((long)J)%16==0); + } + inline ~EFResidual() + { + delete J; + } + + + void takeDataF(); + + + void fixLinearizationF(EnergyFunctional* ef); + + + // structural pointers + PointFrameResidual* data; + int hostIDX, targetIDX; + EFPoint* point; + EFFrame* host; + EFFrame* target; + int idxInAll; + + RawResidualJacobian* J; + + VecNRf res_toZeroF; + Vec8f JpJdF; + + + // status. + bool isLinearized; + + // if residual is not OOB & not OUTLIER & should be used during accumulations + bool isActiveAndIsGoodNEW; + inline const bool &isActive() const {return isActiveAndIsGoodNEW;} +}; + + +enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP}; + +class EFPoint +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_) + { + takeData(); + stateFlag=EFPointStatus::PS_GOOD; + } + void takeData(); + + PointHessian* data; + + + + float priorF; + float deltaF; + + + // constant info (never changes in-between). + int idxInPoints; + EFFrame* host; + + // contains all residuals. + std::vector residualsAll; + + float bdSumF; + float HdiF; + float Hdd_accLF; + VecCf Hcd_accLF; + float bd_accLF; + float Hdd_accAF; + VecCf Hcd_accAF; + float bd_accAF; + + + EFPointStatus stateFlag; +}; + + + +class EFFrame +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EFFrame(FrameHessian* d) : data(d) + { + takeData(); + } + void takeData(); + + + Vec8 prior; // prior hessian (diagonal) + Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) + Vec8 delta; // state - state_zero. + + + + std::vector points; + FrameHessian* data; + int idx; // idx in frames. + + int frameID; +}; + +} + diff --git a/src/dso/OptimizationBackend/MatrixAccumulators.h b/src/dso/OptimizationBackend/MatrixAccumulators.h new file mode 100644 index 0000000..28ed13b --- /dev/null +++ b/src/dso/OptimizationBackend/MatrixAccumulators.h @@ -0,0 +1,1346 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#include "util/NumType.h" + +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#include "SSE2NEON.h" +#endif + +namespace dso +{ + + +template +class AccumulatorXX +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + Eigen::Matrix A; + Eigen::Matrix A1k; + Eigen::Matrix A1m; + size_t num; + + inline void initialize() + { + A.setZero(); + A1k.setZero(); + A1m.setZero(); + num = numIn1 = numIn1k = numIn1m = 0; + } + + inline void finish() + { + shiftUp(true); + num = numIn1 + numIn1k + numIn1m; + } + + + inline void update(const Eigen::Matrix &L, const Eigen::Matrix &R, float w) + { + A += w*L*R.transpose(); + numIn1++; + shiftUp(false); + } + +private: + float numIn1, numIn1k, numIn1m; + + void shiftUp(bool force) + { + if(numIn1 > 1000 || force) + { + A1k += A; + A.setZero(); + numIn1k+=numIn1; + numIn1=0; + } + if(numIn1k > 1000 || force) + { + A1m += A1k; + A1k.setZero(); + numIn1m+=numIn1k; + numIn1k=0; + } + } +}; + +class Accumulator11 +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + float A; + size_t num; + + inline void initialize() + { + A=0; + memset(SSEData,0, sizeof(float)*4*1); + memset(SSEData1k,0, sizeof(float)*4*1); + memset(SSEData1m,0, sizeof(float)*4*1); + num = numIn1 = numIn1k = numIn1m = 0; + } + + inline void finish() + { + shiftUp(true); + A=SSEData1m[0+0] + SSEData1m[0+1] + SSEData1m[0+2] + SSEData1m[0+3]; + } + + + inline void updateSingle( + const float val) + { + SSEData[0] += val; + num++; numIn1++; + shiftUp(false); + } + + inline void updateSSE( + const __m128 val) + { + _mm_store_ps(SSEData, _mm_add_ps(_mm_load_ps(SSEData),val)); + num+=4; + numIn1++; + shiftUp(false); + } + + inline void updateSingleNoShift( + const float val) + { + SSEData[0] += val; + num++; numIn1++; + } + + inline void updateSSENoShift( + const __m128 val) + { + _mm_store_ps(SSEData, _mm_add_ps(_mm_load_ps(SSEData),val)); + num+=4; + numIn1++; + } + + + +private: + EIGEN_ALIGN16 float SSEData[4*1]; + EIGEN_ALIGN16 float SSEData1k[4*1]; + EIGEN_ALIGN16 float SSEData1m[4*1]; + float numIn1, numIn1k, numIn1m; + + + void shiftUp(bool force) + { + if(numIn1 > 1000 || force) + { + _mm_store_ps(SSEData1k, _mm_add_ps(_mm_load_ps(SSEData),_mm_load_ps(SSEData1k))); + numIn1k+=numIn1; numIn1=0; + memset(SSEData,0, sizeof(float)*4*1); + } + + if(numIn1k > 1000 || force) + { + _mm_store_ps(SSEData1m, _mm_add_ps(_mm_load_ps(SSEData1k),_mm_load_ps(SSEData1m))); + numIn1m+=numIn1k; numIn1k=0; + memset(SSEData1k,0, sizeof(float)*4*1); + } + } +}; + + + + +template +class AccumulatorX +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + Eigen::Matrix A; + Eigen::Matrix A1k; + Eigen::Matrix A1m; + size_t num; + + inline void initialize() + { + A.setZero(); + A1k.setZero(); + A1m.setZero(); + num = numIn1 = numIn1k = numIn1m = 0; + } + + inline void finish() + { + shiftUp(true); + num = numIn1+numIn1k+numIn1m; + } + + + inline void update(const Eigen::Matrix &L, float w) + { + A += w*L; + numIn1++; + shiftUp(false); + } + + inline void updateNoWeight(const Eigen::Matrix &L) + { + A += L; + numIn1++; + shiftUp(false); + } + +private: + float numIn1, numIn1k, numIn1m; + + void shiftUp(bool force) + { + if(numIn1 > 1000 || force) + { + A1k += A; + A.setZero(); + numIn1k+=numIn1; + numIn1=0; + } + if(numIn1k > 1000 || force) + { + A1m += A1k; + A1k.setZero(); + numIn1m+=numIn1k; + numIn1k=0; + } + } +}; + + + + + + + + +class Accumulator14 +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + Mat1414f H; + Vec14f b; + size_t num; + + inline void initialize() + { + H.setZero(); + b.setZero(); + memset(SSEData,0, sizeof(float)*4*105); + memset(SSEData1k,0, sizeof(float)*4*105); + memset(SSEData1m,0, sizeof(float)*4*105); + num = numIn1 = numIn1k = numIn1m = 0; + } + + inline void finish() + { + H.setZero(); + shiftUp(true); + assert(numIn1==0); + assert(numIn1k==0); + + int idx=0; + for(int r=0;r<14;r++) + for(int c=r;c<14;c++) + { + float d = SSEData1m[idx+0] + SSEData1m[idx+1] + SSEData1m[idx+2] + SSEData1m[idx+3]; + H(r,c) = H(c,r) = d; + idx+=4; + } + assert(idx==4*105); + num = numIn1 + numIn1k + numIn1m; + } + + + inline void updateSSE( + const __m128 J0,const __m128 J1, + const __m128 J2,const __m128 J3, + const __m128 J4,const __m128 J5, + const __m128 J6,const __m128 J7, + const __m128 J8,const __m128 J9, + const __m128 J10,const __m128 J11, + const __m128 J12,const __m128 J13) + { + float* pt=SSEData; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J0))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J1))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J1))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J8))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J9,J9))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J9,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J9,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J9,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J9,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J10,J10))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J10,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J10,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J10,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J11,J11))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J11,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J11,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J12,J12))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J12,J13))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J13,J13))); pt+=4; + + num+=4; + numIn1++; + shiftUp(false); + } + + + inline void updateSingle( + const float J0,const float J1, + const float J2,const float J3, + const float J4,const float J5, + const float J6,const float J7, + const float J8,const float J9, + const float J10,const float J11, + const float J12, const float J13, + int off=0) + { + float* pt=SSEData+off; + *pt += J0*J0; pt+=4; + *pt += J1*J0; pt+=4; + *pt += J2*J0; pt+=4; + *pt += J3*J0; pt+=4; + *pt += J4*J0; pt+=4; + *pt += J5*J0; pt+=4; + *pt += J6*J0; pt+=4; + *pt += J7*J0; pt+=4; + *pt += J8*J0; pt+=4; + *pt += J9*J0; pt+=4; + *pt += J10*J0; pt+=4; + *pt += J11*J0; pt+=4; + *pt += J12*J0; pt+=4; + *pt += J13*J0; pt+=4; + + *pt += J1*J1; pt+=4; + *pt += J2*J1; pt+=4; + *pt += J3*J1; pt+=4; + *pt += J4*J1; pt+=4; + *pt += J5*J1; pt+=4; + *pt += J6*J1; pt+=4; + *pt += J7*J1; pt+=4; + *pt += J8*J1; pt+=4; + *pt += J9*J1; pt+=4; + *pt += J10*J1; pt+=4; + *pt += J11*J1; pt+=4; + *pt += J12*J1; pt+=4; + *pt += J13*J1; pt+=4; + + *pt += J2*J2; pt+=4; + *pt += J3*J2; pt+=4; + *pt += J4*J2; pt+=4; + *pt += J5*J2; pt+=4; + *pt += J6*J2; pt+=4; + *pt += J7*J2; pt+=4; + *pt += J8*J2; pt+=4; + *pt += J9*J2; pt+=4; + *pt += J10*J2; pt+=4; + *pt += J11*J2; pt+=4; + *pt += J12*J2; pt+=4; + *pt += J13*J2; pt+=4; + + *pt += J3*J3; pt+=4; + *pt += J4*J3; pt+=4; + *pt += J5*J3; pt+=4; + *pt += J6*J3; pt+=4; + *pt += J7*J3; pt+=4; + *pt += J8*J3; pt+=4; + *pt += J9*J3; pt+=4; + *pt += J10*J3; pt+=4; + *pt += J11*J3; pt+=4; + *pt += J12*J3; pt+=4; + *pt += J13*J3; pt+=4; + + *pt += J4*J4; pt+=4; + *pt += J5*J4; pt+=4; + *pt += J6*J4; pt+=4; + *pt += J7*J4; pt+=4; + *pt += J8*J4; pt+=4; + *pt += J9*J4; pt+=4; + *pt += J10*J4; pt+=4; + *pt += J11*J4; pt+=4; + *pt += J12*J4; pt+=4; + *pt += J13*J4; pt+=4; + + *pt += J5*J5; pt+=4; + *pt += J6*J5; pt+=4; + *pt += J7*J5; pt+=4; + *pt += J8*J5; pt+=4; + *pt += J9*J5; pt+=4; + *pt += J10*J5; pt+=4; + *pt += J11*J5; pt+=4; + *pt += J12*J5; pt+=4; + *pt += J13*J5; pt+=4; + + *pt += J6*J6; pt+=4; + *pt += J7*J6; pt+=4; + *pt += J8*J6; pt+=4; + *pt += J9*J6; pt+=4; + *pt += J10*J6; pt+=4; + *pt += J11*J6; pt+=4; + *pt += J12*J6; pt+=4; + *pt += J13*J6; pt+=4; + + *pt += J7*J7; pt+=4; + *pt += J8*J7; pt+=4; + *pt += J9*J7; pt+=4; + *pt += J10*J7; pt+=4; + *pt += J11*J7; pt+=4; + *pt += J12*J7; pt+=4; + *pt += J13*J7; pt+=4; + + *pt += J8*J8; pt+=4; + *pt += J9*J8; pt+=4; + *pt += J10*J8; pt+=4; + *pt += J11*J8; pt+=4; + *pt += J12*J8; pt+=4; + *pt += J13*J8; pt+=4; + + *pt += J9*J9; pt+=4; + *pt += J10*J9; pt+=4; + *pt += J11*J9; pt+=4; + *pt += J12*J9; pt+=4; + *pt += J13*J9; pt+=4; + + *pt += J10*J10; pt+=4; + *pt += J11*J10; pt+=4; + *pt += J12*J10; pt+=4; + *pt += J13*J10; pt+=4; + + *pt += J11*J11; pt+=4; + *pt += J12*J11; pt+=4; + *pt += J13*J11; pt+=4; + + *pt += J12*J12; pt+=4; + *pt += J13*J12; pt+=4; + + *pt += J13*J13; pt+=4; + + num++; + numIn1++; + shiftUp(false); + } + + +private: + EIGEN_ALIGN16 float SSEData[4*105]; + EIGEN_ALIGN16 float SSEData1k[4*105]; + EIGEN_ALIGN16 float SSEData1m[4*105]; + float numIn1, numIn1k, numIn1m; + + + void shiftUp(bool force) + { + if(numIn1 > 1000 || force) + { + for(int i=0;i<105;i++) + _mm_store_ps(SSEData1k+4*i, _mm_add_ps(_mm_load_ps(SSEData+4*i),_mm_load_ps(SSEData1k+4*i))); + numIn1k+=numIn1; + numIn1=0; + memset(SSEData,0, sizeof(float)*4*105); + } + + if(numIn1k > 1000 || force) + { + for(int i=0;i<105;i++) + _mm_store_ps(SSEData1m+4*i, _mm_add_ps(_mm_load_ps(SSEData1k+4*i),_mm_load_ps(SSEData1m+4*i))); + numIn1m+=numIn1k; + numIn1k=0; + memset(SSEData1k,0, sizeof(float)*4*105); + } + } +}; + + + + + +/* + * computes the outer sum of 10x2 matrices, weighted with a 2x2 matrix: + * H = [x y] * [a b; b c] * [x y]^T + * (assuming x,y are column-vectors). + * numerically robust to large sums. + */ +class AccumulatorApprox +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + Mat1313f H; + size_t num; + + inline void initialize() + { + memset(Data,0, sizeof(float)*60); + memset(Data1k,0, sizeof(float)*60); + memset(Data1m,0, sizeof(float)*60); + + memset(TopRight_Data,0, sizeof(float)*32); + memset(TopRight_Data1k,0, sizeof(float)*32); + memset(TopRight_Data1m,0, sizeof(float)*32); + + memset(BotRight_Data,0, sizeof(float)*8); + memset(BotRight_Data1k,0, sizeof(float)*8); + memset(BotRight_Data1m,0, sizeof(float)*8); + num = numIn1 = numIn1k = numIn1m = 0; + } + +inline void finish() +{ + H.setZero(); + shiftUp(true); + assert(numIn1==0); + assert(numIn1k==0); + + int idx=0; + for(int r=0;r<10;r++) + for(int c=r;c<10;c++) + { + H(r,c) = H(c,r) = Data1m[idx]; + idx++; + } + + idx=0; + for(int r=0;r<10;r++) + for(int c=0; c<3;c++) + { + H(r,c+10) = H(c+10,r) = TopRight_Data1m[idx]; + idx++; + } + + H(10,10) = BotRight_Data1m[0]; + H(10,11) = H(11,10) = BotRight_Data1m[1]; + H(10,12) = H(12,10) = BotRight_Data1m[2]; + H(11,11) = BotRight_Data1m[3]; + H(11,12) = H(12,11) = BotRight_Data1m[4]; + H(12,12) = BotRight_Data1m[5]; + + + num = numIn1 + numIn1k + numIn1m; +} + + + + + + inline void updateSSE( + const float* const x, + const float* const y, + const float a, + const float b, + const float c) + { + + Data[0] += a*x[0]*x[0] + c*y[0]*y[0] + b*(x[0]*y[0] + y[0]*x[0]); + Data[1] += a*x[1]*x[0] + c*y[1]*y[0] + b*(x[1]*y[0] + y[1]*x[0]); + Data[2] += a*x[2]*x[0] + c*y[2]*y[0] + b*(x[2]*y[0] + y[2]*x[0]); + Data[3] += a*x[3]*x[0] + c*y[3]*y[0] + b*(x[3]*y[0] + y[3]*x[0]); + Data[4] += a*x[4]*x[0] + c*y[4]*y[0] + b*(x[4]*y[0] + y[4]*x[0]); + Data[5] += a*x[5]*x[0] + c*y[5]*y[0] + b*(x[5]*y[0] + y[5]*x[0]); + Data[6] += a*x[6]*x[0] + c*y[6]*y[0] + b*(x[6]*y[0] + y[6]*x[0]); + Data[7] += a*x[7]*x[0] + c*y[7]*y[0] + b*(x[7]*y[0] + y[7]*x[0]); + Data[8] += a*x[8]*x[0] + c*y[8]*y[0] + b*(x[8]*y[0] + y[8]*x[0]); + Data[9] += a*x[9]*x[0] + c*y[9]*y[0] + b*(x[9]*y[0] + y[9]*x[0]); + + + Data[10] += a*x[1]*x[1] + c*y[1]*y[1] + b*(x[1]*y[1] + y[1]*x[1]); + Data[11] += a*x[2]*x[1] + c*y[2]*y[1] + b*(x[2]*y[1] + y[2]*x[1]); + Data[12] += a*x[3]*x[1] + c*y[3]*y[1] + b*(x[3]*y[1] + y[3]*x[1]); + Data[13] += a*x[4]*x[1] + c*y[4]*y[1] + b*(x[4]*y[1] + y[4]*x[1]); + Data[14] += a*x[5]*x[1] + c*y[5]*y[1] + b*(x[5]*y[1] + y[5]*x[1]); + Data[15] += a*x[6]*x[1] + c*y[6]*y[1] + b*(x[6]*y[1] + y[6]*x[1]); + Data[16] += a*x[7]*x[1] + c*y[7]*y[1] + b*(x[7]*y[1] + y[7]*x[1]); + Data[17] += a*x[8]*x[1] + c*y[8]*y[1] + b*(x[8]*y[1] + y[8]*x[1]); + Data[18] += a*x[9]*x[1] + c*y[9]*y[1] + b*(x[9]*y[1] + y[9]*x[1]); + + + + Data[19] += a*x[2]*x[2] + c*y[2]*y[2] + b*(x[2]*y[2] + y[2]*x[2]); + Data[20] += a*x[3]*x[2] + c*y[3]*y[2] + b*(x[3]*y[2] + y[3]*x[2]); + Data[21] += a*x[4]*x[2] + c*y[4]*y[2] + b*(x[4]*y[2] + y[4]*x[2]); + Data[22] += a*x[5]*x[2] + c*y[5]*y[2] + b*(x[5]*y[2] + y[5]*x[2]); + Data[23] += a*x[6]*x[2] + c*y[6]*y[2] + b*(x[6]*y[2] + y[6]*x[2]); + Data[24] += a*x[7]*x[2] + c*y[7]*y[2] + b*(x[7]*y[2] + y[7]*x[2]); + Data[25] += a*x[8]*x[2] + c*y[8]*y[2] + b*(x[8]*y[2] + y[8]*x[2]); + Data[26] += a*x[9]*x[2] + c*y[9]*y[2] + b*(x[9]*y[2] + y[9]*x[2]); + + + + Data[27] += a*x[3]*x[3] + c*y[3]*y[3] + b*(x[3]*y[3] + y[3]*x[3]); + Data[28] += a*x[4]*x[3] + c*y[4]*y[3] + b*(x[4]*y[3] + y[4]*x[3]); + Data[29] += a*x[5]*x[3] + c*y[5]*y[3] + b*(x[5]*y[3] + y[5]*x[3]); + Data[30] += a*x[6]*x[3] + c*y[6]*y[3] + b*(x[6]*y[3] + y[6]*x[3]); + Data[31] += a*x[7]*x[3] + c*y[7]*y[3] + b*(x[7]*y[3] + y[7]*x[3]); + Data[32] += a*x[8]*x[3] + c*y[8]*y[3] + b*(x[8]*y[3] + y[8]*x[3]); + Data[33] += a*x[9]*x[3] + c*y[9]*y[3] + b*(x[9]*y[3] + y[9]*x[3]); + + + + Data[34] += a*x[4]*x[4] + c*y[4]*y[4] + b*(x[4]*y[4] + y[4]*x[4]); + Data[35] += a*x[5]*x[4] + c*y[5]*y[4] + b*(x[5]*y[4] + y[5]*x[4]); + Data[36] += a*x[6]*x[4] + c*y[6]*y[4] + b*(x[6]*y[4] + y[6]*x[4]); + Data[37] += a*x[7]*x[4] + c*y[7]*y[4] + b*(x[7]*y[4] + y[7]*x[4]); + Data[38] += a*x[8]*x[4] + c*y[8]*y[4] + b*(x[8]*y[4] + y[8]*x[4]); + Data[39] += a*x[9]*x[4] + c*y[9]*y[4] + b*(x[9]*y[4] + y[9]*x[4]); + + + + Data[40] += a*x[5]*x[5] + c*y[5]*y[5] + b*(x[5]*y[5] + y[5]*x[5]); + Data[41] += a*x[6]*x[5] + c*y[6]*y[5] + b*(x[6]*y[5] + y[6]*x[5]); + Data[42] += a*x[7]*x[5] + c*y[7]*y[5] + b*(x[7]*y[5] + y[7]*x[5]); + Data[43] += a*x[8]*x[5] + c*y[8]*y[5] + b*(x[8]*y[5] + y[8]*x[5]); + Data[44] += a*x[9]*x[5] + c*y[9]*y[5] + b*(x[9]*y[5] + y[9]*x[5]); + + + Data[45] += a*x[6]*x[6] + c*y[6]*y[6] + b*(x[6]*y[6] + y[6]*x[6]); + Data[46] += a*x[7]*x[6] + c*y[7]*y[6] + b*(x[7]*y[6] + y[7]*x[6]); + Data[47] += a*x[8]*x[6] + c*y[8]*y[6] + b*(x[8]*y[6] + y[8]*x[6]); + Data[48] += a*x[9]*x[6] + c*y[9]*y[6] + b*(x[9]*y[6] + y[9]*x[6]); + + + Data[49] += a*x[7]*x[7] + c*y[7]*y[7] + b*(x[7]*y[7] + y[7]*x[7]); + Data[50] += a*x[8]*x[7] + c*y[8]*y[7] + b*(x[8]*y[7] + y[8]*x[7]); + Data[51] += a*x[9]*x[7] + c*y[9]*y[7] + b*(x[9]*y[7] + y[9]*x[7]); + + + Data[52] += a*x[8]*x[8] + c*y[8]*y[8] + b*(x[8]*y[8] + y[8]*x[8]); + Data[53] += a*x[9]*x[8] + c*y[9]*y[8] + b*(x[9]*y[8] + y[9]*x[8]); + + Data[54] += a*x[9]*x[9] + c*y[9]*y[9] + b*(x[9]*y[9] + y[9]*x[9]); + + + num++; + numIn1++; + shiftUp(false); + } + + + + +/* + * same as other method, just that x/y are composed of two parts, the first 4 elements are in x4/y4, the last 6 in x6/y6. + */ + inline void update( + const float* const x4, + const float* const x6, + const float* const y4, + const float* const y6, + const float a, + const float b, + const float c) + { + + Data[0] += a*x4[0]*x4[0] + c*y4[0]*y4[0] + b*(x4[0]*y4[0] + y4[0]*x4[0]); + Data[1] += a*x4[1]*x4[0] + c*y4[1]*y4[0] + b*(x4[1]*y4[0] + y4[1]*x4[0]); + Data[2] += a*x4[2]*x4[0] + c*y4[2]*y4[0] + b*(x4[2]*y4[0] + y4[2]*x4[0]); + Data[3] += a*x4[3]*x4[0] + c*y4[3]*y4[0] + b*(x4[3]*y4[0] + y4[3]*x4[0]); + Data[4] += a*x6[0]*x4[0] + c*y6[0]*y4[0] + b*(x6[0]*y4[0] + y6[0]*x4[0]); + Data[5] += a*x6[1]*x4[0] + c*y6[1]*y4[0] + b*(x6[1]*y4[0] + y6[1]*x4[0]); + Data[6] += a*x6[2]*x4[0] + c*y6[2]*y4[0] + b*(x6[2]*y4[0] + y6[2]*x4[0]); + Data[7] += a*x6[3]*x4[0] + c*y6[3]*y4[0] + b*(x6[3]*y4[0] + y6[3]*x4[0]); + Data[8] += a*x6[4]*x4[0] + c*y6[4]*y4[0] + b*(x6[4]*y4[0] + y6[4]*x4[0]); + Data[9] += a*x6[5]*x4[0] + c*y6[5]*y4[0] + b*(x6[5]*y4[0] + y6[5]*x4[0]); + + + + + Data[10] += a*x4[1]*x4[1] + c*y4[1]*y4[1] + b*(x4[1]*y4[1] + y4[1]*x4[1]); + Data[11] += a*x4[2]*x4[1] + c*y4[2]*y4[1] + b*(x4[2]*y4[1] + y4[2]*x4[1]); + Data[12] += a*x4[3]*x4[1] + c*y4[3]*y4[1] + b*(x4[3]*y4[1] + y4[3]*x4[1]); + Data[13] += a*x6[0]*x4[1] + c*y6[0]*y4[1] + b*(x6[0]*y4[1] + y6[0]*x4[1]); + Data[14] += a*x6[1]*x4[1] + c*y6[1]*y4[1] + b*(x6[1]*y4[1] + y6[1]*x4[1]); + Data[15] += a*x6[2]*x4[1] + c*y6[2]*y4[1] + b*(x6[2]*y4[1] + y6[2]*x4[1]); + Data[16] += a*x6[3]*x4[1] + c*y6[3]*y4[1] + b*(x6[3]*y4[1] + y6[3]*x4[1]); + Data[17] += a*x6[4]*x4[1] + c*y6[4]*y4[1] + b*(x6[4]*y4[1] + y6[4]*x4[1]); + Data[18] += a*x6[5]*x4[1] + c*y6[5]*y4[1] + b*(x6[5]*y4[1] + y6[5]*x4[1]); + + + + Data[19] += a*x4[2]*x4[2] + c*y4[2]*y4[2] + b*(x4[2]*y4[2] + y4[2]*x4[2]); + Data[20] += a*x4[3]*x4[2] + c*y4[3]*y4[2] + b*(x4[3]*y4[2] + y4[3]*x4[2]); + Data[21] += a*x6[0]*x4[2] + c*y6[0]*y4[2] + b*(x6[0]*y4[2] + y6[0]*x4[2]); + Data[22] += a*x6[1]*x4[2] + c*y6[1]*y4[2] + b*(x6[1]*y4[2] + y6[1]*x4[2]); + Data[23] += a*x6[2]*x4[2] + c*y6[2]*y4[2] + b*(x6[2]*y4[2] + y6[2]*x4[2]); + Data[24] += a*x6[3]*x4[2] + c*y6[3]*y4[2] + b*(x6[3]*y4[2] + y6[3]*x4[2]); + Data[25] += a*x6[4]*x4[2] + c*y6[4]*y4[2] + b*(x6[4]*y4[2] + y6[4]*x4[2]); + Data[26] += a*x6[5]*x4[2] + c*y6[5]*y4[2] + b*(x6[5]*y4[2] + y6[5]*x4[2]); + + + + Data[27] += a*x4[3]*x4[3] + c*y4[3]*y4[3] + b*(x4[3]*y4[3] + y4[3]*x4[3]); + Data[28] += a*x6[0]*x4[3] + c*y6[0]*y4[3] + b*(x6[0]*y4[3] + y6[0]*x4[3]); + Data[29] += a*x6[1]*x4[3] + c*y6[1]*y4[3] + b*(x6[1]*y4[3] + y6[1]*x4[3]); + Data[30] += a*x6[2]*x4[3] + c*y6[2]*y4[3] + b*(x6[2]*y4[3] + y6[2]*x4[3]); + Data[31] += a*x6[3]*x4[3] + c*y6[3]*y4[3] + b*(x6[3]*y4[3] + y6[3]*x4[3]); + Data[32] += a*x6[4]*x4[3] + c*y6[4]*y4[3] + b*(x6[4]*y4[3] + y6[4]*x4[3]); + Data[33] += a*x6[5]*x4[3] + c*y6[5]*y4[3] + b*(x6[5]*y4[3] + y6[5]*x4[3]); + + + + Data[34] += a*x6[0]*x6[0] + c*y6[0]*y6[0] + b*(x6[0]*y6[0] + y6[0]*x6[0]); + Data[35] += a*x6[1]*x6[0] + c*y6[1]*y6[0] + b*(x6[1]*y6[0] + y6[1]*x6[0]); + Data[36] += a*x6[2]*x6[0] + c*y6[2]*y6[0] + b*(x6[2]*y6[0] + y6[2]*x6[0]); + Data[37] += a*x6[3]*x6[0] + c*y6[3]*y6[0] + b*(x6[3]*y6[0] + y6[3]*x6[0]); + Data[38] += a*x6[4]*x6[0] + c*y6[4]*y6[0] + b*(x6[4]*y6[0] + y6[4]*x6[0]); + Data[39] += a*x6[5]*x6[0] + c*y6[5]*y6[0] + b*(x6[5]*y6[0] + y6[5]*x6[0]); + + + + Data[40] += a*x6[1]*x6[1] + c*y6[1]*y6[1] + b*(x6[1]*y6[1] + y6[1]*x6[1]); + Data[41] += a*x6[2]*x6[1] + c*y6[2]*y6[1] + b*(x6[2]*y6[1] + y6[2]*x6[1]); + Data[42] += a*x6[3]*x6[1] + c*y6[3]*y6[1] + b*(x6[3]*y6[1] + y6[3]*x6[1]); + Data[43] += a*x6[4]*x6[1] + c*y6[4]*y6[1] + b*(x6[4]*y6[1] + y6[4]*x6[1]); + Data[44] += a*x6[5]*x6[1] + c*y6[5]*y6[1] + b*(x6[5]*y6[1] + y6[5]*x6[1]); + + + Data[45] += a*x6[2]*x6[2] + c*y6[2]*y6[2] + b*(x6[2]*y6[2] + y6[2]*x6[2]); + Data[46] += a*x6[3]*x6[2] + c*y6[3]*y6[2] + b*(x6[3]*y6[2] + y6[3]*x6[2]); + Data[47] += a*x6[4]*x6[2] + c*y6[4]*y6[2] + b*(x6[4]*y6[2] + y6[4]*x6[2]); + Data[48] += a*x6[5]*x6[2] + c*y6[5]*y6[2] + b*(x6[5]*y6[2] + y6[5]*x6[2]); + + + Data[49] += a*x6[3]*x6[3] + c*y6[3]*y6[3] + b*(x6[3]*y6[3] + y6[3]*x6[3]); + Data[50] += a*x6[4]*x6[3] + c*y6[4]*y6[3] + b*(x6[4]*y6[3] + y6[4]*x6[3]); + Data[51] += a*x6[5]*x6[3] + c*y6[5]*y6[3] + b*(x6[5]*y6[3] + y6[5]*x6[3]); + + + Data[52] += a*x6[4]*x6[4] + c*y6[4]*y6[4] + b*(x6[4]*y6[4] + y6[4]*x6[4]); + Data[53] += a*x6[5]*x6[4] + c*y6[5]*y6[4] + b*(x6[5]*y6[4] + y6[5]*x6[4]); + + Data[54] += a*x6[5]*x6[5] + c*y6[5]*y6[5] + b*(x6[5]*y6[5] + y6[5]*x6[5]); + + + num++; + numIn1++; + shiftUp(false); + } + + + inline void updateTopRight( + const float* const x4, + const float* const x6, + const float* const y4, + const float* const y6, + const float TR00, const float TR10, + const float TR01, const float TR11, + const float TR02, const float TR12 ) + { + TopRight_Data[0] += x4[0]*TR00 + y4[0]*TR10; + TopRight_Data[1] += x4[0]*TR01 + y4[0]*TR11; + TopRight_Data[2] += x4[0]*TR02 + y4[0]*TR12; + + TopRight_Data[3] += x4[1]*TR00 + y4[1]*TR10; + TopRight_Data[4] += x4[1]*TR01 + y4[1]*TR11; + TopRight_Data[5] += x4[1]*TR02 + y4[1]*TR12; + + TopRight_Data[6] += x4[2]*TR00 + y4[2]*TR10; + TopRight_Data[7] += x4[2]*TR01 + y4[2]*TR11; + TopRight_Data[8] += x4[2]*TR02 + y4[2]*TR12; + + TopRight_Data[9] += x4[3]*TR00 + y4[3]*TR10; + TopRight_Data[10] += x4[3]*TR01 + y4[3]*TR11; + TopRight_Data[11] += x4[3]*TR02 + y4[3]*TR12; + + TopRight_Data[12] += x6[0]*TR00 + y6[0]*TR10; + TopRight_Data[13] += x6[0]*TR01 + y6[0]*TR11; + TopRight_Data[14] += x6[0]*TR02 + y6[0]*TR12; + + TopRight_Data[15] += x6[1]*TR00 + y6[1]*TR10; + TopRight_Data[16] += x6[1]*TR01 + y6[1]*TR11; + TopRight_Data[17] += x6[1]*TR02 + y6[1]*TR12; + + TopRight_Data[18] += x6[2]*TR00 + y6[2]*TR10; + TopRight_Data[19] += x6[2]*TR01 + y6[2]*TR11; + TopRight_Data[20] += x6[2]*TR02 + y6[2]*TR12; + + TopRight_Data[21] += x6[3]*TR00 + y6[3]*TR10; + TopRight_Data[22] += x6[3]*TR01 + y6[3]*TR11; + TopRight_Data[23] += x6[3]*TR02 + y6[3]*TR12; + + TopRight_Data[24] += x6[4]*TR00 + y6[4]*TR10; + TopRight_Data[25] += x6[4]*TR01 + y6[4]*TR11; + TopRight_Data[26] += x6[4]*TR02 + y6[4]*TR12; + + TopRight_Data[27] += x6[5]*TR00 + y6[5]*TR10; + TopRight_Data[28] += x6[5]*TR01 + y6[5]*TR11; + TopRight_Data[29] += x6[5]*TR02 + y6[5]*TR12; + + } + + inline void updateBotRight( + const float a00, + const float a01, + const float a02, + const float a11, + const float a12, + const float a22) + { + BotRight_Data[0] += a00; + BotRight_Data[1] += a01; + BotRight_Data[2] += a02; + BotRight_Data[3] += a11; + BotRight_Data[4] += a12; + BotRight_Data[5] += a22; + } + + + +private: + EIGEN_ALIGN16 float Data[60]; + EIGEN_ALIGN16 float Data1k[60]; + EIGEN_ALIGN16 float Data1m[60]; + + EIGEN_ALIGN16 float TopRight_Data[32]; + EIGEN_ALIGN16 float TopRight_Data1k[32]; + EIGEN_ALIGN16 float TopRight_Data1m[32]; + + EIGEN_ALIGN16 float BotRight_Data[8]; + EIGEN_ALIGN16 float BotRight_Data1k[8]; + EIGEN_ALIGN16 float BotRight_Data1m[8]; + + + float numIn1, numIn1k, numIn1m; + + + + void shiftUp(bool force) + { + if(numIn1 > 1000 || force) + { + for(int i=0;i<60;i+=4) + _mm_store_ps(Data1k+i, _mm_add_ps(_mm_load_ps(Data+i),_mm_load_ps(Data1k+i))); + for(int i=0;i<32;i+=4) + _mm_store_ps(TopRight_Data1k+i, _mm_add_ps(_mm_load_ps(TopRight_Data+i),_mm_load_ps(TopRight_Data1k+i))); + for(int i=0;i<8;i+=4) + _mm_store_ps(BotRight_Data1k+i, _mm_add_ps(_mm_load_ps(BotRight_Data+i),_mm_load_ps(BotRight_Data1k+i))); + + + numIn1k+=numIn1; + numIn1=0; + memset(Data,0, sizeof(float)*60); + memset(TopRight_Data,0, sizeof(float)*32); + memset(BotRight_Data,0, sizeof(float)*8); + } + + if(numIn1k > 1000 || force) + { + for(int i=0;i<60;i+=4) + _mm_store_ps(Data1m+i, _mm_add_ps(_mm_load_ps(Data1k+i),_mm_load_ps(Data1m+i))); + for(int i=0;i<32;i+=4) + _mm_store_ps(TopRight_Data1m+i, _mm_add_ps(_mm_load_ps(TopRight_Data1k+i),_mm_load_ps(TopRight_Data1m+i))); + for(int i=0;i<8;i+=4) + _mm_store_ps(BotRight_Data1m+i, _mm_add_ps(_mm_load_ps(BotRight_Data1k+i),_mm_load_ps(BotRight_Data1m+i))); + + numIn1m+=numIn1k; + numIn1k=0; + memset(Data1k,0, sizeof(float)*60); + memset(TopRight_Data1k,0, sizeof(float)*32); + memset(BotRight_Data1k,0, sizeof(float)*8); + } + } +}; + + + + + + + + + +class Accumulator9 +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + Mat99f H; + Vec9f b; + size_t num; + + inline void initialize() + { + H.setZero(); + b.setZero(); + memset(SSEData,0, sizeof(float)*4*45); + memset(SSEData1k,0, sizeof(float)*4*45); + memset(SSEData1m,0, sizeof(float)*4*45); + num = numIn1 = numIn1k = numIn1m = 0; + } + + inline void finish() + { + H.setZero(); + shiftUp(true); + assert(numIn1==0); + assert(numIn1k==0); + + int idx=0; + for(int r=0;r<9;r++) + for(int c=r;c<9;c++) + { + float d = SSEData1m[idx+0] + SSEData1m[idx+1] + SSEData1m[idx+2] + SSEData1m[idx+3]; + H(r,c) = H(c,r) = d; + idx+=4; + } + assert(idx==4*45); + } + + + inline void updateSSE( + const __m128 J0,const __m128 J1, + const __m128 J2,const __m128 J3, + const __m128 J4,const __m128 J5, + const __m128 J6,const __m128 J7, + const __m128 J8) + { + float* pt=SSEData; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J0))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J1))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J1))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7,J8))); pt+=4; + + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8,J8))); pt+=4; + + num+=4; + numIn1++; + shiftUp(false); + } + + + + + + inline void updateSSE_eighted( + const __m128 J0,const __m128 J1, + const __m128 J2,const __m128 J3, + const __m128 J4,const __m128 J5, + const __m128 J6,const __m128 J7, + const __m128 J8, const __m128 w) + { + float* pt=SSEData; + + __m128 J0w = _mm_mul_ps(J0,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J0))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J1))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J0w,J8))); pt+=4; + + __m128 J1w = _mm_mul_ps(J1,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J1))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J1w,J8))); pt+=4; + + __m128 J2w = _mm_mul_ps(J2,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J2))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J2w,J8))); pt+=4; + + __m128 J3w = _mm_mul_ps(J3,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3w,J3))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3w,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3w,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J3w,J8))); pt+=4; + + __m128 J4w = _mm_mul_ps(J4,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4w,J4))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4w,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J4w,J8))); pt+=4; + + __m128 J5w = _mm_mul_ps(J5,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5w,J5))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J5w,J8))); pt+=4; + + __m128 J6w = _mm_mul_ps(J6,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6w,J6))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J6w,J8))); pt+=4; + + __m128 J7w = _mm_mul_ps(J7,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7w,J7))); pt+=4; + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J7w,J8))); pt+=4; + + __m128 J8w = _mm_mul_ps(J8,w); + _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt),_mm_mul_ps(J8w,J8))); pt+=4; + + num+=4; + numIn1++; + shiftUp(false); + } + + + inline void updateSingle( + const float J0,const float J1, + const float J2,const float J3, + const float J4,const float J5, + const float J6,const float J7, + const float J8, int off=0) + { + float* pt=SSEData+off; + *pt += J0*J0; pt+=4; + *pt += J1*J0; pt+=4; + *pt += J2*J0; pt+=4; + *pt += J3*J0; pt+=4; + *pt += J4*J0; pt+=4; + *pt += J5*J0; pt+=4; + *pt += J6*J0; pt+=4; + *pt += J7*J0; pt+=4; + *pt += J8*J0; pt+=4; + + + *pt += J1*J1; pt+=4; + *pt += J2*J1; pt+=4; + *pt += J3*J1; pt+=4; + *pt += J4*J1; pt+=4; + *pt += J5*J1; pt+=4; + *pt += J6*J1; pt+=4; + *pt += J7*J1; pt+=4; + *pt += J8*J1; pt+=4; + + + *pt += J2*J2; pt+=4; + *pt += J3*J2; pt+=4; + *pt += J4*J2; pt+=4; + *pt += J5*J2; pt+=4; + *pt += J6*J2; pt+=4; + *pt += J7*J2; pt+=4; + *pt += J8*J2; pt+=4; + + + *pt += J3*J3; pt+=4; + *pt += J4*J3; pt+=4; + *pt += J5*J3; pt+=4; + *pt += J6*J3; pt+=4; + *pt += J7*J3; pt+=4; + *pt += J8*J3; pt+=4; + + + *pt += J4*J4; pt+=4; + *pt += J5*J4; pt+=4; + *pt += J6*J4; pt+=4; + *pt += J7*J4; pt+=4; + *pt += J8*J4; pt+=4; + + *pt += J5*J5; pt+=4; + *pt += J6*J5; pt+=4; + *pt += J7*J5; pt+=4; + *pt += J8*J5; pt+=4; + + + *pt += J6*J6; pt+=4; + *pt += J7*J6; pt+=4; + *pt += J8*J6; pt+=4; + + + *pt += J7*J7; pt+=4; + *pt += J8*J7; pt+=4; + + *pt += J8*J8; pt+=4; + + num++; + numIn1++; + shiftUp(false); + } + + inline void updateSingleWeighted( + float J0, float J1, + float J2, float J3, + float J4, float J5, + float J6, float J7, + float J8, float w, + int off=0) + { + + float* pt=SSEData+off; + *pt += J0*J0*w; pt+=4; J0*=w; + *pt += J1*J0; pt+=4; + *pt += J2*J0; pt+=4; + *pt += J3*J0; pt+=4; + *pt += J4*J0; pt+=4; + *pt += J5*J0; pt+=4; + *pt += J6*J0; pt+=4; + *pt += J7*J0; pt+=4; + *pt += J8*J0; pt+=4; + + + *pt += J1*J1*w; pt+=4; J1*=w; + *pt += J2*J1; pt+=4; + *pt += J3*J1; pt+=4; + *pt += J4*J1; pt+=4; + *pt += J5*J1; pt+=4; + *pt += J6*J1; pt+=4; + *pt += J7*J1; pt+=4; + *pt += J8*J1; pt+=4; + + + *pt += J2*J2*w; pt+=4; J2*=w; + *pt += J3*J2; pt+=4; + *pt += J4*J2; pt+=4; + *pt += J5*J2; pt+=4; + *pt += J6*J2; pt+=4; + *pt += J7*J2; pt+=4; + *pt += J8*J2; pt+=4; + + + *pt += J3*J3*w; pt+=4; J3*=w; + *pt += J4*J3; pt+=4; + *pt += J5*J3; pt+=4; + *pt += J6*J3; pt+=4; + *pt += J7*J3; pt+=4; + *pt += J8*J3; pt+=4; + + + *pt += J4*J4*w; pt+=4; J4*=w; + *pt += J5*J4; pt+=4; + *pt += J6*J4; pt+=4; + *pt += J7*J4; pt+=4; + *pt += J8*J4; pt+=4; + + *pt += J5*J5*w; pt+=4; J5*=w; + *pt += J6*J5; pt+=4; + *pt += J7*J5; pt+=4; + *pt += J8*J5; pt+=4; + + + *pt += J6*J6*w; pt+=4; J6*=w; + *pt += J7*J6; pt+=4; + *pt += J8*J6; pt+=4; + + + *pt += J7*J7*w; pt+=4; J7*=w; + *pt += J8*J7; pt+=4; + + *pt += J8*J8*w; pt+=4; + + num++; + numIn1++; + shiftUp(false); + } + + +private: + EIGEN_ALIGN16 float SSEData[4*45]; + EIGEN_ALIGN16 float SSEData1k[4*45]; + EIGEN_ALIGN16 float SSEData1m[4*45]; + float numIn1, numIn1k, numIn1m; + + + void shiftUp(bool force) + { + if(numIn1 > 1000 || force) + { + for(int i=0;i<45;i++) + _mm_store_ps(SSEData1k+4*i, _mm_add_ps(_mm_load_ps(SSEData+4*i),_mm_load_ps(SSEData1k+4*i))); + numIn1k+=numIn1; + numIn1=0; + memset(SSEData,0, sizeof(float)*4*45); + } + + if(numIn1k > 1000 || force) + { + for(int i=0;i<45;i++) + _mm_store_ps(SSEData1m+4*i, _mm_add_ps(_mm_load_ps(SSEData1k+4*i),_mm_load_ps(SSEData1m+4*i))); + numIn1m+=numIn1k; + numIn1k=0; + memset(SSEData1k,0, sizeof(float)*4*45); + } + } +}; +} diff --git a/src/dso/OptimizationBackend/RawResidualJacobian.h b/src/dso/OptimizationBackend/RawResidualJacobian.h new file mode 100644 index 0000000..c245804 --- /dev/null +++ b/src/dso/OptimizationBackend/RawResidualJacobian.h @@ -0,0 +1,63 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + + +#include "util/NumType.h" + +namespace dso +{ +struct RawResidualJacobian +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + // ================== new structure: save independently =============. + VecNRf resF; + + // the two rows of d[x,y]/d[xi]. + Vec6f Jpdxi[2]; // 2x6 + + // the two rows of d[x,y]/d[C]. + VecCf Jpdc[2]; // 2x4 + + // the two rows of d[x,y]/d[idepth]. + Vec2f Jpdd; // 2x1 + + // the two columns of d[r]/d[x,y]. + VecNRf JIdx[2]; // 9x2 + + // = the two columns of d[r] / d[ab] + VecNRf JabF[2]; // 9x2 + + + // = JIdx^T * JIdx (inner product). Only as a shorthand. + Mat22f JIdx2; // 2x2 + // = Jab^T * JIdx (inner product). Only as a shorthand. + Mat22f JabJIdx; // 2x2 + // = Jab^T * Jab (inner product). Only as a shorthand. + Mat22f Jab2; // 2x2 + +}; +} + diff --git a/src/dso/README.md b/src/dso/README.md new file mode 100644 index 0000000..0b64bac --- /dev/null +++ b/src/dso/README.md @@ -0,0 +1,283 @@ +# DSO: Direct Sparse Odometry + +Please note, that the source files in this folder are not the original DSO, but were modified for the inclusion in +DM-VIO. + +For more information see +[https://vision.in.tum.de/dso](https://vision.in.tum.de/dso) + +### 1. Related Papers +* **Direct Sparse Odometry**, *J. Engel, V. Koltun, D. Cremers*, In arXiv:1607.02565, 2016 +* **A Photometrically Calibrated Benchmark For Monocular Visual Odometry**, *J. Engel, V. Usenko, D. Cremers*, In arXiv:1607.02555, 2016 + +Get some datasets from [https://vision.in.tum.de/mono-dataset](https://vision.in.tum.de/mono-dataset) . + +### 2. Installation + + git clone https://github.com/JakobEngel/dso.git + +#### 2.1 Required Dependencies + +##### suitesparse and eigen3 (required). +Required. Install with + + sudo apt-get install libsuitesparse-dev libeigen3-dev libboost-all-dev + + + +#### 2.2 Optional Dependencies + +##### OpenCV (highly recommended). +Used to read / write / display images. +OpenCV is **only** used in `IOWrapper/OpenCV/*`. Without OpenCV, respective +dummy functions from `IOWrapper/*_dummy.cpp` will be compiled into the library, which do nothing. +The main binary will not be created, since it is useless if it can't read the datasets from disk. +Feel free to implement your own version of these functions with your prefered library, +if you want to stay away from OpenCV. + +Install with + + sudo apt-get install libopencv-dev + + +##### Pangolin (highly recommended). +Used for 3D visualization & the GUI. +Pangolin is **only** used in `IOWrapper/Pangolin/*`. You can compile without Pangolin, +however then there is not going to be any visualization / GUI capability. +Feel free to implement your own version of `Output3DWrapper` with your preferred library, +and use it instead of `PangolinDSOViewer` + +Install from [https://github.com/stevenlovegrove/Pangolin](https://github.com/stevenlovegrove/Pangolin) + + +##### ziplib (recommended). +Used to read datasets with images as .zip, as e.g. in the TUM monoVO dataset. +You can compile without this, however then you can only read images directly (i.e., have +to unzip the dataset image archives before loading them). + + sudo apt-get install zlib1g-dev + cd dso/thirdparty + tar -zxvf libzip-1.1.1.tar.gz + cd libzip-1.1.1/ + ./configure + make + sudo make install + sudo cp lib/zipconf.h /usr/local/include/zipconf.h # (no idea why that is needed). + +##### sse2neon (required for ARM builds). +After cloning, just run `git submodule update --init` to include this. It translates Intel-native SSE functions to ARM-native NEON functions during the compilation process. + +#### 2.3 Build + + cd dso + mkdir build + cd build + cmake .. + make -j + +this will compile a library `libdso.a`, which can be linked from external projects. +It will also build a binary `dso_dataset`, to run DSO on datasets. However, for this +OpenCV and Pangolin need to be installed. + + + + + + +### 3 Usage +Run on a dataset from [https://vision.in.tum.de/mono-dataset](https://vision.in.tum.de/mono-dataset) using + + bin/dso_dataset \ + files=XXXXX/sequence_XX/images.zip \ + calib=XXXXX/sequence_XX/camera.txt \ + gamma=XXXXX/sequence_XX/pcalib.txt \ + vignette=XXXXX/sequence_XX/vignette.png \ + preset=0 \ + mode=0 + +See [https://github.com/JakobEngel/dso_ros](https://github.com/JakobEngel/dso_ros) for a minimal example on +how the library can be used from another project. It should be straight forward to implement extentions for +other camera drivers, to use DSO interactively without ROS. + + + +#### 3.1 Dataset Format. +The format assumed is that of [https://vision.in.tum.de/mono-dataset](https://vision.in.tum.de/mono-dataset). +However, it should be easy to adapt it to your needs, if required. The binary is run with: + +- `files=XXX` where XXX is either a folder or .zip archive containing images. They are sorted *alphabetically*. for .zip to work, need to comiple with ziplib support. + +- `gamma=XXX` where XXX is a gamma calibration file, containing a single row with 256 values, mapping [0..255] to the respective irradiance value, i.e. containing the *discretized inverse response function*. See TUM monoVO dataset for an example. + +- `vignette=XXX` where XXX is a monochrome 16bit or 8bit image containing the vignette as pixelwise attenuation factors. See TUM monoVO dataset for an example. + +- `calib=XXX` where XXX is a geometric camera calibration file. See below. + + + +##### Geometric Calibration File. + + +###### Calibration File for Pre-Rectified Images + + Pinhole fx fy cx cy 0 + in_width in_height + "crop" / "full" / "none" / "fx fy cx cy 0" + out_width out_height + +###### Calibration File for FOV camera model: + + FOV fx fy cx cy omega + in_width in_height + "crop" / "full" / "fx fy cx cy 0" + out_width out_height + + +###### Calibration File for Radio-Tangential camera model + + RadTan fx fy cx cy k1 k2 r1 r2 + in_width in_height + "crop" / "full" / "fx fy cx cy 0" + out_width out_height + + +###### Calibration File for Equidistant camera model + + EquiDistant fx fy cx cy k1 k2 r1 r2 + in_width in_height + "crop" / "full" / "fx fy cx cy 0" + out_width out_height + + +(note: for backwards-compatibility, "Pinhole", "FOV" and "RadTan" can be omitted). See the respective +`::distortCoordinates` implementation in `Undistorter.cpp` for the exact corresponding projection function. +Furthermore, it should be straight-forward to implement other camera models. + + +**Explanation:** + Across all models `fx fy cx cy` denotes the focal length / principal point **relative to the image width / height**, +i.e., DSO computes the camera matrix `K` as + + K(0,0) = width * fx + K(1,1) = height * fy + K(0,2) = width * cx - 0.5 + K(1,2) = height * cy - 0.5 +For backwards-compatibility, if the given `cx` and `cy` are larger than 1, DSO assumes all four parameters to directly be the entries of K, +and ommits the above computation. + + +**That strange "0.5" offset:** + Internally, DSO uses the convention that the pixel at integer position (1,1) in the image, i.e. the pixel in the second row and second column, +contains the integral over the continuous image function from (0.5,0.5) to (1.5,1.5), i.e., approximates a "point-sample" of the +continuous image functions at (1.0, 1.0). +In turn, there seems to be no unifying convention across calibration toolboxes whether the pixel at integer position (1,1) +contains the integral over (0.5,0.5) to (1.5,1.5), or the integral over (1,1) to (2,2). The above conversion assumes that +the given calibration in the calibration file uses the latter convention, and thus applies the -0.5 correction. +Note that this also is taken into account when creating the scale-pyramid (see `globalCalib.cpp`). + + +**Rectification modes:** + For image rectification, DSO either supports rectification to a user-defined pinhole model (`fx fy cx cy 0`), +or has an option to automatically crop the image to the maximal rectangular, well-defined region (`crop`). +`full` will preserve the full original field of view and is mainly meant for debugging - it will create black +borders in undefined image regions, which DSO does NOT ignore (i.e., this option will generate additional +outliers along those borders, and corrupt the scale-pyramid). + + + + +#### 3.2 Commandline Options +there are many command line options available, see `main_dso_pangolin.cpp`. some examples include +- `mode=X`: + - `mode=0` use iff a photometric calibration exists (e.g. TUM monoVO dataset). + - `mode=1` use iff NO photometric calibration exists (e.g. ETH EuRoC MAV dataset). + - `mode=2` use iff images are not photometrically distorted (e.g. synthetic datasets). + +- `preset=X` + - `preset=0`: default settings (2k pts etc.), not enforcing real-time execution + - `preset=1`: default settings (2k pts etc.), enforcing 1x real-time execution + - `preset=2`: fast settings (800 pts etc.), not enforcing real-time execution. WARNING: overwrites image resolution with 424 x 320. + - `preset=3`: fast settings (800 pts etc.), enforcing 5x real-time execution. WARNING: overwrites image resolution with 424 x 320. + +- `nolog=1`: disable logging of eigenvalues etc. (good for performance) +- `reverse=1`: play sequence in reverse +- `nogui=1`: disable gui (good for performance) +- `nomt=1`: single-threaded execution +- `start=X`: start at frame X +- `end=X`: end at frame X +- `speed=X`: force execution at X times real-time speed (0 = not enforcing real-time) +- `save=1`: save lots of images for video creation +- `quiet=1`: disable most console output (good for performance) +- `sampleoutput=1`: register a "SampleOutputWrapper", printing some sample output data to the commandline. meant as example. + + + +#### 3.3 Runtime Options +Some parameters can be reconfigured from the Pangolin GUI at runtime. Feel free to add more. + + +#### 3.4 Accessing Data. +The easiest way to access the Data (poses, pointclouds, etc.) computed by DSO (in real-time) +is to create your own `Output3DWrapper`, and add it to the system, i.e., to `FullSystem.outputWrapper`. +The respective member functions will be called on various occations (e.g., when a new KF is created, +when a new frame is tracked, etc.), exposing the relevant data. + +See `IOWrapper/Output3DWrapper.h` for a description of the different callbacks available, +and some basic notes on where to find which data in the used classes. +See `IOWrapper/OutputWrapper/SampleOutputWrapper.h` for an example implementation, which just prints +some example data to the commandline (use the options `sampleoutput=1 quiet=1` to see the result). + +Note that these callbacks block the respective DSO thread, thus expensive computations should not +be performed in the callbacks, a better practice is to just copy over / publish / output the data you need. + +Per default, `dso_dataset` writes all keyframe poses to a file `result.txt` at the end of a sequence, +using the TUM RGB-D / TUM monoVO format ([timestamp x y z qx qy qz qw] of the cameraToWorld transformation). + + + +#### 3.5 Notes +- the initializer is very slow, and does not work very reliably. Maybe replace by your own way to get an initialization. +- see [https://github.com/JakobEngel/dso_ros](https://github.com/JakobEngel/dso_ros) for a minimal example project on how to use the library with your own input / output procedures. +- see `settings.cpp` for a LOT of settings parameters. Most of which you shouldn't touch. +- `setGlobalCalib(...)` needs to be called once before anything is initialized, and globally sets the camera intrinsics and video resolution for convenience. probably not the most portable way of doing this though. + + + + +### 4 General Notes for Good Results + +#### Accurate Geometric Calibration +- Please have a look at Chapter 4.3 from the DSO paper, in particular Figure 20 (Geometric Noise). Direct approaches suffer a LOT from bad geometric calibrations: Geometric distortions of 1.5 pixel already reduce the accuracy by factor 10. + +- **Do not use a rolling shutter camera**, the geometric distortions from a rolling shutter camera are huge. Even for high frame-rates (over 60fps). + +- Note that the reprojection RMSE reported by most calibration tools is the reprojection RMSE on the "training data", i.e., overfitted to the the images you used for calibration. If it is low, that does not imply that your calibration is good, you may just have used insufficient images. + +- try different camera / distortion models, not all lenses can be modelled by all models. + + +#### Photometric Calibration +Use a photometric calibration (e.g. using [https://github.com/tum-vision/mono_dataset_code](https://github.com/tum-vision/mono_dataset_code) ). + +#### Translation vs. Rotation +DSO cannot do magic: if you rotate the camera too much without translation, it will fail. Since it is a pure visual odometry, it cannot recover by re-localizing, or track through strong rotations by using previously triangulated geometry.... everything that leaves the field of view is marginalized immediately. + + +#### Computation Speed +If your computer is slow, try to use "fast" settings. Or run DSO on a dataset, without enforcing real-time. + + +#### Initialization +The current initializer is not very good... it is very slow and occasionally fails. +Make sure, the initial camera motion is slow and "nice" (i.e., a lot of translation and +little rotation) during initialization. +Possibly replace by your own initializer. + + +### 5 License +DSO was developed at the Technical University of Munich and Intel. +The open-source version is licensed under the GNU General Public License +Version 3 (GPLv3). +For commercial purposes, we also offer a professional version, see +[http://vision.in.tum.de/dso](http://vision.in.tum.de/dso) for +details. diff --git a/src/dso/util/DatasetReader.h b/src/dso/util/DatasetReader.h new file mode 100644 index 0000000..de17140 --- /dev/null +++ b/src/dso/util/DatasetReader.h @@ -0,0 +1,608 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#include "util/settings.h" +#include "util/globalFuncs.h" +#include "util/globalCalib.h" + +#include "util/GTData.hpp" +#include "IMU/IMUTypes.h" + +#include +#include +#include +#include + +#include "util/Undistort.h" +#include "IOWrapper/ImageRW.h" + +#if HAS_ZIPLIB + #include "zip.h" +#endif + +#include + +using namespace dso; + + + +inline int getdir (std::string dir, std::vector &files) +{ + DIR *dp; + struct dirent *dirp; + if((dp = opendir(dir.c_str())) == NULL) + { + return -1; + } + + while ((dirp = readdir(dp)) != NULL) { + std::string name = std::string(dirp->d_name); + + if(name != "." && name != "..") + files.push_back(name); + } + closedir(dp); + + + std::sort(files.begin(), files.end()); + + if(dir.at( dir.length() - 1 ) != '/') dir = dir+"/"; + for(unsigned int i=0;ipath = path; + this->calibfile = calibFile; + use16Bit = use16BitPassed; + +#if HAS_ZIPLIB + ziparchive=0; + databuffer=0; +#endif + + isZipped = (path.length()>4 && path.substr(path.length()-4) == ".zip"); + + + + + + if(isZipped) + { +#if HAS_ZIPLIB + int ziperror=0; + ziparchive = zip_open(path.c_str(), ZIP_RDONLY, &ziperror); + if(ziperror!=0) + { + printf("ERROR %d reading archive %s!\n", ziperror, path.c_str()); + exit(1); + } + + files.clear(); + int numEntries = zip_get_num_entries(ziparchive, 0); + for(int k=0;kgetOriginalSize()[0]; + heightOrg = undistort->getOriginalSize()[1]; + width=undistort->getSize()[0]; + height=undistort->getSize()[1]; + + + // load timestamps if possible. + loadTimestamps(); + printf("ImageFolderReader: got %d files in %s!\n", (int)files.size(), path.c_str()); + + } + ~ImageFolderReader() + { +#if HAS_ZIPLIB + if(ziparchive!=0) zip_close(ziparchive); + if(databuffer!=0) delete databuffer; +#endif + + + delete undistort; + }; + + Eigen::VectorXf getOriginalCalib() + { + return undistort->getOriginalParameter().cast(); + } + Eigen::Vector2i getOriginalDimensions() + { + return undistort->getOriginalSize(); + } + + void getCalibMono(Eigen::Matrix3f &K, int &w, int &h) + { + K = undistort->getK().cast(); + w = undistort->getSize()[0]; + h = undistort->getSize()[1]; + } + + void setGlobalCalibration() + { + int w_out, h_out; + Eigen::Matrix3f K; + getCalibMono(K, w_out, h_out); + setGlobalCalib(w_out, h_out, K); + } + + int getNumImages() + { + return files.size(); + } + + double getTimestamp(int id) + { + if(timestamps.size()==0) return id*0.1f; + if(id >= (int)timestamps.size()) return 0; + if(id < 0) return 0; + return timestamps[id]; + } + + std::string getFilename(int id) + { + return files[id]; + } + + void prepImage(int id, bool as8U=false) + { + + } + + + MinimalImageB* getImageRaw(int id) + { + return getImageRaw_internal(id,0); + } + + ImageAndExposure* getImage(int id, bool forceLoadDirectly=false) + { + return getImage_internal(id, 0); + } + + + inline float* getPhotometricGamma() + { + if(undistort==0 || undistort->photometricUndist==0) return 0; + return undistort->photometricUndist->getG(); + } + + dmvio::IMUData getIMUData(int i) + { + // returning IMU data between frame i-1 and frame i! + return imuDataAllFrames[i - 1]; + } + + dmvio::GTData getGTData(int id, bool &foundOut) + { + long long idReal = ids[id]; + auto it = gtData.lower_bound(idReal); + long long firstDist, secondDist; + long long dist = std::abs(idReal - it->first); + firstDist = dist; + if(it == gtData.end()) + { + foundOut = false; + return dmvio::GTData(); + } + + if(it != gtData.begin()) + { + it--; + secondDist = std::abs(idReal - it->first); + if(secondDist >= firstDist) + { + it++; + }else + { + dist = secondDist; + } + } + double distSeconds = (double) dist * 1e-9; + std::cout << "GTData distance (seconds): " << distSeconds << std::endl; + if(distSeconds > 0.01) + { + return dmvio::GTData{}; + } + + foundOut = true; + return it->second; + } + + bool loadGTData(std::string gtFile) + { + std::string defaultFile = path.substr(0, path.find_last_of('/')) + "/../state_groundtruth_estimate0/data.csv"; + std::cout << "Loading gt data" << std::endl; + + if(gtFile == "") + { + gtFile = defaultFile; + } + + std::ifstream tr; + tr.open(gtFile.c_str()); + + if(!tr.good()) + { + return false; + } + while(!tr.eof() && tr.good()) + { + std::string line; + char buf[1000]; + tr.getline(buf, 1000); + + long long id; + double p1, p2, p3, qw, qx, qy, qz, v1, v2, v3, br1, br2, br3, bp1, bp2, bp3; + if(17 == sscanf(buf, "%lld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", &id, &p1, &p2, &p3, &qw, &qx, &qy, &qz, &v1, &v2, &v3, &br1, &br2, &br3, &bp1, &bp2, &bp3)) + { + // EuRoC format with bias GT. + Eigen::Vector3d translation(p1, p2, p3); + Eigen::Quaterniond quat(qw, qx, qy, qz); + Sophus::SE3 pose(quat, translation); + Eigen::Vector3d velocity(v1, v2, v3); + Eigen::Vector3d biasRot(br1, br2, br3); + Eigen::Vector3d biasPos(bp1, bp2, bp3); + + gtData[id] = dmvio::GTData(pose, velocity, biasRot, biasPos); + + } else if(8 == sscanf(buf, "%lld,%lf,%lf,%lf,%lf,%lf,%lf,%lf", &id, &p1, &p2, &p3, &qw, &qx, &qy, &qz)) + { + // TUM-VI format + Eigen::Vector3d translation(p1, p2, p3); + Eigen::Quaterniond quat(qw, qx, qy, qz); + Sophus::SE3 pose(quat, translation); + Eigen::Vector3d velocity(0.0, 0.0, 0.0); + Eigen::Vector3d biasRot(0.0, 0.0, 0.0); + Eigen::Vector3d biasPos(0.0, 0.0, 0.0); + + gtData[id] = dmvio::GTData(pose, velocity, biasRot, biasPos); + } + } + tr.close(); + return true; + } + + + void loadIMUData(std::string imuFile = "") + { + // Important: This IMU loading method expects that for each image there is an IMU 'measurement' with exactly the same timestamp (the VI-sensor does this). + // If the sensor does not output this, a fake measurement with this timestamp has to be interpolated in advance. + // The DM-VIO Python tools have a script to do this. + if(imuFile == "") + { + imuFile = path.substr(0,path.find_last_of('/')) + "/imu.txt"; + } + std::ifstream imuStream(imuFile); + if(imuStream.good()) + { + std::string line; + std::getline(imuStream, line); + // At the moment only comments at the beginning of the file are supported. + while(line[0] == '#') + { + std::cout << "Skipping comment line in IMU data.\n"; + std::getline(imuStream, line); + } + std::stringstream lineStream(line); + long long imuStamp; + double wx, wy, wz, ax, ay, az; + lineStream >> imuStamp >> wx >> wy >> wz >> ax >> ay + >> az; + std::cout << "IMU Id: " << imuStamp << std::endl; + + // Find first frame with IMU data. + int startFrame = -1; + for(size_t j = 0; j < getNumImages(); ++j) + { + long long imageTimestamp = ids[j]; + while(imuStamp < imageTimestamp) + { + imuStream >> imuStamp >> wx >> wy >> wz >> ax >> ay >> az; + } + if(imuStamp == imageTimestamp) + { + // Success + startFrame = j; + break; + } + if(imuStamp > imageTimestamp) + { + std::cout << "IMU-data too old -> skipping frame" << std::endl; + imuDataAllFrames.push_back(dmvio::IMUData{}); + continue; + } + } + + if(startFrame == -1) + { + std::cout << "Found no start frame for IMU-data!" << std::endl; + imuStream.close(); + return; + } + + // For each image, we will save the IMU data between it, and the next frame, so no IMU data is needed for the last frame. + // Note that when later accessing the imu data in the method getIMUData we output the imu data between the given frame and the previous frame. + for(size_t j = startFrame; j < getNumImages() - 1; j++) + { + long long imageTimestamp = ids[j]; + long long nextTimestamp = ids[j+1]; + + assert(imuStamp == imageTimestamp); // Otherwise we would need to interpolate IMU data which is not implemented atm. + dmvio::IMUData imuData; + long long previousIMUTime = imuStamp; + + // Each frame should get the all IMU data with: + // thisTimestamp < imuStamp <= nextTimestamp + while(imuStamp < nextTimestamp) + { + // Get next IMU-Data. + imuStream >> imuStamp >> wx >> wy >> wz >> ax >> ay >> az; + + if(imuStamp > nextTimestamp) + { + // If this happens we would have to interpolate IMU data which is not implemented at the moment. + assert(false); + } + + Eigen::Vector3d accMeas, gyrMeas; + accMeas << ax, ay, az; + gyrMeas << wx, wy, wz; + // For each measurement GTSAM wants the time between it, and the previous measurement. + // The timestamps are in nanoseconds -> convert! + double integrationTime = (double) (imuStamp - previousIMUTime) * 1e-9; + imuData.push_back(dmvio::IMUMeasurement(accMeas, gyrMeas, integrationTime)); + + previousIMUTime = imuStamp; + } + + imuDataAllFrames.push_back(imuData); + } + }else + { + std::cout << "Found no IMU-data." << std::endl; + } + + + imuStream.close(); + + } + + // undistorter. [0] always exists, [1-2] only when MT is enabled. + Undistort* undistort; +private: + + + MinimalImageB* getImageRaw_internal(int id, int unused) + { + assert(!use16Bit); + if(!isZipped) + { + // CHANGE FOR ZIP FILE + return IOWrap::readImageBW_8U(files[id]); + } + else + { +#if HAS_ZIPLIB + if(databuffer==0) databuffer = new char[widthOrg*heightOrg*6+10000]; + zip_file_t* fle = zip_fopen(ziparchive, files[id].c_str(), 0); + long readbytes = zip_fread(fle, databuffer, (long)widthOrg*heightOrg*6+10000); + + if(readbytes > (long)widthOrg*heightOrg*6) + { + printf("read %ld/%ld bytes for file %s. increase buffer!!\n", readbytes,(long)widthOrg*heightOrg*6+10000, files[id].c_str()); + delete[] databuffer; + databuffer = new char[(long)widthOrg*heightOrg*30]; + fle = zip_fopen(ziparchive, files[id].c_str(), 0); + readbytes = zip_fread(fle, databuffer, (long)widthOrg*heightOrg*30+10000); + + if(readbytes > (long)widthOrg*heightOrg*30) + { + printf("buffer still to small (read %ld/%ld). abort.\n", readbytes,(long)widthOrg*heightOrg*30+10000); + exit(1); + } + } + + return IOWrap::readStreamBW_8U(databuffer, readbytes); +#else + printf("ERROR: cannot read .zip archive, as compile without ziplib!\n"); + exit(1); +#endif + } + } + + + ImageAndExposure* getImage_internal(int id, int unused) + { + if(use16Bit) + { + MinimalImage* minimg = IOWrap::readImageBW_16U(files[id]); + assert(minimg); + ImageAndExposure* ret2 = undistort->undistort( + minimg, + (exposures.size() == 0 ? 1.0f : exposures[id]), + (timestamps.size() == 0 ? 0.0 : timestamps[id]), + 1.0f / 256.0f); + delete minimg; + return ret2; + }else + { + MinimalImageB* minimg = getImageRaw_internal(id, 0); + ImageAndExposure* ret2 = undistort->undistort( + minimg, + (exposures.size() == 0 ? 1.0f : exposures[id]), + (timestamps.size() == 0 ? 0.0 : timestamps[id])); + delete minimg; + return ret2; + } + } + + inline void loadTimestamps() + { + std::ifstream tr; + std::string timesFile = path.substr(0,path.find_last_of('/')) + "/times.txt"; + tr.open(timesFile.c_str()); + while(!tr.eof() && tr.good()) + { + std::string line; + char buf[1000]; + tr.getline(buf, 1000); + + long long id; + double stamp; + float exposure = 0; + if(3 == sscanf(buf, "%lld %lf %f", &id, &stamp, &exposure)) + { + ids.push_back(id); + timestamps.push_back(stamp); + exposures.push_back(exposure); + } + + else if(2 == sscanf(buf, "%lld %lf", &id, &stamp)) + { + ids.push_back(id); + timestamps.push_back(stamp); + exposures.push_back(exposure); + } + } + tr.close(); + + // check if exposures are correct, (possibly skip) + bool exposuresGood = ((int)exposures.size()==(int)getNumImages()) ; + for(int i=0;i<(int)exposures.size();i++) + { + if(exposures[i] == 0) + { + // fix! + float sum=0,num=0; + if(i>0 && exposures[i-1] > 0) {sum += exposures[i-1]; num++;} + if(i+1<(int)exposures.size() && exposures[i+1] > 0) {sum += exposures[i+1]; num++;} + + if(num>0) + exposures[i] = sum/num; + } + + if(exposures[i] == 0) exposuresGood=false; + } + + + if((int)getNumImages() != (int)timestamps.size()) + { + printf("set timestamps and exposures to zero!\n"); + exposures.clear(); + timestamps.clear(); + } + + if((int)getNumImages() != (int)exposures.size() || !exposuresGood) + { + printf("set EXPOSURES to zero!\n"); + exposures.clear(); + } + + printf("got %d images and %d timestamps and %d exposures.!\n", (int)getNumImages(), (int)timestamps.size(), (int)exposures.size()); + } + + std::map gtData; + + std::vector preloadedImages; + std::vector files; + std::vector timestamps; + std::vector exposures; + std::vector ids; // Saves the ids that are used by e.g. the EuRoC dataset. + + std::vector imuDataAllFrames; + + int width, height; + int widthOrg, heightOrg; + + std::string path; + std::string calibfile; + + bool isZipped; + bool use16Bit; + +#if HAS_ZIPLIB + zip_t* ziparchive; + char* databuffer; +#endif +}; + diff --git a/src/dso/util/FrameShell.h b/src/dso/util/FrameShell.h new file mode 100644 index 0000000..78489b9 --- /dev/null +++ b/src/dso/util/FrameShell.h @@ -0,0 +1,82 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "util/NumType.h" +#include "algorithm" +#include + +namespace dso +{ + + +class FrameShell +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + int id; // INTERNAL ID, starting at zero. + int incoming_id; // ID passed into DSO + double timestamp; // timestamp passed into DSO. + + // set once after tracking + SE3 camToTrackingRef; + FrameShell* trackingRef; + + // constantly adapted. + SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex]. + AffLight aff_g2l; + bool poseValid; + bool trackingWasGood; + + int keyframeId = -1; // Id of the KF or -1 for non-KFs. + + // statisitcs + int statistics_outlierResOnThis; + int statistics_goodResOnThis; + int marginalizedAt; + double movedByOpt; + + static boost::mutex shellPoseMutex; + + inline FrameShell() + { + id=0; + poseValid=true; + trackingWasGood = true; + camToWorld = SE3(); + timestamp=0; + marginalizedAt=-1; + movedByOpt=0; + statistics_outlierResOnThis=statistics_goodResOnThis=0; + trackingRef=0; + camToTrackingRef = SE3(); + } +}; + + +} + diff --git a/src/dso/util/ImageAndExposure.h b/src/dso/util/ImageAndExposure.h new file mode 100644 index 0000000..75423d3 --- /dev/null +++ b/src/dso/util/ImageAndExposure.h @@ -0,0 +1,67 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once +#include +#include + + +namespace dso +{ + + +class ImageAndExposure +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + float* image; // irradiance. between 0 and 256 + int w,h; // width and height; + double timestamp; + float exposure_time; // exposure time in ms. + inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_) + { + image = new float[w*h]; + exposure_time=1; + } + inline ~ImageAndExposure() + { + delete[] image; + } + + inline void copyMetaTo(ImageAndExposure &other) + { + other.exposure_time = exposure_time; + } + + inline ImageAndExposure* getDeepCopy() + { + ImageAndExposure* img = new ImageAndExposure(w,h,timestamp); + img->exposure_time = exposure_time; + memcpy(img->image, image, w*h*sizeof(float)); + return img; + } +}; + + +} diff --git a/src/dso/util/IndexThreadReduce.h b/src/dso/util/IndexThreadReduce.h new file mode 100644 index 0000000..8bf459a --- /dev/null +++ b/src/dso/util/IndexThreadReduce.h @@ -0,0 +1,218 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#pragma once +#include "util/settings.h" +#include "boost/thread.hpp" +#include +#include + + + +namespace dso +{ +using namespace boost::placeholders; +template +class IndexThreadReduce +{ + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + inline IndexThreadReduce() + { + nextIndex = 0; + maxIndex = 0; + stepSize = 1; + callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); + + running = true; + for(int i=0;i callPerIndex, int first, int end, int stepSize = 0) + { + + memset(&stats, 0, sizeof(Running)); + +// if(!multiThreading) +// { +// callPerIndex(first, end, &stats, 0); +// return; +// } + + + + if(stepSize == 0) + stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS; + + + //printf("reduce called\n"); + + boost::unique_lock lock(exMutex); + + // save + this->callPerIndex = callPerIndex; + nextIndex = first; + maxIndex = end; + this->stepSize = stepSize; + + // go worker threads! + for(int i=0;icallPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); + + //printf("reduce done (all threads finished)\n"); + } + + Running stats; + +private: + boost::thread workerThreads[NUM_THREADS]; + bool isDone[NUM_THREADS]; + bool gotOne[NUM_THREADS]; + + boost::mutex exMutex; + boost::condition_variable todo_signal; + boost::condition_variable done_signal; + + int nextIndex; + int maxIndex; + int stepSize; + + bool running; + + boost::function callPerIndex; + + void callPerIndexDefault(int i, int j,Running* k, int tid) + { + printf("ERROR: should never be called....\n"); + assert(false); + } + + void workerLoop(int idx) + { + boost::unique_lock lock(exMutex); + + while(running) + { + // try to get something to do. + int todo = 0; + bool gotSomething = false; + if(nextIndex < maxIndex) + { + // got something! + todo = nextIndex; + nextIndex+=stepSize; + gotSomething = true; + } + + // if got something: do it (unlock in the meantime) + if(gotSomething) + { + lock.unlock(); + + assert(callPerIndex != 0); + + Running s; memset(&s, 0, sizeof(Running)); + callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx); + gotOne[idx] = true; + lock.lock(); + stats += s; + } + + // otherwise wait on signal, releasing lock in the meantime. + else + { + if(!gotOne[idx]) + { + lock.unlock(); + assert(callPerIndex != 0); + Running s; memset(&s, 0, sizeof(Running)); + callPerIndex(0, 0, &s, idx); + gotOne[idx] = true; + lock.lock(); + stats += s; + } + isDone[idx] = true; + //printf("worker %d waiting..\n", idx); + done_signal.notify_all(); + todo_signal.wait(lock); + } + } + } +}; +} diff --git a/src/dso/util/MinimalImage.h b/src/dso/util/MinimalImage.h new file mode 100644 index 0000000..0bfbe53 --- /dev/null +++ b/src/dso/util/MinimalImage.h @@ -0,0 +1,167 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "util/NumType.h" +#include "algorithm" + +namespace dso +{ + +template +class MinimalImage +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + int w; + int h; + T* data; + + /* + * creates minimal image with own memory + */ + inline MinimalImage(int w_, int h_) : w(w_), h(h_) + { + data = new T[w*h]; + ownData=true; + } + + /* + * creates minimal image wrapping around existing memory + */ + inline MinimalImage(int w_, int h_, T* data_) : w(w_), h(h_) + { + data = data_; + ownData=false; + } + + inline ~MinimalImage() + { + if(ownData) delete [] data; + } + + inline MinimalImage* getClone() + { + MinimalImage* clone = new MinimalImage(w,h); + memcpy(clone->data, data, sizeof(T)*w*h); + return clone; + } + + + inline T& at(int x, int y) {return data[(int)x+((int)y)*w];} + inline T& at(int i) {return data[i];} + + inline void setBlack() + { + memset(data, 0, sizeof(T)*w*h); + } + + inline void setConst(T val) + { + for(int i=0;i Vec3b; +typedef MinimalImage MinimalImageF; +typedef MinimalImage MinimalImageF3; +typedef MinimalImage MinimalImageB; +typedef MinimalImage MinimalImageB3; +typedef MinimalImage MinimalImageB16; + +} + diff --git a/src/dso/util/NumType.h b/src/dso/util/NumType.h new file mode 100644 index 0000000..d943b39 --- /dev/null +++ b/src/dso/util/NumType.h @@ -0,0 +1,195 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "Eigen/Core" +#include "sophus/sim3.hpp" +#include "sophus/se3.hpp" + + +namespace dso +{ + +// CAMERA MODEL TO USE + + +#define SSEE(val,idx) (*(((float*)&val)+idx)) + + +#define MAX_RES_PER_POINT 8 +#define NUM_THREADS 6 + + +#define todouble(x) (x).cast() + + +typedef Sophus::SE3d SE3; +typedef Sophus::Sim3d Sim3; +typedef Sophus::SO3d SO3; + + + +#define CPARS 4 + + +typedef Eigen::Matrix MatXX; +typedef Eigen::Matrix MatCC; +#define MatToDynamic(x) MatXX(x) + + + +typedef Eigen::Matrix MatC10; +typedef Eigen::Matrix Mat1010; +typedef Eigen::Matrix Mat1313; + +typedef Eigen::Matrix Mat810; +typedef Eigen::Matrix Mat83; +typedef Eigen::Matrix Mat66; +typedef Eigen::Matrix Mat53; +typedef Eigen::Matrix Mat43; +typedef Eigen::Matrix Mat42; +typedef Eigen::Matrix Mat33; +typedef Eigen::Matrix Mat22; +typedef Eigen::Matrix Mat8C; +typedef Eigen::Matrix MatC8; +typedef Eigen::Matrix Mat8Cf; +typedef Eigen::Matrix MatC8f; + +typedef Eigen::Matrix Mat88; +typedef Eigen::Matrix Mat77; + +typedef Eigen::Matrix VecC; +typedef Eigen::Matrix VecCf; +typedef Eigen::Matrix Vec13; +typedef Eigen::Matrix Vec10; +typedef Eigen::Matrix Vec9; +typedef Eigen::Matrix Vec8; +typedef Eigen::Matrix Vec7; +typedef Eigen::Matrix Vec6; +typedef Eigen::Matrix Vec5; +typedef Eigen::Matrix Vec4; +typedef Eigen::Matrix Vec3; +typedef Eigen::Matrix Vec2; +typedef Eigen::Matrix VecX; + +typedef Eigen::Matrix Mat33f; +typedef Eigen::Matrix Mat103f; +typedef Eigen::Matrix Mat22f; +typedef Eigen::Matrix Vec3f; +typedef Eigen::Matrix Vec2f; +typedef Eigen::Matrix Vec6f; + + + +typedef Eigen::Matrix Mat49; +typedef Eigen::Matrix Mat89; + +typedef Eigen::Matrix Mat94; +typedef Eigen::Matrix Mat98; + +typedef Eigen::Matrix Mat81; +typedef Eigen::Matrix Mat18; +typedef Eigen::Matrix Mat91; +typedef Eigen::Matrix Mat19; + + +typedef Eigen::Matrix Mat84; +typedef Eigen::Matrix Mat48; +typedef Eigen::Matrix Mat44; + + +typedef Eigen::Matrix VecNRf; +typedef Eigen::Matrix Vec12f; +typedef Eigen::Matrix Mat18f; +typedef Eigen::Matrix Mat66f; +typedef Eigen::Matrix Mat88f; +typedef Eigen::Matrix Mat84f; +typedef Eigen::Matrix Vec8f; +typedef Eigen::Matrix Vec10f; +typedef Eigen::Matrix Mat66f; +typedef Eigen::Matrix Vec4f; +typedef Eigen::Matrix Mat44f; +typedef Eigen::Matrix Mat1212f; +typedef Eigen::Matrix Vec12f; +typedef Eigen::Matrix Mat1313f; +typedef Eigen::Matrix Mat1010f; +typedef Eigen::Matrix Vec13f; +typedef Eigen::Matrix Mat99f; +typedef Eigen::Matrix Vec9f; + +typedef Eigen::Matrix Mat42f; +typedef Eigen::Matrix Mat62f; +typedef Eigen::Matrix Mat12f; + +typedef Eigen::Matrix VecXf; +typedef Eigen::Matrix MatXXf; + + +typedef Eigen::Matrix MatPCPC; +typedef Eigen::Matrix MatPCPCf; +typedef Eigen::Matrix VecPC; +typedef Eigen::Matrix VecPCf; + +typedef Eigen::Matrix Mat1414f; +typedef Eigen::Matrix Vec14f; +typedef Eigen::Matrix Mat1414; +typedef Eigen::Matrix Vec14; + + + + + + +// transforms points from one frame to another. +struct AffLight +{ + AffLight(double a_, double b_) : a(a_), b(b_) {}; + AffLight() : a(0), b(0) {}; + + // Affine Parameters: + double a,b; // I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b). + + static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T) + { + if(exposureF==0 || exposureT==0) + { + exposureT = exposureF = 1; + //printf("got exposure value of 0! please choose the correct model.\n"); + //assert(setting_brightnessTransferFunc < 2); + } + + double a = exp(g2T.a-g2F.a) * exposureT / exposureF; + double b = g2T.b - a*g2F.b; + return Vec2(a,b); + } + + Vec2 vec() + { + return Vec2(a,b); + } +}; + +} + diff --git a/src/dso/util/Undistort.cpp b/src/dso/util/Undistort.cpp new file mode 100644 index 0000000..9182aaa --- /dev/null +++ b/src/dso/util/Undistort.cpp @@ -0,0 +1,1236 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + + +#include +#include +#include + +#include +#include +#include "util/settings.h" +#include "util/globalFuncs.h" +#include "IOWrapper/ImageDisplay.h" +#include "IOWrapper/ImageRW.h" +#include "util/Undistort.h" + + +namespace dso +{ + + + + + + + + +PhotometricUndistorter::PhotometricUndistorter( + std::string file, + std::string noiseImage, + std::string vignetteImage, + int w_, int h_) +{ + valid=false; + vignetteMap=0; + vignetteMapInv=0; + w = w_; + h = h_; + output = new ImageAndExposure(w,h); + if(file=="" || vignetteImage=="") + { + printf("NO PHOTOMETRIC Calibration!\n"); + } + + + // read G. + std::ifstream f(file.c_str()); + printf("Reading Photometric Calibration from file %s\n",file.c_str()); + if (!f.good()) + { + printf("PhotometricUndistorter: Could not open file!\n"); + return; + } + + + + { + std::string line; + std::getline( f, line ); + std::istringstream l1i( line ); + std::vector Gvec = std::vector( std::istream_iterator(l1i), std::istream_iterator() ); + + + + GDepth = Gvec.size(); + + if(GDepth < 256) + { + printf("PhotometricUndistorter: invalid format! got %d entries in first line, expected at least 256!\n",(int)Gvec.size()); + return; + } + + + for(int i=0;i 0..255. + } + + if(setting_photometricCalibration==0) + { + for(int i=0;i* vm16 = IOWrap::readImageBW_16U(vignetteImage.c_str()); + MinimalImageB* vm8 = IOWrap::readImageBW_8U(vignetteImage.c_str()); + vignetteMap = new float[w*h]; + vignetteMapInv = new float[w*h]; + + if(vm16 != 0) + { + if(vm16->w != w ||vm16->h != h) + { + printf("PhotometricUndistorter: Invalid vignette image size! got %d x %d, expected %d x %d\n", + vm16->w, vm16->h, w, h); + if(vm16!=0) delete vm16; + if(vm8!=0) delete vm8; + return; + } + + float maxV=0; + for(int i=0;iat(i) > maxV) maxV = vm16->at(i); + + for(int i=0;iat(i) / maxV; + } + else if(vm8 != 0) + { + if(vm8->w != w ||vm8->h != h) + { + printf("PhotometricUndistorter: Invalid vignette image size! got %d x %d, expected %d x %d\n", + vm8->w, vm8->h, w, h); + if(vm16!=0) delete vm16; + if(vm8!=0) delete vm8; + return; + } + + float maxV=0; + for(int i=0;iat(i) > maxV) maxV = vm8->at(i); + + for(int i=0;iat(i) / maxV; + } + else + { + printf("PhotometricUndistorter: Invalid vignette image\n"); + if(vm16!=0) delete vm16; + if(vm8!=0) delete vm8; + return; + } + + if(vm16!=0) delete vm16; + if(vm8!=0) delete vm8; + + + for(int i=0;i GDepth-1.01f) + BinvC=GDepth-1.1; + else + { + int c = color; + float a = color-c; + BinvC=G[c]*(1-a) + G[c+1]*a; + } + + float val = BinvC; + if(val < 0) val = 0; + image[i] = val; + } +} + +template +void PhotometricUndistorter::processFrame(T* image_in, float exposure_time, float factor) +{ + int wh=w*h; + float* data = output->image; + assert(output->w == w && output->h == h); + assert(data != 0); + + + if(!valid || exposure_time <= 0 || setting_photometricCalibration==0) // disable full photometric calibration. + { + for(int i=0; iexposure_time = exposure_time; + output->timestamp = 0; + } + else + { + for(int i=0; iexposure_time = exposure_time; + output->timestamp = 0; + } + + + if(!setting_useExposure) + output->exposure_time = 1; + +} +template void PhotometricUndistorter::processFrame(unsigned char* image_in, float exposure_time, float factor); +template void PhotometricUndistorter::processFrame(unsigned short* image_in, float exposure_time, float factor); + + + + + +Undistort::~Undistort() +{ + if(remapX != 0) delete[] remapX; + if(remapY != 0) delete[] remapY; +} + +Undistort* Undistort::getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename) +{ + printf("Reading Calibration from file %s",configFilename.c_str()); + + std::ifstream f(configFilename.c_str()); + if (!f.good()) + { + f.close(); + printf(" ... not found. Cannot operate without calibration, shutting down.\n"); + f.close(); + return 0; + } + + printf(" ... found!\n"); + std::string l1; + std::getline(f,l1); + f.close(); + + float ic[10]; + + Undistort* u; + + // for backwards-compatibility: Use RadTan model for 8 parameters. + if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], + &ic[4], &ic[5], &ic[6], &ic[7]) == 8) + { + printf("found RadTan (OpenCV) camera model, building rectifier.\n"); + u = new UndistortRadTan(configFilename.c_str(), true); + if(!u->isValid()) {delete u; return 0; } + } + + // for backwards-compatibility: Use Pinhole / FoV model for 5 parameter. + else if(std::sscanf(l1.c_str(), "%f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], &ic[4]) == 5) + { + if(ic[4]==0) + { + printf("found PINHOLE camera model, building rectifier.\n"); + u = new UndistortPinhole(configFilename.c_str(), true); + if(!u->isValid()) {delete u; return 0; } + } + else + { + printf("found ATAN camera model, building rectifier.\n"); + u = new UndistortFOV(configFilename.c_str(), true); + if(!u->isValid()) {delete u; return 0; } + } + } + + + + + + // clean model selection implementation. + else if(std::sscanf(l1.c_str(), "KannalaBrandt %f %f %f %f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], + &ic[4], &ic[5], &ic[6], &ic[7]) == 8) + { + u = new UndistortKB(configFilename.c_str(), false); + if(!u->isValid()) {delete u; return 0; } + } + + + else if(std::sscanf(l1.c_str(), "RadTan %f %f %f %f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], + &ic[4], &ic[5], &ic[6], &ic[7]) == 8) + { + u = new UndistortRadTan(configFilename.c_str(), false); + if(!u->isValid()) {delete u; return 0; } + } + + + else if(std::sscanf(l1.c_str(), "EquiDistant %f %f %f %f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], + &ic[4], &ic[5], &ic[6], &ic[7]) == 8) + { + u = new UndistortEquidistant(configFilename.c_str(), false); + if(!u->isValid()) {delete u; return 0; } + } + + + else if(std::sscanf(l1.c_str(), "FOV %f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], + &ic[4]) == 5) + { + u = new UndistortFOV(configFilename.c_str(), false); + if(!u->isValid()) {delete u; return 0; } + } + + + else if(std::sscanf(l1.c_str(), "Pinhole %f %f %f %f %f", + &ic[0], &ic[1], &ic[2], &ic[3], + &ic[4]) == 5) + { + u = new UndistortPinhole(configFilename.c_str(), false); + if(!u->isValid()) {delete u; return 0; } + } + + + else + { + printf("could not read calib file! exit."); + exit(1); + } + + u->loadPhotometricCalibration( + gammaFilename, + "", + vignetteFilename); + + return u; +} + +void Undistort::loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage) +{ + photometricUndist = new PhotometricUndistorter(file, noiseImage, vignetteImage,getOriginalSize()[0], getOriginalSize()[1]); +} + +template +ImageAndExposure* Undistort::undistort(const MinimalImage* image_raw, float exposure, double timestamp, float factor) const +{ + if(image_raw->w != wOrg || image_raw->h != hOrg) + { + printf("Undistort::undistort: wrong image size (%d %d instead of %d %d) \n", image_raw->w, image_raw->h, w, h); + exit(1); + } + + photometricUndist->processFrame(image_raw->data, exposure, factor); + ImageAndExposure* result = new ImageAndExposure(w, h, timestamp); + photometricUndist->output->copyMetaTo(*result); + + if (!passthrough) + { + float* out_data = result->image; + float* in_data = photometricUndist->output->image; + + float* noiseMapX=0; + float* noiseMapY=0; + if(benchmark_varNoise>0) + { + int numnoise=(benchmark_noiseGridsize+8)*(benchmark_noiseGridsize+8); + noiseMapX=new float[numnoise]; + noiseMapY=new float[numnoise]; + memset(noiseMapX,0,sizeof(float)*numnoise); + memset(noiseMapY,0,sizeof(float)*numnoise); + + for(int i=0;i=0;idx--) + { + // get interp. values + float xx = remapX[idx]; + float yy = remapY[idx]; + + + + if(benchmark_varNoise>0) + { + float deltax = getInterpolatedElement11BiCub(noiseMapX, 4+(xx/(float)wOrg)*benchmark_noiseGridsize, 4+(yy/(float)hOrg)*benchmark_noiseGridsize, benchmark_noiseGridsize+8 ); + float deltay = getInterpolatedElement11BiCub(noiseMapY, 4+(xx/(float)wOrg)*benchmark_noiseGridsize, 4+(yy/(float)hOrg)*benchmark_noiseGridsize, benchmark_noiseGridsize+8 ); + float x = idx%w + deltax; + float y = idx/w + deltay; + if(x < 0.01) x = 0.01; + if(y < 0.01) y = 0.01; + if(x > w-1.01) x = w-1.01; + if(y > h-1.01) y = h-1.01; + + xx = getInterpolatedElement(remapX, x, y, w); + yy = getInterpolatedElement(remapY, x, y, w); + } + + + if(xx<0) + out_data[idx] = 0; + else + { + // get integer and rational parts + int xxi = xx; + int yyi = yy; + xx -= xxi; + yy -= yyi; + float xxyy = xx*yy; + + // get array base pointer + const float* src = in_data + xxi + yyi * wOrg; + + // interpolate (bilinear) + out_data[idx] = xxyy * src[1+wOrg] + + (yy-xxyy) * src[wOrg] + + (xx-xxyy) * src[1] + + (1-xx-yy+xxyy) * src[0]; + } + } + + if(benchmark_varNoise>0) + { + delete[] noiseMapX; + delete[] noiseMapY; + } + + } + else + { + memcpy(result->image, photometricUndist->output->image, sizeof(float)*w*h); + } + + applyBlurNoise(result->image); + + return result; +} +template ImageAndExposure* Undistort::undistort(const MinimalImage* image_raw, float exposure, double timestamp, float factor) const; +template ImageAndExposure* Undistort::undistort(const MinimalImage* image_raw, float exposure, double timestamp, float factor) const; + + +void Undistort::applyBlurNoise(float* img) const +{ + if(benchmark_varBlurNoise==0) return; + + int numnoise=(benchmark_noiseGridsize+8)*(benchmark_noiseGridsize+8); + float* noiseMapX=new float[numnoise]; + float* noiseMapY=new float[numnoise]; + float* blutTmp=new float[w*h]; + + if(benchmark_varBlurNoise>0) + { + for(int i=0;i 900 ) gmid = 900; + float gw = gaussMap[gmid]; + + if(x+dx>0 && x+dxw]; + } + + if(x-dx>0 && x-dxw]; + } + } + + blutTmp[x+y*this->w] = sumCW / sumW; + } + + // y-blur. + for(int x=0;x 900 ) gmid = 900; + float gw = gaussMap[gmid]; + + if(y+dy>0 && y+dyw]; + } + + if(y-dy>0 && y-dyw]; + } + } + img[x+y*this->w] = sumCW / sumW; + } + + + + delete[] noiseMapX; + delete[] noiseMapY; +} + +void Undistort::makeOptimalK_crop() +{ + printf("finding CROP optimal new model!\n"); + K.setIdentity(); + + // 1. stretch the center lines as far as possible, to get initial coarse quess. + float* tgX = new float[100000]; + float* tgY = new float[100000]; + float minX = 0; + float maxX = 0; + float minY = 0; + float maxY = 0; + + for(int x=0; x<100000;x++) + {tgX[x] = (x-50000.0f) / 10000.0f; tgY[x] = 0;} + distortCoordinates(tgX, tgY,tgX, tgY,100000); + for(int x=0; x<100000;x++) + { + if(tgX[x] > 0 && tgX[x] < wOrg-1) + { + if(minX==0) minX = (x-50000.0f) / 10000.0f; + maxX = (x-50000.0f) / 10000.0f; + } + } + for(int y=0; y<100000;y++) + {tgY[y] = (y-50000.0f) / 10000.0f; tgX[y] = 0;} + distortCoordinates(tgX, tgY,tgX, tgY,100000); + for(int y=0; y<100000;y++) + { + if(tgY[y] > 0 && tgY[y] < hOrg-1) + { + if(minY==0) minY = (y-50000.0f) / 10000.0f; + maxY = (y-50000.0f) / 10000.0f; + } + } + delete[] tgX; + delete[] tgY; + + minX *= 1.01; + maxX *= 1.01; + minY *= 1.01; + maxY *= 1.01; + + + + printf("initial range: x: %.4f - %.4f; y: %.4f - %.4f!\n", minX, maxX, minY, maxY); + + + + // 2. while there are invalid pixels at the border: shrink square at the side that has invalid pixels, + // if several to choose from, shrink the wider dimension. + bool oobLeft=true, oobRight=true, oobTop=true, oobBottom=true; + int iteration=0; + while(oobLeft || oobRight || oobTop || oobBottom) + { + oobLeft=oobRight=oobTop=oobBottom=false; + for(int y=0;y 0 && remapX[2*y] < wOrg-1)) + oobLeft = true; + if(!(remapX[2*y+1] > 0 && remapX[2*y+1] < wOrg-1)) + oobRight = true; + } + + + + for(int x=0;x 0 && remapY[2*x] < hOrg-1)) + oobTop = true; + if(!(remapY[2*x+1] > 0 && remapY[2*x+1] < hOrg-1)) + oobBottom = true; + } + + + if((oobLeft || oobRight) && (oobTop || oobBottom)) + { + if((maxX-minX) > (maxY-minY)) + oobBottom = oobTop = false; // only shrink left/right + else + oobLeft = oobRight = false; // only shrink top/bottom + } + + if(oobLeft) minX *= 0.995; + if(oobRight) maxX *= 0.995; + if(oobTop) minY *= 0.995; + if(oobBottom) maxY *= 0.995; + + iteration++; + + + printf("iteration %05d: range: x: %.4f - %.4f; y: %.4f - %.4f!\n", iteration, minX, maxX, minY, maxY); + if(iteration > 500) + { + printf("FAILED TO COMPUTE GOOD CAMERA MATRIX - SOMETHING IS SERIOUSLY WRONG. ABORTING \n"); + exit(1); + } + } + + K(0,0) = ((float)w-1.0f)/(maxX-minX); + K(1,1) = ((float)h-1.0f)/(maxY-minY); + K(0,2) = -minX*K(0,0); + K(1,2) = -minY*K(1,1); + +} + +void Undistort::makeOptimalK_full() +{ + // todo + assert(false); +} + +void Undistort::readFromFile(const char* configFileName, int nPars, std::string prefix) +{ + photometricUndist=0; + valid = false; + passthrough=false; + remapX = 0; + remapY = 0; + + float outputCalibration[5]; + + parsOrg = VecX(nPars); + + // read parameters + std::ifstream infile(configFileName); + assert(infile.good()); + + std::string l1,l2,l3,l4; + + std::getline(infile,l1); + std::getline(infile,l2); + std::getline(infile,l3); + std::getline(infile,l4); + + // l1 & l2 + if(nPars == 5) // fov model + { + char buf[1000]; + snprintf(buf, 1000, "%s%%lf %%lf %%lf %%lf %%lf", prefix.c_str()); + + if(std::sscanf(l1.c_str(), buf, &parsOrg[0], &parsOrg[1], &parsOrg[2], &parsOrg[3], &parsOrg[4]) == 5 && + std::sscanf(l2.c_str(), "%d %d", &wOrg, &hOrg) == 2) + { + printf("Input resolution: %d %d\n",wOrg, hOrg); + printf("In: %f %f %f %f %f\n", + parsOrg[0], parsOrg[1], parsOrg[2], parsOrg[3], parsOrg[4]); + } + else + { + printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); + infile.close(); + return; + } + } + else if(nPars == 8) // KB, equi & radtan model + { + char buf[1000]; + snprintf(buf, 1000, "%s%%lf %%lf %%lf %%lf %%lf %%lf %%lf %%lf %%lf %%lf", prefix.c_str()); + + if(std::sscanf(l1.c_str(), buf, + &parsOrg[0], &parsOrg[1], &parsOrg[2], &parsOrg[3], &parsOrg[4], + &parsOrg[5], &parsOrg[6], &parsOrg[7]) == 8 && + std::sscanf(l2.c_str(), "%d %d", &wOrg, &hOrg) == 2) + { + printf("Input resolution: %d %d\n",wOrg, hOrg); + printf("In: %s%f %f %f %f %f %f %f %f\n", + prefix.c_str(), + parsOrg[0], parsOrg[1], parsOrg[2], parsOrg[3], parsOrg[4], parsOrg[5], parsOrg[6], parsOrg[7]); + } + else + { + printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); + infile.close(); + return; + } + } + else + { + printf("called with invalid number of parameters.... forgot to implement me?\n"); + infile.close(); + return; + } + + + + if(parsOrg[2] < 1 && parsOrg[3] < 1) + { + printf("\n\nFound fx=%f, fy=%f, cx=%f, cy=%f.\n I'm assuming this is the \"relative\" calibration file format," + "and will rescale this by image width / height to fx=%f, fy=%f, cx=%f, cy=%f.\n\n", + parsOrg[0], parsOrg[1], parsOrg[2], parsOrg[3], + parsOrg[0] * wOrg, parsOrg[1] * hOrg, parsOrg[2] * wOrg - 0.5, parsOrg[3] * hOrg - 0.5 ); + + // rescale and substract 0.5 offset. + // the 0.5 is because I'm assuming the calibration is given such that the pixel at (0,0) + // contains the integral over intensity over [0,0]-[1,1], whereas I assume the pixel (0,0) + // to contain a sample of the intensity ot [0,0], which is best approximated by the integral over + // [-0.5,-0.5]-[0.5,0.5]. Thus, the shift by -0.5. + parsOrg[0] = parsOrg[0] * wOrg; + parsOrg[1] = parsOrg[1] * hOrg; + parsOrg[2] = parsOrg[2] * wOrg - 0.5; + parsOrg[3] = parsOrg[3] * hOrg - 0.5; + } + + + + // l3 + if(l3 == "crop") + { + outputCalibration[0] = -1; + printf("Out: Rectify Crop\n"); + } + else if(l3 == "full") + { + outputCalibration[0] = -2; + printf("Out: Rectify Full\n"); + } + else if(l3 == "none") + { + outputCalibration[0] = -3; + printf("Out: No Rectification\n"); + } + else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) + { + printf("Out: %f %f %f %f %f\n", + outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); + + } + else + { + printf("Out: Failed to Read Output pars... not rectifying.\n"); + infile.close(); + return; + } + + + // l4 + if(std::sscanf(l4.c_str(), "%d %d", &w, &h) == 2) + { + if(benchmarkSetting_width != 0) + { + w = benchmarkSetting_width; + if(outputCalibration[0] == -3) + outputCalibration[0] = -1; // crop instead of none, since probably resolution changed. + } + if(benchmarkSetting_height != 0) + { + h = benchmarkSetting_height; + if(outputCalibration[0] == -3) + outputCalibration[0] = -1; // crop instead of none, since probably resolution changed. + } + + printf("Output resolution: %d %d\n",w, h); + } + else + { + printf("Out: Failed to Read Output resolution... not rectifying.\n"); + valid = false; + } + + remapX = new float[w*h]; + remapY = new float[w*h]; + + if(outputCalibration[0] == -1) + makeOptimalK_crop(); + else if(outputCalibration[0] == -2) + makeOptimalK_full(); + else if(outputCalibration[0] == -3) + { + if(w != wOrg || h != hOrg) + { + printf("ERROR: rectification mode none requires input and output dimenstions to match!\n\n"); + exit(1); + } + K.setIdentity(); + K(0,0) = parsOrg[0]; + K(1,1) = parsOrg[1]; + K(0,2) = parsOrg[2]; + K(1,2) = parsOrg[3]; + passthrough = true; + } + else + { + + + if(outputCalibration[2] > 1 || outputCalibration[3] > 1) + { + printf("\n\n\nWARNING: given output calibration (%f %f %f %f) seems wrong. It needs to be relative to image width / height!\n\n\n", + outputCalibration[0],outputCalibration[1],outputCalibration[2],outputCalibration[3]); + } + + + K.setIdentity(); + K(0,0) = outputCalibration[0] * w; + K(1,1) = outputCalibration[1] * h; + K(0,2) = outputCalibration[2] * w - 0.5; + K(1,2) = outputCalibration[3] * h - 0.5; + } + + if(benchmarkSetting_fxfyfac != 0) + { + K(0,0) = fmax(benchmarkSetting_fxfyfac, (float)K(0,0)); + K(1,1) = fmax(benchmarkSetting_fxfyfac, (float)K(1,1)); + passthrough = false; // cannot pass through when fx / fy have been overwritten. + } + + + for(int y=0;y 0 && iy > 0 && ix < wOrg-1 && iy < wOrg-1) + { + remapX[x+y*w] = ix; + remapY[x+y*w] = iy; + } + else + { + remapX[x+y*w] = -1; + remapY[x+y*w] = -1; + } + } + + valid = true; + + + + + printf("\nRectified Kamera Matrix:\n"); + std::cout << K << "\n\n"; + +} + + +UndistortFOV::UndistortFOV(const char* configFileName, bool noprefix) +{ + printf("Creating FOV undistorter\n"); + + if(noprefix) + readFromFile(configFileName, 5); + else + readFromFile(configFileName, 5, "FOV "); +} +UndistortFOV::~UndistortFOV() +{ +} + +void UndistortFOV::distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const +{ + float dist = parsOrg[4]; + float d2t = 2.0f * tan(dist / 2.0f); + + + + // current camera parameters + float fx = parsOrg[0]; + float fy = parsOrg[1]; + float cx = parsOrg[2]; + float cy = parsOrg[3]; + + + + float ofx = K(0,0); + float ofy = K(1,1); + float ocx = K(0,2); + float ocy = K(1,2); + + for(int i=0;i 1e-8) ? thetad / r : 1.0; + float ox = fx*ix*scaling + cx; + float oy = fy*iy*scaling + cy; + + out_x[i] = ox; + out_y[i] = oy; + } +} + + + +UndistortKB::UndistortKB(const char* configFileName, bool noprefix) +{ + printf("Creating KannalaBrandt undistorter\n"); + + if(noprefix) + readFromFile(configFileName, 8); + else + readFromFile(configFileName, 8,"KannalaBrandt "); +} +UndistortKB::~UndistortKB() +{ +} + +void UndistortKB::distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const +{ + const float fx = parsOrg[0]; + const float fy = parsOrg[1]; + const float cx = parsOrg[2]; + const float cy = parsOrg[3]; + const float k0 = parsOrg[4]; + const float k1 = parsOrg[5]; + const float k2 = parsOrg[6]; + const float k3 = parsOrg[7]; + + const float ofx = K(0,0); + const float ofy = K(1,1); + const float ocx = K(0,2); + const float ocy = K(1,2); + + + for(int i=0;i, +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +#pragma once + +#include "util/ImageAndExposure.h" +#include "util/MinimalImage.h" +#include "util/NumType.h" +#include "Eigen/Core" + + + + + +namespace dso +{ + + +class PhotometricUndistorter +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + PhotometricUndistorter(std::string file, std::string noiseImage, std::string vignetteImage, int w_, int h_); + ~PhotometricUndistorter(); + + // removes readout noise, and converts to irradiance. + // affine normalizes values to 0 <= I < 256. + // raw irradiance = a*I + b. + // output will be written in [output]. + template void processFrame(T* image_in, float exposure_time, float factor=1); + void unMapFloatImage(float* image); + + ImageAndExposure* output; + + float* getG() {if(!valid) return 0; else return G;}; +private: + float G[256*256]; + int GDepth; + float* vignetteMap; + float* vignetteMapInv; + int w,h; + bool valid; +}; + + +class Undistort +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + virtual ~Undistort(); + + virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0; + + + inline const Mat33 getK() const {return K;}; + inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);}; + inline const VecX getOriginalParameter() const {return parsOrg;}; + inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);}; + inline bool isValid() {return valid;}; + + template + ImageAndExposure* undistort(const MinimalImage* image_raw, float exposure=0, double timestamp=0, float factor=1) const; + static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename); + + void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage); + + PhotometricUndistorter* photometricUndist; + +protected: + int w, h, wOrg, hOrg, wUp, hUp; + int upsampleUndistFactor; + Mat33 K; + VecX parsOrg; + bool valid; + bool passthrough; + + float* remapX; + float* remapY; + + void applyBlurNoise(float* img) const; + + void makeOptimalK_crop(); + void makeOptimalK_full(); + + void readFromFile(const char* configFileName, int nPars, std::string prefix = ""); +}; + +class UndistortFOV : public Undistort +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + UndistortFOV(const char* configFileName, bool noprefix); + ~UndistortFOV(); + void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; + +}; + +class UndistortRadTan : public Undistort +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + UndistortRadTan(const char* configFileName, bool noprefix); + ~UndistortRadTan(); + void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; + +}; + +class UndistortEquidistant : public Undistort +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + UndistortEquidistant(const char* configFileName, bool noprefix); + ~UndistortEquidistant(); + void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; + +}; + +class UndistortPinhole : public Undistort +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + UndistortPinhole(const char* configFileName, bool noprefix); + ~UndistortPinhole(); + void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; + +private: + float inputCalibration[8]; +}; + +class UndistortKB : public Undistort +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + UndistortKB(const char* configFileName, bool noprefix); + ~UndistortKB(); + void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; + +}; + +} + diff --git a/src/dso/util/globalCalib.cpp b/src/dso/util/globalCalib.cpp new file mode 100644 index 0000000..3851a34 --- /dev/null +++ b/src/dso/util/globalCalib.cpp @@ -0,0 +1,108 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "util/globalCalib.h" +#include "stdio.h" +#include + +namespace dso +{ + int wG[PYR_LEVELS], hG[PYR_LEVELS]; + float fxG[PYR_LEVELS], fyG[PYR_LEVELS], + cxG[PYR_LEVELS], cyG[PYR_LEVELS]; + + float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], + cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; + + Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; + + + float wM3G; + float hM3G; + + void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K) + { + int wlvl=w; + int hlvl=h; + pyrLevelsUsed=1; + while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS) + { + wlvl /=2; + hlvl /=2; + pyrLevelsUsed++; + } + printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", + pyrLevelsUsed-1, wlvl, hlvl); + if(wlvl>100 && hlvl > 100) + { + printf("\n\n===============WARNING!===================\n " + "using not enough pyramid levels.\n" + "Consider scaling to a resolution that is a multiple of a power of 2.\n"); + } + if(pyrLevelsUsed < 3) + { + printf("\n\n===============WARNING!===================\n " + "I need higher resolution.\n" + "I will probably segfault.\n"); + } + + wM3G = w-3; + hM3G = h-3; + + wG[0] = w; + hG[0] = h; + KG[0] = K; + fxG[0] = K(0,0); + fyG[0] = K(1,1); + cxG[0] = K(0,2); + cyG[0] = K(1,2); + KiG[0] = KG[0].inverse(); + fxiG[0] = KiG[0](0,0); + fyiG[0] = KiG[0](1,1); + cxiG[0] = KiG[0](0,2); + cyiG[0] = KiG[0](1,2); + + for (int level = 1; level < pyrLevelsUsed; ++ level) + { + wG[level] = w >> level; + hG[level] = h >> level; + + fxG[level] = fxG[level-1] * 0.5; + fyG[level] = fyG[level-1] * 0.5; + cxG[level] = (cxG[0] + 0.5) / ((int)1<, +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#pragma once +#include "util/settings.h" +#include "util/NumType.h" + +namespace dso +{ + extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; + extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], + cxG[PYR_LEVELS], cyG[PYR_LEVELS]; + + extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], + cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; + + extern Eigen::Matrix3f KG[PYR_LEVELS],KiG[PYR_LEVELS]; + + extern float wM3G; + extern float hM3G; + + void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K ); +} diff --git a/src/dso/util/globalFuncs.h b/src/dso/util/globalFuncs.h new file mode 100644 index 0000000..9a2acca --- /dev/null +++ b/src/dso/util/globalFuncs.h @@ -0,0 +1,421 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#pragma once +#include "util/settings.h" +#include "util/NumType.h" +#include "IOWrapper/ImageDisplay.h" +#include "fstream" +#include + +#ifdef STACKTRACE +#include +#endif + +namespace dso +{ + + +// Check bounds (assuming that the elements (ix, iy); (ix+1, iy); (ix, iy+1); (ix+1, iy+1) are accessed. +inline void checkBoundsPlus1(int ix, int iy, const int width) +{ +#ifdef DEBUG +// Note: That only getInterpolatedElement31, getInterpolatedElement33 and getInterpolatedElement33BiLin are actually used in the code. +// + getInterpolatedElement11BiCub and getInterpolatedElement are used in Undistort. + int height = 512; // TODO: Pass the actual height to the function (should be optimized out if not used anyway, so no performance hit). + bool oob = ix < 0 || iy < 0 || ix + 1 >= width || iy + 1 >= height; + if(oob) + { +#ifdef STACKTRACE + std::cout << boost::stacktrace::stacktrace(); +#endif + std::cout << "ERROR: Trying to access invalid element: ix: " << ix << " iy: " << iy << " width: " << width << std::endl; + assert(0); + } +#endif + +} + +// reads interpolated element from a uchar* array +// SSE2 optimization possible +EIGEN_ALWAYS_INLINE float getInterpolatedElement(const float* const mat, const float x, const float y, const int width) +{ + //stats.num_pixelInterpolations++; + + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const float* bp = mat +ix+iy*width; + + checkBoundsPlus1(ix, iy, width); + + float res = dxdy * bp[1+width] + + (dy-dxdy) * bp[width] + + (dx-dxdy) * bp[1] + + (1-dx-dy+dxdy) * bp[0]; + + return res; +} + +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement43(const Eigen::Vector4f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector4f* bp = mat +ix+iy*width; + + checkBoundsPlus1(ix, iy, width); + + return dxdy * *(const Eigen::Vector3f*)(bp+1+width) + + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width) + + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1) + + (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33(const Eigen::Vector3f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector3f* bp = mat +ix+iy*width; + + checkBoundsPlus1(ix, iy, width); + + return dxdy * *(const Eigen::Vector3f*)(bp+1+width) + + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width) + + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1) + + (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33OverAnd(const Eigen::Vector3f* const mat, const bool* overMat, const float x, const float y, const int width, bool& over_out) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector3f* bp = mat +ix+iy*width; + + const bool* bbp = overMat +ix+iy*width; + over_out = bbp[1+width] && bbp[1] && bbp[width] && bbp[0]; + + checkBoundsPlus1(ix, iy, width); + + return dxdy * *(const Eigen::Vector3f*)(bp+1+width) + + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width) + + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1) + + (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp); +} +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33OverOr(const Eigen::Vector3f* const mat, const bool* overMat, const float x, const float y, const int width, bool& over_out) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector3f* bp = mat +ix+iy*width; + + const bool* bbp = overMat +ix+iy*width; + over_out = bbp[1+width] || bbp[1] || bbp[width] || bbp[0]; + + checkBoundsPlus1(ix, iy, width); + + return dxdy * *(const Eigen::Vector3f*)(bp+1+width) + + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width) + + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1) + + (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp); +} + + +EIGEN_ALWAYS_INLINE float getInterpolatedElement31(const Eigen::Vector3f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector3f* bp = mat +ix+iy*width; + + checkBoundsPlus1(ix, iy, width); + + return dxdy * (*(const Eigen::Vector3f*)(bp+1+width))[0] + + (dy-dxdy) * (*(const Eigen::Vector3f*)(bp+width))[0] + + (dx-dxdy) * (*(const Eigen::Vector3f*)(bp+1))[0] + + (1-dx-dy+dxdy) * (*(const Eigen::Vector3f*)(bp))[0]; +} + +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement13BiLin(const float* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + const float* bp = mat +ix+iy*width; + + checkBoundsPlus1(ix, iy, width); + + float tl = *(bp); + float tr = *(bp+1); + float bl = *(bp+width); + float br = *(bp+width+1); + + float dx = x - ix; + float dy = y - iy; + float topInt = dx * tr + (1-dx) * tl; + float botInt = dx * br + (1-dx) * bl; + float leftInt = dy * bl + (1-dy) * tl; + float rightInt = dy * br + (1-dy) * tr; + + return Eigen::Vector3f( + dx * rightInt + (1-dx) * leftInt, + rightInt-leftInt, + botInt-topInt); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33BiLin(const Eigen::Vector3f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + const Eigen::Vector3f* bp = mat +ix+iy*width; + + checkBoundsPlus1(ix, iy, width); + + float tl = (*(bp))[0]; + float tr = (*(bp+1))[0]; + float bl = (*(bp+width))[0]; + float br = (*(bp+width+1))[0]; + + float dx = x - ix; + float dy = y - iy; + float topInt = dx * tr + (1-dx) * tl; + float botInt = dx * br + (1-dx) * bl; + float leftInt = dy * bl + (1-dy) * tl; + float rightInt = dy * br + (1-dy) * tr; + + return Eigen::Vector3f( + dx * rightInt + (1-dx) * leftInt, + rightInt-leftInt, + botInt-topInt); +} +EIGEN_ALWAYS_INLINE float getInterpolatedElement11Cub(const float* const p, const float x) // for x=0, this returns p[1]. +{ + return p[1] + 0.5f * x*(p[2] - p[0] + x*(2.0f*p[0] - 5.0f*p[1] + 4.0f*p[2] - p[3] + x*(3.0f*(p[1] - p[2]) + p[3] - p[0]))); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement12Cub(const float* const p, const float x) // for x=0, this returns p[1]. +{ + float c1 = 0.5f * (p[2] - p[0]); + float c2 = p[0]-2.5f*p[1]+2*p[2]-0.5f*p[3]; + float c3 = 0.5f*(3.0f*(p[1]-p[2])+p[3]-p[0]); + float xx = x*x; + float xxx = xx*x; + return Eigen::Vector2f(p[1] + x*c1 + xx*c2 + xxx*c3, c1 + x*2.0f*c2 + xx*3.0f*c3); +} +EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement32Cub(const Eigen::Vector3f* const p, const float x) // for x=0, this returns p[1]. +{ + float c1 = 0.5f * (p[2][0] - p[0][0]); + float c2 = p[0][0]-2.5f*p[1][0]+2*p[2][0]-0.5f*p[3][0]; + float c3 = 0.5f*(3.0f*(p[1][0]-p[2][0])+p[3][0]-p[0][0]); + float xx = x*x; + float xxx = xx*x; + return Eigen::Vector2f(p[1][0] + x*c1 + xx*c2 + xxx*c3, c1 + x*2.0f*c2 + xx*3.0f*c3); +} + +EIGEN_ALWAYS_INLINE float getInterpolatedElement11BiCub(const float* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + const float* bp = mat +ix+iy*width; + + float val[4]; + val[0] =getInterpolatedElement11Cub(bp-width-1, dx); + val[1] =getInterpolatedElement11Cub(bp-1, dx); + val[2] =getInterpolatedElement11Cub(bp+width-1, dx); + val[3] =getInterpolatedElement11Cub(bp+2*width-1, dx); + + float dy = y - iy; + return getInterpolatedElement11Cub(val, dy); +} +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement13BiCub(const float* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + const float* bp = mat +ix+iy*width; + + float val[4]; + float grad[4]; + Eigen::Vector2f v = getInterpolatedElement12Cub(bp-width-1, dx); + val[0] = v[0]; grad[0] = v[1]; + + v = getInterpolatedElement12Cub(bp-1, dx); + val[1] = v[0]; grad[1] = v[1]; + + v = getInterpolatedElement12Cub(bp+width-1, dx); + val[2] = v[0]; grad[2] = v[1]; + + v = getInterpolatedElement12Cub(bp+2*width-1, dx); + val[3] = v[0]; grad[3] = v[1]; + + float dy = y - iy; + v = getInterpolatedElement12Cub(val, dy); + + return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33BiCub(const Eigen::Vector3f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + const Eigen::Vector3f* bp = mat +ix+iy*width; + + float val[4]; + float grad[4]; + Eigen::Vector2f v = getInterpolatedElement32Cub(bp-width-1, dx); + val[0] = v[0]; grad[0] = v[1]; + + v = getInterpolatedElement32Cub(bp-1, dx); + val[1] = v[0]; grad[1] = v[1]; + + v = getInterpolatedElement32Cub(bp+width-1, dx); + val[2] = v[0]; grad[2] = v[1]; + + v = getInterpolatedElement32Cub(bp+2*width-1, dx); + val[3] = v[0]; grad[3] = v[1]; + + float dy = y - iy; + v = getInterpolatedElement12Cub(val, dy); + + return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector4f getInterpolatedElement44(const Eigen::Vector4f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector4f* bp = mat +ix+iy*width; + + + return dxdy * *(bp+1+width) + + (dy-dxdy) * *(bp+width) + + (dx-dxdy) * *(bp+1) + + (1-dx-dy+dxdy) * *(bp); +} + +EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement42(const Eigen::Vector4f* const mat, const float x, const float y, const int width) +{ + int ix = (int)x; + int iy = (int)y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx*dy; + const Eigen::Vector4f* bp = mat +ix+iy*width; + + + return dxdy * *(const Eigen::Vector2f*)(bp+1+width) + + (dy-dxdy) * *(const Eigen::Vector2f*)(bp+width) + + (dx-dxdy) * *(const Eigen::Vector2f*)(bp+1) + + (1-dx-dy+dxdy) * *(const Eigen::Vector2f*)(bp); +} + + + +inline Vec3f makeRainbowf3F(float id) +{ + id *= freeDebugParam3; + if(id < 0) + return Vec3f(1,1,1); + + int icP = id; + float ifP = id-icP; + icP = icP%3; + + if(icP == 0) return Vec3f((1-ifP), ifP, 0); + if(icP == 1) return Vec3f(0, (1-ifP), ifP); + if(icP == 2) return Vec3f(ifP, 0, (1-ifP)); + assert(false); + return Vec3f(1,1,1); +} + +inline Vec3b makeRainbow3B(float id) +{ + id *= freeDebugParam3; + if(!(id > 0)) + return Vec3b(255,255,255); + + int icP = id; + float ifP = id-icP; + icP = icP%3; + + if(icP == 0) return Vec3b(255*(1-ifP), 255*ifP, 0); + if(icP == 1) return Vec3b(0, 255*(1-ifP), 255*ifP); + if(icP == 2) return Vec3b(255*ifP, 0, 255*(1-ifP)); + return Vec3b(255,255,255); +} + +inline Vec3b makeJet3B(float id) +{ + if(id <= 0) return Vec3b(128,0,0); + if(id >= 1) return Vec3b(0,0,128); + + int icP = (id*8); + float ifP = (id*8)-icP; + + if(icP == 0) return Vec3b(255*(0.5+0.5*ifP), 0, 0); + if(icP == 1) return Vec3b(255, 255*(0.5*ifP), 0); + if(icP == 2) return Vec3b(255, 255*(0.5+0.5*ifP), 0); + if(icP == 3) return Vec3b(255*(1-0.5*ifP), 255, 255*(0.5*ifP)); + if(icP == 4) return Vec3b(255*(0.5-0.5*ifP), 255, 255*(0.5+0.5*ifP)); + if(icP == 5) return Vec3b(0, 255*(1-0.5*ifP), 255); + if(icP == 6) return Vec3b(0, 255*(0.5-0.5*ifP), 255); + if(icP == 7) return Vec3b(0, 0, 255*(1-0.5*ifP)); + return Vec3b(255,255,255); +} + +inline Vec3b makeRedGreen3B(float val) // 0 = red, 1=green, 0.5=yellow. +{ + if(val < 0) return Vec3b(0,0,255); + else if(val < 0.5) return Vec3b(0,255*2*val,255); + else if(val < 1) return Vec3b(0,255,255-255*2*(val-0.5)); + else return Vec3b(0,255,0); + +} + + + + + +} diff --git a/src/dso/util/nanoflann.h b/src/dso/util/nanoflann.h new file mode 100644 index 0000000..f4aaca5 --- /dev/null +++ b/src/dso/util/nanoflann.h @@ -0,0 +1,1412 @@ +/** +* This file is part of DSO. +* +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2014 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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. + *************************************************************************/ + +/** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + */ + +#pragma once +#include +#include +#include +#include +#include // for fwrite() +#include // for fabs(),... +#include + + + + +namespace nanoflann +{ +/** @addtogroup nanoflann_grp nanoflann C++ library for ANN + * @{ */ + + /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ + #define NANOFLANN_VERSION 0x119 + + /** @addtogroup result_sets_grp Result set classes + * @{ */ + template + class KNNResultSet + { + IndexType * indices; + DistanceType* dists; + CountType capacity; + CountType count; + + public: + inline KNNResultSet(CountType capacity_) : indices(0), dists(0), capacity(capacity_), count(0) + { + } + + inline void init(IndexType* indices_, DistanceType* dists_) + { + indices = indices_; + dists = dists_; + count = 0; + if (capacity) + dists[capacity-1] = (std::numeric_limits::max)(); + } + + inline CountType size() const + { + return count; + } + + inline bool full() const + { + return count == capacity; + } + + + inline void addPoint(DistanceType dist, IndexType index) + { + CountType i; + for (i=count; i>0; --i) { +#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same distance, the one with the lowest-index will be returned first. + if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) { +#else + if (dists[i-1]>dist) { +#endif + if (i + class RadiusResultSet + { + public: + const DistanceType radius; + + std::vector >& m_indices_dists; + + inline RadiusResultSet(DistanceType radius_, std::vector >& indices_dists) : radius(radius_), m_indices_dists(indices_dists) + { + init(); + } + + inline ~RadiusResultSet() { } + + inline void init() { clear(); } + inline void clear() { m_indices_dists.clear(); } + + inline size_t size() const { return m_indices_dists.size(); } + + inline bool full() const { return true; } + + inline void addPoint(DistanceType dist, IndexType index) + { + if (dist 0 + */ + std::pair worst_item() const + { + if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results."); + typedef typename std::vector >::const_iterator DistIt; + DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end()); + return *it; + } + }; + + /** operator "<" for std::sort() */ + struct IndexDist_Sorter + { + /** PairType will be typically: std::pair */ + template + inline bool operator()(const PairType &p1, const PairType &p2) const { + return p1.second < p2.second; + } + }; + + /** @} */ + + + /** @addtogroup loadsave_grp Load/save auxiliary functions + * @{ */ + template + void save_value(FILE* stream, const T& value, size_t count = 1) + { + fwrite(&value, sizeof(value),count, stream); + } + + template + void save_value(FILE* stream, const std::vector& value) + { + size_t size = value.size(); + fwrite(&size, sizeof(size_t), 1, stream); + fwrite(&value[0], sizeof(T), size, stream); + } + + template + void load_value(FILE* stream, T& value, size_t count = 1) + { + size_t read_cnt = fread(&value, sizeof(value), count, stream); + if (read_cnt != count) { + throw std::runtime_error("Cannot read from file"); + } + } + + + template + void load_value(FILE* stream, std::vector& value) + { + size_t size; + size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); + if (read_cnt!=1) { + throw std::runtime_error("Cannot read from file"); + } + value.resize(size); + read_cnt = fread(&value[0], sizeof(T), size, stream); + if (read_cnt!=size) { + throw std::runtime_error("Cannot read from file"); + } + } + /** @} */ + + + /** @addtogroup metric_grp Metric (distance) classes + * @{ */ + + template inline T abs(T x) { return (x<0) ? -x : x; } + template<> inline int abs(int x) { return ::abs(x); } + template<> inline float abs(float x) { return fabsf(x); } + template<> inline double abs(double x) { return fabs(x); } + template<> inline long double abs(long double x) { return fabsl(x); } + + /** Manhattan distance functor (generic version, optimized for high-dimensionality data sets). + * Corresponding distance traits: nanoflann::metric_L1 + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) + */ + template + struct L1_Adaptor + { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } + + inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const + { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = nanoflann::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++)); + const DistanceType diff1 = nanoflann::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++)); + const DistanceType diff2 = nanoflann::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++)); + const DistanceType diff3 = nanoflann::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if ((worst_dist>0)&&(result>worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + result += nanoflann::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) ); + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, int ) const + { + return nanoflann::abs(a-b); + } + }; + + /** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets). + * Corresponding distance traits: nanoflann::metric_L2 + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) + */ + template + struct L2_Adaptor + { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } + + inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const + { + DistanceType result = DistanceType(); + const T* last = a + size; + const T* lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++); + const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++); + const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++); + const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++); + result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if ((worst_dist>0)&&(result>worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++); + result += diff0 * diff0; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, int ) const + { + return (a-b)*(a-b); + } + }; + + /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds) + * Corresponding distance traits: nanoflann::metric_L2_Simple + * \tparam T Type of the elements (e.g. double, float, uint8_t) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) + */ + template + struct L2_Simple_Adaptor + { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } + + inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const { + return data_source.kdtree_distance(a,b_idx,size); + } + + template + inline DistanceType accum_dist(const U a, const V b, int ) const + { + return (a-b)*(a-b); + } + }; + + /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ + struct metric_L1 { + template + struct traits { + typedef L1_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ + struct metric_L2 { + template + struct traits { + typedef L2_Adaptor distance_t; + }; + }; + /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ + struct metric_L2_Simple { + template + struct traits { + typedef L2_Simple_Adaptor distance_t; + }; + }; + + /** @} */ + + /** @addtogroup param_grp Parameter structs + * @{ */ + + /** Parameters (see README.md) */ + struct KDTreeSingleIndexAdaptorParams + { + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) : + leaf_max_size(_leaf_max_size) + {} + + size_t leaf_max_size; + }; + + /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ + struct SearchParams + { + /** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */ + SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) : + checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} + + int checks; //!< Ignored parameter (Kept for compatibility with the FLANN interface). + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true) + }; + /** @} */ + + + /** @addtogroup memalloc_grp Memory allocation + * @{ */ + + /** + * Allocates (using C's malloc) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template + inline T* allocate(size_t count = 1) + { + T* mem = static_cast( ::malloc(sizeof(T)*count)); + return mem; + } + + + /** + * Pooled storage allocator + * + * The following routines allow for the efficient allocation of storage in + * small chunks from a specified pool. Rather than allowing each structure + * to be freed individually, an entire pool of storage is freed at once. + * This method has two advantages over just using malloc() and free(). First, + * it is far more efficient for allocating small objects, as there is + * no overhead for remembering all the information needed to free each + * object or consolidating fragmented memory. Second, the decision about + * how long to keep an object is made at the time of allocation, and there + * is no need to track down all the objects to free them. + * + */ + + const size_t WORDSIZE=16; + const size_t BLOCKSIZE=8192; + + class PooledAllocator + { + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */ + + + size_t remaining; /* Number of bytes left in current block of storage. */ + void* base; /* Pointer to base of current block of storage. */ + void* loc; /* Current location in block to next allocate memory. */ + + void internal_init() + { + remaining = 0; + base = NULL; + usedMemory = 0; + wastedMemory = 0; + } + + public: + size_t usedMemory; + size_t wastedMemory; + + /** + Default constructor. Initializes a new pool. + */ + PooledAllocator() { + internal_init(); + } + + /** + * Destructor. Frees all the memory allocated in this pool. + */ + ~PooledAllocator() { + free_all(); + } + + /** Frees all allocated memory chunks */ + void free_all() + { + while (base != NULL) { + void *prev = *(static_cast( base)); /* Get pointer to prev block. */ + ::free(base); + base = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void* malloc(const size_t req_size) + { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits of + incremented size to zero. + */ + const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first word + of a block is reserved for a pointer to the previous block. + */ + if (size > remaining) { + + wastedMemory += remaining; + + /* Allocate new storage. */ + const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ? + size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE; + + // use the standard C malloc to allocate memory + void* m = ::malloc(blocksize); + if (!m) { + fprintf(stderr,"Failed to allocate memory.\n"); + return NULL; + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + size_t shift = 0; + //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1); + + remaining = blocksize - sizeof(void*) - shift; + loc = (static_cast(m) + sizeof(void*) + shift); + } + void* rloc = loc; + loc = static_cast(loc) + size; + remaining -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template + T* allocate(const size_t count = 1) + { + T* mem = static_cast(this->malloc(sizeof(T)*count)); + return mem; + } + + }; + /** @} */ + + /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff + * @{ */ + + // ---------------- CArray ------------------------- + /** A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from the MRPT project) + * This code is an adapted version from Boost, modifed for its integration + * within MRPT (JLBC, Dec/2009) (Renamed array -> CArray to avoid possible potential conflicts). + * See + * http://www.josuttis.com/cppcode + * for details and the latest version. + * See + * http://www.boost.org/libs/array for Documentation. + * for documentation. + * + * (C) Copyright Nicolai M. Josuttis 2001. + * Permission to copy, use, modify, sell and distribute this software + * is granted provided this copyright notice appears in all copies. + * This software is provided "as is" without express or implied + * warranty, and with no claim as to its suitability for any purpose. + * + * 29 Jan 2004 - minor fixes (Nico Josuttis) + * 04 Dec 2003 - update to synch with library TR1 (Alisdair Meredith) + * 23 Aug 2002 - fix for Non-MSVC compilers combined with MSVC libraries. + * 05 Aug 2001 - minor update (Nico Josuttis) + * 20 Jan 2001 - STLport fix (Beman Dawes) + * 29 Sep 2000 - Initial Revision (Nico Josuttis) + * + * Jan 30, 2004 + */ + template + class CArray { + public: + T elems[N]; // fixed-size array of elements of type T + + public: + // type definitions + typedef T value_type; + typedef T* iterator; + typedef const T* const_iterator; + typedef T& reference; + typedef const T& const_reference; + typedef std::size_t size_type; + typedef std::ptrdiff_t difference_type; + + // iterator support + inline iterator begin() { return elems; } + inline const_iterator begin() const { return elems; } + inline iterator end() { return elems+N; } + inline const_iterator end() const { return elems+N; } + + // reverse iterator support +#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS) + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; +#elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310) + // workaround for broken reverse_iterator in VC7 + typedef std::reverse_iterator > reverse_iterator; + typedef std::reverse_iterator > const_reverse_iterator; +#else + // workaround for broken reverse_iterator implementations + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; +#endif + + reverse_iterator rbegin() { return reverse_iterator(end()); } + const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); } + reverse_iterator rend() { return reverse_iterator(begin()); } + const_reverse_iterator rend() const { return const_reverse_iterator(begin()); } + // operator[] + inline reference operator[](size_type i) { return elems[i]; } + inline const_reference operator[](size_type i) const { return elems[i]; } + // at() with range check + reference at(size_type i) { rangecheck(i); return elems[i]; } + const_reference at(size_type i) const { rangecheck(i); return elems[i]; } + // front() and back() + reference front() { return elems[0]; } + const_reference front() const { return elems[0]; } + reference back() { return elems[N-1]; } + const_reference back() const { return elems[N-1]; } + // size is constant + static inline size_type size() { return N; } + static bool empty() { return false; } + static size_type max_size() { return N; } + enum { static_size = N }; + /** This method has no effects in this class, but raises an exception if the expected size does not match */ + inline void resize(const size_t nElements) { if (nElements!=N) throw std::logic_error("Try to change the size of a CArray."); } + // swap (note: linear complexity in N, constant for given instantiation) + void swap (CArray& y) { std::swap_ranges(begin(),end(),y.begin()); } + // direct access to data (read-only) + const T* data() const { return elems; } + // use array as C array (direct read/write access to data) + T* data() { return elems; } + // assignment with type conversion + template CArray& operator= (const CArray& rhs) { + std::copy(rhs.begin(),rhs.end(), begin()); + return *this; + } + // assign one value to all elements + inline void assign (const T& value) { for (size_t i=0;i= size()) { throw std::out_of_range("CArray<>: index out of range"); } } + }; // end of CArray + + /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1. + * Fixed size version for a generic DIM: + */ + template + struct array_or_vector_selector + { + typedef CArray container_t; + }; + /** Dynamic size version */ + template + struct array_or_vector_selector<-1,T> { + typedef std::vector container_t; + }; + /** @} */ + + /** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + + /** kd-tree index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * // [Only if using the metric_L2_Simple type] Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: + * inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... } + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, int dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) + * template + * bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + * \tparam DIM Dimensionality of data points (e.g. 3 for 3D points) + * \tparam IndexType Will be typically size_t or int + */ + template + class KDTreeSingleIndexAdaptor + { + private: + /** Hidden copy constructor, to disallow copying indices (Not implemented) */ + KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor&); + public: + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + protected: + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + size_t m_leaf_max_size; + + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + size_t m_size; //!< Number of current poins in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the index was built + int dim; //!< Dimensionality of each data point + + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node + { + /** Union used because a node can be either a LEAF node or a non-leaf node, so both data fields are never used simultaneously */ + union { + struct { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + }; + Node* child1, * child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + typedef Node* NodePtr; + + + struct Interval + { + ElementType low, high; + }; + + /** Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM" */ + typedef typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM" */ + typedef typename array_or_vector_selector::container_t distance_vector_t; + + /** The KD-tree used to find neighbours */ + NodePtr root_node; + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool; + + public: + + Distance distance; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 for 3D points) + * is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) : + dataset(inputData), index_params(params), root_node(NULL), distance(inputData) + { + m_size = dataset.kdtree_get_point_count(); + m_size_at_index_build = m_size; + dim = dimensionality; + if (DIM>0) dim=DIM; + m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** Standard destructor */ + ~KDTreeSingleIndexAdaptor() { } + + /** Frees the previously-built index. Automatically called within buildIndex(). */ + void freeIndex() + { + pool.free_all(); + root_node=NULL; + m_size_at_index_build = 0; + } + + /** + * Builds the index + */ + void buildIndex() + { + init_vind(); + freeIndex(); + m_size_at_index_build = m_size; + if(m_size == 0) return; + computeBoundingBox(root_bbox); + root_node = divideTree(0, m_size, root_bbox ); // construct the tree + } + + /** Returns number of points in dataset */ + size_t size() const { return m_size; } + + /** Returns the length of each point in the dataset */ + size_t veclen() const { + return static_cast(DIM>0 ? DIM : dim); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + size_t usedMemory() const + { + return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside + * the result object. + * + * Params: + * result = the result object in which the indices of the nearest-neighbors are stored + * vec = the vector for which to search the nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const + { + assert(vec); + if (size() == 0) + return false; + if (!root_node) + throw std::runtime_error("[nanoflann] findNeighbors() called before building the index."); + float epsError = 1+searchParams.eps; + + distance_vector_t dists; // fixed or variable-sized container (depending on DIM) + dists.assign((DIM>0 ? DIM : dim) ,0); // Fill it with zeros. + DistanceType distsq = computeInitialDistances(vec, dists); + searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside + * the result object. + * \sa radiusSearch, findNeighbors + * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + */ + inline void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance. + * Previous contents of \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() or dists.size() ) + */ + size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector >& IndicesDists, const SearchParams& searchParams) const + { + RadiusResultSet resultSet(radius,IndicesDists); + const size_t nFound = radiusSearchCustomCallback(query_point,resultSet,searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() ); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point found in the radius of the query. + * See the source of RadiusResultSet<> as a start point for your own classes. + * \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback(const ElementType *query_point,SEARCH_CALLBACK &resultSet, const SearchParams& searchParams = SearchParams() ) const + { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + + private: + /** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */ + void init_vind() + { + // Create a permutable array of indices to the input vectors. + m_size = dataset.kdtree_get_point_count(); + if (vind.size()!=m_size) vind.resize(m_size); + for (size_t i = 0; i < m_size; i++) vind[i] = i; + } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(size_t idx, int component) const { + return dataset.kdtree_get_pt(idx,component); + } + + + void save_tree(FILE* stream, NodePtr tree) + { + save_value(stream, *tree); + if (tree->child1!=NULL) { + save_tree(stream, tree->child1); + } + if (tree->child2!=NULL) { + save_tree(stream, tree->child2); + } + } + + + void load_tree(FILE* stream, NodePtr& tree) + { + tree = pool.allocate(); + load_value(stream, *tree); + if (tree->child1!=NULL) { + load_tree(stream, tree->child1); + } + if (tree->child2!=NULL) { + load_tree(stream, tree->child2); + } + } + + + void computeBoundingBox(BoundingBox& bbox) + { + bbox.resize((DIM>0 ? DIM : dim)); + if (dataset.kdtree_get_bbox(bbox)) + { + // Done! It was implemented in derived class + } + else + { + const size_t N = dataset.kdtree_get_point_count(); + if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but no data points found."); + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + bbox[i].low = + bbox[i].high = dataset_get(0,i); + } + for (size_t k=1; k0 ? DIM : dim); ++i) { + if (dataset_get(k,i)bbox[i].high) bbox[i].high = dataset_get(k,i); + } + } + } + } + + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox) + { + NodePtr node = pool.allocate(); // allocate memory + + /* If too few exemplars remain, then make this a leaf node. */ + if ( (right-left) <= m_leaf_max_size) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->lr.left = left; + node->lr.right = right; + + // compute bounding-box of leaf points + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + bbox[i].low = dataset_get(vind[left],i); + bbox[i].high = dataset_get(vind[left],i); + } + for (IndexType k=left+1; k0 ? DIM : dim); ++i) { + if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i); + if (bbox[i].highsub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = divideTree(left, left+idx, left_bbox); + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = divideTree(left+idx, right, right_bbox); + + node->sub.divlow = left_bbox[cutfeat].high; + node->sub.divhigh = right_bbox[cutfeat].low; + + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + + void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem) + { + min_elem = dataset_get(ind[0],element); + max_elem = dataset_get(ind[0],element); + for (IndexType i=1; imax_elem) max_elem = val; + } + } + + void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox) + { + const DistanceType EPS=static_cast(0.00001); + ElementType max_span = bbox[0].high-bbox[0].low; + for (int i=1; i<(DIM>0 ? DIM : dim); ++i) { + ElementType span = bbox[i].high-bbox[i].low; + if (span>max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { + ElementType span = bbox[i].high-bbox[i].low; + if (span>(1-EPS)*max_span) { + ElementType min_elem, max_elem; + computeMinMax(ind, count, cutfeat, min_elem, max_elem); + ElementType spread = max_elem-min_elem;; + if (spread>max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2; + ElementType min_elem, max_elem; + computeMinMax(ind, count, cutfeat, min_elem, max_elem); + + if (split_valmax_elem) cutval = max_elem; + else cutval = split_val; + + IndexType lim1, lim2; + planeSplit(ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1>count/2) index = lim1; + else if (lim2cutval + */ + void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2) + { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count-1; + for (;; ) { + while (left<=right && dataset_get(ind[left],cutfeat)=cutval) --right; + if (left>right || !right) break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count-1; + for (;; ) { + while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left; + while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right; + if (left>right || !right) break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const ElementType* vec, distance_vector_t& dists) const + { + assert(vec); + DistanceType distsq = DistanceType(); + + for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) { + if (vec[i] < root_bbox[i].low) { + dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i); + distsq += dists[i]; + } + if (vec[i] > root_bbox[i].high) { + dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i); + distsq += dists[i]; + } + } + + return distsq; + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq, + distance_vector_t& dists, const float epsError) const + { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL)&&(node->child2 == NULL)) { + //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i=node->lr.left; ilr.right; ++i) { + const IndexType index = vind[i];// reorder... : i; + DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim)); + if (distsub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->sub.divlow; + DistanceType diff2 = val - node->sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1+diff2)<0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->sub.divhigh, idx); + } + else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist( val, node->sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq*epsError<=result_set.worstDist()) { + searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); + } + dists[idx] = dst; + } + + public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when loading the index object it must be constructed associated to the same source of data points used while building it. + * See the example: examples/saveload_example.cpp + * \sa loadIndex */ + void saveIndex(FILE* stream) + { + save_value(stream, m_size); + save_value(stream, dim); + save_value(stream, root_bbox); + save_value(stream, m_leaf_max_size); + save_value(stream, vind); + save_tree(stream, root_node); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the index object must be constructed associated to the same source of data points used while building the index. + * See the example: examples/saveload_example.cpp + * \sa loadIndex */ + void loadIndex(FILE* stream) + { + load_value(stream, m_size); + load_value(stream, dim); + load_value(stream, root_bbox); + load_value(stream, m_leaf_max_size); + load_value(stream, vind); + load_tree(stream, root_node); + } + + }; // class KDTree + + + /** An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage. + * Each row in the matrix represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * // Fill out "mat"... + * + * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > my_kd_tree_t; + * const int max_leaf = 10; + * my_kd_tree_t mat_index(dimdim, mat, max_leaf ); + * mat_index.index->buildIndex(); + * mat_index.index->... + * \endcode + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. + * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + */ + template + struct KDTreeEigenMatrixAdaptor + { + typedef KDTreeEigenMatrixAdaptor self_t; + typedef typename MatrixType::Scalar num_t; + typedef typename MatrixType::Index IndexType; + typedef typename Distance::template traits::distance_t metric_t; + typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; + + index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat) + { + const IndexType dims = mat.cols(); + if (dims!=dimensionality) throw std::runtime_error("Error: 'dimensionality' must match column count in data matrix"); + if (DIM>0 && static_cast(dims)!=DIM) + throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); + index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); + index->buildIndex(); + } + private: + /** Hidden copy constructor, to disallow copying this class (Not implemented) */ + KDTreeEigenMatrixAdaptor(const self_t&); + public: + + ~KDTreeEigenMatrixAdaptor() { + delete index; + } + + const MatrixType &m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). + * Note that this is a short-cut method for index->findNeighbors(). + * The user can also call index->... methods as desired. + * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + */ + inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t & derived() const { + return *this; + } + self_t & derived() { + return *this; + } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + return m_data_matrix.rows(); + } + + // Returns the L2 distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: + inline num_t kdtree_distance(const num_t *p1, const IndexType idx_p2,IndexType size) const + { + num_t s=0; + for (IndexType i=0; i + bool kdtree_get_bbox(BBOX& /*bb*/) const { + return false; + } + + /** @} */ + + }; // end of KDTreeEigenMatrixAdaptor + /** @} */ + +/** @} */ // end of grouping +} // end of NS + + diff --git a/src/dso/util/settings.cpp b/src/dso/util/settings.cpp new file mode 100644 index 0000000..8633c38 --- /dev/null +++ b/src/dso/util/settings.cpp @@ -0,0 +1,334 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#include "util/settings.h" +#include + + +namespace dso +{ +int pyrLevelsUsed = PYR_LEVELS; + +bool setting_useIMU = true; // Use IMU data (false will disable all IMU integration). +bool setting_useGTSAMIntegration = true; // Use the GTSAM integration for integrating addtional factors to the BA. Needed when useIMU==true). + +// If non-zero we set a prior to the x or y direction of the translation during the coarse visual initializer (useful for car datasets). +double setting_weightZeroPriorDSOInitY = 0.0; +double setting_weightZeroPriorDSOInitX = 0.0; +double setting_forceNoKFTranslationThresh = 0.0; // Force to create no KF if translation (in metric) is smaller than this. + +double setting_maxTimeBetweenKeyframes = 0; + +// If negative, the respective positive value will be used only if in non-RT mode. +// The idea of this parameter is that in non-RT mode the systems otherwise can make successive frames keyframes, which only rarely happens in RT mode. +// Default is -0.5 with means that the parameter is 0.5 in non-RT mode and inactive in RT mode. +// Fractional values are also possible. +double setting_minFramesBetweenKeyframes = -0.5; + +// minimum idepth for keeping points in the optimization window. +float setting_minIdepth = 0.02f; + +/* Parameters controlling when KF's are taken */ +float setting_keyframesPerSecond = 0; // if !=0, takes a fixed number of KF per second. +bool setting_realTimeMaxKF = false; // if true, takes as many KF's as possible (will break the system if the camera stays stationary) +float setting_maxShiftWeightT= 0.04f * (640+480); +float setting_maxShiftWeightR= 0.0f * (640+480); +float setting_maxShiftWeightRT= 0.02f * (640+480); +float setting_kfGlobalWeight = 1; // general weight on threshold, the larger the more KF's are taken (e.g., 2 = double the amount of KF's). +float setting_maxAffineWeight= 2; + + +/* initial hessian values to fix unobservable dimensions / priors on affine lighting parameters. + */ +float setting_idepthFixPrior = 50*50;// * 1000; +float setting_idepthFixPriorMargFac = 600*600; // 30000*30000; +float setting_initialRotPrior = 1e11; // 5e7;// 1e11; +float setting_initialTransPrior = 1e10;// 1e10; +float setting_initialAffBPrior = 1e14; +float setting_initialAffAPrior = 1e14; +float setting_initialCalibHessian = 5e9; + + + + + +/* some modes for solving the resulting linear system (e.g. orthogonalize wrt. unobservable dimensions) */ +//int setting_solverMode = SOLVER_FIX_LAMBDA | SOLVER_ORTHOGONALIZE_X_LATER; +int setting_solverMode = SOLVER_ORTHOGONALIZE_X_LATER; +double setting_solverModeDelta = 0.00001; +bool setting_forceAceptStep = false; + + + +/* some thresholds on when to activate / marginalize points */ +float setting_minIdepthH_act = 100; +float setting_minIdepthH_marg = 50; + + + +float setting_desiredImmatureDensity = 1500; // immature points per frame +float setting_desiredPointDensity = 2000; // aimed total points in the active window. +float setting_minPointsRemaining = 0.05; // marg a frame if less than X% points remain. +float setting_maxLogAffFacInWindow = 0.7; // marg a frame if factor between intensities to current frame is larger than 1/X or X. + + +int setting_minFrames = 5; // min frames in window. +int setting_maxFrames = 7; // max frames in window. +int setting_minFrameAge = 1; +int setting_maxOptIterations=6; // max GN iterations. +int setting_minOptIterations=1; // min GN iterations. +float setting_thOptIterations=1.2; // factor on break threshold for GN iteration (larger = break earlier) + + + + + +/* Outlier Threshold on photometric energy */ +float setting_outlierTH = 12*12; // higher -> less strict +float setting_outlierTHSumComponent = 50*50; // higher -> less strong gradient-based reweighting . + + + + +int setting_pattern = 8; // point pattern used. DISABLED. +float setting_margWeightFac = 0.5*0.5; // factor on hessian when marginalizing, to account for inaccurate linearization points. + + +/* when to re-track a frame */ +float setting_reTrackThreshold = 1.5; // (larger = re-track more often) + + + +/* require some minimum number of residuals for a point to become valid */ +int setting_minGoodActiveResForMarg=3; +int setting_minGoodResForMarg=4; + + + + + + +// 0 = nothing. +// 1 = apply inv. response. +// 2 = apply inv. response & remove V. +int setting_photometricCalibration = 2; +bool setting_useExposure = true; +float setting_affineOptModeA = 1e12; //-1: fix. >=0: optimize (with prior, if > 0). +float setting_affineOptModeB = 1e8; //-1: fix. >=0: optimize (with prior, if > 0). +float setting_affineOptModeA_huberTH = 10000; +float setting_affineOptModeB_huberTH = 10000; +int setting_gammaWeightsPixelSelect = 1; // 1 = use original intensity for pixel selection; 0 = use gamma-corrected intensity. + + + + +float setting_huberTH = 9; // Huber Threshold + + + + + +// parameters controlling adaptive energy threshold computation. +float setting_frameEnergyTHConstWeight = 0.5; +float setting_frameEnergyTHN = 0.7f; +float setting_frameEnergyTHFacMedian = 1.5; +float setting_overallEnergyTHWeight = 1; +float setting_coarseCutoffTH = 20; + + + + + +// parameters controlling pixel selection +float setting_minGradHistCut = 0.5; +float setting_minGradHistAdd = 7; +float setting_gradDownweightPerLevel = 0.75; +bool setting_selectDirectionDistribution = true; + + + + + + +/* settings controling initial immature point tracking */ +float setting_maxPixSearch = 0.027; // max length of the ep. line segment searched during immature point tracking. relative to image resolution. +float setting_minTraceQuality = 3; +int setting_minTraceTestRadius = 2; +int setting_GNItsOnPointActivation = 3; +float setting_trace_stepsize = 1.0; // stepsize for initial discrete search. +int setting_trace_GNIterations = 3; // max # GN iterations +float setting_trace_GNThreshold = 0.1; // GN stop after this stepsize. +float setting_trace_extraSlackOnTH = 1.2; // for energy-based outlier check, be slightly more relaxed by this factor. +float setting_trace_slackInterval = 1.5; // if pixel-interval is smaller than this, leave it be. +float setting_trace_minImprovementFactor = 2; // if pixel-interval is smaller than this, leave it be. + + + + +// for benchmarking different undistortion settings +float benchmarkSetting_fxfyfac = 0; +int benchmarkSetting_width = 0; +int benchmarkSetting_height = 0; +float benchmark_varNoise = 0; +float benchmark_varBlurNoise = 0; +float benchmark_initializerSlackFactor = 1; +int benchmark_noiseGridsize = 3; + + +float freeDebugParam1 = 1; +float freeDebugParam2 = 1; +float freeDebugParam3 = 1; +float freeDebugParam4 = 1; +float freeDebugParam5 = 1; + + + +bool debugSaveImages = false; +bool multiThreading = true; +bool disableAllDisplay = false; +bool setting_logStuff = true; + + + +bool goStepByStep = false; + + +bool setting_render_displayCoarseTrackingFull=false; +bool setting_render_renderWindowFrames=true; +bool setting_render_plotTrackingFull = false; +bool setting_render_display3D = true; +bool setting_render_displayResidual = true; +bool setting_render_displayVideo = true; +bool setting_render_displayDepth = true; + +bool setting_fullResetRequested = false; + +bool setting_debugout_runquiet = false; + +int sparsityFactor = 5; // not actually a setting, only some legacy stuff for coarse initializer. + + +void handleKey(char k) +{ + char kkk = k; + switch(kkk) + { + case 'd': case 'D': + freeDebugParam5 = ((int)(freeDebugParam5+1))%10; + printf("new freeDebugParam5: %f!\n", freeDebugParam5); + break; + case 's': case 'S': + freeDebugParam5 = ((int)(freeDebugParam5-1+10))%10; + printf("new freeDebugParam5: %f!\n", freeDebugParam5); + break; + } + +} + + + + +int staticPattern[10][40][2] = { + {{0,0}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, // . + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{0,-1}, {-1,0}, {0,0}, {1,0}, {0,1}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, // + + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{-1,-1}, {1,1}, {0,0}, {-1,1}, {1,-1}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, // x + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{-1,-1}, {-1,0}, {-1,1}, {-1,0}, {0,0}, {0,1}, {1,-1}, {1,0}, {1,1}, {-100,-100}, // full-tight + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{0,-2}, {-1,-1}, {1,-1}, {-2,0}, {0,0}, {2,0}, {-1,1}, {1,1}, {0,2}, {-100,-100}, // full-spread-9 + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{0,-2}, {-1,-1}, {1,-1}, {-2,0}, {0,0}, {2,0}, {-1,1}, {1,1}, {0,2}, {-2,-2}, // full-spread-13 + {-2,2}, {2,-2}, {2,2}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{-2,-2}, {-2,-1}, {-2,-0}, {-2,1}, {-2,2}, {-1,-2}, {-1,-1}, {-1,-0}, {-1,1}, {-1,2}, // full-25 + {-0,-2}, {-0,-1}, {-0,-0}, {-0,1}, {-0,2}, {+1,-2}, {+1,-1}, {+1,-0}, {+1,1}, {+1,2}, + {+2,-2}, {+2,-1}, {+2,-0}, {+2,1}, {+2,2}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{0,-2}, {-1,-1}, {1,-1}, {-2,0}, {0,0}, {2,0}, {-1,1}, {1,1}, {0,2}, {-2,-2}, // full-spread-21 + {-2,2}, {2,-2}, {2,2}, {-3,-1}, {-3,1}, {3,-1}, {3,1}, {1,-3}, {-1,-3}, {1,3}, + {-1,3}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{0,-2}, {-1,-1}, {1,-1}, {-2,0}, {0,0}, {2,0}, {-1,1}, {0,2}, {-100,-100}, {-100,-100}, // 8 for SSE efficiency + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, + {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}}, + + {{-4,-4}, {-4,-2}, {-4,-0}, {-4,2}, {-4,4}, {-2,-4}, {-2,-2}, {-2,-0}, {-2,2}, {-2,4}, // full-45-SPREAD + {-0,-4}, {-0,-2}, {-0,-0}, {-0,2}, {-0,4}, {+2,-4}, {+2,-2}, {+2,-0}, {+2,2}, {+2,4}, + {+4,-4}, {+4,-2}, {+4,-0}, {+4,2}, {+4,4}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, + {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}}, +}; + +int staticPatternNum[10] = { + 1, + 5, + 5, + 9, + 9, + 13, + 25, + 21, + 8, + 25 +}; + +int staticPatternPadding[10] = { + 1, + 1, + 1, + 1, + 2, + 2, + 2, + 3, + 2, + 4 +}; + + +} diff --git a/src/dso/util/settings.h b/src/dso/util/settings.h new file mode 100644 index 0000000..9bdc757 --- /dev/null +++ b/src/dso/util/settings.h @@ -0,0 +1,243 @@ +/** +* This file is part of DSO, written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + + + +#pragma once + +#include +#include +#include + + +namespace dso +{ +#define SOLVER_SVD (int)1 +#define SOLVER_ORTHOGONALIZE_SYSTEM (int)2 +#define SOLVER_ORTHOGONALIZE_POINTMARG (int)4 +#define SOLVER_ORTHOGONALIZE_FULL (int)8 +#define SOLVER_SVD_CUT7 (int)16 +#define SOLVER_REMOVE_POSEPRIOR (int)32 +#define SOLVER_USE_GN (int)64 +#define SOLVER_FIX_LAMBDA (int)128 +#define SOLVER_ORTHOGONALIZE_X (int)256 +#define SOLVER_MOMENTUM (int)512 +#define SOLVER_STEPMOMENTUM (int)1024 +#define SOLVER_ORTHOGONALIZE_X_LATER (int)2048 + + +// ============== PARAMETERS TO BE DECIDED ON COMPILE TIME ================= +#define PYR_LEVELS 6 +extern int pyrLevelsUsed; + +extern bool setting_useIMU; +extern bool setting_useGTSAMIntegration; +extern double setting_weightZeroPriorDSOInitY; +extern double setting_weightZeroPriorDSOInitX; +extern double setting_forceNoKFTranslationThresh; + +extern double setting_maxTimeBetweenKeyframes; + +extern double setting_minFramesBetweenKeyframes; + +extern float setting_minIdepth; + +extern float setting_keyframesPerSecond; +extern bool setting_realTimeMaxKF; +extern float setting_maxShiftWeightT; +extern float setting_maxShiftWeightR; +extern float setting_maxShiftWeightRT; +extern float setting_maxAffineWeight; +extern float setting_kfGlobalWeight; + + + +extern float setting_idepthFixPrior; +extern float setting_idepthFixPriorMargFac; +extern float setting_initialRotPrior; +extern float setting_initialTransPrior; +extern float setting_initialAffBPrior; +extern float setting_initialAffAPrior; +extern float setting_initialCalibHessian; + +extern int setting_solverMode; +extern double setting_solverModeDelta; + + +extern float setting_minIdepthH_act; +extern float setting_minIdepthH_marg; + + + +extern float setting_maxIdepth; +extern float setting_maxPixSearch; +extern float setting_desiredImmatureDensity; // done +extern float setting_desiredPointDensity; // done +extern float setting_minPointsRemaining; +extern float setting_maxLogAffFacInWindow; +extern int setting_minFrames; +extern int setting_maxFrames; +extern int setting_minFrameAge; +extern int setting_maxOptIterations; +extern int setting_minOptIterations; +extern float setting_thOptIterations; +extern float setting_outlierTH; +extern float setting_outlierTHSumComponent; + + + +extern int setting_pattern; +extern float setting_margWeightFac; +extern int setting_GNItsOnPointActivation; + + +extern float setting_minTraceQuality; +extern int setting_minTraceTestRadius; +extern float setting_reTrackThreshold; + + +extern int setting_minGoodActiveResForMarg; +extern int setting_minGoodResForMarg; +extern int setting_minInlierVotesForMarg; + + + + +extern int setting_photometricCalibration; +extern bool setting_useExposure; +extern float setting_affineOptModeA; +extern float setting_affineOptModeB; +extern float setting_affineOptModeA_huberTH; +extern float setting_affineOptModeB_huberTH; +extern int setting_gammaWeightsPixelSelect; + + + +extern bool setting_forceAceptStep; + + + +extern float setting_huberTH; + + +extern bool setting_logStuff; +extern float benchmarkSetting_fxfyfac; +extern int benchmarkSetting_width; +extern int benchmarkSetting_height; +extern float benchmark_varNoise; +extern float benchmark_varBlurNoise; +extern int benchmark_noiseGridsize; +extern float benchmark_initializerSlackFactor; + +extern float setting_frameEnergyTHConstWeight; +extern float setting_frameEnergyTHN; + +extern float setting_frameEnergyTHFacMedian; +extern float setting_overallEnergyTHWeight; +extern float setting_coarseCutoffTH; + +extern float setting_minGradHistCut; +extern float setting_minGradHistAdd; +extern float setting_gradDownweightPerLevel; +extern bool setting_selectDirectionDistribution; + + + +extern float setting_trace_stepsize; +extern int setting_trace_GNIterations; +extern float setting_trace_GNThreshold; +extern float setting_trace_extraSlackOnTH; +extern float setting_trace_slackInterval; +extern float setting_trace_minImprovementFactor; + + +extern bool setting_render_displayCoarseTrackingFull; +extern bool setting_render_renderWindowFrames; +extern bool setting_render_plotTrackingFull; +extern bool setting_render_display3D; +extern bool setting_render_displayResidual; +extern bool setting_render_displayVideo; +extern bool setting_render_displayDepth; + +extern bool setting_fullResetRequested; + +extern bool setting_debugout_runquiet; + +extern bool disableAllDisplay; + + + + + + +extern bool debugSaveImages; + + +extern int sparsityFactor; +extern bool goStepByStep; +extern bool plotStereoImages; +extern bool multiThreading; + +extern float freeDebugParam1; +extern float freeDebugParam2; +extern float freeDebugParam3; +extern float freeDebugParam4; +extern float freeDebugParam5; + + +void handleKey(char k); + + + + +extern int staticPattern[10][40][2]; +extern int staticPatternNum[10]; +extern int staticPatternPadding[10]; + + + + +//#define patternNum staticPatternNum[setting_pattern] +//#define patternP staticPattern[setting_pattern] +//#define patternPadding staticPatternPadding[setting_pattern] + +// +#define patternNum 8 +#define patternP staticPattern[8] +#define patternPadding 2 + + + + + + + + + + + + + +} diff --git a/src/main_dmvio_dataset.cpp b/src/main_dmvio_dataset.cpp new file mode 100644 index 0000000..283d795 --- /dev/null +++ b/src/main_dmvio_dataset.cpp @@ -0,0 +1,691 @@ +/** +* This file is based on the file main_dso_pangolin.cpp of the project DSO written by Jakob Engel. +* It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). +* +* Copyright 2022 Lukas von Stumberg +* Copyright 2016 Technical University of Munich and Intel. +* Developed by Jakob Engel , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DSO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DSO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DSO. If not, see . +*/ + +// Main file for running on datasets, based on the main file of DSO. + +#include +#include +#include +#include +#include +#include + +#include "IOWrapper/Output3DWrapper.h" +#include "IOWrapper/ImageDisplay.h" + + +#include +#include "dso/util/settings.h" +#include "dso/util/globalFuncs.h" +#include "dso/util/DatasetReader.h" +#include "dso/util/globalCalib.h" +#include "util/TimeMeasurement.h" + +#include "dso/util/NumType.h" +#include "FullSystem/FullSystem.h" +#include "OptimizationBackend/MatrixAccumulators.h" +#include "FullSystem/PixelSelector2.h" + +#include + +#include "IOWrapper/Pangolin/PangolinDSOViewer.h" +#include "IOWrapper/OutputWrapper/SampleOutputWrapper.h" + +std::string gtFile = ""; +std::string vignette = ""; +std::string gammaCalib = ""; +std::string source = ""; +std::string calib = ""; +std::string imuFile = ""; +std::string imuCalibFile = ""; +bool reverse = false; +int start = 0; +int end = 100000; +float playbackSpeed = 0; // 0 for linearize (play as fast as possible, while sequentializing tracking & mapping). otherwise, factor on timestamps. +bool preload = false; +int maxPreloadImages = 0; // If set we only preload if there are less images to be loade. +bool useSampleOutput = false; + + +int mode = 0; + + +using namespace dso; + + +dmvio::IMUCalibration imuCalibration; +dmvio::IMUSettings imuSettings; + +void my_exit_handler(int s) +{ + printf("Caught signal %d\n", s); + exit(1); +} + +void exitThread() +{ + struct sigaction sigIntHandler; + sigIntHandler.sa_handler = my_exit_handler; + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); + + while(true) pause(); +} + + +void settingsDefault(int preset) +{ + printf("\n=============== PRESET Settings: ===============\n"); + if(preset == 0 || preset == 1) + { + printf("DEFAULT settings:\n" + "- %s real-time enforcing\n" + "- 2000 active points\n" + "- 5-7 active frames\n" + "- 1-6 LM iteration each KF\n" + "- original image resolution\n", preset == 0 ? "no " : "1x"); + + playbackSpeed = (preset == 0 ? 0 : 1.0); + preload = preset == 1; + setting_desiredImmatureDensity = 1500; + // setting_desiredPointDensity = 2000; + setting_desiredPointDensity = 1000; + setting_minFrames = 5; + setting_maxFrames = 7; + setting_maxOptIterations = 6; + setting_minOptIterations = 1; + + setting_logStuff = false; + } + + if(preset == 2 || preset == 3) + { + printf("FAST settings:\n" + "- %s real-time enforcing\n" + "- 800 active points\n" + "- 4-6 active frames\n" + "- 1-4 LM iteration each KF\n" + "- 424 x 320 image resolution\n", preset == 0 ? "no " : "5x"); + + playbackSpeed = (preset == 2 ? 0 : 5); + preload = preset == 3; + setting_desiredImmatureDensity = 600; + setting_desiredPointDensity = 800; + setting_minFrames = 4; + setting_maxFrames = 6; + setting_maxOptIterations = 4; + setting_minOptIterations = 1; + + benchmarkSetting_width = 424; + benchmarkSetting_height = 320; + + setting_logStuff = false; + } + + printf("==============================================\n"); +} + + +void parseArgument(char* arg, dmvio::SettingsUtil& settingsUtil) +{ + int option; + float foption; + char buf[1000]; + + // -------------------------------------------------- + // These are mostly the original DSO commandline arguments which also work for DM-VIO. + // The DM-VIO specific settings can also be set with commandline arguments (and also with the yaml settings file) + // and have been registered in the main function. See IMUSettings and its members for details. + // -------------------------------------------------- + + if(1 == sscanf(arg, "sampleoutput=%d", &option)) + { + if(option == 1) + { + useSampleOutput = true; + printf("USING SAMPLE OUTPUT WRAPPER!\n"); + } + return; + } + + if(1 == sscanf(arg, "quiet=%d", &option)) + { + if(option == 1) + { + setting_debugout_runquiet = true; + printf("QUIET MODE, I'll shut up!\n"); + } + return; + } + + if(1 == sscanf(arg, "preset=%d", &option)) + { + settingsDefault(option); + return; + } + + + if(1 == sscanf(arg, "nolog=%d", &option)) + { + if(option == 1) + { + setting_logStuff = false; + printf("DISABLE LOGGING!\n"); + } + return; + } + if(1 == sscanf(arg, "reverse=%d", &option)) + { + if(option == 1) + { + reverse = true; + printf("REVERSE!\n"); + } + return; + } + if(1 == sscanf(arg, "nogui=%d", &option)) + { + if(option == 1) + { + disableAllDisplay = true; + printf("NO GUI!\n"); + } + return; + } + if(1 == sscanf(arg, "nomt=%d", &option)) + { + if(option == 1) + { + multiThreading = false; + printf("NO MultiThreading!\n"); + } + return; + } + if(1 == sscanf(arg, "start=%d", &option)) + { + start = option; + printf("START AT %d!\n", start); + return; + } + if(1 == sscanf(arg, "end=%d", &option)) + { + end = option; + printf("END AT %d!\n", start); + return; + } + + if(1 == sscanf(arg, "files=%s", buf)) + { + source = buf; + printf("loading data from %s!\n", source.c_str()); + return; + } + + if(1 == sscanf(arg, "calib=%s", buf)) + { + calib = buf; + printf("loading calibration from %s!\n", calib.c_str()); + return; + } + + if(1 == sscanf(arg, "vignette=%s", buf)) + { + vignette = buf; + printf("loading vignette from %s!\n", vignette.c_str()); + return; + } + + if(1 == sscanf(arg, "gtFile=%s", buf)) + { + gtFile = buf; + printf("loading gtFile from %s!\n", gtFile.c_str()); + return; + } + + if(1 == sscanf(arg, "imuCalib=%s", buf)) + { + imuCalibFile = buf; + printf("Loading imu parameters from %s!\n", imuCalibFile.c_str()); + imuCalibration.loadFromFile(imuCalibFile); + return; + } + + if(1 == sscanf(arg, "imuFile=%s", buf)) + { + imuFile = buf; + printf("IMU file is locatd at: %s!\n", imuFile.c_str()); + return; + } + + if(1 == sscanf(arg, "useimu=%d", &option)) + { + if(option == 0) + { + printf("Disabling IMU integration!\n"); + setting_useIMU = false; + }else if(option == 1) + { + printf("Enabling IMU integration!\n"); + setting_useIMU = true; + } + return; + } + + if(1 == sscanf(arg, "gamma=%s", buf)) + { + gammaCalib = buf; + printf("loading gammaCalib from %s!\n", gammaCalib.c_str()); + return; + } + + if(1 == sscanf(arg, "speed=%f", &foption)) + { + playbackSpeed = foption; + printf("PLAYBACK SPEED %f!\n", playbackSpeed); + return; + } + + if(1 == sscanf(arg, "save=%d", &option)) + { + if(option == 1) + { + debugSaveImages = true; + if(42 == system("rm -rf images_out")) + printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n"); + if(42 == system("mkdir images_out")) + printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n"); + if(42 == system("rm -rf images_out")) + printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n"); + if(42 == system("mkdir images_out")) + printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n"); + printf("SAVE IMAGES!\n"); + } + return; + } + + if(1 == sscanf(arg, "mode=%d", &option)) + { + + mode = option; + if(option == 0) + { + printf("PHOTOMETRIC MODE WITH CALIBRATION!\n"); + } + if(option == 1) + { + printf("PHOTOMETRIC MODE WITHOUT CALIBRATION!\n"); + setting_photometricCalibration = 0; + setting_affineOptModeA = 0; //-1: fix. >=0: optimize (with prior, if > 0). + setting_affineOptModeB = 0; //-1: fix. >=0: optimize (with prior, if > 0). + } + if(option == 2) + { + printf("PHOTOMETRIC MODE WITH PERFECT IMAGES!\n"); + setting_photometricCalibration = 0; + setting_affineOptModeA = -1; //-1: fix. >=0: optimize (with prior, if > 0). + setting_affineOptModeB = -1; //-1: fix. >=0: optimize (with prior, if > 0). + setting_minGradHistAdd = 3; + } + return; + } + + if(1 == sscanf(arg, "settingsFile=%s", buf)) + { + YAML::Node settings = YAML::LoadFile(buf); + settingsUtil.tryReadFromYaml(settings); + printf("Loading settings from yaml file: %s!\n", imuCalibFile.c_str()); + return; + } + + if(settingsUtil.tryReadFromCommandLine(arg)) + { + return; + } + + printf("could not parse argument \"%s\"!!!!\n", arg); + assert(0); +} + + +void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) +{ + + if(setting_photometricCalibration > 0 && reader->getPhotometricGamma() == 0) + { + printf("ERROR: dont't have photometric calibation. Need to use commandline options mode=1 or mode=2 "); + exit(1); + } + + + int lstart = start; + int lend = end; + int linc = 1; + if(reverse) + { + assert(!setting_useIMU); // Reverse is not supported with IMU data at the moment! + printf("REVERSE!!!!"); + lstart = end - 1; + if(lstart >= reader->getNumImages()) + lstart = reader->getNumImages() - 1; + lend = start; + linc = -1; + } + + + bool linearizeOperation = (playbackSpeed == 0); + + if(linearizeOperation && setting_minFramesBetweenKeyframes < 0) + { + setting_minFramesBetweenKeyframes = -setting_minFramesBetweenKeyframes; + std::cout << "Using setting_minFramesBetweenKeyframes=" << setting_minFramesBetweenKeyframes + << " because of non-realtime mode." << std::endl; + } + + FullSystem* fullSystem = new FullSystem(linearizeOperation, imuCalibration, imuSettings); + fullSystem->setGammaFunction(reader->getPhotometricGamma()); + + + if(viewer != 0) + { + fullSystem->outputWrapper.push_back(viewer); + } + + std::unique_ptr sampleOutPutWrapper; + if(useSampleOutput) + { + sampleOutPutWrapper.reset(new IOWrap::SampleOutputWrapper()); + fullSystem->outputWrapper.push_back(sampleOutPutWrapper.get()); + } + + std::vector idsToPlay; + std::vector timesToPlayAt; + for(int i = lstart; i >= 0 && i < reader->getNumImages() && linc * i < linc * lend; i += linc) + { + idsToPlay.push_back(i); + if(timesToPlayAt.size() == 0) + { + timesToPlayAt.push_back((double) 0); + }else + { + double tsThis = reader->getTimestamp(idsToPlay[idsToPlay.size() - 1]); + double tsPrev = reader->getTimestamp(idsToPlay[idsToPlay.size() - 2]); + timesToPlayAt.push_back(timesToPlayAt.back() + fabs(tsThis - tsPrev) / playbackSpeed); + } + } + + if(preload && maxPreloadImages > 0) + { + if(reader->getNumImages() > maxPreloadImages) + { + printf("maxPreloadImages EXCEEDED! NOT PRELOADING!\n"); + preload = false; + } + } + + std::vector preloadedImages; + if(preload) + { + printf("LOADING ALL IMAGES!\n"); + for(int ii = 0; ii < (int) idsToPlay.size(); ii++) + { + int i = idsToPlay[ii]; + preloadedImages.push_back(reader->getImage(i)); + } + } + + struct timeval tv_start; + gettimeofday(&tv_start, NULL); + clock_t started = clock(); + double sInitializerOffset = 0; + + bool gtDataThere = reader->loadGTData(gtFile); + + bool imuDataSkipped = false; + dmvio::IMUData skippedIMUData; + for(int ii = 0; ii < (int) idsToPlay.size(); ii++) + { + if(!fullSystem->initialized) // if not initialized: reset start time. + { + gettimeofday(&tv_start, NULL); + started = clock(); + sInitializerOffset = timesToPlayAt[ii]; + } + + int i = idsToPlay[ii]; + + + ImageAndExposure* img; + if(preload) + img = preloadedImages[ii]; + else + img = reader->getImage(i); + + + bool skipFrame = false; + if(playbackSpeed != 0) + { + struct timeval tv_now; + gettimeofday(&tv_now, NULL); + double sSinceStart = sInitializerOffset + ((tv_now.tv_sec - tv_start.tv_sec) + + (tv_now.tv_usec - tv_start.tv_usec) / (1000.0f * 1000.0f)); + + if(sSinceStart < timesToPlayAt[ii]) + usleep((int) ((timesToPlayAt[ii] - sSinceStart) * 1000 * 1000)); + else if(sSinceStart > timesToPlayAt[ii] + 0.5 + 0.1 * (ii % 2)) + { + printf("SKIPFRAME %d (play at %f, now it is %f)!\n", ii, timesToPlayAt[ii], sSinceStart); + skipFrame = true; + } + } + + dmvio::GTData data; + bool found = false; + if(gtDataThere) + { + data = reader->getGTData(i, found); + } + + std::unique_ptr imuData; + if(setting_useIMU) + { + imuData = std::make_unique(reader->getIMUData(i)); + } + if(!skipFrame) + { + if(imuDataSkipped && imuData) + { + imuData->insert(imuData->begin(), skippedIMUData.begin(), skippedIMUData.end()); + skippedIMUData.clear(); + imuDataSkipped = false; + } + fullSystem->addActiveFrame(img, i, imuData.get(), (gtDataThere && found) ? &data : 0); + if(gtDataThere && found && !disableAllDisplay) + { + viewer->addGTCamPose(data.pose); + } + }else if(imuData) + { + imuDataSkipped = true; + skippedIMUData.insert(skippedIMUData.end(), imuData->begin(), imuData->end()); + } + + + delete img; + + if(fullSystem->initFailed || setting_fullResetRequested) + { + if(ii < 250 || setting_fullResetRequested) + { + printf("RESETTING!\n"); + std::vector wraps = fullSystem->outputWrapper; + delete fullSystem; + for(IOWrap::Output3DWrapper* ow : wraps) ow->reset(); + + fullSystem = new FullSystem(linearizeOperation, imuCalibration, imuSettings); + fullSystem->setGammaFunction(reader->getPhotometricGamma()); + fullSystem->outputWrapper = wraps; + + setting_fullResetRequested = false; + } + } + + if(viewer != nullptr && viewer->shouldQuit()) + { + std::cout << "User closed window -> Quit!" << std::endl; + break; + } + + if(fullSystem->isLost) + { + printf("LOST!!\n"); + break; + } + + } + fullSystem->blockUntilMappingIsFinished(); + clock_t ended = clock(); + struct timeval tv_end; + gettimeofday(&tv_end, NULL); + + + fullSystem->printResult(imuSettings.resultsPrefix + "result.txt", false, false, true); + fullSystem->printResult(imuSettings.resultsPrefix + "resultKFs.txt", true, false, false); + fullSystem->printResult(imuSettings.resultsPrefix + "resultScaled.txt", false, true, true); + + dmvio::TimeMeasurement::saveResults(imuSettings.resultsPrefix + "timings.txt"); + + + int numFramesProcessed = abs(idsToPlay[0] - idsToPlay.back()); + double numSecondsProcessed = fabs(reader->getTimestamp(idsToPlay[0]) - reader->getTimestamp(idsToPlay.back())); + double MilliSecondsTakenSingle = 1000.0f * (ended - started) / (float) (CLOCKS_PER_SEC); + double MilliSecondsTakenMT = sInitializerOffset + ((tv_end.tv_sec - tv_start.tv_sec) * 1000.0f + + (tv_end.tv_usec - tv_start.tv_usec) / 1000.0f); + printf("\n======================" + "\n%d Frames (%.1f fps)" + "\n%.2fms per frame (single core); " + "\n%.2fms per frame (multi core); " + "\n%.3fx (single core); " + "\n%.3fx (multi core); " + "\n======================\n\n", + numFramesProcessed, numFramesProcessed / numSecondsProcessed, + MilliSecondsTakenSingle / numFramesProcessed, + MilliSecondsTakenMT / (float) numFramesProcessed, + 1000 / (MilliSecondsTakenSingle / numSecondsProcessed), + 1000 / (MilliSecondsTakenMT / numSecondsProcessed)); + fullSystem->printFrameLifetimes(); + if(setting_logStuff) + { + std::ofstream tmlog; + tmlog.open("logs/time.txt", std::ios::trunc | std::ios::out); + tmlog << 1000.0f * (ended - started) / (float) (CLOCKS_PER_SEC * reader->getNumImages()) << " " + << ((tv_end.tv_sec - tv_start.tv_sec) * 1000.0f + (tv_end.tv_usec - tv_start.tv_usec) / 1000.0f) / + (float) reader->getNumImages() << "\n"; + tmlog.flush(); + tmlog.close(); + } + + for(IOWrap::Output3DWrapper* ow : fullSystem->outputWrapper) + { + ow->join(); + } + + + printf("DELETE FULLSYSTEM!\n"); + delete fullSystem; + + printf("DELETE READER!\n"); + delete reader; + + printf("EXIT NOW!\n"); +} + +int main(int argc, char** argv) +{ + setlocale(LC_ALL, "C"); + +#ifdef DEBUG + std::cout << "DEBUG MODE!" << std::endl; +#endif + + bool use16Bit = false; + + auto settingsUtil = std::make_shared(); + + // Create Settings files. + imuSettings.registerArgs(*settingsUtil); + imuCalibration.registerArgs(*settingsUtil); + settingsUtil->registerArg("setting_minOptIterations", setting_minOptIterations); + settingsUtil->registerArg("setting_maxOptIterations", setting_maxOptIterations); + settingsUtil->registerArg("setting_minIdepth", setting_minIdepth); + settingsUtil->registerArg("setting_solverMode", setting_solverMode); + settingsUtil->registerArg("use16Bit", use16Bit); + settingsUtil->registerArg("setting_weightZeroPriorDSOInitY", setting_weightZeroPriorDSOInitY); + settingsUtil->registerArg("setting_weightZeroPriorDSOInitX", setting_weightZeroPriorDSOInitX); + settingsUtil->registerArg("setting_forceNoKFTranslationThresh", setting_forceNoKFTranslationThresh); + settingsUtil->registerArg("setting_minFramesBetweenKeyframes", setting_minFramesBetweenKeyframes); + settingsUtil->registerArg("preload", preload); + settingsUtil->registerArg("maxPreloadImages", maxPreloadImages); + + for(int i = 1; i < argc; i++) + parseArgument(argv[i], *settingsUtil); + + // Print settings to commandline and file. + std::cout << "Settings:\n"; + settingsUtil->printAllSettings(std::cout); + { + std::ofstream settingsStream; + settingsStream.open(imuSettings.resultsPrefix + "usedSettingsdso.txt"); + settingsUtil->printAllSettings(settingsStream); + } + + + // hook crtl+C. + boost::thread exThread = boost::thread(exitThread); + + ImageFolderReader* reader = new ImageFolderReader(source, calib, gammaCalib, vignette, use16Bit); + reader->loadIMUData(imuFile); + reader->setGlobalCalibration(); + + if(!disableAllDisplay) + { + IOWrap::PangolinDSOViewer* viewer = new IOWrap::PangolinDSOViewer(wG[0], hG[0], false, settingsUtil); + + boost::thread runThread = boost::thread(boost::bind(run, reader, viewer)); + + viewer->run(); + + delete viewer; + + // Make sure that the destructor of FullSystem, etc. finishes, so all log files are properly flushed. + runThread.join(); + }else + { + run(reader, 0); + } + + + return 0; +} diff --git a/src/util/GTData.hpp b/src/util/GTData.hpp new file mode 100644 index 0000000..4727c43 --- /dev/null +++ b/src/util/GTData.hpp @@ -0,0 +1,55 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef GTData_hpp +#define GTData_hpp + +#include + +#include + +#include +#include + +namespace dmvio +{ +class GTData +{ +public: + + inline GTData() + {} + + inline GTData(Sophus::SE3 pose, Eigen::Vector3d velocity, Eigen::Vector3d biasRotation, + Eigen::Vector3d biasTranslation) + : pose(pose), velocity(velocity), biasRotation(biasRotation), biasTranslation(biasTranslation) + {} + + + Sophus::SE3 pose; + Eigen::Vector3d velocity; // Note: velocities might be in the vicon frame instead of the world frame... + Eigen::Vector3d biasRotation; + Eigen::Vector3d biasTranslation; +}; +} + +#endif /* GTData_hpp */ diff --git a/src/util/SettingsUtil.cpp b/src/util/SettingsUtil.cpp new file mode 100644 index 0000000..965d632 --- /dev/null +++ b/src/util/SettingsUtil.cpp @@ -0,0 +1,103 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "SettingsUtil.h" + +dmvio::SettingsUtil::Parameter::Parameter(void* pointer, + const std::function& commandLineHandler, + const std::function& yamlHandler, + const std::function& printHandler) : pointer( + pointer), commandLineHandler(commandLineHandler), yamlHandler(yamlHandler), printHandler(printHandler) +{} + +void dmvio::SettingsUtil::tryReadFromYaml(const YAML::Node& node) +{ + // Loop through parameters and check if their name is in the node. + for(auto& pair : parameters) + { + // We give preference to commandline over yaml. + if(!pair.second.loadedFromCommandLine) + { + std::string name = pair.first; + if(node[name]) + { + pair.second.yamlHandler(pair.second.pointer, node[name]); + } + } + } +} + +bool dmvio::SettingsUtil::tryReadFromCommandLine(const std::string& arg) +{ + // Extract name as part before the = sign and lookup in map + auto pos = arg.find('='); + if(pos == std::string::npos) + { + return false; + } + std::string name = arg.substr(0, pos); + + auto it = parameters.find(name); + if(it == parameters.end()) + { + return false; + } + + it->second.commandLineHandler(it->second.pointer, arg.substr(pos + 1)); + it->second.loadedFromCommandLine = true; + + return true; +} + +void dmvio::SettingsUtil::printAllSettings(std::ostream& stream) +{ + for(auto& pair : parameters) + { + stream << pair.first << ": "; + pair.second.printHandler(pair.second.pointer, stream); + stream << '\n'; + } +} + +void dmvio::SettingsUtil::createPangolinSettings() +{ + for(auto&& param : parameters) + { + auto* set = param.second.pangolinSetting.get(); + if(set) + { + set->createVar(); + } + } +} + +void dmvio::SettingsUtil::updatePangolinSettings() +{ + for(auto&& param : parameters) + { + auto* set = param.second.pangolinSetting.get(); + if(set) + { + set->updateVar(); + } + } +} diff --git a/src/util/SettingsUtil.h b/src/util/SettingsUtil.h new file mode 100644 index 0000000..9ddf4ca --- /dev/null +++ b/src/util/SettingsUtil.h @@ -0,0 +1,175 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_SETTINGSUTIL_H +#define DMVIO_SETTINGSUTIL_H + +#include "yaml-cpp/yaml.h" +#include +#include +#include +#include + +// This file contains utils for settings. +// Most settings should be in (potentially nested) settings classes which. +// These can call SettingsUtil::registerArg to register their members as settings. +// Registered settings can be set with commandline arguments or with yaml-files (see methods of SettingsUtil). +// Can also create settings which can be modified in the Pangolin GUI. +namespace dmvio +{ +template void defaultCommandLineHandler(void* pointer, std::string arg) +{ + std::stringstream stream(arg); + T* typedPointer = static_cast(pointer); + if(!(stream >> *typedPointer)) + { + std::cerr << "Could not convert argument: " << arg << std::endl; + assert(0); + } +} + +template void defaultYAMLHandler(void* pointer, const YAML::Node& node) +{ + T* typedPointer = static_cast(pointer); + *typedPointer = node.as(); +} + +template void defaultPrintHandler(void* pointer, std::ostream& stream) +{ + T* typedPointer = static_cast(pointer); + stream << *typedPointer; +} + +// Class for a setting that can be set by the GUI in Pangolin. +class PangolinSettingVar +{ +public: + virtual ~PangolinSettingVar() = default; + + // Called in the Pangolin thread + virtual void createVar() = 0; // Create Var + virtual void updateVar() = 0; // Update value of var. +}; + +template class PangolinSetting : public PangolinSettingVar +{ +public: + PangolinSetting(std::string name, T* pointer, bool toggle) + : name(name), pointer(pointer), toggle(toggle), boolConstr(true) + {} + + PangolinSetting(std::string name, T* pointer, double min, double max) + : name(name), pointer(pointer), min(min), max(max), boolConstr(false) + {} + + void createVar() override + { + if(boolConstr) + { + var.reset(new pangolin::Var("ui." + name, *pointer, toggle)); + }else + { + var.reset(new pangolin::Var("ui." + name, *pointer, min, max)); + } + } + + void updateVar() override + { + *pointer = var->Get(); + assert(var); + } + +private: + std::string name; + std::unique_ptr> var; + T* pointer; + bool boolConstr, toggle; + double min, max; +}; + +class SettingsUtil +{ +public: + // Overwrite settings with the ones saved in the yaml file. + // Note that for this we don't check that every element in the yaml file must be read, so typos are not checked. + void tryReadFromYaml(const YAML::Node& node); + + // Set a parameter from a (single) commandline argument. + // Returns true if the setting existed and was set. + // Settings set from commandline have preference over ones set from yaml. + bool tryReadFromCommandLine(const std::string& arg); + + // Register argument with the given name. + template + void registerArg(std::string name, T& arg) + { + if(parameters.find(name) != parameters.end()) + { + std::cerr << "ERROR: Trying to add parameter twice! " << name << std::endl; + assert(0); + } + parameters.emplace(name, + Parameter(static_cast(&arg), defaultCommandLineHandler, defaultYAMLHandler, + defaultPrintHandler)); + } + + // The following 2 methods will also create a GUI item in Pangolin for the setting (either a toggle switch or a slider). + template void registerArg(std::string name, T& arg, bool toggle) + { + registerArg(name, arg); + parameters.at(name).pangolinSetting.reset(new PangolinSetting(name, &arg, toggle)); + } + + template void registerArg(std::string name, T& arg, double min, double max) + { + registerArg(name, arg); + parameters.at(name).pangolinSetting.reset(new PangolinSetting(name, &arg, min, max)); + } + + // Dump all settings to file. + void printAllSettings(std::ostream& stream); + + // Should be called from Pangolin thread. + void createPangolinSettings(); + void updatePangolinSettings(); + +private: + struct Parameter + { + Parameter(void* pointer, const std::function& commandLineHandler, + const std::function& yamlHandler, + const std::function& printHandler); + + void* pointer; + std::function commandLineHandler; + std::function yamlHandler; + std::function printHandler; + bool loadedFromCommandLine{ + false}; // Used to make sure that we don't overwrite parameters set using commandline when reading from yaml. + + std::unique_ptr pangolinSetting; + }; + std::map parameters; +}; +} + +#endif //DMVIO_SETTINGSUTIL_H diff --git a/src/util/TimeMeasurement.cpp b/src/util/TimeMeasurement.cpp new file mode 100644 index 0000000..cf2b646 --- /dev/null +++ b/src/util/TimeMeasurement.cpp @@ -0,0 +1,132 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#include "TimeMeasurement.h" +#include + +using namespace dmvio; +using namespace std::chrono; + + +std::map dmvio::TimeMeasurement::logs = std::map(); +bool dmvio::TimeMeasurement::saveFileOpen = false; + +dmvio::TimeMeasurement::TimeMeasurement(std::string name) + : name(name) +{ + begin = high_resolution_clock::now(); +} + +dmvio::TimeMeasurement::~TimeMeasurement() +{ + end(); +} + +double dmvio::TimeMeasurement::end() +{ + if(ended) + { + return -1; + } + + auto end = high_resolution_clock::now(); + double duration = duration_cast>(end - begin).count(); + + logs[name].addMeasurement(duration); + + ended = true; + + return duration; +} + +void dmvio::TimeMeasurement::saveResults(std::string filename) +{ + std::ofstream saveFile; + saveFile.open(filename); + + for(const auto& pair : logs) + { + saveFile << pair.first << ' ' << pair.second << '\n'; + } + saveFile.close(); +} + +void dmvio::TimeMeasurement::cancel() +{ + ended = true; +} + +void dmvio::MeasurementLog::addMeasurement(double time) +{ + if(num == 0) + { + first = time; + } + sum += time; + num++; + + double shifted = time - first; + sumShifted += shifted; + sumSquared += shifted * shifted; + + if(time > max) + { + max = time; + } +} + +void dmvio::MeasurementLog::writeLogLine(std::ostream& stream) const +{ + double mean = getMean(); + double variance = getVariance(); + + stream << mean << ' ' << variance << ' ' << max << ' ' << num; + +} + +double dmvio::MeasurementLog::getVariance() const +{ + double variance = (sumSquared - (sumShifted * sumShifted) / num) / (num - 1); + return variance; +} + +double dmvio::MeasurementLog::getMean() const +{ + double mean = sum / (double) num; + return mean; +} + +double dmvio::MeasurementLog::getMax() const +{ + return max; +} + +int dmvio::MeasurementLog::getNum() const +{ + return num; +} + +std::ostream& operator<<(std::ostream& os, const dmvio::MeasurementLog& obj) +{ + obj.writeLogLine(os); + return os; +} diff --git a/src/util/TimeMeasurement.h b/src/util/TimeMeasurement.h new file mode 100644 index 0000000..c8e362b --- /dev/null +++ b/src/util/TimeMeasurement.h @@ -0,0 +1,93 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + +#ifndef DMVIO_TIMEMEASUREMENT_H +#define DMVIO_TIMEMEASUREMENT_H + +#include +#include +#include +#include +#include + + +namespace dmvio +{ +// Saves mean, maximum, and variance. +class MeasurementLog +{ +public: + MeasurementLog() = default; + + void addMeasurement(double time); + + void writeLogLine(std::ostream& stream) const; + + int getNum() const; + double getMax() const; + double getMean() const; + double getVariance() const; +private: + double sum{0}; + double max{0}; + int num{0}; + + double first{-1}; + // For computing stddev we shift by the first sample. + double sumShifted{0}; + double sumSquared{0}; + +}; + +// Used to measure and log wall time for different code parts. +// Note that this class is only partially thread-safe, meaning there should not be measurements with the same name +// in different threads, otherwise there can be an endless loop / segfault in the first call! +class TimeMeasurement final +{ +public: + TimeMeasurement(std::string name); + TimeMeasurement(const TimeMeasurement&) = delete; + ~TimeMeasurement(); + + // End the measurement interval. Optional, if not called the destructor will call it. + double end(); + + // Cancel the time measurement. + void cancel(); + + static void saveResults(std::string filename); + +private: + static bool saveFileOpen; + static std::ofstream saveFile; + static std::map logs; + + std::string name; + std::chrono::high_resolution_clock::time_point begin; + bool ended{false}; +}; +} + +std::ostream& operator<<(std::ostream& os, const dmvio::MeasurementLog& obj); + + +#endif //DMVIO_TIMEMEASUREMENT_H diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..e8f85fc --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,9 @@ +project(Tests) + +if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/googletest/CMakeLists.txt") + add_subdirectory(googletest) + include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) + + add_executable(Google_Tests_run test_PoseTransformationFactor.cpp) + target_link_libraries(Google_Tests_run gtest gtest_main dmvio ${DMVIO_LINKED_LIBRARIES}) +endif() \ No newline at end of file diff --git a/test/googletest b/test/googletest new file mode 160000 index 0000000..af29db7 --- /dev/null +++ b/test/googletest @@ -0,0 +1 @@ +Subproject commit af29db7ec28d6df1c7f0f745186884091e602e07 diff --git a/test/test_PoseTransformationFactor.cpp b/test/test_PoseTransformationFactor.cpp new file mode 100644 index 0000000..77eba50 --- /dev/null +++ b/test/test_PoseTransformationFactor.cpp @@ -0,0 +1,168 @@ +/** +* This file is part of DM-VIO. +* +* Copyright (c) 2022 Lukas von Stumberg . +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* DM-VIO is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* DM-VIO 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 General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with DM-VIO. If not, see . +*/ + + +#include + +#include +#include +#include +#include "GTSAMIntegration/PoseTransformationFactor.h" +#include +#include +#include +#include + +#include + +using namespace gtsam; +using namespace dmvio; +using symbol_shorthand::P, symbol_shorthand::S; + + +class SimpleGraphTest : public ::testing::Test +{ +protected: + gtsam::NonlinearFactorGraph graph; + Values values; + Values trueValues; + BetweenFactor::shared_ptr between01; + BetweenFactor::shared_ptr between12; + + gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); + gtsam::SharedNoiseModel betweenModel = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); + + void SetUp() override + { + // P0 at (0, 0, 0) + // P1 at (2, 0, 0) + // P2 at (4, 0, 0) + // scale is 2 + // Between factor in IMU scale: + // between P0-P1 and P1-P2: (1, 0, 0) + Pose3 pose0(gtsam::Rot3::identity(), gtsam::Point3(0.0, 0.0, 0.0)); + Pose3 pose1(gtsam::Rot3::identity(), gtsam::Point3(2.0, 0.0, 0.0)); + Pose3 pose2(gtsam::Rot3::identity(), gtsam::Point3(4.0, 0.0, 0.0)); + + // We use the inverse, because TransformDSOToIMUNew also converts from worldToCam to camToWorld. + pose0 = pose0.inverse(); + pose1 = pose1.inverse(); + pose2 = pose2.inverse(); + + trueValues.insert(P(0), pose0); + trueValues.insert(P(1), pose1); + trueValues.insert(P(2), pose2); + + graph.push_back(gtsam::PriorFactor(P(0), pose0, priorModel)); + graph.push_back(gtsam::PriorFactor(P(2), pose2, priorModel)); + + values.insert(P(0), Pose3{}); + values.insert(P(1), Pose3{}); + values.insert(P(2), Pose3{}); + + between01.reset(new BetweenFactor(P(0), P(1), + gtsam::Pose3(gtsam::Rot3::identity(), gtsam::Point3(1.0, 0.0, 0.0)), betweenModel)); + between12.reset(new BetweenFactor(P(1), P(2), + gtsam::Pose3(gtsam::Rot3::identity(), gtsam::Point3(1.0, 0.0, 0.0)), betweenModel)); + } +}; + +class SimpleGraphTestsWithParams : public SimpleGraphTest, public ::testing::WithParamInterface +{ +}; + +TEST_F(SimpleGraphTest, NoPoseTransformation) +{ + graph.push_back(between01); + graph.push_back(between12); + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + EXPECT_FALSE(trueValues.equals(result, 0.1)); +} + +TEST_P(SimpleGraphTestsWithParams, WithTransformDSOToIMUNew) +{ + PoseTransformationFactor::ConversionType conversionType = GetParam(); + std::shared_ptr transform( + new TransformDSOToIMU(gtsam::Pose3::identity(), std::make_shared(true), + std::make_shared(false), std::make_shared(false), true, 0)); + + graph.push_back(boost::make_shared(between01, *transform, conversionType)); + graph.push_back(boost::make_shared(between12, *transform, conversionType)); + + values.insert(S(0), ScaleGTSAM(1.0)); + trueValues.insert(S(0), ScaleGTSAM(0.5)); + LevenbergMarquardtParams params = LevenbergMarquardtParams::CeresDefaults(); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values result = optimizer.optimize(); + EXPECT_TRUE(assert_equal(trueValues, result)); + +} + +TEST(ValuesTest, ValuesPointerChange) +{ + Values values; + values.insert(S(0), ScaleGTSAM(2.0)); + values.insert(S(1), 2.0); + gtsam::Vector3 vector = gtsam::Vector3::Identity(); + values.insert(S(2), vector); + + const Values* pointerBefore = &values; + EXPECT_TRUE(pointerBefore == &values); + + VectorValues incVec; + gtsam::Vector1 inc; + inc(0) = 0.001; + incVec.insert(S(0), inc); + values = values.retract(incVec); +} + +INSTANTIATE_TEST_SUITE_P(PoseTransformationTests, SimpleGraphTestsWithParams, + ::testing::Values(PoseTransformationFactor::JACOBIAN_BAKED_IN, PoseTransformationFactor::JACOBIAN_FACTOR)); + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/thirdparty/Sophus/CMakeLists.txt b/thirdparty/Sophus/CMakeLists.txt new file mode 100644 index 0000000..49a5657 --- /dev/null +++ b/thirdparty/Sophus/CMakeLists.txt @@ -0,0 +1,83 @@ +SET(PROJECT_NAME Sophus) + +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +SET (CMAKE_VERBOSE_MAKEFILE ON) + +# Release by default +# Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" +IF( NOT CMAKE_BUILD_TYPE ) + SET( CMAKE_BUILD_TYPE Release ) +ENDIF() + +IF (CMAKE_COMPILER_IS_GNUCXX ) + SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") + SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ") + + ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable + -Wno-unused-but-set-variable -Wno-unknown-pragmas ") +ENDIF() + +################################################################################ +# Add local path for finding packages, set the local version first +set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" ) +list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" ) + +################################################################################ +# Create variables used for exporting in SophusConfig.cmake +set( Sophus_LIBRARIES "" ) +set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} ) + +################################################################################ + +include(FindEigen3.cmake) +#find_package( Eigen3 REQUIRED ) +INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} ) +SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) + +SET (SOURCE_DIR "sophus") + +SET (TEMPLATES tests + so2 + se2 + so3 + se3 + rxso3 + sim3 +) + +SET (SOURCES ${SOURCE_DIR}/sophus.hpp) + +FOREACH(templ ${TEMPLATES}) + LIST(APPEND SOURCES ${SOURCE_DIR}/${templ}.hpp) +ENDFOREACH(templ) + + +INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) + +# Added ${SOURCES} to executables so they show up in QtCreator (and possibly +# other IDEs). +# ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp ${SOURCES}) +# ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp ${SOURCES}) +# ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp ${SOURCES}) +# ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp ${SOURCES}) +# ADD_EXECUTABLE(test_rxso3 sophus/test_rxso3.cpp ${SOURCES}) +# ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp ${SOURCES}) +# ENABLE_TESTING() +# +# ADD_TEST(test_so2 test_so2) +# ADD_TEST(test_se2 test_se2) +# ADD_TEST(test_so3 test_so3) +# ADD_TEST(test_se3 test_se3) +# ADD_TEST(test_rxso3 test_rxso3) +# ADD_TEST(test_sim3 test_sim3) + +################################################################################ +# Create the SophusConfig.cmake file for other cmake projects. +CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE ) +export( PACKAGE Sophus ) + +INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include + FILES_MATCHING PATTERN "*.hpp" ) \ No newline at end of file diff --git a/thirdparty/Sophus/FindEigen3.cmake b/thirdparty/Sophus/FindEigen3.cmake new file mode 100644 index 0000000..9c546a0 --- /dev/null +++ b/thirdparty/Sophus/FindEigen3.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) + diff --git a/thirdparty/Sophus/README b/thirdparty/Sophus/README new file mode 100644 index 0000000..05213ac --- /dev/null +++ b/thirdparty/Sophus/README @@ -0,0 +1,19 @@ +Sophus (version 0.9a) + +C++ implementation of Lie Groups using Eigen. + +Thanks to Steven Lovegrove, Sophus is now fully templated - using the Curiously Recurring Template Pattern (CRTP). + +(In order to go back to the non-templated/double-only version "git checkout a621ff".) + +Installation guide: + +>>> +cd Sophus +mkdir build +cd build +cmake .. +make +<<< + + diff --git a/thirdparty/Sophus/SophusConfig.cmake.in b/thirdparty/Sophus/SophusConfig.cmake.in new file mode 100644 index 0000000..24ba84f --- /dev/null +++ b/thirdparty/Sophus/SophusConfig.cmake.in @@ -0,0 +1,11 @@ +################################################################################ +# Sophus source dir +set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") + +################################################################################ +# Sophus build dir +set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@") + +################################################################################ +set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" ) +set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" ) \ No newline at end of file diff --git a/thirdparty/Sophus/cmake_modules/FindEigen3.cmake b/thirdparty/Sophus/cmake_modules/FindEigen3.cmake new file mode 100644 index 0000000..469c77d --- /dev/null +++ b/thirdparty/Sophus/cmake_modules/FindEigen3.cmake @@ -0,0 +1,26 @@ +# - Try to find Eigen3 lib +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib +# EIGEN3_INCLUDE_DIR - the eigen include directory + +# Copyright (c) 2006, 2007 Montel Laurent, +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + +if( EIGEN3_INCLUDE_DIR ) + # in cache already + set( EIGEN3_FOUND TRUE ) +else (EIGEN3_INCLUDE_DIR) + find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core + PATH_SUFFIXES eigen3/ + HINTS + ${INCLUDE_INSTALL_DIR} + /usr/local/include + ${KDE4_INCLUDE_DIR} + ) + include( FindPackageHandleStandardArgs ) + find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) + mark_as_advanced( EIGEN3_INCLUDE_DIR ) +endif(EIGEN3_INCLUDE_DIR) + diff --git a/thirdparty/Sophus/sophus/rxso3.hpp b/thirdparty/Sophus/sophus/rxso3.hpp new file mode 100644 index 0000000..a0262a0 --- /dev/null +++ b/thirdparty/Sophus/sophus/rxso3.hpp @@ -0,0 +1,838 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_RXSO3_HPP +#define RXSO3_HPP + +#include "sophus.hpp" +#include "so3.hpp" + +//////////////////////////////////////////////////////////////////////////// +// Forward Declarations / typedefs +//////////////////////////////////////////////////////////////////////////// + +namespace Sophus { +template class RxSO3Group; +typedef RxSO3Group ScSO3 EIGEN_DEPRECATED; +typedef RxSO3Group RxSO3d; /**< double precision RxSO3 */ +typedef RxSO3Group RxSO3f; /**< single precision RxSO3 */ +} + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +namespace Eigen { +namespace internal { + +template +struct traits > { + typedef _Scalar Scalar; + typedef Quaternion QuaternionType; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> QuaternionType; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> QuaternionType; +}; + +} +} + +namespace Sophus { +using namespace Eigen; + +class ScaleNotPositive : public SophusException { +public: + ScaleNotPositive () + : SophusException("Scale factor is not positive") { + } +}; + +/** + * \brief RxSO3 base type - implements RxSO3 class but is storage agnostic + * + * This class implements the group \f$ R^{+} \times SO3 \f$ (RxSO3), the direct + * product of the group of positive scalar matrices (=isomorph to the positive + * real numbers) and the three-dimensional special orthogonal group SO3. + * Geometrically, it is the group of rotation and scaling in three dimensions. + * As a matrix groups, RxSO3 consists of matrices of the form \f$ s\cdot R \f$ + * where \f$ R \f$ is an orthognal matrix with \f$ det(R)=1 \f$ and \f$ s>0 \f$ + * be a positive real number. + * + * Internally, RxSO3 is represented by the group of non-zero quaternions. This + * is a most compact representation since the degrees of freedom (DoF) of RxSO3 + * (=4) equals the number of internal parameters (=4). + * + * [add more detailed description/tutorial] + */ +template +class RxSO3GroupBase { +public: + /** \brief scalar type, use with care since this might be a Map type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief quaternion reference type */ + typedef typename internal::traits::QuaternionType & + QuaternionReference; + /** \brief quaternion const reference type */ + typedef const typename internal::traits::QuaternionType & + ConstQuaternionReference; + + + /** \brief degree of freedom of group + * (three for rotation and one for scaling) */ + static const int DoF = 4; + /** \brief number of internal parameters used + * (quaternion for rotation and scaling) */ + static const int num_parameters = 4; + /** \brief group transformations are NxN matrices */ + static const int N = 3; + /** \brief group transfomation type */ + typedef Matrix Transformation; + /** \brief point type */ + typedef Matrix Point; + /** \brief tangent vector type */ + typedef Matrix Tangent; + /** \brief adjoint transformation type */ + typedef Matrix Adjoint; + + + /** + * \brief Adjoint transformation + * + * This function return the adjoint transformation \f$ Ad \f$ of the + * group instance \f$ A \f$ such that for all \f$ x \f$ + * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ + * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. + * + * For RxSO3, it simply returns the rotation matrix corresponding to + * \f$ A \f$. + */ + inline + const Adjoint Adj() const { + Adjoint res; + res.setIdentity(); + res.template topLeftCorner<3,3>() = rotationMatrix(); + return res; + } + + /** + * \returns copy of instance casted to NewScalarType + */ + template + inline RxSO3Group cast() const { + return RxSO3Group(quaternion() + .template cast() ); + } + + /** + * \returns pointer to internal data + * + * This provides direct read/write access to internal data. RxSO3 is + * represented by an Eigen::Quaternion (four parameters). + * + * Note: The first three Scalars represent the imaginary parts, while the + * forth Scalar represent the real part. + */ + inline Scalar* data() { + return quaternion().coeffs().data(); + } + + /** + * \returns const pointer to internal data + * + * Const version of data(). + */ + inline const Scalar* data() const { + return quaternion().coeffs().data(); + } + + /** + * \brief In-place group multiplication + * + * Same as operator*=() for RxSO3. + * + * \see operator*=() + */ + inline + void fastMultiply(const RxSO3Group& other) { + quaternion() *= other.quaternion(); + } + + + /** + * \returns group inverse of instance + */ + inline + const RxSO3Group inverse() const { + if(quaternion().squaredNorm() <= static_cast(0)) { + throw ScaleNotPositive(); + } + return RxSO3Group(quaternion().inverse()); + } + + /** + * \brief Logarithmic map + * + * \returns tangent space representation (=rotation vector) of instance + * + * \see log(). + */ + inline + const Tangent log() const { + return RxSO3Group::log(*this); + } + + /** + * \returns 3x3 matrix representation of instance + * + * For RxSO3, the matrix representation is a scaled orthogonal + * matrix \f$ sR \f$ with \f$ det(sR)=s^3 \f$, thus a scaled rotation + * matrix \f$ R \f$ with scale s. + */ + inline + const Transformation matrix() const { + //ToDO: implement this directly! + Scalar scale = quaternion().norm(); + Quaternion norm_quad = quaternion(); + norm_quad.coeffs() /= scale; + return scale*norm_quad.toRotationMatrix(); + } + + /** + * \brief Assignment operator + */ + template inline + RxSO3GroupBase& operator= + (const RxSO3GroupBase & other) { + quaternion() = other.quaternion(); + return *this; + } + + /** + * \brief Group multiplication + * \see operator*=() + */ + inline + const RxSO3Group operator*(const RxSO3Group& other) const { + RxSO3Group result(*this); + result *= other; + return result; + } + + /** + * \brief Group action on \f$ \mathbf{R}^3 \f$ + * + * \param p point \f$p \in \mathbf{R}^3 \f$ + * \returns point \f$p' \in \mathbf{R}^3 \f$, + * rotated and scaled version of \f$p\f$ + * + * This function rotates and scales a point \f$ p \f$ in \f$ \mathbf{R}^3 \f$ + * by the RxSO3 transformation \f$sR\f$ (=rotation matrix) + * : \f$ p' = sR\cdot p \f$. + */ + inline + const Point operator*(const Point & p) const { + //ToDO: implement this directly! + Scalar scale = quaternion().norm(); + Quaternion norm_quad = quaternion(); + norm_quad.coeffs() /= scale; + return scale*norm_quad._transformVector(p); + } + + /** + * \brief In-place group multiplication + * \see operator*=() + */ + inline + void operator*=(const RxSO3Group& other) { + quaternion() *= other.quaternion(); + } + + /** + * \brief Mutator of quaternion + */ + EIGEN_STRONG_INLINE + QuaternionReference quaternion() { + return static_cast(this)->quaternion(); + } + + /** + * \brief Accessor of quaternion + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference quaternion() const { + return static_cast(this)->quaternion(); + } + + /** + * \returns rotation matrix + */ + inline + Transformation rotationMatrix() const { + Scalar scale = quaternion().norm(); + Quaternion norm_quad = quaternion(); + norm_quad.coeffs() /= scale; + return norm_quad.toRotationMatrix(); + } + + /** + * \returns scale + */ + EIGEN_STRONG_INLINE + const Scalar scale() const { + return quaternion().norm(); + } + + /** + * \brief Setter of quaternion using rotation matrix, leaves scale untouched + * + * \param R a 3x3 rotation matrix + * \pre the 3x3 matrix should be orthogonal and have a determinant of 1 + */ + inline + void setRotationMatrix(const Transformation & R) { + Scalar saved_scale = scale(); + quaternion() = R; + quaternion().coeffs() *= saved_scale; + } + + /** + * \brief Scale setter + */ + EIGEN_STRONG_INLINE + void setScale(const Scalar & scale) { + quaternion().normalize(); + quaternion().coeffs() *= scale; + } + + /** + * \brief Setter of quaternion using scaled rotation matrix + * + * \param sR a 3x3 scaled rotation matrix + * \pre the 3x3 matrix should be "scaled orthogonal" + * and have a positive determinant + */ + inline + void setScaledRotationMatrix + (const Transformation & sR) { + Transformation squared_sR = sR*sR.transpose(); + Scalar squared_scale + = static_cast(1./3.) + *(squared_sR(0,0)+squared_sR(1,1)+squared_sR(2,2)); + if (squared_scale <= static_cast(0)) { + throw ScaleNotPositive(); + } + Scalar scale = std::sqrt(squared_scale); + if (scale <= static_cast(0)) { + throw ScaleNotPositive(); + } + quaternion() = sR/scale; + quaternion().coeffs() *= scale; + } + + //////////////////////////////////////////////////////////////////////////// + // public static functions + //////////////////////////////////////////////////////////////////////////// + + /** + * \param b 4-vector representation of Lie algebra element + * \returns derivative of Lie bracket + * + * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{rxso3} \f$ + * with \f$ [a, b]_{rxso3} \f$ being the lieBracket() of the Lie + * algebra rxso3. + * + * \see lieBracket() + */ + inline static + const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { + Adjoint res; + res.setZero(); + res.template topLeftCorner<3,3>() = -SO3::hat(b.template head<3>()); + return res; + } + + /** + * \brief Group exponential + * + * \param a tangent space element + * (rotation vector \f$ \omega \f$ and logarithm of scale) + * \returns corresponding element of the group RxSO3 + * + * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ + * with \f$ \exp(\cdot) \f$ being the matrix exponential + * and \f$ \widehat{\cdot} \f$ the hat()-operator of RxSO3. + * + * \see expAndTheta() + * \see hat() + * \see log() + */ + inline static + const RxSO3Group exp(const Tangent & a) { + Scalar theta; + return expAndTheta(a, &theta); + } + + /** + * \brief Group exponential and theta + * + * \param a tangent space element + * (rotation vector \f$ \omega \f$ and logarithm of scale ) + * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ + * \returns corresponding element of the group RxSO3 + * + * \see exp() for details + */ + inline static + const RxSO3Group expAndTheta(const Tangent & a, + Scalar * theta) { + const Matrix & omega = a.template head<3>(); + Scalar sigma = a[3]; + Scalar scale = std::exp(sigma); + Quaternion quat + = SO3Group::expAndTheta(omega, theta).unit_quaternion(); + quat.coeffs() *= scale; + return RxSO3Group(quat); + } + + /** + * \brief Generators + * + * \pre \f$ i \in \{0,1,2,3\} \f$ + * \returns \f$ i \f$th generator \f$ G_i \f$ of RxSO3 + * + * The infinitesimal generators of RxSO3 + * are \f$ + * G_0 = \left( \begin{array}{ccc} + * 0& 0& 0& \\ + * 0& 0& -1& \\ + * 0& 1& 0& + * \end{array} \right), + * G_1 = \left( \begin{array}{ccc} + * 0& 0& 1& \\ + * 0& 0& 0& \\ + * -1& 0& 0& + * \end{array} \right), + * G_2 = \left( \begin{array}{ccc} + * 0& -1& 0& \\ + * 1& 0& 0& \\ + * 0& 0& 0& + * \end{array} \right), + * G_3 = \left( \begin{array}{ccc} + * 1& 0& 0& \\ + * 0& 1& 0& \\ + * 0& 0& 1& + * \end{array} \right). + * \f$ + * \see hat() + */ + inline static + const Transformation generator(int i) { + if (i<0 || i>3) { + throw SophusException("i is not in range [0,3]."); + } + Tangent e; + e.setZero(); + e[i] = static_cast(1); + return hat(e); + } + + /** + * \brief hat-operator + * + * \param a 4-vector representation of Lie algebra element + * \returns 3x3-matrix representatin of Lie algebra element + * + * Formally, the hat-operator of RxSO3 is defined + * as \f$ \widehat{\cdot}: \mathbf{R}^4 \rightarrow \mathbf{R}^{3\times 3}, + * \quad \widehat{a} = \sum_{i=0}^3 G_i a_i \f$ + * with \f$ G_i \f$ being the ith infinitesial generator(). + * + * \see generator() + * \see vee() + */ + inline static + const Transformation hat(const Tangent & a) { + Transformation A; + A << a(3), -a(2), a(1) + , a(2), a(3), -a(0) + ,-a(1), a(0), a(3); + return A; + } + + /** + * \brief Lie bracket + * + * \param a 4-vector representation of Lie algebra element + * \param b 4-vector representation of Lie algebra element + * \returns 4-vector representation of Lie algebra element + * + * It computes the bracket of RxSO3. To be more specific, it + * computes \f$ [a, 2]_{rxso3} + * := [\widehat{a}, \widehat{b}]^\vee \f$ + * with \f$ [A,B] = AB-BA \f$ being the matrix + * commutator, \f$ \widehat{\cdot} \f$ the + * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of RxSO3. + * + * \see hat() + * \see vee() + */ + inline static + const Tangent lieBracket(const Tangent & a, + const Tangent & b) { + const Matrix & omega1 = a.template head<3>(); + const Matrix & omega2 = b.template head<3>(); + Matrix res; + res.template head<3>() = omega1.cross(omega2); + res[3] = static_cast(0); + return res; + } + + /** + * \brief Logarithmic map + * + * \param other element of the group RxSO3 + * \returns corresponding tangent space element + * (rotation vector \f$ \omega \f$ and logarithm of scale) + * + * Computes the logarithmic, the inverse of the group exponential. + * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ + * with \f$ \vee(\cdot) \f$ being the matrix logarithm + * and \f$ \vee{\cdot} \f$ the vee()-operator of RxSO3. + * + * \see exp() + * \see logAndTheta() + * \see vee() + */ + inline static + const Tangent log(const RxSO3Group & other) { + Scalar theta; + return logAndTheta(other, &theta); + } + + /** + * \brief Logarithmic map and theta + * + * \param other element of the group RxSO3 + * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ + * \returns corresponding tangent space element + * (rotation vector \f$ \omega \f$ and logarithm of scale) + * + * \see log() for details + */ + inline static + const Tangent logAndTheta(const RxSO3Group & other, + Scalar * theta) { + const Scalar & scale = other.quaternion().norm(); + Tangent omega_sigma; + omega_sigma[3] = std::log(scale); + omega_sigma.template head<3>() + = SO3Group::logAndTheta(SO3Group(other.quaternion()), + theta); + return omega_sigma; + } + + /** + * \brief vee-operator + * + * \param Omega 3x3-matrix representation of Lie algebra element + * \returns 4-vector representatin of Lie algebra element + * + * This is the inverse of the hat()-operator. + * + * \see hat() + */ + inline static + const Tangent vee(const Transformation & Omega) { + return Tangent( static_cast(0.5) * (Omega(2,1) - Omega(1,2)), + static_cast(0.5) * (Omega(0,2) - Omega(2,0)), + static_cast(0.5) * (Omega(1,0) - Omega(0,1)), + static_cast(1./3.) + * (Omega(0,0) + Omega(1,1) + Omega(2,2)) ); + } +}; + +/** + * \brief RxSO3 default type - Constructors and default storage for RxSO3 Type + */ +template +class RxSO3Group : public RxSO3GroupBase > { + typedef RxSO3GroupBase > Base; +public: + /** \brief scalar type */ + typedef typename internal::traits > + ::Scalar Scalar; + /** \brief quaternion reference type */ + typedef typename internal::traits > + ::QuaternionType & QuaternionReference; + /** \brief quaternion const reference type */ + typedef const typename internal::traits > + ::QuaternionType & ConstQuaternionReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Default constructor + * + * Initialize Quaternion to identity rotation and scale. + */ + inline RxSO3Group() + : quaternion_(static_cast(1), static_cast(0), + static_cast(0), static_cast(0)) { + } + + /** + * \brief Copy constructor + */ + template inline + RxSO3Group(const RxSO3GroupBase & other) + : quaternion_(other.quaternion()) { + } + + /** + * \brief Constructor from scaled rotation matrix + * + * \pre matrix need to be "scaled orthogonal" with positive determinant + */ + inline explicit + RxSO3Group(const Transformation & sR) { + this->setScaledRotationMatrix(sR); + } + + /** + * \brief Constructor from scale factor and rotation matrix + * + * \pre rotation matrix need to be orthogonal with determinant of 1 + * \pre scale need to be not zero + */ + inline + RxSO3Group(const Scalar & scale, const Transformation & R) + : quaternion_(R) { + if(scale <= static_cast(0)) { + throw ScaleNotPositive(); + } + quaternion_.normalize(); + quaternion_.coeffs() *= scale; + } + + /** + * \brief Constructor from scale factor and SO3 + * + * \pre scale need to be not zero + */ + inline + RxSO3Group(const Scalar & scale, const SO3Group & so3) + : quaternion_(so3.unit_quaternion()) { + if (scale <= static_cast(0)) { + throw ScaleNotPositive(); + } + quaternion_.normalize(); + quaternion_.coeffs() *= scale; + } + + /** + * \brief Constructor from quaternion + * + * \pre quaternion must not be zero + */ + inline explicit + RxSO3Group(const Quaternion & quat) : quaternion_(quat) { + if(quaternion_.squaredNorm() <= SophusConstants::epsilon()) { + throw ScaleNotPositive(); + } + } + + /** + * \brief Mutator of quaternion + */ + EIGEN_STRONG_INLINE + QuaternionReference quaternion() { + return quaternion_; + } + + /** + * \brief Accessor of quaternion + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference quaternion() const { + return quaternion_; + } + +protected: + Quaternion quaternion_; +}; + +} // end namespace + + +namespace Eigen { +/** + * \brief Specialisation of Eigen::Map for RxSO3GroupBase + * + * Allows us to wrap RxSO3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::RxSO3GroupBase< + Map,_Options> > { + typedef Sophus::RxSO3GroupBase, _Options> > + Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief quaternion reference type */ + typedef typename internal::traits::QuaternionType & + QuaternionReference; + /** \brief quaternion const reference type */ + typedef const typename internal::traits::QuaternionType & + ConstQuaternionReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(Scalar* coeffs) : quaternion_(coeffs) { + } + + /** + * \brief Mutator of quaternion + */ + EIGEN_STRONG_INLINE + QuaternionReference quaternion() { + return quaternion_; + } + + /** + * \brief Accessor of quaternion + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference quaternion() const { + return quaternion_; + } + +protected: + Map,_Options> quaternion_; +}; + +/** + * \brief Specialisation of Eigen::Map for const RxSO3GroupBase + * + * Allows us to wrap RxSO3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::RxSO3GroupBase< + Map, _Options> > { + typedef Sophus::RxSO3GroupBase< + Map, _Options> > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief quaternion const reference type */ + typedef const typename internal::traits::QuaternionType & + ConstQuaternionReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(const Scalar* coeffs) : quaternion_(coeffs) { + } + + /** + * \brief Accessor of unit quaternion + * + * No direct write access is given to ensure the quaternion stays normalized. + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference quaternion() const { + return quaternion_; + } + +protected: + const Map,_Options> quaternion_; +}; + +} + +#endif // SOPHUS_RXSO3_HPP diff --git a/thirdparty/Sophus/sophus/se2.hpp b/thirdparty/Sophus/sophus/se2.hpp new file mode 100644 index 0000000..32f3ff2 --- /dev/null +++ b/thirdparty/Sophus/sophus/se2.hpp @@ -0,0 +1,907 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_SE2_HPP +#define SOPHUS_SE2_HPP + +#include "so2.hpp" + +//////////////////////////////////////////////////////////////////////////// +// Forward Declarations / typedefs +//////////////////////////////////////////////////////////////////////////// + +namespace Sophus { +template class SE2Group; +typedef SE2Group SE2 EIGEN_DEPRECATED; +typedef SE2Group SE2d; /**< double precision SE2 */ +typedef SE2Group SE2f; /**< single precision SE2 */ +} + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +namespace Eigen { +namespace internal { + +template +struct traits > { + typedef _Scalar Scalar; + typedef Matrix TranslationType; + typedef Sophus::SO2Group SO2Type; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> TranslationType; + typedef Map,_Options> SO2Type; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> TranslationType; + typedef Map,_Options> SO2Type; +}; + +} +} + +namespace Sophus { +using namespace Eigen; +using namespace std; + +/** + * \brief SE2 base type - implements SE2 class but is storage agnostic + * + * [add more detailed description/tutorial] + */ +template +class SE2GroupBase { +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits::TranslationType & + TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief SO2 reference type */ + typedef typename internal::traits::SO2Type & + SO2Reference; + /** \brief SO2 type */ + typedef const typename internal::traits::SO2Type & + ConstSO2Reference; + + /** \brief degree of freedom of group + * (two for translation, one for in-plane rotation) */ + static const int DoF = 3; + /** \brief number of internal parameters used + * (unit complex number for rotation + translation 2-vector) */ + static const int num_parameters = 4; + /** \brief group transformations are NxN matrices */ + static const int N = 3; + /** \brief group transfomation type */ + typedef Matrix Transformation; + /** \brief point type */ + typedef Matrix Point; + /** \brief tangent vector type */ + typedef Matrix Tangent; + /** \brief adjoint transformation type */ + typedef Matrix Adjoint; + + /** + * \brief Adjoint transformation + * + * This function return the adjoint transformation \f$ Ad \f$ of the + * group instance \f$ A \f$ such that for all \f$ x \f$ + * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ + * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. + */ + inline + const Adjoint Adj() const { + const Matrix & R = so2().matrix(); + Transformation res; + res.setIdentity(); + res.template topLeftCorner<2,2>() = R; + res(0,2) = translation()[1]; + res(1,2) = -translation()[0]; + return res; + } + + /** + * \returns copy of instance casted to NewScalarType + */ + template + inline SE2Group cast() const { + return + SE2Group(so2().template cast(), + translation().template cast() ); + } + + /** + * \brief Fast group multiplication + * + * This method is a fast version of operator*=(), since it does not perform + * normalization. It is up to the user to call normalize() once in a while. + * + * \see operator*=() + */ + inline + void fastMultiply(const SE2Group& other) { + translation() += so2()*(other.translation()); + so2().fastMultiply(other.so2()); + } + + /** + * \returns Group inverse of instance + */ + inline + const SE2Group inverse() const { + const SO2Group invR = so2().inverse(); + return SE2Group(invR, invR*(translation() + *static_cast(-1) ) ); + } + + /** + * \brief Logarithmic map + * + * \returns tangent space representation + * (translational part and rotation angle) of instance + * + * \see log(). + */ + inline + const Tangent log() const { + return log(*this); + } + + /** + * \brief Normalize SO2 element + * + * It re-normalizes the SO2 element. This method only needs to + * be called in conjunction with fastMultiply() or data() write access. + */ + inline + void normalize() { + so2().normalize(); + } + + /** + * \returns 3x3 matrix representation of instance + */ + inline + const Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.setIdentity(); + homogenious_matrix.block(0,0,2,2) = rotationMatrix(); + homogenious_matrix.col(2).head(2) = translation(); + return homogenious_matrix; + } + + /** + * \returns 2x3 matrix representation of instance + * + * It returns the three first row of matrix(). + */ + inline + const Matrix matrix2x3() const { + Matrix matrix; + matrix.block(0,0,2,2) = rotationMatrix(); + matrix.col(2) = translation(); + return matrix; + } + + /** + * \brief Assignment operator + */ + template inline + SE2GroupBase& operator= (const SE2GroupBase & other) { + so2() = other.so2(); + translation() = other.translation(); + return *this; + } + + /** + * \brief Group multiplication + * \see operator*=() + */ + inline + const SE2Group operator*(const SE2Group& other) const { + SE2Group result(*this); + result *= other; + return result; + } + + /** + * \brief Group action on \f$ \mathbf{R}^2 \f$ + * + * \param p point \f$p \in \mathbf{R}^2 \f$ + * \returns point \f$p' \in \mathbf{R}^2 \f$, + * rotated and translated version of \f$p\f$ + * + * This function rotates and translates point \f$ p \f$ + * in \f$ \mathbf{R}^2 \f$ by the SE2 transformation \f$R,t\f$ + * (=rotation matrix, translation vector): \f$ p' = R\cdot p + t \f$. + */ + inline + const Point operator*(const Point & p) const { + return so2()*p + translation(); + } + + /** + * \brief In-place group multiplication + * + * \see fastMultiply() + * \see operator*() + */ + inline + void operator*=(const SE2Group& other) { + fastMultiply(other); + normalize(); + } + + + /** + * \returns Rotation matrix + */ + inline + const Matrix rotationMatrix() const { + return so2().matrix(); + } + + /** + * \brief Setter of internal unit complex number representation + * + * \param complex + * \pre the complex number must not be zero + * + * The complex number is normalized to unit length. + */ + inline + void setComplex(const Matrix & complex) { + return so2().setComplex(complex); + } + + /** + * \brief Setter of unit complex number using rotation matrix + * + * \param R a 2x2 matrix + * \pre the 2x2 matrix should be orthogonal and have a determinant of 1 + */ + inline + void setRotationMatrix(const Matrix & R) { + so2().setComplex(static_cast(0.5)*(R(0,0)+R(1,1)), + static_cast(0.5)*(R(1,0)-R(0,1))); + } + + /** + * \brief Mutator of SO2 group + */ + EIGEN_STRONG_INLINE + SO2Reference so2() { + return static_cast(this)->so2(); + } + + /** + * \brief Accessor of SO2 group + */ + EIGEN_STRONG_INLINE + ConstSO2Reference so2() const { + return static_cast(this)->so2(); + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return static_cast(this)->translation(); + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return static_cast(this)->translation(); + } + + /** + * \brief Accessor of unit complex number + * + * No direct write access is given to ensure the complex number stays + * normalized. + */ + inline + typename internal::traits::SO2Type::ConstComplexReference + unit_complex() const { + return so2().unit_complex(); + } + + //////////////////////////////////////////////////////////////////////////// + // public static functions + //////////////////////////////////////////////////////////////////////////// + + /** + * \param b 3-vector representation of Lie algebra element + * \returns derivative of Lie bracket + * + * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{se2} \f$ + * with \f$ [a, b]_{se2} \f$ being the lieBracket() of the Lie algebra se2. + * + * \see lieBracket() + */ + inline static + const Transformation d_lieBracketab_by_d_a(const Tangent & b) { + static const Scalar zero = static_cast(0); + Matrix upsilon2 = b.template head<2>(); + Scalar theta2 = b[2]; + + Transformation res; + res << zero, theta2, -upsilon2[1] + , -theta2, zero, upsilon2[0] + , zero, zero, zero; + return res; + } + + /** + * \brief Group exponential + * + * \param a tangent space element (3-vector) + * \returns corresponding element of the group SE2 + * + * The first two components of \f$ a \f$ represent the translational + * part \f$ \upsilon \f$ in the tangent space of SE2, while the last + * components of \f$ a \f$ is the rotation angle \f$ \theta \f$. + * + * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ + * with \f$ \exp(\cdot) \f$ being the matrix exponential + * and \f$ \widehat{\cdot} \f$ the hat()-operator of SE2. + * + * \see hat() + * \see log() + */ + inline static + const SE2Group exp(const Tangent & a) { + Scalar theta = a[2]; + const SO2Group & so2 = SO2Group::exp(theta); + Scalar sin_theta_by_theta; + Scalar one_minus_cos_theta_by_theta; + + if(std::abs(theta)::epsilon()) { + Scalar theta_sq = theta*theta; + sin_theta_by_theta + = static_cast(1.) - static_cast(1./6.)*theta_sq; + one_minus_cos_theta_by_theta + = static_cast(0.5)*theta + - static_cast(1./24.)*theta*theta_sq; + } else { + sin_theta_by_theta = so2.unit_complex().y()/theta; + one_minus_cos_theta_by_theta + = (static_cast(1.) - so2.unit_complex().x())/theta; + } + Matrix trans + (sin_theta_by_theta*a[0] - one_minus_cos_theta_by_theta*a[1], + one_minus_cos_theta_by_theta * a[0]+sin_theta_by_theta*a[1]); + return SE2Group(so2, trans); + } + + /** + * \brief Generators + * + * \pre \f$ i \in \{0,1,2\} \f$ + * \returns \f$ i \f$th generator \f$ G_i \f$ of SE2 + * + * The infinitesimal generators of SE2 are: \f[ + * G_0 = \left( \begin{array}{ccc} + * 0& 0& 1\\ + * 0& 0& 0\\ + * 0& 0& 0\\ + * \end{array} \right), + * G_1 = \left( \begin{array}{cccc} + * 0& 0& 0\\ + * 0& 0& 1\\ + * 0& 0& 0\\ + * \end{array} \right), + * G_2 = \left( \begin{array}{cccc} + * 0& 0& 0&\\ + * 0& 0& -1&\\ + * 0& 1& 0&\\ + * \end{array} \right), + * \f] + * \see hat() + */ + inline static + const Transformation generator(int i) { + if (i<0 || i>2) { + throw SophusException("i is not in range [0,2]."); + } + Tangent e; + e.setZero(); + e[i] = static_cast(1); + return hat(e); + } + + /** + * \brief hat-operator + * + * \param omega 3-vector representation of Lie algebra element + * \returns 3x3-matrix representatin of Lie algebra element + * + * Formally, the hat-operator of SE2 is defined + * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{2\times 2}, + * \quad \widehat{\omega} = \sum_{i=0}^2 G_i \omega_i \f$ + * with \f$ G_i \f$ being the ith infinitesial generator(). + * + * \see generator() + * \see vee() + */ + inline static + const Transformation hat(const Tangent & v) { + Transformation Omega; + Omega.setZero(); + Omega.template topLeftCorner<2,2>() = SO2Group::hat(v[2]); + Omega.col(2).template head<2>() = v.template head<2>(); + return Omega; + } + + /** + * \brief Lie bracket + * + * \param a 3-vector representation of Lie algebra element + * \param b 3-vector representation of Lie algebra element + * \returns 3-vector representation of Lie algebra element + * + * It computes the bracket of SE2. To be more specific, it + * computes \f$ [a, b]_{se2} + * := [\widehat{a_1}, \widehat{b_2}]^\vee \f$ + * with \f$ [A,B] = AB-BA \f$ being the matrix + * commutator, \f$ \widehat{\cdot} \f$ the + * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SE2. + * + * \see hat() + * \see vee() + */ + inline static + const Tangent lieBracket(const Tangent & a, + const Tangent & b) { + Matrix upsilon1 = a.template head<2>(); + Matrix upsilon2 = b.template head<2>(); + Scalar theta1 = a[2]; + Scalar theta2 = b[2]; + + return Tangent(-theta1*upsilon2[1] + theta2*upsilon1[1], + theta1*upsilon2[0] - theta2*upsilon1[0], + static_cast(0)); + } + + /** + * \brief Logarithmic map + * + * \param other element of the group SE2 + * \returns corresponding tangent space element + * (translational part \f$ \upsilon \f$ + * and rotation vector \f$ \omega \f$) + * + * Computes the logarithmic, the inverse of the group exponential. + * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ + * with \f$ \vee(\cdot) \f$ being the matrix logarithm + * and \f$ \vee{\cdot} \f$ the vee()-operator of SE2. + * + * \see exp() + * \see vee() + */ + inline static + const Tangent log(const SE2Group & other) { + Tangent upsilon_theta; + const SO2Group & so2 = other.so2(); + Scalar theta = SO2Group::log(so2); + upsilon_theta[2] = theta; + Scalar halftheta = static_cast(0.5)*theta; + Scalar halftheta_by_tan_of_halftheta; + + const Matrix & z = so2.unit_complex(); + Scalar real_minus_one = z.x()-static_cast(1.); + if (std::abs(real_minus_one)::epsilon()) { + halftheta_by_tan_of_halftheta + = static_cast(1.) + - static_cast(1./12)*theta*theta; + } else { + halftheta_by_tan_of_halftheta + = -(halftheta*z.y())/(real_minus_one); + } + Matrix V_inv; + V_inv << halftheta_by_tan_of_halftheta, halftheta + , -halftheta, halftheta_by_tan_of_halftheta; + upsilon_theta.template head<2>() = V_inv*other.translation(); + return upsilon_theta; + } + + /** + * \brief vee-operator + * + * \param Omega 3x3-matrix representation of Lie algebra element + * \returns 3-vector representatin of Lie algebra element + * + * This is the inverse of the hat()-operator. + * + * \see hat() + */ + inline static + const Tangent vee(const Transformation & Omega) { + Tangent upsilon_omega; + upsilon_omega.template head<2>() = Omega.col(2).template head<2>(); + upsilon_omega[2] + = SO2Group::vee(Omega.template topLeftCorner<2,2>()); + return upsilon_omega; + } +}; + +/** + * \brief SE2 default type - Constructors and default storage for SE2 Type + */ +template +class SE2Group : public SE2GroupBase > { + typedef SE2GroupBase > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits > + ::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits > + ::TranslationType & TranslationReference; + typedef const typename internal::traits > + ::TranslationType & ConstTranslationReference; + /** \brief SO2 reference type */ + typedef typename internal::traits > + ::SO2Type & SO2Reference; + /** \brief SO2 const reference type */ + typedef const typename internal::traits > + ::SO2Type & ConstSO2Reference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Default constructor + * + * Initialize Complex to identity rotation and translation to zero. + */ + inline + SE2Group() + : translation_( Matrix::Zero() ) + { + } + + /** + * \brief Copy constructor + */ + template inline + SE2Group(const SE2GroupBase & other) + : so2_(other.so2()), translation_(other.translation()) { + } + + /** + * \brief Constructor from SO2 and translation vector + */ + template inline + SE2Group(const SO2GroupBase & so2, + const Point & translation) + : so2_(so2), translation_(translation) { + } + + /** + * \brief Constructor from rotation matrix and translation vector + * + * \pre rotation matrix need to be orthogonal with determinant of 1 + */ + inline + SE2Group(const typename SO2Group::Transformation & rotation_matrix, + const Point & translation) + : so2_(rotation_matrix), translation_(translation) { + } + + /** + * \brief Constructor from rotation angle and translation vector + */ + inline + SE2Group(const Scalar & theta, + const Point & translation) + : so2_(theta), translation_(translation) { + } + + /** + * \brief Constructor from complex number and translation vector + * + * \pre complex must not be zero + */ + inline + SE2Group(const std::complex & complex, + const Point & translation) + : so2_(complex), translation_(translation) { + } + + /** + * \brief Constructor from 3x3 matrix + * + * \pre 2x2 sub-matrix need to be orthogonal with determinant of 1 + */ + inline explicit + SE2Group(const Transformation & T) + : so2_(T.template topLeftCorner<2,2>()), + translation_(T.template block<2,1>(0,2)) { + } + + /** + * \returns pointer to internal data + * + * This provides unsafe read/write access to internal data. SE2 is represented + * by a pair of an SO2 element (two parameters) and a translation vector (two + * parameters). The user needs to take care of that the complex + * stays normalized. + * + * /see normalize() + */ + EIGEN_STRONG_INLINE + Scalar* data() { + // so2_ and translation_ are layed out sequentially with no padding + return so2_.data(); + } + + /** + * \returns const pointer to internal data + * + * Const version of data(). + */ + EIGEN_STRONG_INLINE + const Scalar* data() const { + // so2_ and translation_ are layed out sequentially with no padding + return so2_.data(); + } + + /** + * \brief Accessor of SO2 + */ + EIGEN_STRONG_INLINE + SO2Reference so2() { + return so2_; + } + + /** + * \brief Mutator of SO2 + */ + EIGEN_STRONG_INLINE + ConstSO2Reference so2() const { + return so2_; + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return translation_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + Sophus::SO2Group so2_; + Matrix translation_; +}; + + +} // end namespace + + +namespace Eigen { +/** + * \brief Specialisation of Eigen::Map for SE2GroupBase + * + * Allows us to wrap SE2 Objects around POD array + * (e.g. external c style complex) + */ +template +class Map, _Options> + : public Sophus::SE2GroupBase, _Options> > +{ + typedef Sophus::SE2GroupBase, _Options> > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits::TranslationType & + TranslationReference; + /** \brief translation reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief SO2 reference type */ + typedef typename internal::traits::SO2Type & SO2Reference; + /** \brief SO2 const reference type */ + typedef const typename internal::traits::SO2Type & ConstSO2Reference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(Scalar* coeffs) + : so2_(coeffs), + translation_(coeffs+Sophus::SO2Group::num_parameters) { + } + + /** + * \brief Mutator of SO2 + */ + EIGEN_STRONG_INLINE + SO2Reference so2() { + return so2_; + } + + /** + * \brief Accessor of SO2 + */ + EIGEN_STRONG_INLINE + ConstSO2Reference so2() const { + return so2_; + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return translation_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + Map,_Options> so2_; + Map,_Options> translation_; +}; + +/** + * \brief Specialisation of Eigen::Map for const SE2GroupBase + * + * Allows us to wrap SE2 Objects around POD array + * (e.g. external c style complex) + */ +template +class Map, _Options> + : public Sophus::SE2GroupBase< + Map, _Options> > { + typedef Sophus::SE2GroupBase, _Options> > + Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief SO2 const reference type */ + typedef const typename internal::traits::SO2Type & ConstSO2Reference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(const Scalar* coeffs) + : so2_(coeffs), + translation_(coeffs+Sophus::SO2Group::num_parameters) { + } + + EIGEN_STRONG_INLINE + Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs) + : translation_(trans_coeffs), so2_(rot_coeffs){ + } + + /** + * \brief Accessor of SO2 + */ + EIGEN_STRONG_INLINE + ConstSO2Reference so2() const { + return so2_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + const Map,_Options> so2_; + const Map,_Options> translation_; +}; + +} + +#endif diff --git a/thirdparty/Sophus/sophus/se3.hpp b/thirdparty/Sophus/sophus/se3.hpp new file mode 100644 index 0000000..d2a20cc --- /dev/null +++ b/thirdparty/Sophus/sophus/se3.hpp @@ -0,0 +1,947 @@ +// This file is part of Sophus. +// +// Copyright 2011-2013 Hauke Strasdat +// 2012-2013 Steven Lovegrove +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_SE3_HPP +#define SOPHUS_SE3_HPP + +#include "so3.hpp" + +//////////////////////////////////////////////////////////////////////////// +// Forward Declarations / typedefs +//////////////////////////////////////////////////////////////////////////// + +namespace Sophus { +template class SE3Group; +typedef SE3Group SE3 EIGEN_DEPRECATED; +typedef SE3Group SE3d; /**< double precision SE3 */ +typedef SE3Group SE3f; /**< single precision SE3 */ +typedef Matrix Vector6d; +typedef Matrix Matrix6d; +typedef Matrix Vector6f; +typedef Matrix Matrix6f; +} + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +namespace Eigen { +namespace internal { + +template +struct traits > { + typedef _Scalar Scalar; + typedef Matrix TranslationType; + typedef Sophus::SO3Group SO3Type; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> TranslationType; + typedef Map,_Options> SO3Type; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> TranslationType; + typedef Map,_Options> SO3Type; +}; + +} +} + +namespace Sophus { +using namespace Eigen; +using namespace std; + +/** + * \brief SE3 base type - implements SE3 class but is storage agnostic + * + * [add more detailed description/tutorial] + */ +template +class SE3GroupBase { +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits::TranslationType & + TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief SO3 reference type */ + typedef typename internal::traits::SO3Type & + SO3Reference; + /** \brief SO3 const reference type */ + typedef const typename internal::traits::SO3Type & + ConstSO3Reference; + + /** \brief degree of freedom of group + * (three for translation, three for rotation) */ + static const int DoF = 6; + /** \brief number of internal parameters used + * (unit quaternion for rotation + translation 3-vector) */ + static const int num_parameters = 7; + /** \brief group transformations are NxN matrices */ + static const int N = 4; + /** \brief group transfomation type */ + typedef Matrix Transformation; + /** \brief point type */ + typedef Matrix Point; + /** \brief tangent vector type */ + typedef Matrix Tangent; + /** \brief adjoint transformation type */ + typedef Matrix Adjoint; + + + /** + * \brief Adjoint transformation + * + * This function return the adjoint transformation \f$ Ad \f$ of the + * group instance \f$ A \f$ such that for all \f$ x \f$ + * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ + * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. + */ + inline + const Adjoint Adj() const { + const Matrix & R = so3().matrix(); + Adjoint res; + res.block(0,0,3,3) = R; + res.block(3,3,3,3) = R; + res.block(0,3,3,3) = SO3Group::hat(translation())*R; + res.block(3,0,3,3) = Matrix::Zero(3,3); + return res; + } + + /** + * \returns copy of instance casted to NewScalarType + */ + template + inline SE3Group cast() const { + return + SE3Group(so3().template cast(), + translation().template cast() ); + } + + /** + * \brief Fast group multiplication + * + * This method is a fast version of operator*=(), since it does not perform + * normalization. It is up to the user to call normalize() once in a while. + * + * \see operator*=() + */ + inline + void fastMultiply(const SE3Group& other) { + translation() += so3()*(other.translation()); + so3().fastMultiply(other.so3()); + } + + /** + * \returns Group inverse of instance + */ + inline + const SE3Group inverse() const { + const SO3Group invR = so3().inverse(); + return SE3Group(invR, invR*(translation() + *static_cast(-1) ) ); + } + + /** + * \brief Logarithmic map + * + * \returns tangent space representation + * (translational part and rotation vector) of instance + * + * \see log(). + */ + inline + const Tangent log() const { + return log(*this); + } + + /** + * \brief Normalize SO3 element + * + * It re-normalizes the SO3 element. This method only needs to + * be called in conjunction with fastMultiply() or data() write access. + */ + inline + void normalize() { + so3().normalize(); + } + + /** + * \returns 4x4 matrix representation of instance + */ + inline + const Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.setIdentity(); + homogenious_matrix.block(0,0,3,3) = rotationMatrix(); + homogenious_matrix.col(3).head(3) = translation(); + return homogenious_matrix; + } + + /** + * \returns 3x4 matrix representation of instance + * + * It returns the three first row of matrix(). + */ + inline + const Matrix matrix3x4() const { + Matrix matrix; + matrix.block(0,0,3,3) = rotationMatrix(); + matrix.col(3) = translation(); + return matrix; + } + + /** + * \brief Assignment operator + */ + template inline + SE3GroupBase& operator= (const SE3GroupBase & other) { + so3() = other.so3(); + translation() = other.translation(); + return *this; + } + + /** + * \brief Group multiplication + * \see operator*=() + */ + inline + const SE3Group operator*(const SE3Group& other) const { + SE3Group result(*this); + result *= other; + return result; + } + + /** + * \brief Group action on \f$ \mathbf{R}^3 \f$ + * + * \param p point \f$p \in \mathbf{R}^3 \f$ + * \returns point \f$p' \in \mathbf{R}^3 \f$, + * rotated and translated version of \f$p\f$ + * + * This function rotates and translates point \f$ p \f$ + * in \f$ \mathbf{R}^3 \f$ by the SE3 transformation \f$R,t\f$ + * (=rotation matrix, translation vector): \f$ p' = R\cdot p + t \f$. + */ + inline + const Point operator*(const Point & p) const { + return so3()*p + translation(); + } + + /** + * \brief In-place group multiplication + * + * \see fastMultiply() + * \see operator*() + */ + inline + void operator*=(const SE3Group& other) { + fastMultiply(other); + normalize(); + } + + + /** + * \returns Rotation matrix + * + * deprecated: use rotationMatrix() instead. + */ + typedef Transformation M3_marcos_dont_like_commas; + inline + EIGEN_DEPRECATED const M3_marcos_dont_like_commas rotation_matrix() const { + return so3().matrix(); + } + + /** + * \returns Rotation matrix + */ + inline + const Matrix rotationMatrix() const { + return so3().matrix(); + } + + + /** + * \brief Mutator of SO3 group + */ + EIGEN_STRONG_INLINE + SO3Reference so3() { + return static_cast(this)->so3(); + } + + /** + * \brief Accessor of SO3 group + */ + EIGEN_STRONG_INLINE + ConstSO3Reference so3() const { + return static_cast(this)->so3(); + } + + /** + * \brief Setter of internal unit quaternion representation + * + * \param quaternion + * \pre the quaternion must not be zero + * + * The quaternion is normalized to unit length. + */ + inline + void setQuaternion(const Quaternion & quat) { + return so3().setQuaternion(quat); + } + + /** + * \brief Setter of unit quaternion using rotation matrix + * + * \param rotation_matrix a 3x3 rotation matrix + * \pre the 3x3 matrix should be orthogonal and have a determinant of 1 + */ + inline + void setRotationMatrix + (const Matrix & rotation_matrix) { + so3().setQuaternion(Quaternion(rotation_matrix)); + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return static_cast(this)->translation(); + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return static_cast(this)->translation(); + } + + /** + * \brief Accessor of unit quaternion + * + * No direct write access is given to ensure the quaternion stays normalized. + */ + inline + typename internal::traits::SO3Type::ConstQuaternionReference + unit_quaternion() const { + return so3().unit_quaternion(); + } + + //////////////////////////////////////////////////////////////////////////// + // public static functions + //////////////////////////////////////////////////////////////////////////// + + /** + * \param b 6-vector representation of Lie algebra element + * \returns derivative of Lie bracket + * + * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{se3} \f$ + * with \f$ [a, b]_{se3} \f$ being the lieBracket() of the Lie algebra se3. + * + * \see lieBracket() + */ + inline static + const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { + Adjoint res; + res.setZero(); + + const Matrix & upsilon2 = b.template head<3>(); + const Matrix & omega2 = b.template tail<3>(); + + res.template topLeftCorner<3,3>() = -SO3Group::hat(omega2); + res.template topRightCorner<3,3>() = -SO3Group::hat(upsilon2); + res.template bottomRightCorner<3,3>() = -SO3Group::hat(omega2); + return res; + } + + /** + * \brief Group exponential + * + * \param a tangent space element (6-vector) + * \returns corresponding element of the group SE3 + * + * The first three components of \f$ a \f$ represent the translational + * part \f$ \upsilon \f$ in the tangent space of SE3, while the last three + * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$. + * + * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ + * with \f$ \exp(\cdot) \f$ being the matrix exponential + * and \f$ \widehat{\cdot} \f$ the hat()-operator of SE3. + * + * \see hat() + * \see log() + */ + inline static + const SE3Group exp(const Tangent & a) { + const Matrix & omega = a.template tail<3>(); + + Scalar theta; + const SO3Group & so3 + = SO3Group::expAndTheta(omega, &theta); + + const Matrix & Omega = SO3Group::hat(omega); + const Matrix & Omega_sq = Omega*Omega; + Matrix V; + + if(theta::epsilon()) { + V = so3.matrix(); + //Note: That is an accurate expansion! + } else { + Scalar theta_sq = theta*theta; + V = (Matrix::Identity() + + (static_cast(1)-std::cos(theta))/(theta_sq)*Omega + + (theta-std::sin(theta))/(theta_sq*theta)*Omega_sq); + } + return SE3Group(so3,V*a.template head<3>()); + } + + /** + * \brief Generators + * + * \pre \f$ i \in \{0,1,2,3,4,5\} \f$ + * \returns \f$ i \f$th generator \f$ G_i \f$ of SE3 + * + * The infinitesimal generators of SE3 are: \f[ + * G_0 = \left( \begin{array}{cccc} + * 0& 0& 0& 1\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_1 = \left( \begin{array}{cccc} + * 0& 0& 0& 0\\ + * 0& 0& 0& 1\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_2 = \left( \begin{array}{cccc} + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 1\\ + * 0& 0& 0& 0\\ + * \end{array} \right). + * G_3 = \left( \begin{array}{cccc} + * 0& 0& 0& 0\\ + * 0& 0& -1& 0\\ + * 0& 1& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_4 = \left( \begin{array}{cccc} + * 0& 0& 1& 0\\ + * 0& 0& 0& 0\\ + * -1& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_5 = \left( \begin{array}{cccc} + * 0& -1& 0& 0\\ + * 1& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right). + * \f] + * \see hat() + */ + inline static + const Transformation generator(int i) { + if (i<0 || i>5) { + throw SophusException("i is not in range [0,5]."); + } + Tangent e; + e.setZero(); + e[i] = static_cast(1); + return hat(e); + } + + /** + * \brief hat-operator + * + * \param omega 6-vector representation of Lie algebra element + * \returns 4x4-matrix representatin of Lie algebra element + * + * Formally, the hat-operator of SE3 is defined + * as \f$ \widehat{\cdot}: \mathbf{R}^6 \rightarrow \mathbf{R}^{4\times 4}, + * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$ + * with \f$ G_i \f$ being the ith infinitesial generator(). + * + * \see generator() + * \see vee() + */ + inline static + const Transformation hat(const Tangent & v) { + Transformation Omega; + Omega.setZero(); + Omega.template topLeftCorner<3,3>() + = SO3Group::hat(v.template tail<3>()); + Omega.col(3).template head<3>() = v.template head<3>(); + return Omega; + } + + /** + * \brief Lie bracket + * + * \param a 6-vector representation of Lie algebra element + * \param b 6-vector representation of Lie algebra element + * \returns 6-vector representation of Lie algebra element + * + * It computes the bracket of SE3. To be more specific, it + * computes \f$ [a, b]_{se3} + * := [\widehat{a}, \widehat{b}]^\vee \f$ + * with \f$ [A,B] = AB-BA \f$ being the matrix + * commutator, \f$ \widehat{\cdot} \f$ the + * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SE3. + * + * \see hat() + * \see vee() + */ + inline static + const Tangent lieBracket(const Tangent & a, + const Tangent & b) { + Matrix upsilon1 = a.template head<3>(); + Matrix upsilon2 = b.template head<3>(); + Matrix omega1 = a.template tail<3>(); + Matrix omega2 = b.template tail<3>(); + + Tangent res; + res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2); + res.template tail<3>() = omega1.cross(omega2); + + return res; + } + + /** + * \brief Logarithmic map + * + * \param other element of the group SE3 + * \returns corresponding tangent space element + * (translational part \f$ \upsilon \f$ + * and rotation vector \f$ \omega \f$) + * + * Computes the logarithmic, the inverse of the group exponential. + * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ + * with \f$ \vee(\cdot) \f$ being the matrix logarithm + * and \f$ \vee{\cdot} \f$ the vee()-operator of SE3. + * + * \see exp() + * \see vee() + */ + inline static + const Tangent log(const SE3Group & se3) { + Tangent upsilon_omega; + Scalar theta; + upsilon_omega.template tail<3>() + = SO3Group::logAndTheta(se3.so3(), &theta); + + if (std::abs(theta)::epsilon()) { + const Matrix & Omega + = SO3Group::hat(upsilon_omega.template tail<3>()); + const Matrix & V_inv = + Matrix::Identity() - + static_cast(0.5)*Omega + + static_cast(1./12.)*(Omega*Omega); + + upsilon_omega.template head<3>() = V_inv*se3.translation(); + } else { + const Matrix & Omega + = SO3Group::hat(upsilon_omega.template tail<3>()); + const Matrix & V_inv = + ( Matrix::Identity() - static_cast(0.5)*Omega + + ( static_cast(1) + - theta/(static_cast(2)*tan(theta/Scalar(2)))) / + (theta*theta)*(Omega*Omega) ); + upsilon_omega.template head<3>() = V_inv*se3.translation(); + } + return upsilon_omega; + } + + /** + * \brief vee-operator + * + * \param Omega 4x4-matrix representation of Lie algebra element + * \returns 6-vector representatin of Lie algebra element + * + * This is the inverse of the hat()-operator. + * + * \see hat() + */ + inline static + const Tangent vee(const Transformation & Omega) { + Tangent upsilon_omega; + upsilon_omega.template head<3>() = Omega.col(3).template head<3>(); + upsilon_omega.template tail<3>() + = SO3Group::vee(Omega.template topLeftCorner<3,3>()); + return upsilon_omega; + } +}; + +/** + * \brief SE3 default type - Constructors and default storage for SE3 Type + */ +template +class SE3Group : public SE3GroupBase > { + typedef SE3GroupBase > Base; +public: + /** \brief scalar type */ + typedef typename internal::traits > + ::Scalar Scalar; + /** \brief SO3 reference type */ + typedef typename internal::traits > + ::SO3Type & SO3Reference; + /** \brief SO3 const reference type */ + typedef const typename internal::traits > + ::SO3Type & ConstSO3Reference; + /** \brief translation reference type */ + typedef typename internal::traits > + ::TranslationType & TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits > + ::TranslationType & ConstTranslationReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Default constructor + * + * Initialize Quaternion to identity rotation and translation to zero. + */ + inline + SE3Group() + : translation_( Matrix::Zero() ) + { + } + + /** + * \brief Copy constructor + */ + template inline + SE3Group(const SE3GroupBase & other) + : so3_(other.so3()), translation_(other.translation()) { + } + + /** + * \brief Constructor from SO3 and translation vector + */ + template inline + SE3Group(const SO3GroupBase & so3, + const Point & translation) + : so3_(so3), translation_(translation) { + } + + /** + * \brief Constructor from rotation matrix and translation vector + * + * \pre rotation matrix need to be orthogonal with determinant of 1 + */ + inline + SE3Group(const Matrix & rotation_matrix, + const Point & translation) + : so3_(rotation_matrix), translation_(translation) { + } + + /** + * \brief Constructor from quaternion and translation vector + * + * \pre quaternion must not be zero + */ + inline + SE3Group(const Quaternion & quaternion, + const Point & translation) + : so3_(quaternion), translation_(translation) { + } + + /** + * \brief Constructor from 4x4 matrix + * + * \pre top-left 3x3 sub-matrix need to be orthogonal with determinant of 1 + */ + inline explicit + SE3Group(const Eigen::Matrix& T) + : so3_(T.template topLeftCorner<3,3>()), + translation_(T.template block<3,1>(0,3)) { + } + + /** + * \returns pointer to internal data + * + * This provides unsafe read/write access to internal data. SE3 is represented + * by a pair of an SO3 element (4 parameters) and translation vector (three + * parameters). The user needs to take care of that the quaternion + * stays normalized. + * + * Note: The first three Scalars represent the imaginary parts, while the + * forth Scalar represent the real part. + * + * /see normalize() + */ + EIGEN_STRONG_INLINE + Scalar* data() { + // so3_ and translation_ are layed out sequentially with no padding + return so3_.data(); + } + + /** + * \returns const pointer to internal data + * + * Const version of data(). + */ + EIGEN_STRONG_INLINE + const Scalar* data() const { + // so3_ and translation_ are layed out sequentially with no padding + return so3_.data(); + } + + /** + * \brief Accessor of SO3 + */ + EIGEN_STRONG_INLINE + SO3Reference so3() { + return so3_; + } + + /** + * \brief Mutator of SO3 + */ + EIGEN_STRONG_INLINE + ConstSO3Reference so3() const { + return so3_; + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return translation_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + Sophus::SO3Group so3_; + Matrix translation_; +}; + + +} // end namespace + + +namespace Eigen { +/** + * \brief Specialisation of Eigen::Map for SE3GroupBase + * + * Allows us to wrap SE3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::SE3GroupBase, _Options> > { + typedef Sophus::SE3GroupBase, _Options> > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits::TranslationType & + TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief SO3 reference type */ + typedef typename internal::traits::SO3Type & + SO3Reference; + /** \brief SO3 const reference type */ + typedef const typename internal::traits::SO3Type & + ConstSO3Reference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(Scalar* coeffs) + : so3_(coeffs), + translation_(coeffs+Sophus::SO3Group::num_parameters) { + } + + /** + * \brief Mutator of SO3 + */ + EIGEN_STRONG_INLINE + SO3Reference so3() { + return so3_; + } + + /** + * \brief Accessor of SO3 + */ + EIGEN_STRONG_INLINE + ConstSO3Reference so3() const { + return so3_; + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return translation_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + Map,_Options> so3_; + Map,_Options> translation_; +}; + +/** + * \brief Specialisation of Eigen::Map for const SE3GroupBase + * + * Allows us to wrap SE3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::SE3GroupBase< + Map, _Options> > { + typedef Sophus::SE3GroupBase, _Options> > + Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation const reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief SO3 const reference type */ + typedef const typename internal::traits::SO3Type & + ConstSO3Reference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(const Scalar* coeffs) + : so3_(coeffs), + translation_(coeffs+Sophus::SO3Group::num_parameters) { + } + + EIGEN_STRONG_INLINE + Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs) + : translation_(trans_coeffs), so3_(rot_coeffs){ + } + + /** + * \brief Accessor of SO3 + */ + EIGEN_STRONG_INLINE + ConstSO3Reference so3() const { + return so3_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + const Map,_Options> so3_; + const Map,_Options> translation_; +}; + +} + +#endif diff --git a/thirdparty/Sophus/sophus/sim3.hpp b/thirdparty/Sophus/sophus/sim3.hpp new file mode 100644 index 0000000..97087c3 --- /dev/null +++ b/thirdparty/Sophus/sophus/sim3.hpp @@ -0,0 +1,976 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_SIM3_HPP +#define SOPHUS_SIM3_HPP + +#include "rxso3.hpp" + +//////////////////////////////////////////////////////////////////////////// +// Forward Declarations / typedefs +//////////////////////////////////////////////////////////////////////////// + +namespace Sophus { +template class Sim3Group; +typedef Sim3Group Sim3 EIGEN_DEPRECATED; +typedef Sim3Group Sim3d; /**< double precision Sim3 */ +typedef Sim3Group Sim3f; /**< single precision Sim3 */ +typedef Matrix Vector7d; +typedef Matrix Matrix7d; +typedef Matrix Vector7f; +typedef Matrix Matrix7f; +} + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +namespace Eigen { +namespace internal { + +template +struct traits > { + typedef _Scalar Scalar; + typedef Matrix TranslationType; + typedef Sophus::RxSO3Group RxSO3Type; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> TranslationType; + typedef Map,_Options> RxSO3Type; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> TranslationType; + typedef Map,_Options> RxSO3Type; +}; + +} +} + +namespace Sophus { +using namespace Eigen; +using namespace std; + +/** + * \brief Sim3 base type - implements Sim3 class but is storage agnostic + * + * [add more detailed description/tutorial] + */ +template +class Sim3GroupBase { +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits::TranslationType & + TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief RxSO3 reference type */ + typedef typename internal::traits::RxSO3Type & + RxSO3Reference; + /** \brief RxSO3 const reference type */ + typedef const typename internal::traits::RxSO3Type & + ConstRxSO3Reference; + + + /** \brief degree of freedom of group + * (three for translation, three for rotation, one for scale) */ + static const int DoF = 7; + /** \brief number of internal parameters used + * (quaternion for rotation and scale + translation 3-vector) */ + static const int num_parameters = 7; + /** \brief group transformations are NxN matrices */ + static const int N = 4; + /** \brief group transfomation type */ + typedef Matrix Transformation; + /** \brief point type */ + typedef Matrix Point; + /** \brief tangent vector type */ + typedef Matrix Tangent; + /** \brief adjoint transformation type */ + typedef Matrix Adjoint; + + /** + * \brief Adjoint transformation + * + * This function return the adjoint transformation \f$ Ad \f$ of the + * group instance \f$ A \f$ such that for all \f$ x \f$ + * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ + * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. + */ + inline + const Adjoint Adj() const { + const Matrix & R = rxso3().rotationMatrix(); + Adjoint res; + res.setZero(); + res.block(0,0,3,3) = scale()*R; + res.block(0,3,3,3) = SO3Group::hat(translation())*R; + res.block(0,6,3,1) = -translation(); + res.block(3,3,3,3) = R; + res(6,6) = 1; + return res; + } + + /** + * \returns copy of instance casted to NewScalarType + */ + template + inline Sim3Group cast() const { + return + Sim3Group(rxso3().template cast(), + translation().template cast() ); + } + + /** + * \brief In-place group multiplication + * + * Same as operator*=() for Sim3. + * + * \see operator*() + */ + inline + void fastMultiply(const Sim3Group& other) { + translation() += (rxso3() * other.translation()); + rxso3() *= other.rxso3(); + } + + /** + * \returns Group inverse of instance + */ + inline + const Sim3Group inverse() const { + const RxSO3Group invR = rxso3().inverse(); + return Sim3Group(invR, invR*(translation() + *static_cast(-1) ) ); + } + + /** + * \brief Logarithmic map + * + * \returns tangent space representation + * (translational part and rotation vector) of instance + * + * \see log(). + */ + inline + const Tangent log() const { + return log(*this); + } + + /** + * \returns 4x4 matrix representation of instance + */ + inline + const Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.setIdentity(); + homogenious_matrix.block(0,0,3,3) = rxso3().matrix(); + homogenious_matrix.col(3).head(3) = translation(); + return homogenious_matrix; + } + + /** + * \returns 3x4 matrix representation of instance + * + * It returns the three first row of matrix(). + */ + inline + const Matrix matrix3x4() const { + Matrix matrix; + matrix.block(0,0,3,3) = rxso3().matrix(); + matrix.col(3) = translation(); + return matrix; + } + + /** + * \brief Assignment operator + */ + template inline + Sim3GroupBase& operator= + (const Sim3GroupBase & other) { + rxso3() = other.rxso3(); + translation() = other.translation(); + return *this; + } + + /** + * \brief Group multiplication + * \see operator*=() + */ + inline + const Sim3Group operator*(const Sim3Group& other) const { + Sim3Group result(*this); + result *= other; + return result; + } + + /** + * \brief Group action on \f$ \mathbf{R}^3 \f$ + * + * \param p point \f$p \in \mathbf{R}^3 \f$ + * \returns point \f$p' \in \mathbf{R}^3 \f$, + * rotated, scaled and translated version of \f$p\f$ + * + * This function scales, rotates and translates point \f$ p \f$ + * in \f$ \mathbf{R}^3 \f$ by the Sim3 transformation \f$sR,t\f$ + * (=scaled rotation matrix, translation vector): \f$ p' = sR\cdot p + t \f$. + */ + inline + const Point operator*(const Point & p) const { + return rxso3()*p + translation(); + } + + /** + * \brief In-place group multiplication + * + * \see operator*() + */ + inline + void operator*=(const Sim3Group& other) { + translation() += (rxso3() * other.translation()); + rxso3() *= other.rxso3(); + } + + /** + * \brief Mutator of quaternion + */ + inline + typename internal::traits::RxSO3Type::QuaternionReference + quaternion() { + return rxso3().quaternion(); + } + + /** + * \brief Accessor of quaternion + */ + inline + typename internal::traits::RxSO3Type::ConstQuaternionReference + quaternion() const { + return rxso3().quaternion(); + } + + /** + * \returns Rotation matrix + * + * deprecated: use rotationMatrix() instead. + */ + inline + EIGEN_DEPRECATED const Transformation rotation_matrix() const { + return rxso3().rotationMatrix(); + } + + /** + * \returns Rotation matrix + */ + inline + const Matrix rotationMatrix() const { + return rxso3().rotationMatrix(); + } + + /** + * \brief Mutator of RxSO3 group + */ + EIGEN_STRONG_INLINE + RxSO3Reference rxso3() { + return static_cast(this)->rxso3(); + } + + /** + * \brief Accessor of RxSO3 group + */ + EIGEN_STRONG_INLINE + ConstRxSO3Reference rxso3() const { + return static_cast(this)->rxso3(); + } + + /** + * \returns scale + */ + EIGEN_STRONG_INLINE + const Scalar scale() const { + return rxso3().scale(); + } + + /** + * \brief Setter of quaternion using rotation matrix, leaves scale untouched + * + * \param R a 3x3 rotation matrix + * \pre the 3x3 matrix should be orthogonal and have a determinant of 1 + */ + inline + void setRotationMatrix + (const Matrix & R) { + rxso3().setRotationMatrix(R); + } + + /** + * \brief Scale setter + */ + EIGEN_STRONG_INLINE + void setScale(const Scalar & scale) { + rxso3().setScale(scale); + } + + /** + * \brief Setter of quaternion using scaled rotation matrix + * + * \param sR a 3x3 scaled rotation matrix + * \pre the 3x3 matrix should be "scaled orthogonal" + * and have a positive determinant + */ + inline + void setScaledRotationMatrix + (const Matrix & sR) { + rxso3().setScaledRotationMatrix(sR); + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return static_cast(this)->translation(); + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return static_cast(this)->translation(); + } + + //////////////////////////////////////////////////////////////////////////// + // public static functions + //////////////////////////////////////////////////////////////////////////// + + /** + * \param b 7-vector representation of Lie algebra element + * \returns derivative of Lie bracket + * + * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{sim3} \f$ + * with \f$ [a, b]_{sim3} \f$ being the lieBracket() of the Lie algebra sim3. + * + * \see lieBracket() + */ + inline static + const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { + const Matrix & upsilon2 = b.template head<3>(); + const Matrix & omega2 = b.template segment<3>(3); + Scalar sigma2 = b[6]; + + Adjoint res; + res.setZero(); + res.template topLeftCorner<3,3>() + = -SO3::hat(omega2)-sigma2*Matrix3d::Identity(); + res.template block<3,3>(0,3) = -SO3::hat(upsilon2); + res.template topRightCorner<3,1>() = upsilon2; + res.template block<3,3>(3,3) = -SO3::hat(omega2); + return res; + } + + /** + * \brief Group exponential + * + * \param a tangent space element (7-vector) + * \returns corresponding element of the group Sim3 + * + * The first three components of \f$ a \f$ represent the translational + * part \f$ \upsilon \f$ in the tangent space of Sim3, while the last three + * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$. + * + * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ + * with \f$ \exp(\cdot) \f$ being the matrix exponential + * and \f$ \widehat{\cdot} \f$ the hat()-operator of Sim3. + * + * \see hat() + * \see log() + */ + inline static + const Sim3Group exp(const Tangent & a) { + const Matrix & upsilon = a.segment(0,3); + const Matrix & omega = a.segment(3,3); + Scalar sigma = a[6]; + Scalar theta; + RxSO3Group rxso3 + = RxSO3Group::expAndTheta(a.template tail<4>(), &theta); + const Matrix & Omega = SO3Group::hat(omega); + const Matrix & W = calcW(theta, sigma, rxso3.scale(), Omega); + return Sim3Group(rxso3, W*upsilon); + } + + /** + * \brief Generators + * + * \pre \f$ i \in \{0,1,2,3,4,5,6\} \f$ + * \returns \f$ i \f$th generator \f$ G_i \f$ of Sim3 + * + * The infinitesimal generators of Sim3 are: \f[ + * G_0 = \left( \begin{array}{cccc} + * 0& 0& 0& 1\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_1 = \left( \begin{array}{cccc} + * 0& 0& 0& 0\\ + * 0& 0& 0& 1\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_2 = \left( \begin{array}{cccc} + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 1\\ + * 0& 0& 0& 0\\ + * \end{array} \right). + * G_3 = \left( \begin{array}{cccc} + * 0& 0& 0& 0\\ + * 0& 0& -1& 0\\ + * 0& 1& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_4 = \left( \begin{array}{cccc} + * 0& 0& 1& 0\\ + * 0& 0& 0& 0\\ + * -1& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_5 = \left( \begin{array}{cccc} + * 0& -1& 0& 0\\ + * 1& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right), + * G_6 = \left( \begin{array}{cccc} + * 1& 0& 0& 0\\ + * 0& 1& 0& 0\\ + * 0& 0& 1& 0\\ + * 0& 0& 0& 0\\ + * \end{array} \right). + * \f] + * \see hat() + */ + inline static + const Transformation generator(int i) { + if (i<0 || i>6) { + throw SophusException("i is not in range [0,6]."); + } + Tangent e; + e.setZero(); + e[i] = static_cast(1); + return hat(e); + } + + /** + * \brief hat-operator + * + * \param omega 7-vector representation of Lie algebra element + * \returns 4x4-matrix representatin of Lie algebra element + * + * Formally, the hat-operator of Sim3 is defined + * as \f$ \widehat{\cdot}: \mathbf{R}^7 \rightarrow \mathbf{R}^{4\times 4}, + * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$ + * with \f$ G_i \f$ being the ith infinitesial generator(). + * + * \see generator() + * \see vee() + */ + inline static + const Transformation hat(const Tangent & v) { + Transformation Omega; + Omega.template topLeftCorner<3,3>() + = RxSO3Group::hat(v.template tail<4>()); + Omega.col(3).template head<3>() = v.template head<3>(); + Omega.row(3).setZero(); + return Omega; + } + + /** + * \brief Lie bracket + * + * \param a 7-vector representation of Lie algebra element + * \param b 7-vector representation of Lie algebra element + * \returns 7-vector representation of Lie algebra element + * + * It computes the bracket of Sim3. To be more specific, it + * computes \f$ [a, b]_{sim3} + * := [\widehat{a}, \widehat{b}]^\vee \f$ + * with \f$ [A,B] = AB-BA \f$ being the matrix + * commutator, \f$ \widehat{\cdot} \f$ the + * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of Sim3. + * + * \see hat() + * \see vee() + */ + inline static + const Tangent lieBracket(const Tangent & a, + const Tangent & b) { + const Matrix & upsilon1 = a.template head<3>(); + const Matrix & upsilon2 = b.template head<3>(); + const Matrix & omega1 = a.template segment<3>(3); + const Matrix & omega2 = b.template segment<3>(3); + Scalar sigma1 = a[6]; + Scalar sigma2 = b[6]; + + Tangent res; + res.template head<3>() = + SO3Group::hat(omega1)*upsilon2 + + SO3Group::hat(upsilon1)*omega2 + + sigma1*upsilon2 - sigma2*upsilon1; + res.template segment<3>(3) = omega1.cross(omega2); + res[6] = static_cast(0); + + return res; + } + + /** + * \brief Logarithmic map + * + * \param other element of the group Sim3 + * \returns corresponding tangent space element + * (translational part \f$ \upsilon \f$ + * and rotation vector \f$ \omega \f$) + * + * Computes the logarithmic, the inverse of the group exponential. + * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ + * with \f$ \vee(\cdot) \f$ being the matrix logarithm + * and \f$ \vee{\cdot} \f$ the vee()-operator of Sim3. + * + * \see exp() + * \see vee() + */ + inline static + const Tangent log(const Sim3Group & other) { + Tangent res; + Scalar theta; + const Matrix & omega_sigma + = RxSO3Group::logAndTheta(other.rxso3(), &theta); + const Matrix & omega = omega_sigma.template head<3>(); + Scalar sigma = omega_sigma[3]; + const Matrix & W + = calcW(theta, sigma, other.scale(), SO3Group::hat(omega)); + res.segment(0,3) = W.partialPivLu().solve(other.translation()); + res.segment(3,3) = omega; + res[6] = sigma; + return res; + } + + /** + * \brief vee-operator + * + * \param Omega 4x4-matrix representation of Lie algebra element + * \returns 7-vector representatin of Lie algebra element + * + * This is the inverse of the hat()-operator. + * + * \see hat() + */ + inline static + const Tangent vee(const Transformation & Omega) { + Tangent upsilon_omega_sigma; + upsilon_omega_sigma.template head<3>() + = Omega.col(3).template head<3>(); + upsilon_omega_sigma.template tail<4>() + = RxSO3Group::vee(Omega.template topLeftCorner<3,3>()); + return upsilon_omega_sigma; + } + +private: + static + Matrix calcW(const Scalar & theta, + const Scalar & sigma, + const Scalar & scale, + const Matrix & Omega){ + static const Matrix I + = Matrix::Identity(); + static const Scalar one = static_cast(1.); + static const Scalar half = static_cast(1./2.); + Matrix Omega2 = Omega*Omega; + + Scalar A,B,C; + if (std::abs(sigma)::epsilon()) { + C = one; + if (std::abs(theta)::epsilon()) { + A = half; + B = static_cast(1./6.); + } else { + Scalar theta_sq = theta*theta; + A = (one-std::cos(theta))/theta_sq; + B = (theta-std::sin(theta))/(theta_sq*theta); + } + } else { + C = (scale-one)/sigma; + if (std::abs(theta)::epsilon()) { + Scalar sigma_sq = sigma*sigma; + A = ((sigma-one)*scale+one)/sigma_sq; + B = ((half*sigma*sigma-sigma+one)*scale)/(sigma_sq*sigma); + } else { + Scalar theta_sq = theta*theta; + Scalar a = scale*std::sin(theta); + Scalar b = scale*std::cos(theta); + Scalar c = theta_sq+sigma*sigma; + A = (a*sigma+ (one-b)*theta)/(theta*c); + B = (C-((b-one)*sigma+a*theta)/(c))*one/(theta_sq); + } + } + return A*Omega + B*Omega2 + C*I; + } +}; + +/** + * \brief Sim3 default type - Constructors and default storage for Sim3 Type + */ +template +class Sim3Group : public Sim3GroupBase > { + typedef Sim3GroupBase > Base; +public: + /** \brief scalar type */ + typedef typename internal::traits > + ::Scalar Scalar; + /** \brief RxSO3 reference type */ + typedef typename internal::traits > + ::RxSO3Type & RxSO3Reference; + /** \brief RxSO3 const reference type */ + typedef const typename internal::traits > + ::RxSO3Type & ConstRxSO3Reference; + /** \brief translation reference type */ + typedef typename internal::traits > + ::TranslationType & TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits > + ::TranslationType & ConstTranslationReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Default constructor + * + * Initialize Quaternion to identity rotation and translation to zero. + */ + inline + Sim3Group() + : translation_( Matrix::Zero() ) + { + } + + /** + * \brief Copy constructor + */ + template inline + Sim3Group(const Sim3GroupBase & other) + : rxso3_(other.rxso3()), translation_(other.translation()) { + } + + /** + * \brief Constructor from RxSO3 and translation vector + */ + template inline + Sim3Group(const RxSO3GroupBase & rxso3, + const Point & translation) + : rxso3_(rxso3), translation_(translation) { + } + + /** + * \brief Constructor from quaternion and translation vector + * + * \pre quaternion must not be zero + */ + inline + Sim3Group(const Quaternion & quaternion, + const Point & translation) + : rxso3_(quaternion), translation_(translation) { + } + + /** + * \brief Constructor from 4x4 matrix + * + * \pre top-left 3x3 sub-matrix need to be "scaled orthogonal" + * with positive determinant of + */ + inline explicit + Sim3Group(const Eigen::Matrix& T) + : rxso3_(T.template topLeftCorner<3,3>()), + translation_(T.template block<3,1>(0,3)) { + } + + /** + * \returns pointer to internal data + * + * This provides unsafe read/write access to internal data. Sim3 is + * represented by a pair of an RxSO3 element (4 parameters) and translation + * vector (three parameters). + * + * Note: The first three Scalars represent the imaginary parts, while the + */ + EIGEN_STRONG_INLINE + Scalar* data() { + // rxso3_ and translation_ are layed out sequentially with no padding + return rxso3_.data(); + } + + /** + * \returns const pointer to internal data + * + * Const version of data(). + */ + EIGEN_STRONG_INLINE + const Scalar* data() const { + // rxso3_ and translation_ are layed out sequentially with no padding + return rxso3_.data(); + } + + /** + * \brief Accessor of RxSO3 + */ + EIGEN_STRONG_INLINE + RxSO3Reference rxso3() { + return rxso3_; + } + + /** + * \brief Mutator of RxSO3 + */ + EIGEN_STRONG_INLINE + ConstRxSO3Reference rxso3() const { + return rxso3_; + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return translation_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + Sophus::RxSO3Group rxso3_; + Matrix translation_; +}; + + +} // end namespace + + +namespace Eigen { +/** + * \brief Specialisation of Eigen::Map for Sim3GroupBase + * + * Allows us to wrap Sim3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::Sim3GroupBase, _Options> > { + typedef Sophus::Sim3GroupBase, _Options> > + Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation reference type */ + typedef typename internal::traits::TranslationType & + TranslationReference; + /** \brief translation const reference type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief RxSO3 reference type */ + typedef typename internal::traits::RxSO3Type & + RxSO3Reference; + /** \brief RxSO3 const reference type */ + typedef const typename internal::traits::RxSO3Type & + ConstRxSO3Reference; + + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(Scalar* coeffs) + : rxso3_(coeffs), + translation_(coeffs+Sophus::RxSO3Group::num_parameters) { + } + + /** + * \brief Mutator of RxSO3 + */ + EIGEN_STRONG_INLINE + RxSO3Reference rxso3() { + return rxso3_; + } + + /** + * \brief Accessor of RxSO3 + */ + EIGEN_STRONG_INLINE + ConstRxSO3Reference rxso3() const { + return rxso3_; + } + + /** + * \brief Mutator of translation vector + */ + EIGEN_STRONG_INLINE + TranslationReference translation() { + return translation_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + Map,_Options> rxso3_; + Map,_Options> translation_; +}; + +/** + * \brief Specialisation of Eigen::Map for const Sim3GroupBase + * + * Allows us to wrap Sim3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::Sim3GroupBase< + Map, _Options> > { + typedef Sophus::Sim3GroupBase< + Map, _Options> > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief translation type */ + typedef const typename internal::traits::TranslationType & + ConstTranslationReference; + /** \brief RxSO3 const reference type */ + typedef const typename internal::traits::RxSO3Type & + ConstRxSO3Reference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(const Scalar* coeffs) + : rxso3_(coeffs), + translation_(coeffs+Sophus::RxSO3Group::num_parameters) { + } + + EIGEN_STRONG_INLINE + Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs) + : translation_(trans_coeffs), rxso3_(rot_coeffs){ + } + + /** + * \brief Accessor of RxSO3 + */ + EIGEN_STRONG_INLINE + ConstRxSO3Reference rxso3() const { + return rxso3_; + } + + /** + * \brief Accessor of translation vector + */ + EIGEN_STRONG_INLINE + ConstTranslationReference translation() const { + return translation_; + } + +protected: + const Map,_Options> rxso3_; + const Map,_Options> translation_; +}; + +} + +#endif diff --git a/thirdparty/Sophus/sophus/so2.hpp b/thirdparty/Sophus/sophus/so2.hpp new file mode 100644 index 0000000..d2e8160 --- /dev/null +++ b/thirdparty/Sophus/sophus/so2.hpp @@ -0,0 +1,701 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_SO2_HPP +#define SOPHUS_SO2_HPP + +#include + +#include "sophus.hpp" + +//////////////////////////////////////////////////////////////////////////// +// Forward Declarations / typedefs +//////////////////////////////////////////////////////////////////////////// + +namespace Sophus { +template class SO2Group; +typedef SO2Group SO2 EIGEN_DEPRECATED; +typedef SO2Group SO2d; /**< double precision SO2 */ +typedef SO2Group SO2f; /**< single precision SO2 */ +} + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +namespace Eigen { +namespace internal { + +template +struct traits > { + typedef _Scalar Scalar; + typedef Matrix ComplexType; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> ComplexType; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> ComplexType; +}; + +} +} + +namespace Sophus { +using namespace Eigen; + +/** + * \brief SO2 base type - implements SO2 class but is storage agnostic + * + * [add more detailed description/tutorial] + */ +template +class SO2GroupBase { +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief complex number reference type */ + typedef typename internal::traits::ComplexType & + ComplexReference; + /** \brief complex number const reference type */ + typedef const typename internal::traits::ComplexType & + ConstComplexReference; + + /** \brief degree of freedom of group + * (one for in-plane rotation) */ + static const int DoF = 1; + /** \brief number of internal parameters used + * (unit complex number for rotation) */ + static const int num_parameters = 2; + /** \brief group transformations are NxN matrices */ + static const int N = 2; + /** \brief group transfomation type */ + typedef Matrix Transformation; + /** \brief point type */ + typedef Matrix Point; + /** \brief tangent vector type */ + typedef Scalar Tangent; + /** \brief adjoint transformation type */ + typedef Scalar Adjoint; + + /** + * \brief Adjoint transformation + * + * This function return the adjoint transformation \f$ Ad \f$ of the + * group instance \f$ A \f$ such that for all \f$ x \f$ + * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ + * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. + * + * For SO2, it simply returns 1. + */ + inline + const Adjoint Adj() const { + return 1; + } + + /** + * \returns copy of instance casted to NewScalarType + */ + template + inline SO2Group cast() const { + return SO2Group(unit_complex() + .template cast() ); + } + + /** + * \returns pointer to internal data + * + * This provides unsafe read/write access to internal data. SO2 is represented + * by a complex number with unit length (two parameters). When using direct + * write access, the user needs to take care of that the complex number stays + * normalized. + * + * \see normalize() + */ + inline Scalar* data() { + return unit_complex_nonconst().data(); + } + + /** + * \returns const pointer to internal data + * + * Const version of data(). + */ + inline const Scalar* data() const { + return unit_complex().data(); + } + + /** + * \brief Fast group multiplication + * + * This method is a fast version of operator*=(), since it does not perform + * normalization. It is up to the user to call normalize() once in a while. + * + * \see operator*=() + */ + inline + void fastMultiply(const SO2Group& other) { + Scalar lhs_real = unit_complex().x(); + Scalar lhs_imag = unit_complex().y(); + const Scalar & rhs_real = other.unit_complex().x(); + const Scalar & rhs_imag = other.unit_complex().y(); + // complex multiplication + unit_complex_nonconst().x() = lhs_real*rhs_real - lhs_imag*rhs_imag; + unit_complex_nonconst().y() = lhs_real*rhs_imag + lhs_imag*rhs_real; + } + + /** + * \returns group inverse of instance + */ + inline + const SO2Group inverse() const { + return SO2Group(unit_complex().x(), -unit_complex().y()); + } + + /** + * \brief Logarithmic map + * + * \returns tangent space representation (=rotation angle) of instance + * + * \see log(). + */ + inline + const Scalar log() const { + return SO2Group::log(*this); + } + + /** + * \brief Normalize complex number + * + * It re-normalizes complex number to unit length. This method only needs to + * be called in conjunction with fastMultiply() or data() write access. + */ + inline + void normalize() { + Scalar length = + std::sqrt(unit_complex().x()*unit_complex().x() + + unit_complex().y()*unit_complex().y()); + if(length < SophusConstants::epsilon()) { + throw SophusException("Complex number is (near) zero!"); + } + unit_complex_nonconst().x() /= length; + unit_complex_nonconst().y() /= length; + } + + /** + * \returns 2x2 matrix representation of instance + * + * For SO2, the matrix representation is an orthogonal matrix R with det(R)=1, + * thus the so-called rotation matrix. + */ + inline + const Transformation matrix() const { + const Scalar & real = unit_complex().x(); + const Scalar & imag = unit_complex().y(); + Transformation R; + R << real, -imag + ,imag, real; + return R; + } + + /** + * \brief Assignment operator + */ + template inline + SO2GroupBase& operator=(const SO2GroupBase & other) { + unit_complex_nonconst() = other.unit_complex(); + return *this; + } + + /** + * \brief Group multiplication + * \see operator*=() + */ + inline + const SO2Group operator*(const SO2Group& other) const { + SO2Group result(*this); + result *= other; + return result; + } + + /** + * \brief Group action on \f$ \mathbf{R}^2 \f$ + * + * \param p point \f$p \in \mathbf{R}^2 \f$ + * \returns point \f$p' \in \mathbf{R}^2 \f$, rotated version of \f$p\f$ + * + * This function rotates a point \f$ p \f$ in \f$ \mathbf{R}^2 \f$ by the + * SO2 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$. + */ + inline + const Point operator*(const Point & p) const { + const Scalar & real = unit_complex().x(); + const Scalar & imag = unit_complex().y(); + return Point(real*p[0] - imag*p[1], imag*p[0] + real*p[1]); + } + + /** + * \brief In-place group multiplication + * + * \see fastMultiply() + * \see operator*() + */ + inline + void operator*=(const SO2Group& other) { + fastMultiply(other); + normalize(); + } + + /** + * \brief Setter of internal unit complex number representation + * + * \param complex + * \pre the complex number must not be near zero + * + * The complex number is normalized to unit length. + */ + inline + void setComplex(const Point & complex) { + unit_complex() = complex; + normalize(); + } + + /** + * \brief Accessor of unit complex number + * + * No direct write access is given to ensure the complex stays normalized. + */ + EIGEN_STRONG_INLINE + ConstComplexReference unit_complex() const { + return static_cast(this)->unit_complex(); + } + + //////////////////////////////////////////////////////////////////////////// + // public static functions + //////////////////////////////////////////////////////////////////////////// + + /** + * \brief Group exponential + * + * \param theta tangent space element (=rotation angle \f$ \theta \f$) + * \returns corresponding element of the group SO2 + * + * To be more specific, this function computes \f$ \exp(\widehat{\theta}) \f$ + * with \f$ \exp(\cdot) \f$ being the matrix exponential + * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO2. + * + * \see hat() + * \see log() + */ + inline static + const SO2Group exp(const Tangent & theta) { + return SO2Group(std::cos(theta), std::sin(theta)); + } + + /** + * \brief Generator + * + * The infinitesimal generator of SO2 + * is \f$ + * G_0 = \left( \begin{array}{ccc} + * 0& -1& \\ + * 1& 0& + * \end{array} \right). + * \f$ + * \see hat() + */ + inline static + const Transformation generator() { + return hat(1); + } + + /** + * \brief hat-operator + * + * \param theta scalar representation of Lie algebra element + * \returns 2x2-matrix representatin of Lie algebra element + * + * Formally, the hat-operator of SO2 is defined + * as \f$ \widehat{\cdot}: \mathbf{R}^2 \rightarrow \mathbf{R}^{2\times 2}, + * \quad \widehat{\theta} = G_0\cdot \theta \f$ + * with \f$ G_0 \f$ being the infinitesial generator(). + * + * \see generator() + * \see vee() + */ + inline static + const Transformation hat(const Tangent & theta) { + Transformation Omega; + Omega << static_cast(0), -theta + , theta, static_cast(0); + return Omega; + } + + /** + * \brief Lie bracket + * + * \param theta1 scalar representation of Lie algebra element + * \param theta2 scalar representation of Lie algebra element + * \returns zero + * + * It computes the bracket. For the Lie algebra so2, the Lie bracket is + * simply \f$ [\theta_1, \theta_2]_{so2} = 0 \f$ since SO2 is a + * commutative group. + * + * \see hat() + * \see vee() + */ + inline static + const Tangent lieBracket(const Tangent & theta1, + const Tangent & theta2) { + return static_cast(0); + } + + /** + * \brief Logarithmic map + * + * \param other element of the group SO2 + * \returns corresponding tangent space element + * (=rotation angle \f$ \theta \f$) + * + * Computes the logarithmic, the inverse of the group exponential. + * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ + * with \f$ \vee(\cdot) \f$ being the matrix logarithm + * and \f$ \vee{\cdot} \f$ the vee()-operator of SO2. + * + * \see exp() + * \see vee() + */ + inline static + const Tangent log(const SO2Group & other) { + // todo: general implementation for Scalar not being float or double. + return atan2(other.unit_complex_.y(), other.unit_complex().x()); + } + + /** + * \brief vee-operator + * + * \param Omega 2x2-matrix representation of Lie algebra element + * \pre Omega need to be a skew-symmetric matrix + * \returns scalar representatin of Lie algebra element + *s + * This is the inverse of the hat()-operator. + * + * \see hat() + */ + inline static + const Tangent vee(const Transformation & Omega) { + return static_cast(0.5)*(Omega(1,0) - Omega(0,1)); + } + +private: + // Mutator of complex number is private so users are hampered + // from setting non-unit complex numbers. + EIGEN_STRONG_INLINE + ComplexReference unit_complex_nonconst() { + return static_cast(this)->unit_complex_nonconst(); + } + +}; + +/** + * \brief SO2 default type - Constructors and default storage for SO2 Type + */ +template +class SO2Group : public SO2GroupBase > { + typedef SO2GroupBase > Base; +public: + /** \brief scalar type */ + typedef typename internal::traits > + ::Scalar Scalar; + /** \brief complex number reference type */ + typedef typename internal::traits > + ::ComplexType & ComplexReference; + /** \brief complex number const reference type */ + typedef const typename internal::traits > + ::ComplexType & ConstComplexReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + // base is friend so unit_complex_nonconst can be accessed from base + friend class SO2GroupBase >; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Default constructor + * + * Initialize complex number to identity rotation. + */ + inline SO2Group() + : unit_complex_(static_cast(1), static_cast(0)) { + } + + /** + * \brief Copy constructor + */ + template inline + SO2Group(const SO2GroupBase & other) + : unit_complex_(other.unit_complex()) { + } + + /** + * \brief Constructor from rotation matrix + * + * \pre rotation matrix need to be orthogonal with determinant of 1 + */ + inline explicit + SO2Group(const Transformation & R) + : unit_complex_(static_cast(0.5)*(R(0,0)+R(1,1)), + static_cast(0.5)*(R(1,0)-R(0,1))) { + if (std::abs(R.determinant()-static_cast(1)) + > SophusConstants::epsilon()) { + throw SophusException("det(R) is not near 1."); + } + } + + /** + * \brief Constructor from pair of real and imaginary number + * + * \pre pair must not be zero + */ + inline SO2Group(const Scalar & real, const Scalar & imag) + : unit_complex_(real, imag) { + Base::normalize(); + } + + /** + * \brief Constructor from 2-vector + * + * \pre vector must not be zero + */ + inline explicit + SO2Group(const Matrix & complex) + : unit_complex_(complex) { + Base::normalize(); + } + + /** + * \brief Constructor from std::complex + * + * \pre complex number must not be zero + */ + inline explicit + SO2Group(const std::complex & complex) + : unit_complex_(complex.real(), complex.imag()) { + Base::normalize(); + } + + /** + * \brief Constructor from an angle + */ + inline explicit + SO2Group(Scalar theta) { + unit_complex_nonconst() = SO2Group::exp(theta).unit_complex(); + } + + /** + * \brief Accessor of unit complex number + * + * No direct write access is given to ensure the complex number stays + * normalized. + */ + EIGEN_STRONG_INLINE + ConstComplexReference unit_complex() const { + return unit_complex_; + } + +protected: + // Mutator of complex number is protected so users are hampered + // from setting non-unit complex numbers. + EIGEN_STRONG_INLINE + ComplexReference unit_complex_nonconst() { + return unit_complex_; + } + + static bool isNearZero(const Scalar & real, const Scalar & imag) { + return (real*real + imag*imag < SophusConstants::epsilon()); + } + + Matrix unit_complex_; +}; + +} // end namespace + + +namespace Eigen { +/** + * \brief Specialisation of Eigen::Map for SO2GroupBase + * + * Allows us to wrap SO2 Objects around POD array + * (e.g. external c style complex number) + */ +template +class Map, _Options> + : public Sophus::SO2GroupBase, _Options> > { + typedef Sophus::SO2GroupBase, _Options> > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief complex number reference type */ + typedef typename internal::traits::ComplexType & ComplexReference; + /** \brief complex number const reference type */ + typedef const typename internal::traits::ComplexType & + ConstComplexReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + // base is friend so unit_complex_nonconst can be accessed from base + friend class Sophus::SO2GroupBase, _Options> >; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(Scalar* coeffs) : unit_complex_(coeffs) { + } + + /** + * \brief Accessor of unit complex number + * + * No direct write access is given to ensure the complex number stays + * normalized. + */ + EIGEN_STRONG_INLINE + ConstComplexReference unit_complex() const { + return unit_complex_; + } + +protected: + // Mutator of complex number is protected so users are hampered + // from setting non-unit complex number. + EIGEN_STRONG_INLINE + ComplexReference unit_complex_nonconst() { + return unit_complex_; + } + + Map,_Options> unit_complex_; +}; + +/** + * \brief Specialisation of Eigen::Map for const SO2GroupBase + * + * Allows us to wrap SO2 Objects around POD array + * (e.g. external c style complex number) + */ +template +class Map, _Options> + : public Sophus::SO2GroupBase< + Map, _Options> > { + typedef Sophus::SO2GroupBase, _Options> > + Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief complex number const reference type */ + typedef const typename internal::traits::ComplexType & + ConstComplexReference; + + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(const Scalar* coeffs) : unit_complex_(coeffs) { + } + + /** + * \brief Accessor of unit complex number + * + * No direct write access is given to ensure the complex number stays + * normalized. + */ + EIGEN_STRONG_INLINE + ConstComplexReference unit_complex() const { + return unit_complex_; + } + +protected: + const Map,_Options> unit_complex_; +}; + +} + + +#endif // SOPHUS_SO2_HPP diff --git a/thirdparty/Sophus/sophus/so3.hpp b/thirdparty/Sophus/sophus/so3.hpp new file mode 100644 index 0000000..88a362e --- /dev/null +++ b/thirdparty/Sophus/sophus/so3.hpp @@ -0,0 +1,811 @@ +// This file is part of Sophus. +// +// Copyright 2011-2013 Hauke Strasdat +// Copyrifht 2012-2013 Steven Lovegrove +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_SO3_HPP +#define SOPHUS_SO3_HPP + +#include "sophus.hpp" + +//////////////////////////////////////////////////////////////////////////// +// Forward Declarations / typedefs +//////////////////////////////////////////////////////////////////////////// + +namespace Sophus { +template class SO3Group; +typedef EIGEN_DEPRECATED SO3Group SO3; +typedef SO3Group SO3d; /**< double precision SO3 */ +typedef SO3Group SO3f; /**< single precision SO3 */ +} + +//////////////////////////////////////////////////////////////////////////// +// Eigen Traits (For querying derived types in CRTP hierarchy) +//////////////////////////////////////////////////////////////////////////// + +namespace Eigen { +namespace internal { + +template +struct traits > { + typedef _Scalar Scalar; + typedef Quaternion QuaternionType; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> QuaternionType; +}; + +template +struct traits, _Options> > + : traits > { + typedef _Scalar Scalar; + typedef Map,_Options> QuaternionType; +}; + +} +} + +namespace Sophus { +using namespace Eigen; + +/** + * \brief SO3 base type - implements SO3 class but is storage agnostic + * + * [add more detailed description/tutorial] + */ +template +class SO3GroupBase { +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief quaternion reference type */ + typedef typename internal::traits::QuaternionType & + QuaternionReference; + /** \brief quaternion const reference type */ + typedef const typename internal::traits::QuaternionType & + ConstQuaternionReference; + + /** \brief degree of freedom of group + * (three for rotation) */ + static const int DoF = 3; + /** \brief number of internal parameters used + * (unit quaternion for rotation) */ + static const int num_parameters = 4; + /** \brief group transformations are NxN matrices */ + static const int N = 3; + /** \brief group transfomation type */ + typedef Matrix Transformation; + /** \brief point type */ + typedef Matrix Point; + /** \brief tangent vector type */ + typedef Matrix Tangent; + /** \brief adjoint transformation type */ + typedef Matrix Adjoint; + + /** + * \brief Adjoint transformation + * + * This function return the adjoint transformation \f$ Ad \f$ of the + * group instance \f$ A \f$ such that for all \f$ x \f$ + * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ + * with \f$\ \widehat{\cdot} \f$ being the hat()-Vector4Scalaror. + * + * For SO3, it simply returns the rotation matrix corresponding to \f$ A \f$. + */ + inline + const Adjoint Adj() const { + return matrix(); + } + + /** + * \returns copy of instance casted to NewScalarType + */ + template + inline SO3Group cast() const { + return SO3Group(unit_quaternion() + .template cast() ); + } + + /** + * \returns pointer to internal data + * + * This provides unsafe read/write access to internal data. SO3 is represented + * by an Eigen::Quaternion (four parameters). When using direct write access, + * the user needs to take care of that the quaternion stays normalized. + * + * Note: The first three Scalars represent the imaginary parts, while the + * forth Scalar represent the real part. + * + * \see normalize() + */ + inline Scalar* data() { + return unit_quaternion_nonconst().coeffs().data(); + } + + /** + * \returns const pointer to internal data + * + * Const version of data(). + */ + inline const Scalar* data() const { + return unit_quaternion().coeffs().data(); + } + + /** + * \brief Fast group multiplication + * + * This method is a fast version of operator*=(), since it does not perform + * normalization. It is up to the user to call normalize() once in a while. + * + * \see operator*=() + */ + inline + void fastMultiply(const SO3Group& other) { + unit_quaternion_nonconst() *= other.unit_quaternion(); + } + + /** + * \returns group inverse of instance + */ + inline + const SO3Group inverse() const { + return SO3Group(unit_quaternion().conjugate()); + } + + /** + * \brief Logarithmic map + * + * \returns tangent space representation (=rotation vector) of instance + * + * \see log(). + */ + inline + const Tangent log() const { + return SO3Group::log(*this); + } + + /** + * \brief Normalize quaternion + * + * It re-normalizes unit_quaternion to unit length. This method only needs to + * be called in conjunction with fastMultiply() or data() write access. + */ + inline + void normalize() { + Scalar length = unit_quaternion_nonconst().norm(); + if (length < SophusConstants::epsilon()) { + throw SophusException("Quaternion is (near) zero!"); + } + unit_quaternion_nonconst().coeffs() /= length; + } + + /** + * \returns 3x3 matrix representation of instance + * + * For SO3, the matrix representation is an orthogonal matrix R with det(R)=1, + * thus the so-called rotation matrix. + */ + inline + const Transformation matrix() const { + return unit_quaternion().toRotationMatrix(); + } + + /** + * \brief Assignment operator + */ + template inline + SO3GroupBase& operator=(const SO3GroupBase & other) { + unit_quaternion_nonconst() = other.unit_quaternion(); + return *this; + } + + /** + * \brief Group multiplication + * \see operator*=() + */ + inline + const SO3Group operator*(const SO3Group& other) const { + SO3Group result(*this); + result *= other; + return result; + } + + /** + * \brief Group action on \f$ \mathbf{R}^3 \f$ + * + * \param p point \f$p \in \mathbf{R}^3 \f$ + * \returns point \f$p' \in \mathbf{R}^3 \f$, rotated version of \f$p\f$ + * + * This function rotates a point \f$ p \f$ in \f$ \mathbf{R}^3 \f$ by the + * SO3 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$. + * + * + * Since SO3 is intenally represented by a unit quaternion \f$q\f$, it is + * implemented as \f$ p' = q\cdot p \cdot q^{*} \f$ + * with \f$ q^{*} \f$ being the quaternion conjugate of \f$ q \f$. + * + * Geometrically, \f$ p \f$ is rotated by angle \f$|\omega|\f$ around the + * axis \f$\frac{\omega}{|\omega|}\f$ with \f$ \omega = \log(R)^\vee \f$. + * + * \see log() + */ + inline + const Point operator*(const Point & p) const { + return unit_quaternion()._transformVector(p); + } + + /** + * \brief In-place group multiplication + * + * \see fastMultiply() + * \see operator*() + */ + inline + void operator*=(const SO3Group& other) { + fastMultiply(other); + normalize(); + } + + /** + * \brief Setter of internal unit quaternion representation + * + * \param quaternion + * \pre the quaternion must not be zero + * + * The quaternion is normalized to unit length. + */ + inline + void setQuaternion(const Quaternion& quaternion) { + unit_quaternion_nonconst() = quaternion; + normalize(); + } + + /** + * \brief Accessor of unit quaternion + * + * No direct write access is given to ensure the quaternion stays normalized. + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference unit_quaternion() const { + return static_cast(this)->unit_quaternion(); + } + + //////////////////////////////////////////////////////////////////////////// + // public static functions + //////////////////////////////////////////////////////////////////////////// + + /** + * \param b 3-vector representation of Lie algebra element + * \returns derivative of Lie bracket + * + * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{so3} \f$ + * with \f$ [a, b]_{so3} \f$ being the lieBracket() of the Lie algebra so3. + * + * \see lieBracket() + */ + inline static + const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { + return -hat(b); + } + + /** + * \brief Group exponential + * + * \param omega tangent space element (=rotation vector \f$ \omega \f$) + * \returns corresponding element of the group SO3 + * + * To be more specific, this function computes \f$ \exp(\widehat{\omega}) \f$ + * with \f$ \exp(\cdot) \f$ being the matrix exponential + * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO3. + * + * \see expAndTheta() + * \see hat() + * \see log() + */ + inline static + const SO3Group exp(const Tangent & omega) { + Scalar theta; + return expAndTheta(omega, &theta); + } + + /** + * \brief Group exponential and theta + * + * \param omega tangent space element (=rotation vector \f$ \omega \f$) + * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ + * \returns corresponding element of the group SO3 + * + * \see exp() for details + */ + inline static + const SO3Group expAndTheta(const Tangent & omega, + Scalar * theta) { + const Scalar theta_sq = omega.squaredNorm(); + *theta = std::sqrt(theta_sq); + const Scalar half_theta = static_cast(0.5)*(*theta); + + Scalar imag_factor; + Scalar real_factor;; + if((*theta)::epsilon()) { + const Scalar theta_po4 = theta_sq*theta_sq; + imag_factor = static_cast(0.5) + - static_cast(1.0/48.0)*theta_sq + + static_cast(1.0/3840.0)*theta_po4; + real_factor = static_cast(1) + - static_cast(0.5)*theta_sq + + static_cast(1.0/384.0)*theta_po4; + } else { + const Scalar sin_half_theta = std::sin(half_theta); + imag_factor = sin_half_theta/(*theta); + real_factor = std::cos(half_theta); + } + + return SO3Group(Quaternion(real_factor, + imag_factor*omega.x(), + imag_factor*omega.y(), + imag_factor*omega.z())); + } + + /** + * \brief Generators + * + * \pre \f$ i \in \{0,1,2\} \f$ + * \returns \f$ i \f$th generator \f$ G_i \f$ of SO3 + * + * The infinitesimal generators of SO3 + * are \f$ + * G_0 = \left( \begin{array}{ccc} + * 0& 0& 0& \\ + * 0& 0& -1& \\ + * 0& 1& 0& + * \end{array} \right), + * G_1 = \left( \begin{array}{ccc} + * 0& 0& 1& \\ + * 0& 0& 0& \\ + * -1& 0& 0& + * \end{array} \right), + * G_2 = \left( \begin{array}{ccc} + * 0& -1& 0& \\ + * 1& 0& 0& \\ + * 0& 0& 0& + * \end{array} \right). + * \f$ + * \see hat() + */ + inline static + const Transformation generator(int i) { + if (i<0 || i>2) { + throw SophusException("i is not in range [0,2]."); + } + Tangent e; + e.setZero(); + e[i] = static_cast(1); + return hat(e); + } + + /** + * \brief hat-operator + * + * \param omega 3-vector representation of Lie algebra element + * \returns 3x3-matrix representatin of Lie algebra element + * + * Formally, the hat-operator of SO3 is defined + * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{3\times 3}, + * \quad \widehat{\omega} = \sum_{i=0}^2 G_i \omega_i \f$ + * with \f$ G_i \f$ being the ith infinitesial generator(). + * + * \see generator() + * \see vee() + */ + inline static + const Transformation hat(const Tangent & omega) { + Transformation Omega; + Omega << static_cast(0), -omega(2), omega(1) + , omega(2), static_cast(0), -omega(0) + , -omega(1), omega(0), static_cast(0); + return Omega; + } + + /** + * \brief Lie bracket + * + * \param omega1 3-vector representation of Lie algebra element + * \param omega2 3-vector representation of Lie algebra element + * \returns 3-vector representation of Lie algebra element + * + * It computes the bracket of SO3. To be more specific, it + * computes \f$ [\omega_1, \omega_2]_{so3} + * := [\widehat{\omega_1}, \widehat{\omega_2}]^\vee \f$ + * with \f$ [A,B] = AB-BA \f$ being the matrix + * commutator, \f$ \widehat{\cdot} \f$ the + * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SO3. + * + * For the Lie algebra so3, the Lie bracket is simply the + * cross product: \f$ [\omega_1, \omega_2]_{so3} + * = \omega_1 \times \omega_2 \f$. + * + * \see hat() + * \see vee() + */ + inline static + const Tangent lieBracket(const Tangent & omega1, + const Tangent & omega2) { + return omega1.cross(omega2); + } + + /** + * \brief Logarithmic map + * + * \param other element of the group SO3 + * \returns corresponding tangent space element + * (=rotation vector \f$ \omega \f$) + * + * Computes the logarithmic, the inverse of the group exponential. + * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ + * with \f$ \vee(\cdot) \f$ being the matrix logarithm + * and \f$ \vee{\cdot} \f$ the vee()-operator of SO3. + * + * \see exp() + * \see logAndTheta() + * \see vee() + */ + inline static + const Tangent log(const SO3Group & other) { + Scalar theta; + return logAndTheta(other, &theta); + } + + /** + * \brief Logarithmic map and theta + * + * \param other element of the group SO3 + * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ + * \returns corresponding tangent space element + * (=rotation vector \f$ \omega \f$) + * + * \see log() for details + */ + inline static + const Tangent logAndTheta(const SO3Group & other, + Scalar * theta) { + const Scalar squared_n + = other.unit_quaternion().vec().squaredNorm(); + const Scalar n = std::sqrt(squared_n); + const Scalar w = other.unit_quaternion().w(); + + Scalar two_atan_nbyw_by_n; + + // Atan-based log thanks to + // + // C. Hertzberg et al.: + // "Integrating Generic Sensor Fusion Algorithms with Sound State + // Representation through Encapsulation of Manifolds" + // Information Fusion, 2011 + + if (n < SophusConstants::epsilon()) { + // If quaternion is normalized and n=0, then w should be 1; + // w=0 should never happen here! + if (std::abs(w) < SophusConstants::epsilon()) { + throw SophusException("Quaternion is not normalized!"); + } + const Scalar squared_w = w*w; + two_atan_nbyw_by_n = static_cast(2) / w + - static_cast(2)*(squared_n)/(w*squared_w); + } else { + if (std::abs(w)::epsilon()) { + if (w > static_cast(0)) { + two_atan_nbyw_by_n = M_PI/n; + } else { + two_atan_nbyw_by_n = -M_PI/n; + } + }else{ + two_atan_nbyw_by_n = static_cast(2) * atan(n/w) / n; + } + } + + *theta = two_atan_nbyw_by_n*n; + + return two_atan_nbyw_by_n * other.unit_quaternion().vec(); + } + + /** + * \brief vee-operator + * + * \param Omega 3x3-matrix representation of Lie algebra element + * \pr Omega must be a skew-symmetric matrix + * \returns 3-vector representatin of Lie algebra element + * + * This is the inverse of the hat()-operator. + * + * \see hat() + */ + inline static + const Tangent vee(const Transformation & Omega) { + return static_cast(0.5) * Tangent(Omega(2,1) - Omega(1,2), + Omega(0,2) - Omega(2,0), + Omega(1,0) - Omega(0,1)); + } + +private: + // Mutator of unit_quaternion is private so users are hampered + // from setting non-unit quaternions. + EIGEN_STRONG_INLINE + QuaternionReference unit_quaternion_nonconst() { + return static_cast(this)->unit_quaternion_nonconst(); + } + +}; + +/** + * \brief SO3 default type - Constructors and default storage for SO3 Type + */ +template +class SO3Group : public SO3GroupBase > { + typedef SO3GroupBase > Base; +public: + /** \brief scalar type */ + typedef typename internal::traits > + ::Scalar Scalar; + /** \brief quaternion type */ + typedef typename internal::traits > + ::QuaternionType & QuaternionReference; + typedef const typename internal::traits > + ::QuaternionType & ConstQuaternionReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + // base is friend so unit_quaternion_nonconst can be accessed from base + friend class SO3GroupBase >; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Default constructor + * + * Initialize Quaternion to identity rotation. + */ + inline + SO3Group() + : unit_quaternion_(static_cast(1), static_cast(0), + static_cast(0), static_cast(0)) { + } + + /** + * \brief Copy constructor + */ + template inline + SO3Group(const SO3GroupBase & other) + : unit_quaternion_(other.unit_quaternion()) { + } + + /** + * \brief Constructor from rotation matrix + * + * \pre rotation matrix need to be orthogonal with determinant of 1 + */ + inline SO3Group(const Transformation & R) + : unit_quaternion_(R) { + } + + /** + * \brief Constructor from quaternion + * + * \pre quaternion must not be zero + */ + inline explicit + SO3Group(const Quaternion & quat) : unit_quaternion_(quat) { + Base::normalize(); + } + + /** + * \brief Constructor from Euler angles + * + * \param alpha1 rotation around x-axis + * \param alpha2 rotation around y-axis + * \param alpha3 rotation around z-axis + * + * Since rotations in 3D do not commute, the order of the individual rotations + * matter. Here, the following convention is used. We calculate a SO3 member + * corresponding to the rotation matrix \f$ R \f$ such + * that \f$ R=\exp\left(\begin{array}{c}\alpha_1\\ 0\\ 0\end{array}\right) + * \cdot \exp\left(\begin{array}{c}0\\ \alpha_2\\ 0\end{array}\right) + * \cdot \exp\left(\begin{array}{c}0\\ 0\\ \alpha_3\end{array}\right)\f$. + */ + inline + SO3Group(Scalar alpha1, Scalar alpha2, Scalar alpha3) { + const static Scalar zero = static_cast(0); + unit_quaternion_ + = ( SO3Group::exp(Tangent(alpha1, zero, zero)) + *SO3Group::exp(Tangent( zero, alpha2, zero)) + *SO3Group::exp(Tangent( zero, zero, alpha3)) ) + .unit_quaternion_; + } + + /** + * \brief Accessor of unit quaternion + * + * No direct write access is given to ensure the quaternion stays normalized. + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference unit_quaternion() const { + return unit_quaternion_; + } + +protected: + // Mutator of unit_quaternion is protected so users are hampered + // from setting non-unit quaternions. + EIGEN_STRONG_INLINE + QuaternionReference unit_quaternion_nonconst() { + return unit_quaternion_; + } + + Quaternion unit_quaternion_; +}; + +} // end namespace + + +namespace Eigen { +/** + * \brief Specialisation of Eigen::Map for SO3GroupBase + * + * Allows us to wrap SO3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::SO3GroupBase, _Options> > { + typedef Sophus::SO3GroupBase, _Options> > Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief quaternion reference type */ + typedef typename internal::traits::QuaternionType & + QuaternionReference; + /** \brief quaternion const reference type */ + typedef const typename internal::traits::QuaternionType & + ConstQuaternionReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + // base is friend so unit_quaternion_nonconst can be accessed from base + friend class Sophus::SO3GroupBase, _Options> >; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(Scalar* coeffs) : unit_quaternion_(coeffs) { + } + + /** + * \brief Accessor of unit quaternion + * + * No direct write access is given to ensure the quaternion stays normalized. + */ + EIGEN_STRONG_INLINE + ConstQuaternionReference unit_quaternion() const { + return unit_quaternion_; + } + +protected: + // Mutator of unit_quaternion is protected so users are hampered + // from setting non-unit quaternions. + EIGEN_STRONG_INLINE + QuaternionReference unit_quaternion_nonconst() { + return unit_quaternion_; + } + + Map,_Options> unit_quaternion_; +}; + +/** + * \brief Specialisation of Eigen::Map for const SO3GroupBase + * + * Allows us to wrap SO3 Objects around POD array + * (e.g. external c style quaternion) + */ +template +class Map, _Options> + : public Sophus::SO3GroupBase< + Map, _Options> > { + typedef Sophus::SO3GroupBase, _Options> > + Base; + +public: + /** \brief scalar type */ + typedef typename internal::traits::Scalar Scalar; + /** \brief quaternion const reference type */ + typedef const typename internal::traits::QuaternionType & + ConstQuaternionReference; + + /** \brief degree of freedom of group */ + static const int DoF = Base::DoF; + /** \brief number of internal parameters used */ + static const int num_parameters = Base::num_parameters; + /** \brief group transformations are NxN matrices */ + static const int N = Base::N; + /** \brief group transfomation type */ + typedef typename Base::Transformation Transformation; + /** \brief point type */ + typedef typename Base::Point Point; + /** \brief tangent vector type */ + typedef typename Base::Tangent Tangent; + /** \brief adjoint transformation type */ + typedef typename Base::Adjoint Adjoint; + + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + using Base::operator*=; + using Base::operator*; + + EIGEN_STRONG_INLINE + Map(const Scalar* coeffs) : unit_quaternion_(coeffs) { + } + + /** + * \brief Accessor of unit quaternion + * + * No direct write access is given to ensure the quaternion stays normalized. + */ + EIGEN_STRONG_INLINE + const ConstQuaternionReference unit_quaternion() const { + return unit_quaternion_; + } + +protected: + const Map,_Options> unit_quaternion_; +}; + +} + +#endif diff --git a/thirdparty/Sophus/sophus/sophus.hpp b/thirdparty/Sophus/sophus/sophus.hpp new file mode 100644 index 0000000..76e142e --- /dev/null +++ b/thirdparty/Sophus/sophus/sophus.hpp @@ -0,0 +1,77 @@ +// This file is part of Sophus. +// +// Copyright 2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef SOPHUS_HPP +#define SOPHUS_HPP + +#include + +// fix log1p not being found on Android in Eigen +#if defined( ANDROID ) +#include +namespace std { + using ::log1p; +} +#endif + +#include +#include + +namespace Sophus { +using namespace Eigen; + +template +struct SophusConstants { + EIGEN_ALWAYS_INLINE static + const Scalar epsilon() { + return static_cast(1e-10); + } + + EIGEN_ALWAYS_INLINE static + const Scalar pi() { + return static_cast(M_PI); + } +}; + +template<> +struct SophusConstants { + EIGEN_ALWAYS_INLINE static + float epsilon() { + return static_cast(1e-5); + } + + EIGEN_ALWAYS_INLINE static + float pi() { + return static_cast(M_PI); + } +}; + +class SophusException : public std::runtime_error { +public: + SophusException (const std::string& str) + : runtime_error("Sophus exception: " + str) { + } +}; + +} + +#endif diff --git a/thirdparty/Sophus/sophus/test_rxso3.cpp b/thirdparty/Sophus/sophus/test_rxso3.cpp new file mode 100644 index 0000000..46134d3 --- /dev/null +++ b/thirdparty/Sophus/sophus/test_rxso3.cpp @@ -0,0 +1,92 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + + +#include "rxso3.hpp" +#include "tests.hpp" + +using namespace Sophus; +using namespace std; + +template +void tests() { + + typedef RxSO3Group RxSO3Type; + typedef typename RxSO3Group::Point Point; + typedef typename RxSO3Group::Tangent Tangent; + + vector rxso3_vec; + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0)) + *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0)) + *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0))); + rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0)) + *RxSO3Type::exp(Tangent(M_PI, 0, 0,0)) + *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0))); + + vector tangent_vec; + Tangent tmp; + tmp << 0,0,0,0; + tangent_vec.push_back(tmp); + tmp << 1,0,0,0; + tangent_vec.push_back(tmp); + tmp << 1,0,0,0.1; + tangent_vec.push_back(tmp); + tmp << 0,1,0,0.1; + tangent_vec.push_back(tmp); + tmp << 0,0,1,-0.1; + tangent_vec.push_back(tmp); + tmp << -1,1,0,-0.1; + tangent_vec.push_back(tmp); + tmp << 20,-1,0,2; + tangent_vec.push_back(tmp); + + vector point_vec; + point_vec.push_back(Point(1,2,4)); + + Tests tests; + tests.setGroupElements(rxso3_vec); + tests.setTangentVectors(tangent_vec); + tests.setPoints(point_vec); + + tests.runAllTests(); +} + +int main() { + cerr << "Test RxSO3" << endl << endl; + + cerr << "Double tests: " << endl; + tests(); + + cerr << "Float tests: " << endl; + tests(); + return 0; +} diff --git a/thirdparty/Sophus/sophus/test_se2.cpp b/thirdparty/Sophus/sophus/test_se2.cpp new file mode 100644 index 0000000..1d9d293 --- /dev/null +++ b/thirdparty/Sophus/sophus/test_se2.cpp @@ -0,0 +1,90 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include +#include "se2.hpp" +#include "tests.hpp" + +using namespace Sophus; +using namespace std; + +template +void tests() { + + typedef SO2Group SO2Type; + typedef SE2Group SE2Type; + typedef typename SE2Group::Point Point; + typedef typename SE2Group::Tangent Tangent; + + vector se2_vec; + se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0))); + se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0))); + se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100))); + se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1))); + se2_vec.push_back(SE2Type(SO2Type(0.00001), + Point(-0.00000001,0.0000000001))); + se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0)) + *SE2Type(SO2Type(M_PI),Point(0,0)) + *SE2Type(SO2Type(-0.2),Point(0,0))); + se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0)) + *SE2Type(SO2Type(M_PI),Point(0,0)) + *SE2Type(SO2Type(-0.3),Point(0,6))); + + vector tangent_vec; + Tangent tmp; + tmp << 0,0,0; + tangent_vec.push_back(tmp); + tmp << 1,0,0; + tangent_vec.push_back(tmp); + tmp << 0,1,1; + tangent_vec.push_back(tmp); + tmp << -1,1,0; + tangent_vec.push_back(tmp); + tmp << 20,-1,-1; + tangent_vec.push_back(tmp); + tmp << 30,5,20; + tangent_vec.push_back(tmp); + + vector point_vec; + point_vec.push_back(Point(1,2)); + + Tests tests; + tests.setGroupElements(se2_vec); + tests.setTangentVectors(tangent_vec); + tests.setPoints(point_vec); + + tests.runAllTests(); +} + +int main() { + cerr << "Test SE2" << endl << endl; + + cerr << "Double tests: " << endl; + tests(); + + cerr << "Float tests: " << endl; + tests(); + return 0; +} diff --git a/thirdparty/Sophus/sophus/test_se3.cpp b/thirdparty/Sophus/sophus/test_se3.cpp new file mode 100644 index 0000000..4d9313b --- /dev/null +++ b/thirdparty/Sophus/sophus/test_se3.cpp @@ -0,0 +1,105 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "se3.hpp" +#include "tests.hpp" + +using namespace Sophus; +using namespace std; + +template +void tests() { + + typedef SO3Group SO3Type; + typedef SE3Group SE3Type; + typedef typename SE3Group::Point Point; + typedef typename SE3Group::Tangent Tangent; + + vector se3_vec; + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), + Point(0,0,0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), + Point(10,0,0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), + Point(0,100,5))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), + Point(0,0,0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), + Point(0,-0.00000001,0.0000000001))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), + Point(0.01,0,0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), + Point(4,-5,0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), + Point(0,0,0)) + *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), + Point(0,0,0)) + *SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), + Point(0,0,0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), + Point(2,0,-7)) + *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), + Point(0,0,0)) + *SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), + Point(0,6,0))); + vector tangent_vec; + Tangent tmp; + tmp << 0,0,0,0,0,0; + tangent_vec.push_back(tmp); + tmp << 1,0,0,0,0,0; + tangent_vec.push_back(tmp); + tmp << 0,1,0,1,0,0; + tangent_vec.push_back(tmp); + tmp << 0,-5,10,0,0,0; + tangent_vec.push_back(tmp); + tmp << -1,1,0,0,0,1; + tangent_vec.push_back(tmp); + tmp << 20,-1,0,-1,1,0; + tangent_vec.push_back(tmp); + tmp << 30,5,-1,20,-1,0; + tangent_vec.push_back(tmp); + + vector point_vec; + point_vec.push_back(Point(1,2,4)); + + Tests tests; + tests.setGroupElements(se3_vec); + tests.setTangentVectors(tangent_vec); + tests.setPoints(point_vec); + + tests.runAllTests(); + cerr << "passed." << endl << endl; +} + +int main() { + cerr << "Test SE3" << endl << endl; + + cerr << "Double tests: " << endl; + tests(); + + cerr << "Float tests: " << endl; + tests(); + return 0; +} diff --git a/thirdparty/Sophus/sophus/test_sim3.cpp b/thirdparty/Sophus/sophus/test_sim3.cpp new file mode 100644 index 0000000..97c2c8c --- /dev/null +++ b/thirdparty/Sophus/sophus/test_sim3.cpp @@ -0,0 +1,109 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include + +#include "sim3.hpp" +#include "tests.hpp" + +using namespace Sophus; +using namespace std; + +template +void tests() { + + typedef Sim3Group Sim3Type; + typedef RxSO3Group RxSO3Type; + typedef typename Sim3Group::Point Point; + typedef typename Sim3Group::Tangent Tangent; + typedef Matrix Vector4Type; + + vector sim3_vec; + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,1.)), + Point(0,0,0))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, -1.0,1.1)), + Point(10,0,0))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.,1.1)), + Point(0,10,5))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0.)), + Point(0,0,0))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp( + Vector4Type(0., 0., 0.00001, 0.0000001)), + Point(1,-1.00000001,2.0000000001))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0)), + Point(0.01,0,0))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0.9)), + Point(4,-5,0))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,0)), + Point(0,0,0)) + *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), + Point(0,0,0)) + *Sim3Type(RxSO3Type::exp(Vector4Type(-0.2, -0.5, -0.0,0)), + Point(0,0,0))); + sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.3, 0.5, 0.1,0)), + Point(2,0,-7)) + *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), + Point(0,0,0)) + *Sim3Type(RxSO3Type::exp(Vector4Type(-0.3, -0.5, -0.1,0)), + Point(0,6,0))); + vector tangent_vec; + Tangent tmp; + tmp << 0,0,0,0,0,0,0; + tangent_vec.push_back(tmp); + tmp << 1,0,0,0,0,0,0; + tangent_vec.push_back(tmp); + tmp << 0,1,0,1,0,0,0.1; + tangent_vec.push_back(tmp); + tmp << 0,0,1,0,1,0,0.1; + tangent_vec.push_back(tmp); + tmp << -1,1,0,0,0,1,-0.1; + tangent_vec.push_back(tmp); + tmp << 20,-1,0,-1,1,0,-0.1; + tangent_vec.push_back(tmp); + tmp << 30,5,-1,20,-1,0,1.5; + tangent_vec.push_back(tmp); + + + vector point_vec; + point_vec.push_back(Point(1,2,4)); + + Tests tests; + tests.setGroupElements(sim3_vec); + tests.setTangentVectors(tangent_vec); + tests.setPoints(point_vec); + + tests.runAllTests(); +} + +int main() { + cerr << "Test Sim3" << endl << endl; + + cerr << "Double tests: " << endl; + tests(); + + cerr << "Float tests: " << endl; + tests(); + return 0; +} diff --git a/thirdparty/Sophus/sophus/test_so2.cpp b/thirdparty/Sophus/sophus/test_so2.cpp new file mode 100644 index 0000000..1685b2b --- /dev/null +++ b/thirdparty/Sophus/sophus/test_so2.cpp @@ -0,0 +1,91 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + + +#include +#include + +#include "so2.hpp" +#include "tests.hpp" + +using namespace Sophus; +using namespace std; + +template +void tests() { + + typedef SO2Group SO2Type; + typedef typename SO2Group::Point Point; + typedef typename SO2Group::Tangent Tangent; + + vector so2_vec; + so2_vec.push_back(SO2Type::exp(0.0)); + so2_vec.push_back(SO2Type::exp(0.2)); + so2_vec.push_back(SO2Type::exp(10.)); + so2_vec.push_back(SO2Type::exp(0.00001)); + so2_vec.push_back(SO2Type::exp(M_PI)); + so2_vec.push_back(SO2Type::exp(0.2) + *SO2Type::exp(M_PI) + *SO2Type::exp(-0.2)); + so2_vec.push_back(SO2Type::exp(-0.3) + *SO2Type::exp(M_PI) + *SO2Type::exp(0.3)); + + vector tangent_vec; + tangent_vec.push_back(Tangent(0)); + tangent_vec.push_back(Tangent(1)); + tangent_vec.push_back(Tangent(M_PI_2)); + tangent_vec.push_back(Tangent(-1)); + tangent_vec.push_back(Tangent(20)); + tangent_vec.push_back(Tangent(M_PI_2+0.0001)); + + vector point_vec; + point_vec.push_back(Point(1,2)); + + Tests tests; + tests.setGroupElements(so2_vec); + tests.setTangentVectors(tangent_vec); + tests.setPoints(point_vec); + + tests.runAllTests(); + + cerr << "Exception test: "; + try { + SO2Type so2(0., 0.); + } catch(SophusException & e) { + cerr << "passed." << endl << endl; + return; + } + cerr << "failed!" << endl << endl; + exit(-1); +} + +int main() { + cerr << "Test SO2" << endl << endl; + + cerr << "Double tests: " << endl; + tests(); + + cerr << "Float tests: " << endl; + tests(); + return 0; +} diff --git a/thirdparty/Sophus/sophus/test_so3.cpp b/thirdparty/Sophus/sophus/test_so3.cpp new file mode 100644 index 0000000..e961e1c --- /dev/null +++ b/thirdparty/Sophus/sophus/test_so3.cpp @@ -0,0 +1,84 @@ +// This file is part of Sophus. +// +// Copyright 2012-2013 Hauke Strasdat +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "so3.hpp" +#include "tests.hpp" + +using namespace Sophus; +using namespace std; + +template +void tests() { + + typedef SO3Group SO3Type; + typedef typename SO3Group::Point Point; + typedef typename SO3Group::Tangent Tangent; + + vector so3_vec; + + so3_vec.push_back(SO3Type(Quaternion(0.1e-11, 0., 1., 0.))); + so3_vec.push_back(SO3Type(Quaternion(-1,0.00001,0.0,0.0))); + so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0))); + so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, -1.0))); + so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.))); + so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.00001))); + so3_vec.push_back(SO3Type::exp(Point(M_PI, 0, 0))); + so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0)) + *SO3Type::exp(Point(M_PI, 0, 0)) + *SO3Type::exp(Point(-0.2, -0.5, -0.0))); + so3_vec.push_back(SO3Type::exp(Point(0.3, 0.5, 0.1)) + *SO3Type::exp(Point(M_PI, 0, 0)) + *SO3Type::exp(Point(-0.3, -0.5, -0.1))); + + vector tangent_vec; + tangent_vec.push_back(Tangent(0,0,0)); + tangent_vec.push_back(Tangent(1,0,0)); + tangent_vec.push_back(Tangent(0,1,0)); + tangent_vec.push_back(Tangent(M_PI_2,M_PI_2,0.0)); + tangent_vec.push_back(Tangent(-1,1,0)); + tangent_vec.push_back(Tangent(20,-1,0)); + tangent_vec.push_back(Tangent(30,5,-1)); + + vector point_vec; + point_vec.push_back(Point(1,2,4)); + + Tests tests; + tests.setGroupElements(so3_vec); + tests.setTangentVectors(tangent_vec); + tests.setPoints(point_vec); + + tests.runAllTests(); +} + +int main() { + cerr << "Test SO3" << endl << endl; + + cerr << "Double tests: " << endl; + tests(); + + cerr << "Float tests: " << endl; + tests(); + return 0; +} diff --git a/thirdparty/Sophus/sophus/tests.hpp b/thirdparty/Sophus/sophus/tests.hpp new file mode 100644 index 0000000..0ec3c4a --- /dev/null +++ b/thirdparty/Sophus/sophus/tests.hpp @@ -0,0 +1,264 @@ +#ifndef SOPUHS_TESTS_HPP +#define SOPUHS_TESTS_HPP + +#include +#include + +#include "sophus.hpp" + +namespace Sophus { + +using namespace std; +using namespace Eigen; + +template +class Tests { + +public: + typedef typename LieGroup::Scalar Scalar; + typedef typename LieGroup::Transformation Transformation; + typedef typename LieGroup::Tangent Tangent; + typedef typename LieGroup::Point Point; + typedef typename LieGroup::Adjoint Adjoint; + static const int N = LieGroup::N; + static const int DoF = LieGroup::DoF; + + const Scalar SMALL_EPS; + + Tests() : SMALL_EPS(SophusConstants::epsilon()) { + } + + void setGroupElements(const vector & group_vec) { + group_vec_ = group_vec; + } + + void setTangentVectors(const vector & tangent_vec) { + tangent_vec_ = tangent_vec; + } + + void setPoints(const vector & point_vec) { + point_vec_ = point_vec; + } + + bool adjointTest() { + bool passed = true; + for (size_t i=0; i20.*SMALL_EPS) { + cerr << "Adjoint" << endl; + cerr << "Test case: " << i << "," << j <SMALL_EPS) { + cerr << "G - exp(log(G))" << endl; + cerr << "Test case: " << i << endl; + cerr << DiffT <10.*SMALL_EPS) { + cerr << "expmap(hat(x)) - exp(x)" << endl; + cerr << "Test case: " << i << endl; + cerr << exp_x <SMALL_EPS) { + cerr << "Transform vector" << endl; + cerr << "Test case: " << i << endl; + cerr << (res1-res2) <SMALL_EPS) { + cerr << "Lie Bracket Test" << endl; + cerr << "Test case: " << i << ", " < fastmul_res(fastmul_res_raw); + Eigen::Map group_j_constmap(group_vec_[j].data()); + fastmul_res = group_vec_[i]; + fastmul_res.fastMultiply(group_j_constmap); + Transformation diff = mul_resmat-fastmul_res.matrix(); + Scalar nrm = diff.norm(); + if (isnan(nrm) || nrm>SMALL_EPS) { + cerr << "Map & Multiply" << endl; + cerr << "Test case: " << i << "," << j << endl; + cerr << diff <SMALL_EPS) { + cerr << "Hat-vee Test" << endl; + cerr << "Test case: " << i << endl; + cerr << resDiff << endl; + cerr << endl; + passed = false; + } + } + return passed; + } + + + + void runAllTests() { + bool passed = adjointTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + passed = expLogTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + passed = expMapTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + passed = groupActionTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + passed = lieBracketTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + passed = mapAndMultTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + passed = veeHatTest(); + if (!passed) { + cerr << "failed!" << endl << endl; + exit(-1); + } + cerr << "passed." << endl << endl; + } + +private: + Matrix map(const Matrix & T, + const Matrix & p) { + return T.template topLeftCorner()*p + + T.template topRightCorner(); + } + + Matrix map(const Matrix & T, + const Matrix & p) { + return T*p; + } + + Scalar norm(const Scalar & v) { + return std::abs(v); + } + + Scalar norm(const Matrix & T) { + return T.norm(); + } + + std::vector group_vec_; + std::vector tangent_vec_; + std::vector point_vec_; +}; +} +#endif // TESTS_HPP diff --git a/thirdparty/libzip-1.1.1.tar.gz b/thirdparty/libzip-1.1.1.tar.gz new file mode 100644 index 0000000..e604128 Binary files /dev/null and b/thirdparty/libzip-1.1.1.tar.gz differ diff --git a/thirdparty/sse2neon b/thirdparty/sse2neon new file mode 160000 index 0000000..8372526 --- /dev/null +++ b/thirdparty/sse2neon @@ -0,0 +1 @@ +Subproject commit 83725268b2fd1855740f3dfda5aff8ebab357c6c