diff --git a/mola_demos/mola-cli-launchs/kitti360_just_replay.yaml b/mola_demos/mola-cli-launchs/kitti360_just_replay.yaml new file mode 100644 index 00000000..3029509f --- /dev/null +++ b/mola_demos/mola-cli-launchs/kitti360_just_replay.yaml @@ -0,0 +1,42 @@ +# ----------------------------------------------------------------------------- +# SLAM system definition for MOLA +# +# This file just plays the raw data of a sequence of the KITTI-360 dataset +# ----------------------------------------------------------------------------- + +modules: + # ===================== + # Kitti360Dataset + # ===================== + - type: mola::Kitti360Dataset + name: dataset_input + execution_rate: 20 # Hz + #export_to_rawlog: kitti_${KITTI360_SEQ}.rawlog + #verbosity_level: INFO + gui_preview_sensors: + - raw_sensor_label: lidar + decimation: 1 + win_pos: 5 70 400 400 + - raw_sensor_label: image_0 + decimation: 1 + win_pos: 5 370 600 200 + - raw_sensor_label: image_1 + decimation: 1 + win_pos: 5 500 600 200 + params: + base_dir: ${KITTI360_DATASET} + sequence: ${KITTI360_SEQ} + time_warp_scale: 1.0 + start_paused: ${MOLA_DATASET_START_PAUSED|false} + publish_lidar: true + publish_image_0: true + publish_image_1: true + publish_ground_truth: true + + # ===================== + # MolaViz + # ===================== + - name: viz + type: mola::MolaViz + #verbosity_level: DEBUG + params: ~ # none diff --git a/mola_demos/mola-cli-launchs/kitti_just_replay.yaml b/mola_demos/mola-cli-launchs/kitti_just_replay.yaml index 92b3b14d..b9ba6663 100644 --- a/mola_demos/mola-cli-launchs/kitti_just_replay.yaml +++ b/mola_demos/mola-cli-launchs/kitti_just_replay.yaml @@ -11,7 +11,7 @@ modules: - type: mola::KittiOdometryDataset name: dataset_input execution_rate: 20 # Hz - export_to_rawlog: kitti_${KITTI_SEQ}.rawlog + #export_to_rawlog: kitti_${KITTI_SEQ}.rawlog #verbosity_level: INFO gui_preview_sensors: - raw_sensor_label: lidar diff --git a/mola_input_kitti360_dataset/.clang-format b/mola_input_kitti360_dataset/.clang-format new file mode 100644 index 00000000..d1df9d28 --- /dev/null +++ b/mola_input_kitti360_dataset/.clang-format @@ -0,0 +1,83 @@ +Language: Cpp +BasedOnStyle: Google +# --- +#AccessModifierOffset: -4 +AlignAfterOpenBracket: AlwaysBreak # Values: Align, DontAlign, AlwaysBreak +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: true +#AlignEscapedNewlinesLeft: true +#AlignOperands: false +AlignTrailingComments: false # Should be off, causes many dummy problems!! +#AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: true +#AllowShortCaseLabelsOnASingleLine: false +#AllowShortFunctionsOnASingleLine: Empty +#AllowShortIfStatementsOnASingleLine: false +#AllowShortLoopsOnASingleLine: false +#AlwaysBreakAfterDefinitionReturnType: None +#AlwaysBreakAfterReturnType: None +#AlwaysBreakBeforeMultilineStrings: true +#AlwaysBreakTemplateDeclarations: true +#BinPackArguments: false +#BinPackParameters: false +#BraceWrapping: + #AfterClass: false + #AfterControlStatement: false + #AfterEnum: false + #AfterFunction: false + #AfterNamespace: false + #AfterObjCDeclaration: false + #AfterStruct: false + #AfterUnion: false + #BeforeCatch: false + #BeforeElse: true + #IndentBraces: false +#BreakBeforeBinaryOperators: None +BreakBeforeBraces: Allman +#BreakBeforeTernaryOperators: true +#BreakConstructorInitializersBeforeComma: false +ColumnLimit: 80 +#CommentPragmas: '' +#ConstructorInitializerAllOnOneLineOrOnePerLine: true +#ConstructorInitializerIndentWidth: 4 +#ContinuationIndentWidth: 4 +#Cpp11BracedListStyle: true +#DerivePointerAlignment: false +#DisableFormat: false +#ExperimentalAutoDetectBinPacking: false +##FixNamespaceComments: true # Not applicable in 3.8 +#ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +#IncludeCategories: + #- Regex: '.*' + #Priority: 1 +IndentCaseLabels: true +IndentWidth: 4 +IndentWrappedFunctionNames: true +#KeepEmptyLinesAtTheStartOfBlocks: true +#MacroBlockBegin: '' +#MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +#PenaltyBreakBeforeFirstCallParameter: 19 +#PenaltyBreakComment: 300 +#PenaltyBreakFirstLessLess: 120 +#PenaltyBreakString: 1000 +#PenaltyExcessCharacter: 1000000 +#PenaltyReturnTypeOnItsOwnLine: 200 +DerivePointerAlignment: false +#PointerAlignment: Left +ReflowComments: true # Should be true, otherwise clang-format doesn't touch comments +SortIncludes: true +#SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +#SpaceBeforeParens: ControlStatements +#SpaceInEmptyParentheses: false +#SpacesBeforeTrailingComments: 2 +#SpacesInAngles: false +#SpacesInContainerLiterals: true +#SpacesInCStyleCastParentheses: false +#SpacesInParentheses: false +#SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 4 +UseTab: Never # Available options are Never, Always, ForIndentation diff --git a/mola_input_kitti360_dataset/CHANGELOG.rst b/mola_input_kitti360_dataset/CHANGELOG.rst new file mode 100644 index 00000000..770383f7 --- /dev/null +++ b/mola_input_kitti360_dataset/CHANGELOG.rst @@ -0,0 +1,18 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mola_input_kitti_dataset +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.2 (2023-09-08) +------------------ +* Correct references to license +* Fix published ground truth axis of reference +* Fix wrong ground truth matrix indexing. +* Contributors: Jose Luis Blanco-Claraco + +0.2.1 (2023-09-02) +------------------ + +* Implement ground-truth interface for KITTI +* Update copyright date +* Update to new colcon ROS2 build system +* Contributors: Jose Luis Blanco-Claraco diff --git a/mola_input_kitti360_dataset/CMakeLists.txt b/mola_input_kitti360_dataset/CMakeLists.txt new file mode 100644 index 00000000..0217b3a4 --- /dev/null +++ b/mola_input_kitti360_dataset/CMakeLists.txt @@ -0,0 +1,40 @@ +# ------------------------------------------------------------------------------ +# A Modular Optimization framework for Localization and mApping +# (MOLA) +# +# Copyright (C) 2018-2024, Jose Luis Blanco-Claraco, contributors (AUTHORS.md) +# All rights reserved. +# Released under GNU GPL v3. See LICENSE file +# ------------------------------------------------------------------------------ + +# Minimum CMake vesion: limited by CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS +cmake_minimum_required(VERSION 3.4) + +# Tell CMake we'll use C++ for use in its tests/flags +project(mola_input_kitti360_dataset LANGUAGES CXX) + +# MOLA CMake scripts: "mola_xxx()" +find_package(mola_common REQUIRED) + +# find dependencies: +find_package(mrpt-math) # for MRPT Eigen utilities +find_package(mrpt-maps) + +find_mola_package(mola_kernel) + +# ----------------------- +# define lib: +file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) +file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) + +mola_add_library( + TARGET ${PROJECT_NAME} + SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS} + PUBLIC_LINK_LIBRARIES + mola::mola_kernel + PRIVATE_LINK_LIBRARIES + mrpt::maps + mrpt::math + CMAKE_DEPENDENCIES + mola_kernel +) diff --git a/mola_input_kitti360_dataset/LICENSE b/mola_input_kitti360_dataset/LICENSE new file mode 100644 index 00000000..f288702d --- /dev/null +++ b/mola_input_kitti360_dataset/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. + + + Copyright (C) + + 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: + + Copyright (C) + 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/mola_input_kitti360_dataset/README.md b/mola_input_kitti360_dataset/README.md new file mode 100644 index 00000000..052c747f --- /dev/null +++ b/mola_input_kitti360_dataset/README.md @@ -0,0 +1,11 @@ +# mola_input_kitti360_dataset +Offline RawDataSource from Kitti-360 SLAM datasets + +Provided MOLA modules: +* `Kitti360Dataset`, type RawDataSourceBase. + +## Build and install +Refer to the [root MOLA repository](https://github.com/MOLAorg/mola). + +## License +This package is released under the GNU GPL v3 license. Other options available upon request. diff --git a/mola_input_kitti360_dataset/include/mola_input_kitti360_dataset/Kitti360Dataset.h b/mola_input_kitti360_dataset/include/mola_input_kitti360_dataset/Kitti360Dataset.h new file mode 100644 index 00000000..5a77d7e8 --- /dev/null +++ b/mola_input_kitti360_dataset/include/mola_input_kitti360_dataset/Kitti360Dataset.h @@ -0,0 +1,237 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file Kitti360Dataset.h + * @brief RawDataSource from Kitti-360 dataset + * @author Jose Luis Blanco Claraco + * @date Feb 23, 2024 + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +// fwrd decls: +namespace mrpt::obs +{ +class CObservationPointCloud; +} + +namespace mola +{ +/** RawDataSource from [Kitti-360 + * datasets](https://www.cvlibs.net/datasets/kitti-360/). + * + * Each "sequence" directory contains these sensor streams: + * - `image_0` & `image_1`: Perspective cameras + * - `image_2` & `image_3`: Fish-eye cameras + * - `lidar`: Velodyne 3D LIDAR. Published as mrpt::obs::CObservationPointCloud + * with X,Y,Z,I channels. + * - Ground truth poses + * + * The sequence to load is determined by the `sequence` parameter (e.g. via + * config yaml file), from these possible values: + * + * \code + * sequence: 00|02|03|04|05|06|07|08|09|10|18|test_0|test_1|test_2|test_3 + * \endcode + * + * + * Expected contents under `base_dir` directory (the result of using the + * download scripts provided by the KITTI-360 web): + * + * \code + * KITTI-360/ + * ├── calibration + * │ ├── calib_cam_to_pose.txt + * │ ├── calib_cam_to_velo.txt + * │ ├── calib_sick_to_velo.txt + * │ ├── image_02.yaml + * │ ├── image_03.yaml + * │ └── perspective.txt + * │ + * ├── data_3d_raw + * │   ├ 2013_05_28_drive_00xx_sync + * │   │ ├── cam0_to_world.txt + * │   │ ├── poses.txt + * │   │ └── velodyne_points + * │   │    ├── timestamps.txt + * │   │   └── data + * │   │ ├── 0000000000.bin + * │   │ ├── 0000000001.bin + * ... ... ... + * ├── data_2d_raw + * │   ├── 2013_05_28_drive_00xx_sync + * │ │   ├── image_00 + * │ │   │   ├── data_rect + * │ │   │ │ ├ 0000000000.png + * │ │   │ │ ├ 0000000001.png + * │ │   │ │ ... + * │ │   │ └── timestamps.txt + * │ │   └── image_01 + * │ │   ├── data_rect + * │ │   │ ├ 0000000000.png + * │ │   │ ├ 0000000001.png + * │ │   │ ... + * │ │   └── timestamps.txt + * ... ... + * ├── data_poses/ + * │   ├── 2013_05_28_drive_00xx_sync + * │   │   ├── cam0_to_world.txt + * │   │   └── poses.txt + * ... ... + * └── data_3d_test_slam/ + *    ├── test_0 + *    │   └── 2013_05_28_drive_0008_sync + *    │   └── velodyne_points + *    │   ├── data + *    │   └── timestamps.txt + *    ├── test_1 + *    │   └── 2013_05_28_drive_0008_sync + *    │   └── velodyne_points + *    │   ├── data + *    │   └── timestamps.txt + *    ├── test_2 + *    │   └── 2013_05_28_drive_0004_sync + *    │   └── velodyne_points + *    │   ├── data + *    │   └── timestamps.txt + *    └── test_3 + *    └── 2013_05_28_drive_0002_sync + *    └── velodyne_points + *    ├── data + *    └── timestamps.txt + * \endcode + * + * - Example `base_dir`: `/mnt/storage/KITTI-360/` (normally read from + * environment variable KITTI360_DATASET in mola-cli launch files). + * - Sequences: `01`, `02`, etc. + * + * \ingroup mola_input_kitti360_dataset_grp + */ +class Kitti360Dataset : public RawDataSourceBase, + public OfflineDatasetSource, + public Dataset_UI +{ + DEFINE_MRPT_OBJECT(Kitti360Dataset, mola) + + public: + Kitti360Dataset(); + ~Kitti360Dataset() override = default; + + // See docs in base class + void initialize(const Yaml& cfg) override; + void spinOnce() override; + bool hasGroundTruthTrajectory() const override + { + return !groundTruthTrajectory_.empty(); + } + trajectory_t getGroundTruthTrajectory() const override + { + return groundTruthTrajectory_; + } + + /** Direct programmatic access to dataset observations. + * The type of the lidar observation can be either: + * - mrpt::obs::CObservationPointCloud (`clouds_as_organized_points_`=false) + * - mrpt::obs::CObservationRotatingScan + * (`clouds_as_organized_points_`=true) + */ + std::shared_ptr getPointCloud( + timestep_t step) const; + std::shared_ptr getImage( + const unsigned int cam_idx, timestep_t step) const; + + // See docs in base class: + size_t datasetSize() const override; + + mrpt::obs::CSensoryFrame::Ptr datasetGetObservations( + size_t timestep) const override; + + // Virtual interface of Dataset_UI (see docs in derived class) + size_t datasetUI_size() const override { return datasetSize(); } + size_t datasetUI_lastQueriedTimestep() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return last_used_tim_index_; + } + double datasetUI_playback_speed() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return time_warp_scale_; + } + void datasetUI_playback_speed(double speed) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + time_warp_scale_ = speed; + } + bool datasetUI_paused() const override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + return paused_; + } + void datasetUI_paused(bool paused) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + paused_ = paused; + } + void datasetUI_teleport(size_t timestep) override + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + teleport_here_ = timestep; + } + + private: + bool initialized_ = false; + std::string base_dir_; //!< base dir for "sequences/*". + std::string sequence_; //!< "00", "01", ... + timestep_t replay_next_tim_index_{0}; + bool publish_lidar_{true}; + bool publish_ground_truth_{true}; + std::array publish_image_{{true, true, true, true}}; + std::array cam_intrinsics_; + std::array cam_poses_; //!< wrt vehicle origin + mrpt::poses::CPose3D velodyne_pose_; //!< wrt vehicle origin + + std::optional last_play_wallclock_time_; + double last_dataset_time_ = 0; + + std::array, 4> lst_image_; + std::array lst_image_basedir_; + + std::vector lst_velodyne_; + std::string lst_velodyne_basedir_; + + mrpt::math::CMatrixDouble groundTruthPoses_; + trajectory_t groundTruthTrajectory_; + mutable std::map + read_ahead_lidar_obs_; + mutable std::map> + read_ahead_image_obs_; + + std::vector lstLidarTimestamps_; + double replay_time_{.0}; + + mutable timestep_t last_used_tim_index_ = 0; + bool paused_ = false; + double time_warp_scale_ = 1.0; + std::optional teleport_here_; + mutable std::mutex dataset_ui_mtx_; + + void load_img(const unsigned int cam_idx, const timestep_t step) const; + void load_lidar(timestep_t step) const; + + void autoUnloadOldEntries() const; +}; + +} // namespace mola diff --git a/mola_input_kitti360_dataset/package.xml b/mola_input_kitti360_dataset/package.xml new file mode 100644 index 00000000..d86327a7 --- /dev/null +++ b/mola_input_kitti360_dataset/package.xml @@ -0,0 +1,31 @@ + + + + + mola_input_kitti360_dataset + 0.2.2 + Offline RawDataSource from Kitti-360 datasets + + Jose-Luis Blanco-Claraco + Jose-Luis Blanco-Claraco + + https://github.com/MOLAorg/ + + GPLv3 + + mola_common + mola_kernel + mrpt2 + + doxygen + + + cmake + + cmake + + + + diff --git a/mola_input_kitti360_dataset/src/Kitti360Dataset.cpp b/mola_input_kitti360_dataset/src/Kitti360Dataset.cpp new file mode 100644 index 00000000..57461f0e --- /dev/null +++ b/mola_input_kitti360_dataset/src/Kitti360Dataset.cpp @@ -0,0 +1,702 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file Kitti360Dataset.cpp + * @brief RawDataSource from Kitti-360 dataset + * @author Jose Luis Blanco Claraco + * @date Feb 23, 2024 + */ + +/** \defgroup mola_input_kitti360_dataset_grp mola-input-kitti360-dataset. + * RawDataSource from Kitti-360 datasets. + * + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include //ASSERT_DIRECTORY_EXISTS_() + +#include +#include + +using namespace mola; + +// arguments: class_name, parent_class, class namespace +IMPLEMENTS_MRPT_OBJECT(Kitti360Dataset, RawDataSourceBase, mola) + +MRPT_INITIALIZER(do_register_Kitti360Dataset) +{ + MOLA_REGISTER_MODULE(Kitti360Dataset); +} + +Kitti360Dataset::Kitti360Dataset() = default; + +namespace +{ +void build_list_files( + const std::string& dir, const std::string& file_extension, + std::vector& out_lst) +{ + out_lst.clear(); + if (!mrpt::system::directoryExists(dir)) return; + + using direxpl = mrpt::system::CDirectoryExplorer; + direxpl::TFileInfoList lstFiles; + direxpl::explore(dir, FILE_ATTRIB_ARCHIVE, lstFiles); + direxpl::sortByName(lstFiles); + direxpl::filterByExtension(lstFiles, file_extension); + out_lst.resize(lstFiles.size()); + std::transform( + lstFiles.begin(), lstFiles.end(), out_lst.begin(), + [](auto& fil) { return fil.name; }); +} + +template +void parse_calib_line(const std::string& line, MATRIX& M) +{ + MRPT_TRY_START + + std::istringstream ss(line); + for (Eigen::Index r = 0; r < M.rows(); r++) + for (Eigen::Index c = 0; c < M.cols(); c++) + { + if (!(ss >> M(r, c))) + { + THROW_EXCEPTION_FMT( + "Error parsing calib line: `%s`", line.c_str()); + } + } + MRPT_TRY_END +} + +// format: +// 2013-05-28 08:46:02.904295089 +// 2013-05-28 08:46:03.008671417 +// ... +std::vector parseKitti360Timestamps(const std::string& fil) +{ + std::vector ts; + + mrpt::io::CTextFileLinesParser parser(fil); + + std::string line; + while (parser.getNextLine(line)) + { + ASSERT_(line.size() > 25); + unsigned int year, month, day, hour, min; + double sec; + + int nParsedFields = ::sscanf( + line.c_str(), "%04u-%02u-%02u %02u:%02u:%lf", &year, &month, &day, + &hour, &min, &sec); + ASSERT_EQUAL_(nParsedFields, 6); + + mrpt::system::TTimeParts tp; + tp.year = year; + tp.month = month; + tp.day = day; + tp.hour = hour; + tp.minute = min; + tp.second = sec; + + const auto t = mrpt::system::buildTimestampFromParts(tp); + + ts.push_back(mrpt::Clock::toDouble(t)); + } + + return ts; +} + +} // namespace + +void Kitti360Dataset::initialize(const Yaml& c) +{ + using namespace std::string_literals; + + MRPT_START + ProfilerEntry tle(profiler_, "initialize"); + + MRPT_LOG_DEBUG_STREAM("Initializing with these params:\n" << c); + + // Mandatory parameters: + ENSURE_YAML_ENTRY_EXISTS(c, "params"); + auto cfg = c["params"]; + + YAML_LOAD_MEMBER_REQ(base_dir, std::string); + YAML_LOAD_MEMBER_REQ(sequence, std::string); + + const std::string regExpr = + "(00|02|03|04|05|06|07|08|09|10|18|test_0|test_1|test_2|test_3)"; + const auto seqRegex = std::regex(regExpr); + + const std::string regExprTests = "(test_0|test_1|test_2|test_3)"; + const auto seqRegexTests = std::regex(regExprTests); + const bool seqIsTest = std::regex_search(sequence_, seqRegexTests); + + if (!std::regex_search(sequence_, seqRegex)) + { + THROW_EXCEPTION_FMT( + "Error: sequence is '%s' but must be one of '%s'", + sequence_.c_str(), regExpr.c_str()); + } + + // Build path to lidar and images (see .h for expected tree layout) + + // the "velodyne_points" dir, if it exists + std::string lidar_dir; + // the image_?? dirs, if they exists + std::array images_dirs; + std::string gt_file; + + if (seqIsTest) + { + const std::map syncDirs = { + {'0', "2013_05_28_drive_0008_sync"}, + {'1', "2013_05_28_drive_0008_sync"}, + {'2', "2013_05_28_drive_0004_sync"}, + {'3', "2013_05_28_drive_0002_sync"}, + }; + const std::string syncDir = syncDirs.at(sequence_.back()); + + lidar_dir = mrpt::system::pathJoin( + {base_dir_, "data_3d_test_slam", sequence_, syncDir, + "velodyne_points"}); + MRPT_LOG_DEBUG_STREAM("Expected LIDAR dir: " << lidar_dir); + + for (int i = 0; i < 4; i++) + images_dirs[i] = mrpt::system::pathJoin( + {base_dir_, "data_2d_test_slam", sequence_, syncDir, + mrpt::format("image_%02i", i)}); + } + else + { + const std::string syncDir = + mrpt::format("2013_05_28_drive_00%s_sync", sequence_.c_str()); + + lidar_dir = mrpt::system::pathJoin( + {base_dir_, "data_3d_raw", syncDir, "velodyne_points"}); + MRPT_LOG_DEBUG_STREAM("Expected LIDAR dir: " << lidar_dir); + + for (int i = 0; i < 4; i++) + { + images_dirs[i] = mrpt::system::pathJoin( + {base_dir_, "data_2d_raw", syncDir, + mrpt::format("image_%02i", i)}); + + MRPT_LOG_DEBUG_STREAM("Expected images dir: " << images_dirs[i]); + } + + gt_file = mrpt::system::pathJoin( + {base_dir_, "data_3d_raw", syncDir, "poses.txt"}); + MRPT_LOG_DEBUG_STREAM("Expected GT file: " << gt_file); + } + + // clear if does not exist: + if (!mrpt::system::directoryExists(lidar_dir)) lidar_dir.clear(); + for (auto& imDir : images_dirs) + if (!mrpt::system::directoryExists(imDir)) imDir.clear(); + if (!mrpt::system::fileExists(gt_file)) gt_file.clear(); + + // Optional params with default values: + YAML_LOAD_MEMBER_OPT(time_warp_scale, double); + paused_ = cfg.getOrDefault("start_paused", paused_); + + YAML_LOAD_MEMBER_OPT(publish_lidar, bool); + YAML_LOAD_MEMBER_OPT(publish_ground_truth, bool); + + for (unsigned int i = 0; i < 4; i++) + publish_image_[i] = cfg.getOrDefault( + mrpt::format("publish_image_%u", i), publish_image_[i]); + + // Make list of all existing files and preload everything we may need later + // to quickly replay the dataset in realtime: + MRPT_LOG_INFO_STREAM("Loading kitti-360 dataset sequence: " << sequence_); + + // Load LIDAR timestamps: + auto filLidarTimes = mrpt::system::pathJoin({lidar_dir, "timestamps.txt"}); + ASSERT_FILE_EXISTS_(filLidarTimes); // TODO: make lidar optional? + + lstLidarTimestamps_ = parseKitti360Timestamps(filLidarTimes); + + const auto N = lstLidarTimestamps_.size(); + + lst_velodyne_basedir_ = mrpt::system::pathJoin({lidar_dir, "data"}); + build_list_files(lst_velodyne_basedir_, "bin", lst_velodyne_); + if (!lst_velodyne_.empty()) + ASSERTMSG_(lst_velodyne_.size() == N, "Velodyne: invalid file count"); + + MRPT_LOG_INFO_STREAM( + "Velodyne pointclouds: " + << (!lst_velodyne_.empty() + ? "Found ("s + std::to_string(lst_velodyne_.size()) + ")"s + : "Not found")); + + for (unsigned int i = 0; i < 4; i++) + { + lst_image_basedir_[i] = + mrpt::system::pathJoin({images_dirs[i], "data_rect"}); + build_list_files(lst_image_basedir_[i], "png", lst_image_[i]); + if (!lst_image_[i].empty()) + ASSERTMSG_( + lst_image_[i].size() == N, + mrpt::format("image_%u: invalid file count", i)); + + MRPT_LOG_INFO_STREAM( + "Camera channel `image_" + << i << "`: " + << (!lst_image_[i].empty() + ? "Found ("s + std::to_string(lst_image_[i].size()) + ")"s + : "Not found")); + + // Override user choice if not possible to publish images: + if (lst_image_[i].empty()) publish_image_[i] = false; + } + + // Load sensors calibration: + const std::string filCalibCamToPose = lidar_dir = mrpt::system::pathJoin( + {base_dir_, "calibration", "calib_cam_to_pose.txt"}); + const std::string filCalibCamToVelo = lidar_dir = mrpt::system::pathJoin( + {base_dir_, "calibration", "calib_cam_to_velo.txt"}); + const std::string filCalibCameras00_01 = lidar_dir = + mrpt::system::pathJoin({base_dir_, "calibration", "perspective.txt"}); + + ASSERT_FILE_EXISTS_(filCalibCamToPose); + ASSERT_FILE_EXISTS_(filCalibCamToVelo); + ASSERT_FILE_EXISTS_(filCalibCameras00_01); + + // Load projection matrices: + const auto calib = mrpt::containers::yaml::FromFile(filCalibCameras00_01); + ENSURE_YAML_ENTRY_EXISTS(calib, "P_rect_00"); + ENSURE_YAML_ENTRY_EXISTS(calib, "R_rect_00"); + ENSURE_YAML_ENTRY_EXISTS(calib, "P_rect_01"); + ENSURE_YAML_ENTRY_EXISTS(calib, "R_rect_01"); + + std::array, 2> P01; // proj matrix (3x4) + std::array, 2> S01; // resolution (1x2) + + // (T,R): not needed for rectified images + // std::array, 2> T01; // translation (3x1) + // std::array, 2> R01; // rotation (3x3) + + for (int i = 0; i < 2; i++) + { + parse_calib_line( + calib[mrpt::format("P_rect_%02i", i)].as(), P01[i]); + parse_calib_line( + calib[mrpt::format("S_rect_%02i", i)].as(), S01[i]); + + // parse_calib_line(calib[mrpt::format("T%02i", i)].as(), + // T01[i]); + // parse_calib_line(calib[mrpt::format("R_rect_%02i", + // i)].as(), R01[i]); + } + + // IMU/GPS is the reference pose, but it's flipped (+Z downwards): + using namespace mrpt::literals; // _deg + const auto T_veh_gps = + mrpt::poses::CPose3D::FromYawPitchRoll(0.0_deg, 0.0_deg, 180.0_deg); + + // Camera intrinsics: + for (unsigned int i = 0; i < 2; i++) + { + const double fx = P01[i](0, 0), fy = P01[i](1, 1), cx = P01[i](0, 2), + cy = P01[i](1, 2); + cam_intrinsics_[i].setIntrinsicParamsFromValues(fx, fy, cx, cy); + + // Resolution: + cam_intrinsics_[i].ncols = S01[i][0]; + cam_intrinsics_[i].nrows = S01[i][1]; + } + + // Velodyne pose wrt vehicle = + const auto yamlCamToPose = + mrpt::containers::yaml::FromFile(filCalibCamToPose); + for (int i = 0; i < 4; i++) + { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + auto Tr = Eigen::Matrix(); + parse_calib_line( + yamlCamToPose[mrpt::format("image_%02i", i)].as(), Tr); + T.block<3, 4>(0, 0) = Tr; + + cam_poses_[i] = mrpt::poses::CPose3D::FromHomogeneousMatrix(T); + cam_poses_[i] = T_veh_gps + cam_poses_[i]; + + MRPT_LOG_DEBUG_STREAM("Camera 0" << i << " pose: " << cam_poses_[i]); + } + + // velodyne -> image_00: + { + auto Tr = Eigen::Matrix(); + parse_calib_line(mrpt::io::file_get_contents(filCalibCamToVelo), Tr); + + Eigen::Matrix4d T_cam00_to_velo = Eigen::Matrix4d::Identity(); + T_cam00_to_velo.block<3, 4>(0, 0) = Tr; + + const auto cam00_to_velo = + mrpt::poses::CPose3D::FromHomogeneousMatrix(T_cam00_to_velo); + + // T_velo = cam00 (+) ((-) cam00_to_velo) + velodyne_pose_ = cam_poses_[0] + (-cam00_to_velo); + + MRPT_LOG_DEBUG_STREAM("cam00_to_velo : " << cam00_to_velo); + MRPT_LOG_DEBUG_STREAM("cam00_to_velo^-1 : " << (-cam00_to_velo)); + MRPT_LOG_DEBUG_STREAM("Velodyne pose: " << velodyne_pose_); + } + + // Load ground truth poses, if available: +#if 0 + const auto gtFile = base_dir_ + "/poses/"s + sequence_ + ".txt"s; + if (mrpt::system::fileExists(gtFile)) + { + groundTruthPoses_.loadFromTextFile(gtFile); + + ASSERT_EQUAL_(groundTruthPoses_.cols(), 12U); + ASSERT_EQUAL_( + static_cast(groundTruthPoses_.rows()), + lstLidarTimestamps_.size()); + + // Convert into the format expected by MOLA generic interface: + mrpt::math::CMatrixDouble44 m = mrpt::math::CMatrixDouble44::Identity(); + const auto cam0PoseInv = -mrpt::poses::CPose3D(cam_poses_.at(0)); + + using namespace mrpt::literals; // _deg + const auto axisChange = mrpt::poses::CPose3D::FromYawPitchRoll( + -90.0_deg, 0.0_deg, -90.0_deg); + + for (size_t i = 0; i < lstLidarTimestamps_.size(); i++) + { + for (int row = 0, ij_idx = 0; row < 3; row++) + for (int col = 0; col < 4; col++, ij_idx++) + m(row, col) = groundTruthPoses_(i, ij_idx); + + // ground truth is for cam0: + const auto gtCam0Pose = + mrpt::poses::CPose3D::FromHomogeneousMatrix(m); + + // Convert it to the vehicle frame, for consistency with all MOLA + // datasets: + const auto gtPose = axisChange + gtCam0Pose + cam0PoseInv; + + groundTruthTrajectory_.insert( + mrpt::Clock::fromDouble(lstLidarTimestamps_.at(i)), gtPose); + } + + MRPT_LOG_INFO("Ground truth poses: Found"); + } + else + { + MRPT_LOG_WARN_STREAM( + "Ground truth poses: not found. Expected file: " << gtFile); + } +#endif + + initialized_ = true; + + MRPT_END +} // end initialize() + +void Kitti360Dataset::spinOnce() +{ + MRPT_START + + ASSERT_(initialized_); + + ProfilerEntry tleg(profiler_, "spinOnce"); + + const auto tNow = mrpt::Clock::now(); + + // Starting time: + if (!last_play_wallclock_time_) last_play_wallclock_time_ = tNow; + + // get current replay time: + auto lckUIVars = mrpt::lockHelper(dataset_ui_mtx_); + const double time_warp_scale = time_warp_scale_; + const bool paused = paused_; + const auto teleport_here = teleport_here_; + teleport_here_.reset(); + lckUIVars.unlock(); + + double dt = mrpt::system::timeDifference(*last_play_wallclock_time_, tNow) * + time_warp_scale; + last_play_wallclock_time_ = tNow; + + // override by an special teleport order? + if (teleport_here.has_value() && + *teleport_here < lstLidarTimestamps_.size()) + { + replay_next_tim_index_ = *teleport_here; + last_dataset_time_ = lstLidarTimestamps_[replay_next_tim_index_]; + } + else + { + if (paused) return; + // move forward replayed dataset time: + last_dataset_time_ += dt; + } + + if (replay_next_tim_index_ >= lstLidarTimestamps_.size()) + { + onDatasetPlaybackEnds(); // notify base class + + MRPT_LOG_THROTTLE_INFO( + 10.0, + "End of dataset reached! Nothing else to publish (CTRL+C to quit)"); + return; + } + else if (!lstLidarTimestamps_.empty()) + { + MRPT_LOG_THROTTLE_INFO_FMT( + 5.0, "Dataset replay progress: %lu / %lu (%4.02f%%)", + static_cast(replay_next_tim_index_), + static_cast(lstLidarTimestamps_.size()), + (100.0 * replay_next_tim_index_) / (lstLidarTimestamps_.size())); + } + + const double t0 = lstLidarTimestamps_.front(); + + // We have to publish all observations until "t": + while (replay_next_tim_index_ < lstLidarTimestamps_.size() && + last_dataset_time_ >= + (lstLidarTimestamps_[replay_next_tim_index_] - t0)) + { + MRPT_LOG_DEBUG_STREAM( + "Sending observations for replay time: " + << mrpt::system::formatTimeInterval(last_dataset_time_)); + + // Save one single timestamp for all observations, since they are in + // theory shynchronized in the Kitti datasets: + const auto obs_tim = mrpt::Clock::fromDouble( + lstLidarTimestamps_[replay_next_tim_index_]); + + if (publish_lidar_) + { + ProfilerEntry tle(profiler_, "spinOnce.publishLidar"); + load_lidar(replay_next_tim_index_); + auto o = read_ahead_lidar_obs_[replay_next_tim_index_]; + // o->timestamp = obs_tim; // already done in load_lidar() + this->sendObservationsToFrontEnds(o); + } + + for (unsigned int i = 0; i < 4; i++) + { + if (!publish_image_[i]) continue; + ProfilerEntry tle(profiler_, "spinOnce.publishImage"); + load_img(i, replay_next_tim_index_); + auto o = read_ahead_image_obs_[replay_next_tim_index_][i]; + // o->timestamp = obs_tim; // already done in load_img() + this->sendObservationsToFrontEnds(o); + } + + if (publish_ground_truth_ && + replay_next_tim_index_ < groundTruthTrajectory_.size()) + { + // Get GT pose: it's already stored and correctly transformed + // into groundTruthTrajectory_: + auto it = groundTruthTrajectory_.begin(); + std::advance(it, replay_next_tim_index_); + + // Publish as robot pose observation: + auto o = mrpt::obs::CObservationRobotPose::Create(); + o->sensorLabel = "ground_truth"; + o->pose.mean = mrpt::poses::CPose3D(it->second); + // o->pose.cov? don't use + o->timestamp = obs_tim; + + this->sendObservationsToFrontEnds(o); + } + + // Free memory in read-ahead buffers: + read_ahead_lidar_obs_.erase(replay_next_tim_index_); + read_ahead_image_obs_.erase(replay_next_tim_index_); + + replay_next_tim_index_++; + } + + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + last_used_tim_index_ = + replay_next_tim_index_ > 0 ? replay_next_tim_index_ - 1 : 0; + } + + // Read ahead to save delays in the next iteration: + if (replay_next_tim_index_ < lstLidarTimestamps_.size()) + { + ProfilerEntry tle(profiler_, "spinOnce.read_ahead"); + if (0 == read_ahead_image_obs_.count(replay_next_tim_index_)) + { + for (unsigned int i = 0; i < 4; i++) + { + if (!publish_image_[i]) continue; + load_img(i, replay_next_tim_index_); + } + } + if (0 == read_ahead_lidar_obs_.count(replay_next_tim_index_)) + { + if (publish_lidar_) load_lidar(replay_next_tim_index_); + } + } + + MRPT_END +} + +void Kitti360Dataset::load_img( + const unsigned int cam_idx, const timestep_t step) const +{ + MRPT_START + + // unload() very old observations. + autoUnloadOldEntries(); + + // Already loaded? + if (read_ahead_image_obs_[step][cam_idx]) return; + + ProfilerEntry tleg(profiler_, "load_img"); + + auto obs = mrpt::obs::CObservationImage::Create(); + obs->sensorLabel = std::string("image_") + std::to_string(cam_idx); + + ASSERTMSG_( + lst_image_[cam_idx].size() > replay_next_tim_index_, + mrpt::format("Missing image files for image_%u", cam_idx)); + const auto f = mrpt::system::pathJoin( + {lst_image_basedir_[cam_idx], lst_image_[cam_idx][step]}); + + obs->image.setExternalStorage(f); + + // Use this thread time to load images from disk, instead of + // delegating it to the first use of the image in the consumer: + obs->image.forceLoad(); + + obs->cameraParams = cam_intrinsics_[cam_idx]; + obs->setSensorPose(mrpt::poses::CPose3D(cam_poses_[cam_idx])); + obs->timestamp = mrpt::Clock::fromDouble(lstLidarTimestamps_.at(step)); + + auto o = mrpt::ptr_cast::from(obs); + read_ahead_image_obs_[step][cam_idx] = std::move(o); + + MRPT_END +} + +void Kitti360Dataset::load_lidar(timestep_t step) const +{ + MRPT_START + + // unload() very old observations. + autoUnloadOldEntries(); + + // Already loaded? + if (read_ahead_lidar_obs_[step]) return; + + ProfilerEntry tleg(profiler_, "load_lidar"); + + // Load velodyne pointcloud: + const auto f = + mrpt::system::pathJoin({lst_velodyne_basedir_, lst_velodyne_[step]}); + + auto obs = mrpt::obs::CObservationPointCloud::Create(); + obs->sensorLabel = "lidar"; + obs->sensorPose = velodyne_pose_; + obs->setAsExternalStorage( + f, + mrpt::obs::CObservationPointCloud::ExternalStorageFormat::KittiBinFile); + obs->load(); // force loading now from disk + ASSERTMSG_( + obs->pointcloud, + mrpt::format("Error loading kitti scan file: '%s'", f.c_str())); + + // Pose: velodyne is at the origin of the vehicle coordinates in + // Kitti datasets. + obs->sensorPose = mrpt::poses::CPose3D(); + obs->timestamp = mrpt::Clock::fromDouble(lstLidarTimestamps_.at(step)); + +#if 0 // Export clouds to txt for debugging externally (e.g. python, matlab) + obs->pointcloud->save3D_to_text_file( + mrpt::format("kitti_%s_%06zu.txt", sequence_.c_str(), step)); +#endif + + mrpt::obs::CObservation::Ptr o; + + o = std::dynamic_pointer_cast(obs); + + // Store in the output queue: + read_ahead_lidar_obs_[step] = std::move(o); + + MRPT_END +} + +mrpt::obs::CObservation::Ptr Kitti360Dataset::getPointCloud( + timestep_t step) const +{ + ASSERT_(initialized_); + ASSERT_LT_(step, lstLidarTimestamps_.size()); + + load_lidar(step); + auto o = read_ahead_lidar_obs_.at(step); + return o; +} + +std::shared_ptr Kitti360Dataset::getImage( + const unsigned int cam_idx, timestep_t step) const +{ + ASSERT_(initialized_); + ASSERT_LT_(step, lstLidarTimestamps_.size()); + + load_img(cam_idx, step); + auto o = std::dynamic_pointer_cast( + read_ahead_image_obs_.at(step).at(cam_idx)); + ASSERT_(o); + return o; +} + +size_t Kitti360Dataset::datasetSize() const +{ + ASSERT_(initialized_); + return lstLidarTimestamps_.size(); +} + +mrpt::obs::CSensoryFrame::Ptr Kitti360Dataset::datasetGetObservations( + size_t timestep) const +{ + { + auto lck = mrpt::lockHelper(dataset_ui_mtx_); + last_used_tim_index_ = timestep; + } + + auto sf = mrpt::obs::CSensoryFrame::Create(); + + for (size_t i = 0; i < publish_image_.size(); i++) + { + if (!publish_image_[i]) continue; + sf->insert(getImage(i, timestep)); + } + + if (publish_lidar_) { sf->insert(getPointCloud(timestep)); } + + return sf; +} + +constexpr size_t MAX_UNLOAD_LEN = 250; + +void Kitti360Dataset::autoUnloadOldEntries() const +{ + while (read_ahead_lidar_obs_.size() > MAX_UNLOAD_LEN) + read_ahead_lidar_obs_.erase(read_ahead_lidar_obs_.begin()); + + while (read_ahead_image_obs_.size() > MAX_UNLOAD_LEN) + read_ahead_image_obs_.erase(read_ahead_image_obs_.begin()); +}