diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..339cee9 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..1ef27e7 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/rpg_svo.iml b/.idea/rpg_svo.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/.idea/rpg_svo.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..94a25f7 --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/CPU_version/Forster2014ICRA.pdf b/CPU_version/Forster2014ICRA.pdf new file mode 100755 index 0000000..9567178 Binary files /dev/null and b/CPU_version/Forster2014ICRA.pdf differ diff --git a/CPU_version/LICENCE b/CPU_version/LICENCE new file mode 100755 index 0000000..70fc6da --- /dev/null +++ b/CPU_version/LICENCE @@ -0,0 +1,675 @@ +GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. {http://fsf.org/} + 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 {http://www.gnu.org/licenses/}. + +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: + + PTAM-GPL Copyright (C) 2013 Oxford-PTAM + 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 +{http://www.gnu.org/licenses/}. + + 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 +{http://www.gnu.org/philosophy/why-not-lgpl.html}. + diff --git a/CPU_version/README.md b/CPU_version/README.md new file mode 100755 index 0000000..8387b0e --- /dev/null +++ b/CPU_version/README.md @@ -0,0 +1,64 @@ +SVO +=== + +This code implements a semi-direct monocular visual odometry pipeline which has been integrated with GTSAM to infused IMU as well. + + +Paper svo: http://rpg.ifi.uzh.ch/docs/ICRA14_Forster.pdf +Paper Svo+GTSAM: https://arxiv.org/abs/1512.02363 + + +#### Disclaimer + +SVO has been tested under ROS Groovy, Hydro and Indigo with Ubuntu 12.04, 13.04 and 14.04. This is research code, any fitness for a particular purpose is disclaimed. + + +#### Licence + +The source code is released under a GPLv3 licence. A closed-source professional edition is available for commercial purposes. In this case, please contact the authors for further info. + + +#### Citing + +If you use SVO in an academic context, please cite the following publication: + + @inproceedings{Forster2014ICRA, + author = {Forster, Christian and Pizzoli, Matia and Scaramuzza, Davide}, + title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry}, + booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, + year = {2014} + } + + +#### Documentation + +The API is documented here: http://uzh-rpg.github.io/rpg_svo/doc/ +The GTSAM documentation https://gtsam.org/tutorials/ + +#### Instructions + +First install gtsam library => https://github.com/borglab/gtsam + +Then follow the SVO instalation + +See the Wiki for more instructions. https://github.com/uzh-rpg/rpg_svo/wiki and https://gtsam.org/get_started/ + +#### Contributing + +You are very welcome to contribute to SVO by opening a pull request via Github. +I try to follow the ROS C++ style guide http://wiki.ros.org/CppStyleGuide + +## Setting + +You can edit the camera position in the launch file as (The IMU frame will rotate to align with camera frame based on the follwoing Eular angles): +CPU_version/vio_svo/launch/Px30.launch + + + + + + +You can also edit the algorithm parameters in the follwoing file, please look at the config file to underestand the meaning of the parameters (CPU_version/svo/include/svo/config.h) + +CPU_version/vio_svo/param/vo_fast.yaml + diff --git a/CPU_version/__init__.py b/CPU_version/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/CPU_version/svo/.idea/.gitignore b/CPU_version/svo/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/CPU_version/svo/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/CPU_version/svo/.idea/misc.xml b/CPU_version/svo/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/CPU_version/svo/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/CPU_version/svo/.idea/modules.xml b/CPU_version/svo/.idea/modules.xml new file mode 100644 index 0000000..4c8cf2b --- /dev/null +++ b/CPU_version/svo/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/CPU_version/svo/.idea/svo.iml b/CPU_version/svo/.idea/svo.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/CPU_version/svo/.idea/svo.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/CPU_version/svo/.idea/vcs.xml b/CPU_version/svo/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/CPU_version/svo/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/CPU_version/svo/CMakeLists.txt b/CPU_version/svo/CMakeLists.txt new file mode 100644 index 0000000..c3ace90 --- /dev/null +++ b/CPU_version/svo/CMakeLists.txt @@ -0,0 +1,156 @@ +################################################################################ +# user build settings + +SET(TRACE TRUE) +SET(HAVE_G2O FALSE) +SET(USE_ROS TRUE) # Set FALSE if you want to build this package without Catkin +SET(DEBUG_OUTPUT FALSE) # Only relevant if build without ROS + +################################################################################ + +SET(PROJECT_NAME svo) +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_VERBOSE_MAKEFILE OFF) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") +set(CMAKE_C_STANDARD 14) +set(CMAKE_CXX_STANDARD 14) + +# Set definitions +IF(TRACE) + ADD_DEFINITIONS(-DSVO_TRACE) +ENDIF() +IF(HAVE_G2O) + ADD_DEFINITIONS(-DUSE_BUNDLE_ADJUSTMENT) +ENDIF() +IF(USE_ROS) + ADD_DEFINITIONS(-DSVO_USE_ROS) +ENDIF() +IF(DEBUG_OUTPUT) + ADD_DEFINITIONS(-DSVO_DEBUG_OUTPUT) +ENDIF() + +# Set build flags, set ARM_ARCHITECTURE environment variable on Odroid +# Set build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +ENDIF() +IF(CMAKE_COMPILER_IS_GNUCC) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ENDIF() +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Add plain cmake packages +FIND_PACKAGE(OpenCV 3 REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(fast REQUIRED) +FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system) +FIND_PACKAGE(GTSAM REQUIRED) + +set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}") +IF(USE_ROS) + FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros) + catkin_package( + DEPENDS Eigen OpenCV Sophus Boost fast GTSAM + CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros + INCLUDE_DIRS include + LIBRARIES svo + ) +ELSE() + FIND_PACKAGE(vikit_common REQUIRED) + SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +ENDIF() + +# Include dirs +INCLUDE_DIRECTORIES( + include + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${fast_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} +) + +# Set link libraries +LIST(APPEND LINK_LIBS + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + ${Boost_LIBRARIES} + ${fast_LIBRARIES} +) +IF(USE_ROS) + INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS}) + LIST(APPEND LINK_LIBS ${catkin_LIBRARIES}) +ELSE() + INCLUDE_DIRECTORIES(${vikit_common_INCLUDE_DIRS}) + LIST(APPEND LINK_LIBS ${vikit_common_LIBRARIES}) +ENDIF() + +# Set sourcefiles +LIST(APPEND SOURCEFILES + src/frame_handler_mono.cpp + src/frame_handler_base.cpp + src/frame.cpp + src/point.cpp + src/map.cpp + src/pose_optimizer.cpp + src/initialization.cpp + src/matcher.cpp + src/reprojector.cpp + src/feature_alignment.cpp + src/feature_detection.cpp + src/depth_filter.cpp + src/config.cpp + src/sparse_img_align.cpp src/imu_integration.cpp) + +# Add g2o if available +IF(HAVE_G2O) + FIND_PACKAGE(G2O REQUIRED) + INCLUDE_DIRECTORIES( + ${G2O_INCLUDE_DIR} + /usr/include/suitesparse # for cholmod + ) + LIST(APPEND LINK_LIBS + ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE} + ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse + ) + LIST(APPEND SOURCEFILES src/bundle_adjustment.cpp) +ENDIF(HAVE_G2O) + +# Create svo library +ADD_LIBRARY(svo SHARED ${SOURCEFILES}) +TARGET_LINK_LIBRARIES(svo ${LINK_LIBS} gtsam) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) + +################################################################################ +# TESTS + +#ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp) +#TARGET_LINK_LIBRARIES(test_feature_align svo) + +#ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp) +#TARGET_LINK_LIBRARIES(test_pipeline svo) + +#ADD_EXECUTABLE(test_matcher test/test_matcher.cpp) +#TARGET_LINK_LIBRARIES(test_matcher svo) + +#ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp) +#TARGET_LINK_LIBRARIES(test_feature_detection svo) + +#ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp) +#TARGET_LINK_LIBRARIES(test_depth_filter svo) + +#ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp) +#TARGET_LINK_LIBRARIES(test_sparse_img_align svo) + +#ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp) +#TARGET_LINK_LIBRARIES(test_pose_optimizer svo) diff --git a/CPU_version/svo/CMakeModules/FindEigen.cmake b/CPU_version/svo/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/CPU_version/svo/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# 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. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/CPU_version/svo/CMakeModules/FindG2O.cmake b/CPU_version/svo/CMakeModules/FindG2O.cmake new file mode 100644 index 0000000..655600f --- /dev/null +++ b/CPU_version/svo/CMakeModules/FindG2O.cmake @@ -0,0 +1,113 @@ +# Find the header files + +FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h + ${G2O_ROOT}/include + $ENV{G2O_ROOT}/include + $ENV{G2O_ROOT} + /usr/local/include + /usr/include + /opt/local/include + /sw/local/include + /sw/include + NO_DEFAULT_PATH + ) + +# Macro to unify finding both the debug and release versions of the +# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY +# macro. + +MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ${G2O_ROOT}/lib/Debug + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Debug + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ${G2O_ROOT}/lib/Release + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Release + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + IF(NOT ${MYLIBRARY}_DEBUG) + IF(MYLIBRARY) + SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) + ENDIF(MYLIBRARY) + ENDIF( NOT ${MYLIBRARY}_DEBUG) + +ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) + +# Find the core elements +FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) +FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) + +# Find the CLI library +FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) + +# Find the pluggable solvers +FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) +FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) +FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) +FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) +FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) +FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) + +# Find the predefined types +FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) +FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) +FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) +FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) + +# G2O solvers declared found if we found at least one solver +SET(G2O_SOLVERS_FOUND "NO") +IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + SET(G2O_SOLVERS_FOUND "YES") +ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + +# G2O itself declared found if we found the core libraries and at least one solver +SET(G2O_FOUND "NO") +IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) + SET(G2O_FOUND "YES") +ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) diff --git a/CPU_version/svo/Doxyfile b/CPU_version/svo/Doxyfile new file mode 100644 index 0000000..b1b4256 --- /dev/null +++ b/CPU_version/svo/Doxyfile @@ -0,0 +1,1782 @@ +# Doxyfile 1.7.6.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" "). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "svo" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = "Semi-Direct Visual Odometry" + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = "doc" + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = YES + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. +# This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = "src", "include/svo" + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = g2o +EXCLUDE_SYMBOLS += vk + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. +# If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. +# Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. +# The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. +# Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# style sheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = NO + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 4 + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = NO + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the +# mathjax.org site, so you can quickly see the result without installing +# MathJax, but it is strongly recommended to install a local copy of MathJax +# before deployment. + +MATHJAX_RELPATH = http://www.mathjax.org/mathjax + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvantages are that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = YES + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. +# This is useful +# if you want to understand what is going on. +# On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = YES + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES diff --git a/CPU_version/svo/doc/.gitignore b/CPU_version/svo/doc/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/CPU_version/svo/doc/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/CPU_version/svo/doc/notation.ipe b/CPU_version/svo/doc/notation.ipe new file mode 100644 index 0000000..f8c1efe --- /dev/null +++ b/CPU_version/svo/doc/notation.ipe @@ -0,0 +1,312 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +$_w\mathtt{p}$ +$\overrightarrow{\mathcal{F}}_w$ + +64 576 m +96 576 l + + +64 576 m +64 608 l + + +64 576 m +80 592 l + + +144 688 m +176 688 l + + +144 688 m +144 720 l + + +144 688 m +160 704 l + + +285.262 759.615 m +162.017 735.456 +129.232 628.801 s + + +$\mathtt{T}_{f,w}$ + +128.441 626.745 m +320.625 647.224 l + + +331.681 587.399 m +331.681 587.399 l + +$x$ +$z$ +$y$ + +256 736 m +224 704 l +320 704 l +352 736 l +h + + +285.262 759.265 m +322.176 651.119 l + +$\mathtt{px}$ + +285.612 758.915 m +315.723 669.632 l + +$\mathtt{f}$ +$\overrightarrow{\mathcal{F}}_f$ + +255.668 719.918 m +274.728 719.957 l + + +256.107 720.028 m +246.655 710.527 l + +$v$ +$u$ + +$\mathbf{p}$ +$_f\mathtt{p}$ + + diff --git a/CPU_version/svo/doc/notation.pdf b/CPU_version/svo/doc/notation.pdf new file mode 100644 index 0000000..3bc3fcd Binary files /dev/null and b/CPU_version/svo/doc/notation.pdf differ diff --git a/CPU_version/svo/doc/notation.png b/CPU_version/svo/doc/notation.png new file mode 100644 index 0000000..d613b70 Binary files /dev/null and b/CPU_version/svo/doc/notation.png differ diff --git a/CPU_version/svo/include/svo/bundle_adjustment.h b/CPU_version/svo/include/svo/bundle_adjustment.h new file mode 100644 index 0000000..f3c3501 --- /dev/null +++ b/CPU_version/svo/include/svo/bundle_adjustment.h @@ -0,0 +1,109 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_BUNDLE_ADJUSTMENT_H_ +#define SVO_BUNDLE_ADJUSTMENT_H_ + +#include + +namespace g2o { +class EdgeProjectXYZ2UV; +class SparseOptimizer; +class VertexSE3Expmap; +class VertexSBAPointXYZ; +} + +namespace svo { + +typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3; +typedef g2o::VertexSE3Expmap g2oFrameSE3; +typedef g2o::VertexSBAPointXYZ g2oPoint; + +class Frame; +class Point; +class Feature; +class Map; + +/// Local, global and 2-view bundle adjustment with g2o +namespace ba { + +/// Temporary container to hold the g2o edge with reference to frame and point. +struct EdgeContainerSE3 +{ + g2oEdgeSE3* edge; + Frame* frame; + Feature* feature; + bool is_deleted; + EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) : + edge(e), frame(frame), feature(feature), is_deleted(false) + {} +}; + +/// Optimize two camera frames and their observed 3D points. +/// Is used after initialization. +void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map); + +/// Local bundle adjustment. +/// Optimizes core_kfs and their observed map points while keeping the +/// neighbourhood fixed. +void localBA( + Frame* center_kf, + set* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error); + +/// Global bundle adjustment. +/// Optimizes the whole map. Is currently not used in SVO. +void globalBA(Map* map); + +/// Initialize g2o with solver type, optimization strategy and camera model. +void setupG2o(g2o::SparseOptimizer * optimizer); + +/// Run the optimization on the provided graph. +void runSparseBAOptimizer( + g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, + double& final_error); + +/// Create a g2o vertice from a keyframe object. +g2oFrameSE3* createG2oFrameSE3( + Frame* kf, + size_t id, + bool fixed); + +/// Creates a g2o vertice from a mappoint object. +g2oPoint* createG2oPoint( + Vector3d pos, + size_t id, + bool fixed); + +/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. +g2oEdgeSE3* createG2oEdgeSE3( + g2oFrameSE3* v_kf, + g2oPoint* v_mp, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight = 1); + +} // namespace ba +} // namespace svo + +#endif // SVO_BUNDLE_ADJUSTMENT_H_ diff --git a/CPU_version/svo/include/svo/config.h b/CPU_version/svo/include/svo/config.h new file mode 100644 index 0000000..e1c97b1 --- /dev/null +++ b/CPU_version/svo/include/svo/config.h @@ -0,0 +1,177 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_CONFIG_H_ +#define SVO_CONFIG_H_ + +#include +#include +#include + +namespace svo { + +using std::string; + +/// Global configuration file of SVO. +/// Implements the Singleton design pattern to allow global access and to ensure +/// that only one instance exists. +class Config +{ +public: + static Config& getInstance(); + + /// Base-name of the tracefiles. + static string& traceName() { return getInstance().trace_name; } + + /// Directory where the tracefiles are saved. + static string& traceDir() { return getInstance().trace_dir; } + + /// Number of pyramid levels used for features. + static size_t& nPyrLevels() { return getInstance().n_pyr_levels; } + + /// Use the IMU to get relative rotations. + static bool& useImu() { return getInstance().use_imu; } + + /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment. + static size_t& coreNKfs() { return getInstance().core_n_kfs; } + + /// Initial scale of the map. Depends on the distance the camera is moved for the initialization. + static double& mapScale() { return getInstance().map_scale; } + + /// Feature grid size of a cell in [px]. + static size_t& gridSize() { return getInstance().grid_size; } + + /// Initialization: Minimum required disparity between the first two frames. + static double& initMinDisparity() { return getInstance().init_min_disparity; } + + /// Initialization: Minimum number of tracked features. + static size_t& initMinTracked() { return getInstance().init_min_tracked; } + + /// Initialization: Minimum number of inliers after RANSAC. + static size_t& initMinInliers() { return getInstance().init_min_inliers; } + + /// Maximum level of the Lucas Kanade tracker. + static size_t& kltMaxLevel() { return getInstance().klt_max_level; } + + /// Minimum level of the Lucas Kanade tracker. + static size_t& kltMinLevel() { return getInstance().klt_min_level; } + + /// Reprojection threshold [px]. + static double& reprojThresh() { return getInstance().reproj_thresh; } + + /// Reprojection threshold after pose optimization. + static double& poseOptimThresh() { return getInstance().poseoptim_thresh; } + + /// Number of iterations in local bundle adjustment. + static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; } + + /// Maximum number of points to optimize at every iteration. + static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; } + + /// Number of iterations in structure optimization. + static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; } + + /// Reprojection threshold after bundle adjustment. + static double& lobaThresh() { return getInstance().loba_thresh; } + + /// Threshold for the robust Huber kernel of the local bundle adjustment. + static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; } + + /// Number of iterations in the local bundle adjustment. + static size_t& lobaNumIter() { return getInstance().loba_num_iter; } + + /// Minimum distance between two keyframes. Relative to the average height in the map. + static double& kfSelectMinDist() { return getInstance().kfselect_mindist; } + + /// Select only features with a minimum Harris corner score for triangulation. + static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; } + + /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required! + static size_t& subpixNIter() { return getInstance().subpix_n_iter; } + + /// Limit the number of keyframes in the map. This makes nslam essentially. + /// a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. + /// Minimum number of keyframes is 3. + static size_t& maxNKfs() { return getInstance().max_n_kfs; } + + /// How much (in milliseconds) is the camera delayed with respect to the imu. + static double& imgImuDelay() { return getInstance().img_imu_delay; } + + /// Maximum number of features that should be tracked. + static size_t& maxFts() { return getInstance().max_fts; } + + /// If the number of tracked features drops below this threshold. Tracking quality is bad. + static size_t& qualityMinFts() { return getInstance().quality_min_fts; } + + /// If within one frame, this amount of features are dropped. Tracking quality is bad. + static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; } + + /// acc white noise in continuous. + static double& ACC_Noise() { return getInstance().ACC_noise; } + + /// acc white noise in continuous. + static double& GYO_Noise() { return getInstance().GYO_noise; } + /// integration uncertainty continuous + static double& IUC() { return getInstance().iuc; } + // acc bias in continuous + static double& ABC() { return getInstance().abc; } + // gyro bias in continuous + static double& GBC() { return getInstance().gbc; } + // error in the bias used for preintegration + static double& EBP() { return getInstance().ebp; } + // the noise used for projection + static double& NRP() { return getInstance().nrp; } + + +private: + Config(); + Config(Config const&); + void operator=(Config const&); + string trace_name; + string trace_dir; + size_t n_pyr_levels; + bool use_imu; + size_t core_n_kfs; + double map_scale; + size_t grid_size; + double init_min_disparity; + size_t init_min_tracked; + size_t init_min_inliers; + size_t klt_max_level; + size_t klt_min_level; + double reproj_thresh; + double poseoptim_thresh; + size_t poseoptim_num_iter; + size_t structureoptim_max_pts; + size_t structureoptim_num_iter; + double loba_thresh; + double loba_robust_huber_width; + size_t loba_num_iter; + double kfselect_mindist; + double triang_min_corner_score; + size_t triang_half_patch_size; + size_t subpix_n_iter; + size_t max_n_kfs; + double img_imu_delay; + size_t max_fts; + size_t quality_min_fts; + int quality_max_drop_fts; + double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise; +}; + +} // namespace svo + +#endif // SVO_CONFIG_H_ diff --git a/CPU_version/svo/include/svo/depth_filter.h b/CPU_version/svo/include/svo/depth_filter.h new file mode 100644 index 0000000..1612d0e --- /dev/null +++ b/CPU_version/svo/include/svo/depth_filter.h @@ -0,0 +1,171 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_DEPTH_FILTER_H_ +#define SVO_DEPTH_FILTER_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Feature; +class Point; + +/// A seed is a probabilistic depth estimate for a single pixel. +struct Seed +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int batch_counter; + static int seed_counter; + int batch_id; //!< Batch id is the id of the keyframe for which the seed was created. + int id; //!< Seed ID, only used for visualization. + Feature* ftr; //!< Feature in the keyframe for which the depth should be computed. + float a; //!< a of Beta distribution: When high, probability of inlier is large. + float b; //!< b of Beta distribution: When high, probability of outlier is large. + float mu; //!< Mean of normal distribution. + float z_range; //!< Max range of the possible depth. + float sigma2; //!< Variance of normal distribution. + Matrix2d patch_cov; //!< Patch covariance in reference image. + Seed(Feature* ftr, float depth_mean, float depth_min); +}; + +/// Depth filter implements the Bayesian Update proposed in: +/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández. +/// In Image and Vision Computing, 29(7):434-441, 2011. +/// +/// The class uses a callback mechanism such that it can be used also by other +/// algorithms than nslam and for simplified testing. +class DepthFilter +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef boost::unique_lock lock_t; + typedef boost::function callback_t; + + /// Depth-filter config parameters + struct Options + { + bool check_ftr_angle; //!< gradient features are only updated if the epipolar line is orthogonal to the gradient. + bool epi_search_1d; //!< restrict Gauss Newton in the epipolar search to the epipolar line. + bool verbose; //!< display output. + bool use_photometric_disparity_error; //!< use photometric disparity error instead of 1px error in tau computation. + int max_n_kfs; //!< maximum number of keyframes for which we maintain seeds. + double sigma_i_sq; //!< image noise. + double seed_convergence_sigma2_thresh; //!< threshold on depth uncertainty for convergence. + Options() + : check_ftr_angle(false), + epi_search_1d(false), + verbose(false), + use_photometric_disparity_error(false), + max_n_kfs(3), + sigma_i_sq(5e-4), + seed_convergence_sigma2_thresh(200.0) + {} + } options_; + + DepthFilter( + feature_detection::DetectorPtr feature_detector, + callback_t seed_converged_cb); + + virtual ~DepthFilter(); + + /// Start this thread when seed updating should be in a parallel thread. + void startThread(); + + /// Stop the parallel thread that is running. + void stopThread(); + + /// Add frame to the queue to be processed. + void addFrame(FramePtr frame); + + /// Add new keyframe to the queue + void addKeyframe(FramePtr frame, double depth_mean, double depth_min); + + /// Remove all seeds which are initialized from the specified keyframe. This + /// function is used to make sure that no seeds points to a non-existent frame + /// when a frame is removed from the map. + void removeKeyframe(FramePtr frame); + + /// If the map is reset, call this function such that we don't have pointers + /// to old frames. + void reset(); + + /// Returns a copy of the seeds belonging to frame. Thread-safe. + /// Can be used to compute the Next-Best-View in parallel. + /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame + /// so it is not being deleted while you use it. + void getSeedsCopy(const FramePtr& frame, std::list& seeds); + + /// Return a reference to the seeds. This is NOT THREAD SAFE! + //std::list& getSeeds() { return seeds_; } Bug 238 + std::list>& getSeeds() { return seeds_; } + + /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty + static void updateSeed( + const float x, + const float tau2, + Seed* seed); + + /// Compute the uncertainty of the measurement. + static double computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle); + +protected: + feature_detection::DetectorPtr feature_detector_; + callback_t seed_converged_cb_; + //std::list seeds_; + std::list> seeds_; + boost::mutex seeds_mut_; + bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted. + boost::thread* thread_; + std::queue frame_queue_; + boost::mutex frame_queue_mut_; + boost::condition_variable frame_queue_cond_; + FramePtr new_keyframe_; //!< Next keyframe to extract new seeds. + bool new_keyframe_set_; //!< Do we have a new keyframe to process?. + double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. + double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. + vk::PerformanceMonitor permon_; //!< Separate performance monitor since the DepthFilter runs in a parallel thread. + Matcher matcher_; + + /// Initialize new seeds from a frame. + void initializeSeeds(FramePtr frame); + + /// Update all seeds with a new measurement frame. + virtual void updateSeeds(FramePtr frame); + + /// When a new keyframe arrives, the frame queue should be cleared. + void clearFrameQueue(); + + /// A thread that is continuously updating the seeds. + void updateSeedsLoop(); +}; + +} // namespace svo + +#endif // SVO_DEPTH_FILTER_H_ diff --git a/CPU_version/svo/include/svo/feature.h b/CPU_version/svo/include/svo/feature.h new file mode 100644 index 0000000..6f90a9a --- /dev/null +++ b/CPU_version/svo/include/svo/feature.h @@ -0,0 +1,75 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FEATURE_H_ +#define SVO_FEATURE_H_ + +#include + +namespace svo { + +/// A salient image region that is tracked across frames. +struct Feature +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum FeatureType { + CORNER, + EDGELET + }; + + FeatureType type; //!< Type can be corner or edgelet. + Frame* frame; //!< Pointer to frame in which the feature was detected. + Vector2d px; //!< Coordinates in pixels on pyramid level 0. + Vector3d f; //!< Unit-bearing vector of the feature. + int level; //!< Image pyramid level where feature was extracted. + Point* point; //!< Pointer to 3D point which corresponds to the feature. + Vector2d grad; //!< Dominant gradient direction for edglets, normalized. + + Feature(Frame* _frame, const Vector2d& _px, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(frame->cam_->cam2world(px)), + level(_level), + point(NULL), + grad(1.0,0.0) + {} + + Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(NULL), + grad(1.0,0.0) + {} + + Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(_point), + grad(1.0,0.0) + {} +}; + +} // namespace svo + +#endif // SVO_FEATURE_H_ diff --git a/CPU_version/svo/include/svo/feature_alignment.h b/CPU_version/svo/include/svo/feature_alignment.h new file mode 100644 index 0000000..12523e3 --- /dev/null +++ b/CPU_version/svo/include/svo/feature_alignment.h @@ -0,0 +1,63 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FEATURE_ALIGNMENT_H_ +#define SVO_FEATURE_ALIGNMENT_H_ + +#include + +namespace svo { + +/// Subpixel refinement of a reference feature patch with the current image. +/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" +/// paper by Baker. +namespace feature_alignment { + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv); + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd = false); + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +bool align2D_NEON( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +} // namespace feature_alignment +} // namespace svo + +#endif // SVO_FEATURE_ALIGNMENT_H_ diff --git a/CPU_version/svo/include/svo/feature_detection.h b/CPU_version/svo/include/svo/feature_detection.h new file mode 100644 index 0000000..d0990a8 --- /dev/null +++ b/CPU_version/svo/include/svo/feature_detection.h @@ -0,0 +1,107 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FEATURE_DETECTION_H_ +#define SVO_FEATURE_DETECTION_H_ + +#include +#include + +namespace svo { + +/// Implementation of various feature detectors. +namespace feature_detection { + +/// Temporary container used for corner detection. Features are initialized from these. +struct Corner +{ + int x; //!< x-coordinate of corner in the image. + int y; //!< y-coordinate of corner in the image. + int level; //!< pyramid level of the corner. + float score; //!< shi-tomasi score of the corner. + float angle; //!< for gradient-features: dominant gradient angle. + Corner(int x, int y, float score, int level, float angle) : + x(x), y(y), level(level), score(score), angle(angle) + {} +}; +typedef vector Corners; + +/// All detectors should derive from this abstract class. +class AbstractDetector +{ +public: + AbstractDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels); + + virtual ~AbstractDetector() {}; + + virtual void detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts) = 0; + + /// Flag the grid cell as occupied + void setGridOccpuancy(const Vector2d& px); + + /// Set grid cells of existing features as occupied + void setExistingFeatures(const Features& fts); + +protected: + + static const int border_ = 8; //!< no feature should be within 8px of border. + const int cell_size_; + const int n_pyr_levels_; + const int grid_n_cols_; + const int grid_n_rows_; + vector grid_occupancy_; + + void resetGrid(); + + inline int getCellIndex(int x, int y, int level) + { + const int scale = (1< DetectorPtr; + +/// FAST detector by Edward Rosten. +class FastDetector : public AbstractDetector +{ +public: + FastDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels); + + virtual ~FastDetector() {} + + virtual void detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts); +}; + +} // namespace feature_detection +} // namespace svo + +#endif // SVO_FEATURE_DETECTION_H_ diff --git a/CPU_version/svo/include/svo/frame.h b/CPU_version/svo/include/svo/frame.h new file mode 100644 index 0000000..c926277 --- /dev/null +++ b/CPU_version/svo/include/svo/frame.h @@ -0,0 +1,154 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FRAME_H_ +#define SVO_FRAME_H_ + +#include +#include +#include +#include +#include + +namespace g2o { +class VertexSE3Expmap; +} +typedef g2o::VertexSE3Expmap g2oFrameSE3; + +namespace svo { + +class Point; +struct Feature; + +typedef list Features; +typedef vector ImgPyr; + +/// A frame saves the image, the associated features and the estimated pose. +class Frame : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. + int id_; //!< Unique id of the frame. + double timestamp_; //!< Timestamp of when the image was recorded. + vk::AbstractCamera* cam_; //!< Camera model. + Sophus::SE3 T_f_w_; //!< Transform (f)rame from (w)orld. + Matrix Cov_; //!< Covariance. + ImgPyr img_pyr_; //!< Image Pyramid. + Features fts_; //!< List of features in the image. + vector key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. + bool is_keyframe_; //!< Was this frames selected as keyframe? + g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. + int last_published_ts_; //!< Timestamp of last publishing. + + Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); + ~Frame(); + + /// Initialize new frame and create image pyramid. + void initFrame(const cv::Mat& img); + + /// Select this frame as keyframe. + void setKeyframe(); + + /// Add a feature to the image + void addFeature(Feature* ftr); + + /// The KeyPoints are those five features which are closest to the 4 image corners + /// and to the center and which have a 3D point assigned. These points are used + /// to quickly check whether two frames have overlapping field of view. + void setKeyPoints(); + + /// Check if we can select five better key-points. + void checkKeyPoints(Feature* ftr); + + /// If a point is deleted, we must remove the corresponding key-point. + void removeKeyPoint(Feature* ftr); + + /// Return number of point observations. + inline size_t nObs() const { return fts_.size(); } + + /// Check if a point in (w)orld coordinate frame is visible in the image. + bool isVisible(const Vector3d& xyz_w) const; + + /// Full resolution image stored in the frame. + inline const cv::Mat& img() const { return img_pyr_[0]; } + + /// Was this frame selected as keyframe? + inline bool isKeyframe() const { return is_keyframe_; } + + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). + inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); } + + /// Transforms point coordinates in world-frame (w) to camera-frams (f). + inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; } + + /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). + inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; } + + /// Projects Point from unit sphere (f) in camera pixels (c). + inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); } + + /// Return the pose of the frame in the (w)orld coordinate frame. + inline Vector3d pos() const { return T_f_w_.inverse().translation(); } + + /// Frame jacobian for projection of 3D point in (f)rame coordinate to + /// unit plane coordinates uv (focal length = 1). + inline static void jacobian_xyz2uv( + const Vector3d& xyz_in_f, + Matrix& J) + { + const double x = xyz_in_f[0]; + const double y = xyz_in_f[1]; + const double z_inv = 1./xyz_in_f[2]; + const double z_inv_2 = z_inv*z_inv; + + J(0,0) = -z_inv; // -1/z + J(0,1) = 0.0; // 0 + J(0,2) = x*z_inv_2; // x/z^2 + J(0,3) = y*J(0,2); // x*y/z^2 + J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2) + J(0,5) = y*z_inv; // y/z + + J(1,0) = 0.0; // 0 + J(1,1) = -z_inv; // -1/z + J(1,2) = y*z_inv_2; // y/z^2 + J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2 + J(1,4) = -J(0,3); // -x*y/z^2 + J(1,5) = -x*z_inv; // x/z + } +}; + + +/// Some helper functions for the frame object. +namespace frame_utils { + +/// Creates an image pyramid of half-sampled images. +void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr); + +/// Get the average depth of the features in the image. +bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min); + +} // namespace frame_utils +} // namespace svo + +#endif // SVO_FRAME_H_ diff --git a/CPU_version/svo/include/svo/frame_handler_base.h b/CPU_version/svo/include/svo/frame_handler_base.h new file mode 100644 index 0000000..9a78639 --- /dev/null +++ b/CPU_version/svo/include/svo/frame_handler_base.h @@ -0,0 +1,123 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FRAME_HANDLER_BASE_H_ +#define SVO_FRAME_HANDLER_BASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk +{ +class AbstractCamera; +class PerformanceMonitor; +} + +namespace svo +{ +class Point; +class Matcher; +class DepthFilter; + +/// Base class for various VO pipelines. Manages the map and the state machine. +class FrameHandlerBase : boost::noncopyable +{ +public: + enum Stage { + STAGE_PAUSED, + STAGE_FIRST_FRAME, + STAGE_SECOND_FRAME, + STAGE_DEFAULT_FRAME, + STAGE_RELOCALIZING + }; + enum TrackingQuality { + TRACKING_INSUFFICIENT, + TRACKING_BAD, + TRACKING_GOOD + }; + enum UpdateResult { + RESULT_NO_KEYFRAME, + RESULT_IS_KEYFRAME, + RESULT_FAILURE + }; + + FrameHandlerBase(); + + virtual ~FrameHandlerBase(); + + /// Get the current map. + const Map& map() const { return map_; } + + /// Will reset the map as soon as the current frame is finished processing. + void reset() { set_reset_ = true; } + + /// Start processing. + void start() { set_start_ = true; } + + /// Get the current stage of the algorithm. + Stage stage() const { return stage_; } + + /// Get tracking quality. + TrackingQuality trackingQuality() const { return tracking_quality_; } + + /// Get the processing time of the previous iteration. + double lastProcessingTime() const { return timer_.getTime(); } + + /// Get the number of feature observations of the last frame. + size_t lastNumObservations() const { return num_obs_last_; } + +protected: + Stage stage_; //!< Current stage of the algorithm. + bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration. + bool set_start_; //!< Flag the user can set to start the system when the next image is received. + Map map_; //!< Map of keyframes created by the slam system. + vk::Timer timer_; //!< Stopwatch to measure time to process frame. + vk::RingBuffer acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. + vk::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. + size_t num_obs_last_; //!< Number of observations in the previous frame. + TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features. + + /// Before a frame is processed, this function is called. + bool startFrameProcessingCommon(const double timestamp); + + /// When a frame is finished processing, this function is called. + int finishFrameProcessingCommon( + const size_t update_id, + const UpdateResult dropout, + const size_t num_observations); + + /// Reset the map and frame handler to start from scratch. + void resetCommon(); + + /// Reset the frame handler. Implement in derived class. + virtual void resetAll() { resetCommon(); } + + /// Set the tracking quality based on the number of tracked features. + virtual void setTrackingQuality(const size_t num_observations); + + /// Optimize some of the observed 3D points. + virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter); +}; + +} // namespace nslam + +#endif // SVO_FRAME_HANDLER_BASE_H_ diff --git a/CPU_version/svo/include/svo/frame_handler_mono.h b/CPU_version/svo/include/svo/frame_handler_mono.h new file mode 100644 index 0000000..58789c8 --- /dev/null +++ b/CPU_version/svo/include/svo/frame_handler_mono.h @@ -0,0 +1,104 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FRAME_HANDLER_H_ +#define SVO_FRAME_HANDLER_H_ + +#include +#include +#include +#include +#include +#include + +namespace svo { + +/// Monocular Visual Odometry Pipeline as described in the SVO paper. +class FrameHandlerMono : public FrameHandlerBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init); + virtual ~FrameHandlerMono(); + + /// Provide an image. + void addImage(const cv::Mat& img, double timestamp); + + /// Set the first frame (used for synthetic datasets in benchmark node) + void setFirstFrame(const FramePtr& first_frame); + + /// Get the last frame that has been processed. + FramePtr lastFrame() { return last_frame_; } + + /// Get the set of spatially closest keyframes of the last frame. + const set& coreKeyframes() { return core_kfs_; } + + /// Return the feature track to visualize the KLT tracking during initialization. + const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } + const vector& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } + + /// Access the depth filter. + DepthFilter* depthFilter() const { return depth_filter_; } + + /// An external place recognition module may know where to relocalize. + bool relocalizeFrameAtPose( + const int keyframe_id, + const SE3& T_kf_f, + const cv::Mat& img, + const double timestamp); + std::unique_ptr imu_integPtr_; //Integration of IMU + +protected: + vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). + Reprojector reprojector_; //!< Projects points from other keyframes into the current frame + FramePtr new_frame_; //!< Current frame. + FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. + set core_kfs_; //!< Keyframes in the closer neighbourhood. + vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? + initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. + DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. + + + /// Initialize the visual odometry algorithm. + virtual void initialize(); + + /// Processes the first frame and sets it as a keyframe. + virtual UpdateResult processFirstFrame(); + + /// Processes all frames after the first frame until a keyframe is selected. + virtual UpdateResult processSecondFrame(); + + /// Processes all frames after the first two keyframes. + virtual UpdateResult processFrame(); + + /// Try relocalizing the frame at relative position to provided keyframe. + virtual UpdateResult relocalizeFrame( + const SE3& T_cur_ref, + FramePtr ref_keyframe); + + /// Reset the frame handler. Implement in derived class. + virtual void resetAll(); + + /// Keyframe selection criterion. + virtual bool needNewKf(double scene_depth_mean); + + void setCoreKfs(size_t n_closest); +}; + +} // namespace svo + +#endif // SVO_FRAME_HANDLER_H_ diff --git a/CPU_version/svo/include/svo/global.h b/CPU_version/svo/include/svo/global.h new file mode 100644 index 0000000..03aff4e --- /dev/null +++ b/CPU_version/svo/include/svo/global.h @@ -0,0 +1,101 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_GLOBAL_H_ +#define SVO_GLOBAL_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit +#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) +#endif + +#ifdef SVO_USE_ROS + #include + #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x) + #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x) + #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x) + #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x) + #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x) +#else + #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "< // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc. + #define SVO_WARN_STREAM_THROTTLE(rate, x) \ + do { \ + static double __log_stream_throttle__last_hit__ = 0.0; \ + std::chrono::time_point __log_stream_throttle__now__ = \ + std::chrono::system_clock::now(); \ + if (__log_stream_throttle__last_hit__ + rate <= \ + std::chrono::duration_cast( \ + __log_stream_throttle__now__.time_since_epoch()).count()) { \ + __log_stream_throttle__last_hit__ = \ + std::chrono::duration_cast( \ + __log_stream_throttle__now__.time_since_epoch()).count(); \ + SVO_WARN_STREAM(x); \ + } \ + } while(0) +#endif + +namespace svo +{ + using namespace Eigen; + using namespace Sophus; + + const double EPS = 0.0000000001; + const double PI = 3.14159265; + +#ifdef SVO_TRACE + extern vk::PerformanceMonitor* g_permon; + #define SVO_LOG(value) g_permon->log(std::string((#value)),(value)) + #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2) + #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3) + #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4) + #define SVO_START_TIMER(name) g_permon->startTimer((name)) + #define SVO_STOP_TIMER(name) g_permon->stopTimer((name)) +#else + #define SVO_LOG(v) + #define SVO_LOG2(v1, v2) + #define SVO_LOG3(v1, v2, v3) + #define SVO_LOG4(v1, v2, v3, v4) + #define SVO_START_TIMER(name) + #define SVO_STOP_TIMER(name) +#endif + + class Frame; + typedef boost::shared_ptr FramePtr; +} // namespace svo + +#endif // SVO_GLOBAL_H_ diff --git a/CPU_version/svo/include/svo/imu_integration.h b/CPU_version/svo/include/svo/imu_integration.h new file mode 100644 index 0000000..11b9055 --- /dev/null +++ b/CPU_version/svo/include/svo/imu_integration.h @@ -0,0 +1,121 @@ +// +// Created by gufe on 4/9/21. +// + +#ifndef SVO_IMU_INTEGRATION_H +#define SVO_IMU_INTEGRATION_H +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using gtsam::symbol_shorthand::P; // Pose3 (x,y,z,r,p,y) +using gtsam::symbol_shorthand::C; // Camera pose + +//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop +// Wrapper class +template +class enumerate_impl +{ +public: + // The return value of the operator* of the iterator, this + // is what you will get inside of the for loop + struct item + { + size_t index; + typename T::value_type & item; + }; + typedef item value_type; + + // Custom iterator with minimal interface + struct iterator + { + iterator(typename T::iterator _it, size_t counter=0) : + it(_it), counter(counter) + {} + + iterator operator++() + { + return iterator(++it, ++counter); + } + + bool operator!=(iterator other) + { + return it != other.it; + } + + typename T::iterator::value_type item() + { + return *it; + } + + value_type operator*() + { + return value_type{counter, *it}; + } + + size_t index() + { + return counter; + } + + private: + typename T::iterator it; + size_t counter; + }; + + enumerate_impl(T & t) : container(t) {} + + iterator begin() + { + return iterator(container.begin()); + } + + iterator end() + { + return iterator(container.end()); + } + +private: + T & container; +}; + +// A templated free function allows you to create the wrapper class +// conveniently +template +enumerate_impl enumerate(T & t) +{ + return enumerate_impl(t); +} + +class Imu_Integration{ +public: + Imu_Integration(Sophus::SE3& SE_init); + ~Imu_Integration(); + bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame); + bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame); + bool update(double* imu= nullptr); + bool predict(boost::shared_ptr&,std::size_t&,const double reproj_thresh); +private: + std::shared_ptr statePtr; + std::shared_ptr imu_biasPtr; + std::shared_ptr valuesPtr; + std::shared_ptr graphPtr; + boost::shared_ptr parameterPtr; + std::shared_ptr preintegratedPtr; + boost::shared_ptr ProjectNoisePtr; + std::chrono::steady_clock::time_point t_1; + std::uint32_t imu_factor_id=0; + uint imu_n=0; + std::shared_ptr optimizerParamPtr; + bool syn=false; +}; +#endif //SVO_IMU_INTEGRATION_H diff --git a/CPU_version/svo/include/svo/initialization.h b/CPU_version/svo/include/svo/initialization.h new file mode 100644 index 0000000..cb562e5 --- /dev/null +++ b/CPU_version/svo/include/svo/initialization.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_INITIALIZATION_H +#define SVO_INITIALIZATION_H + +#include + +namespace svo { + +class FrameHandlerMono; + +/// Bootstrapping the map from the first two views. +namespace initialization { + +enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; + +/// Tracks features using Lucas-Kanade tracker and then estimates a homography. +class KltHomographyInit { + friend class svo::FrameHandlerMono; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FramePtr frame_ref_; + KltHomographyInit() {}; + ~KltHomographyInit() {}; + InitResult addFirstFrame(FramePtr frame_ref); + InitResult addSecondFrame(FramePtr frame_ref); + void reset(); + +protected: + vector px_ref_; //!< keypoints to be tracked in reference frame. + vector px_cur_; //!< tracked keypoints in current frame. + vector f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. + vector f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. + vector disparities_; //!< disparity between first and second frame. + vector inliers_; //!< inliers after the geometric check (e.g., Homography). + vector xyz_in_cur_; //!< 3D points computed during the geometric check. + SE3 T_cur_from_ref_; //!< computed transformation between the first two frames. +}; + +/// Detect Fast corners in the image. +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec); + +/// Compute optical flow (Lucas Kanade) for selected keypoints. +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities); + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE3& T_cur_from_ref); + +} // namespace initialization +} // namespace svo + +#endif // SVO_INITIALIZATION_H diff --git a/CPU_version/svo/include/svo/map.h b/CPU_version/svo/include/svo/map.h new file mode 100644 index 0000000..458622b --- /dev/null +++ b/CPU_version/svo/include/svo/map.h @@ -0,0 +1,136 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_MAP_H_ +#define SVO_MAP_H_ + +#include +#include +#include +#include + +namespace svo { + +class Point; +class Feature; +class Seed; + +/// Container for converged 3D points that are not already assigned to two keyframes. +class MapPointCandidates +{ +public: + typedef pair PointCandidate; + typedef list PointCandidateList; + + /// The depth-filter is running in a parallel thread and fills the canidate list. + /// This mutex controls concurrent access to point_candidates. + boost::mutex mut_; + + /// Candidate points are created from converged seeds. + /// Until the next keyframe, these points can be used for reprojection and pose optimization. + PointCandidateList candidates_; + list< Point* > trash_points_; + + MapPointCandidates(); + ~MapPointCandidates(); + + /// Add a candidate point. + void newCandidatePoint(Point* point, double depth_sigma2); + + /// Adds the feature to the frame and deletes candidate from list. + void addCandidatePointToFrame(FramePtr frame); + + /// Remove a candidate point from the list of candidates. + bool deleteCandidatePoint(Point* point); + + /// Remove all candidates that belong to a frame. + void removeFrameCandidates(FramePtr frame); + + /// Reset the candidate list, remove and delete all points. + void reset(); + + void deleteCandidate(PointCandidate& c); + + void emptyTrash(); +}; + +/// Map object which saves all keyframes which are in a map. +class Map : boost::noncopyable +{ +public: + list< FramePtr > keyframes_; //!< List of keyframes in the map. + list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also. + MapPointCandidates point_candidates_; + + Map(); + ~Map(); + + /// Reset the map. Delete all keyframes and reset the frame and point counters. + void reset(); + + /// Delete a point in the map and remove all references in keyframes to it. + void safeDeletePoint(Point* pt); + + /// Moves the point to the trash queue which is cleaned now and then. + void deletePoint(Point* pt); + + /// Moves the frame to the trash queue which is cleaned now and then. + bool safeDeleteFrame(FramePtr frame); + + /// Remove the references between a point and a frame. + void removePtFrameRef(Frame* frame, Feature* ftr); + + /// Add a new keyframe to the map. + void addKeyframe(FramePtr new_keyframe); + + /// Given a frame, return all keyframes which have an overlapping field of view. + void getCloseKeyframes(const FramePtr& frame, list< pair >& close_kfs) const; + + /// Return the keyframe which is spatially closest and has overlapping field of view. + FramePtr getClosestKeyframe(const FramePtr& frame) const; + + /// Return the keyframe which is furthest apart from pos. + FramePtr getFurthestKeyframe(const Vector3d& pos) const; + + bool getKeyframeById(const int id, FramePtr& frame) const; + + /// Transform the whole map with rotation R, translation t and scale s. + void transform(const Matrix3d& R, const Vector3d& t, const double& s); + + /// Empty trash bin of deleted keyframes and map points. We don't delete the + /// points immediately to ensure proper cleanup and to provide the visualizer + /// a list of objects which must be removed. + void emptyTrash(); + + /// Return the keyframe which was last inserted in the map. + inline FramePtr lastKeyframe() { return keyframes_.back(); } + + /// Return the number of keyframes in the map + inline size_t size() const { return keyframes_.size(); } +}; + +/// A collection of debug functions to check the data consistency. +namespace map_debug { + +void mapStatistics(Map* map); +void mapValidation(Map* map, int id); +void frameValidation(Frame* frame, int id); +void pointValidation(Point* point, int id); + +} // namespace map_debug +} // namespace svo + +#endif // SVO_MAP_H_ diff --git a/CPU_version/svo/include/svo/matcher.h b/CPU_version/svo/include/svo/matcher.h new file mode 100644 index 0000000..b53340e --- /dev/null +++ b/CPU_version/svo/include/svo/matcher.h @@ -0,0 +1,130 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_MATCHER_H_ +#define SVO_MATCHER_H_ + +#include + +namespace vk { + class AbstractCamera; + namespace patch_score { + template class ZMSSD; + } +} + +namespace svo { + +class Point; +class Frame; +class Feature; + +/// Warp a patch from the reference view to the current view. +namespace warp { + +void getWarpMatrixAffine( + const vk::AbstractCamera& cam_ref, + const vk::AbstractCamera& cam_cur, + const Vector2d& px_ref, + const Vector3d& f_ref, + const double depth_ref, + const SE3& T_cur_ref, + const int level_ref, + Matrix2d& A_cur_ref); + +int getBestSearchLevel( + const Matrix2d& A_cur_ref, + const int max_level); + +void warpAffine( + const Matrix2d& A_cur_ref, + const cv::Mat& img_ref, + const Vector2d& px_ref, + const int level_ref, + const int level_cur, + const int halfpatch_size, + uint8_t* patch); + +} // namespace warp + +/// Patch-matcher for reprojection-matching and epipolar search in triangulation. +class Matcher +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static const int halfpatch_size_ = 4; + static const int patch_size_ = 8; + + typedef vk::patch_score::ZMSSD PatchScore; + + struct Options + { + bool align_1d; //!< in epipolar search: align patch 1D along epipolar line + int align_max_iter; //!< number of iterations for aligning the feature patches in gauss newton + double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align + size_t max_epi_search_steps;//!< max number of evaluations along epipolar line + bool subpix_refinement; //!< do gauss newton feature patch alignment after epipolar search + bool epi_search_edgelet_filtering; + double epi_search_edgelet_max_angle; + Options() : + align_1d(false), + align_max_iter(10), + max_epi_length_optim(2.0), + max_epi_search_steps(1000), + subpix_refinement(true), + epi_search_edgelet_filtering(true), + epi_search_edgelet_max_angle(0.7) + {} + } options_; + + uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16))); + uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16))); + Matrix2d A_cur_ref_; //!< affine warp matrix + Vector2d epi_dir_; + double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search) + double h_inv_; //!< hessian of 1d image alignment along epipolar line + int search_level_; + bool reject_; + Feature* ref_ftr_; + Vector2d px_cur_; + + Matcher() = default; + ~Matcher() = default; + + /// Find a match by directly applying subpix refinement. + /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result! + bool findMatchDirect( + const Point& pt, + const Frame& frame, + Vector2d& px_cur); + + /// Find a match by searching along the epipolar line without using any features. + bool findEpipolarMatchDirect( + const Frame& ref_frame, + const Frame& cur_frame, + const Feature& ref_ftr, + const double d_estimate, + const double d_min, + const double d_max, + double& depth); + + void createPatchFromPatchWithBorder(); +}; + +} // namespace svo + +#endif // SVO_MATCHER_H_ diff --git a/CPU_version/svo/include/svo/point.h b/CPU_version/svo/include/svo/point.h new file mode 100644 index 0000000..e1bb81b --- /dev/null +++ b/CPU_version/svo/include/svo/point.h @@ -0,0 +1,108 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_POINT_H_ +#define SVO_POINT_H_ + +#include +#include + +namespace g2o { + class VertexSBAPointXYZ; +} +typedef g2o::VertexSBAPointXYZ g2oPoint; + +namespace svo { + +class Feature; + +typedef Matrix Matrix23d; + +/// A 3D point on the surface of the scene. +class Point : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum PointType { + TYPE_DELETED, + TYPE_CANDIDATE, + TYPE_UNKNOWN, + TYPE_GOOD + }; + + static int point_counter_; //!< Counts the number of created points. Used to set the unique id. + int id_; //!< Unique ID of the point. + Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. + Vector3d normal_; //!< Surface normal at point. + Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. + bool normal_set_; //!< Flag whether the surface normal was estimated or not. + list obs_; //!< References to keyframes which observe the point. + size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. + g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. + int last_published_ts_; //!< Timestamp of last publishing. + int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice. + PointType type_; //!< Quality of the point. + int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. + int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. + int last_structure_optim_; //!< Timestamp of last point optimization + + Point(const Vector3d& pos); + Point(const Vector3d& pos, Feature* ftr); + ~Point(); + + /// Add a reference to a frame. + void addFrameRef(Feature* ftr); + + /// Remove reference to a frame. + bool deleteFrameRef(Frame* frame); + + /// Initialize point normal. The inital estimate will point towards the frame. + void initNormal(); + + /// Check whether mappoint has reference to a frame. + Feature* findFrameRef(Frame* frame); + + /// Get Frame with similar viewpoint. + bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const; + + /// Get number of observations. + inline size_t nRefs() const { return obs_.size(); } + + /// Optimize point position through minimizing the reprojection error. + void optimize(const size_t n_iter); + + /// Jacobian of point projection on unit plane (focal length = 1) in frame (f). + inline static void jacobian_xyz2uv( + const Vector3d& p_in_f, + const Matrix3d& R_f_w, + Matrix23d& point_jac) + { + const double z_inv = 1.0/p_in_f[2]; + const double z_inv_sq = z_inv*z_inv; + point_jac(0, 0) = z_inv; + point_jac(0, 1) = 0.0; + point_jac(0, 2) = -p_in_f[0] * z_inv_sq; + point_jac(1, 0) = 0.0; + point_jac(1, 1) = z_inv; + point_jac(1, 2) = -p_in_f[1] * z_inv_sq; + point_jac = - point_jac * R_f_w; + } +}; + +} // namespace svo + +#endif // SVO_POINT_H_ diff --git a/CPU_version/svo/include/svo/pose_optimizer.h b/CPU_version/svo/include/svo/pose_optimizer.h new file mode 100644 index 0000000..ca048ae --- /dev/null +++ b/CPU_version/svo/include/svo/pose_optimizer.h @@ -0,0 +1,50 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_POSE_OPTIMIZER_H_ +#define SVO_POSE_OPTIMIZER_H_ + +#include + +namespace svo { + +using namespace Eigen; +using namespace Sophus; +using namespace std; + +typedef Matrix Matrix6d; +typedef Matrix Matrix26d; +typedef Matrix Vector6d; + +class Point; + +/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs); + +} // namespace pose_optimizer +} // namespace svo + +#endif // SVO_POSE_OPTIMIZER_H_ diff --git a/CPU_version/svo/include/svo/reprojector.h b/CPU_version/svo/include/svo/reprojector.h new file mode 100644 index 0000000..e345f0d --- /dev/null +++ b/CPU_version/svo/include/svo/reprojector.h @@ -0,0 +1,101 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_REPROJECTION_H_ +#define SVO_REPROJECTION_H_ + +#include +#include + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Map; +class Point; + +/// Project points from the map into the image and find the corresponding +/// feature (corner). We don't search a match for every point but only for one +/// point per cell. Thereby, we achieve a homogeneously distributed set of +/// matched features and at the same time we can save processing time by not +/// projecting all points. +class Reprojector +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Reprojector config parameters + struct Options { + size_t max_n_kfs; //!< max number of keyframes to reproject from + bool find_match_direct; + Options() + : max_n_kfs(10), + find_match_direct(true) + {} + } options_; + + size_t n_matches_; + size_t n_trials_; + + Reprojector(vk::AbstractCamera* cam, Map& map); + + ~Reprojector(); + + /// Project points from the map into the image. First finds keyframes with + /// overlapping field of view and projects only those map-points. + void reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs); + +private: + + /// A candidate is a point that projects into the image plane and for which we + /// will search a maching feature in the image. + struct Candidate { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Point* pt; //!< 3D point. + Vector2d px; //!< projected 2D pixel location. + Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} + }; + typedef std::list Cell; + typedef std::vector CandidateGrid; + + /// The grid stores a set of candidate matches. For every grid cell we try to find one match. + struct Grid + { + CandidateGrid cells; + vector cell_order; + int cell_size; + int grid_n_cols; + int grid_n_rows; + }; + + Grid grid_; + Matcher matcher_; + Map& map_; + + static bool pointQualityComparator(Candidate& lhs, Candidate& rhs); + void initializeGrid(vk::AbstractCamera* cam); + void resetGrid(); + bool reprojectCell(Cell& cell, FramePtr frame); + bool reprojectPoint(FramePtr frame, Point* point); +}; + +} // namespace svo + +#endif // SVO_REPROJECTION_H_ diff --git a/CPU_version/svo/include/svo/sparse_img_align.h b/CPU_version/svo/include/svo/sparse_img_align.h new file mode 100644 index 0000000..1bedfed --- /dev/null +++ b/CPU_version/svo/include/svo/sparse_img_align.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_SPARSE_IMG_ALIGN_H_ +#define SVO_SPARSE_IMG_ALIGN_H_ + +#include +#include +#include + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Feature; + +/// Optimize the pose of the frame by minimizing the photometric error of feature patches. +class SparseImgAlign : public vk::NLLSSolver<6, SE3> +{ + static const int patch_halfsize_ = 2; + static const int patch_size_ = 2*patch_halfsize_; + static const int patch_area_ = patch_size_*patch_size_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + cv::Mat resimg_; + + SparseImgAlign( + int n_levels, + int min_level, + int n_iter, + Method method, + bool display, + bool verbose); + + size_t run( + FramePtr ref_frame, + FramePtr cur_frame); + + /// Return fisher information matrix, i.e. the Hessian of the log-likelihood + /// at the converged state. + Matrix getFisherInformation(); + +protected: + FramePtr ref_frame_; //!< reference frame, has depth for gradient pixels. + FramePtr cur_frame_; //!< only the image is known! + int level_; //!< current pyramid level on which the optimization runs. + bool display_; //!< display residual image. + int max_level_; //!< coarsest pyramid level for the alignment. + int min_level_; //!< finest pyramid level for the alignment. + + // cache: + Matrix jacobian_cache_; + bool have_ref_patch_cache_; + cv::Mat ref_patch_cache_; + std::vector visible_fts_; + + void precomputeReferencePatches(); + virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false); + virtual int solve(); + virtual void update (const ModelType& old_model, ModelType& new_model); + virtual void startIteration(); + virtual void finishIteration(); +}; + +} // namespace svo + +#endif // SVO_SPARSE_IMG_ALIGN_H_ diff --git a/CPU_version/svo/include/svo/svoVisionFactor.h b/CPU_version/svo/include/svo/svoVisionFactor.h new file mode 100644 index 0000000..cd00816 --- /dev/null +++ b/CPU_version/svo/include/svo/svoVisionFactor.h @@ -0,0 +1,70 @@ +// +// Created by root on 4/13/21. +// + +#ifndef SVO_SVOVISIONFACTOR_H +#define SVO_SVOVISIONFACTOR_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactor: public gtsam::NoiseModelFactor1 { + + private: + // measurement information + gtsam::Point3 feature; /// body_P_sensor_; ///< TODO The pose of the sensor in the body frame + int level; ///< Image pyramid level where feature was extracted. + + public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param feature Unit-bearing vector of the feature. + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param poseKey is the index of the camera pose + * @param landmark 3D position of the landmark + * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param level Image pyramid level where feature was extracted. + */ + VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp, + gtsam::Key poseKey,boost::optional body_P_sensor = boost::none) : + gtsam::NoiseModelFactor1(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){} + virtual ~VisionFactor(){} + + /// error function + /// @param p the Estimated pose in Pose3 + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional Hp = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + gtsam::Matrix26 J; + gtsam::Point3 point(p*landmark); + svo::Frame::jacobian_xyz2uv(point, J); + Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/CPU_version/svo/package.xml b/CPU_version/svo/package.xml new file mode 100644 index 0000000..4d71cb3 --- /dev/null +++ b/CPU_version/svo/package.xml @@ -0,0 +1,27 @@ + + + svo + 0.1.0 + + Monocular Visual Odometry + + Christian Forster + Christian Forster + GPLv3 + + + catkin + + + cmake_modules + roscpp + roslib + vikit_common + vikit_ros + + + roscpp + roslib + vikit_common + vikit_ros + \ No newline at end of file diff --git a/CPU_version/svo/src/bundle_adjustment.cpp b/CPU_version/svo/src/bundle_adjustment.cpp new file mode 100644 index 0000000..eaba2b0 --- /dev/null +++ b/CPU_version/svo/src/bundle_adjustment.cpp @@ -0,0 +1,435 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SCHUR_TRICK 1 + +namespace svo { +namespace ba { + +void twoViewBA( + Frame* frame1, + Frame* frame2, + double reproj_thresh, + Map* map) +{ + // scale reprojection threshold in pixels to unit plane + reproj_thresh /= frame1->cam_->errorMultiplier2(); + + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + size_t v_id = 0; + + // New Keyframe Vertex 1: This Keyframe is set to fixed! + g2oFrameSE3* v_frame1 = createG2oFrameSE3(frame1, v_id++, true); + optimizer.addVertex(v_frame1); + + // New Keyframe Vertex 2 + g2oFrameSE3* v_frame2 = createG2oFrameSE3(frame2, v_id++, false); + optimizer.addVertex(v_frame2); + + // Create Point Vertices + for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr) + { + Point* pt = (*it_ftr)->point; + if(pt == NULL) + continue; + g2oPoint* v_pt = createG2oPoint(pt->pos_, v_id++, false); + optimizer.addVertex(v_pt); + pt->v_pt_ = v_pt; + g2oEdgeSE3* e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth()); + optimizer.addEdge(e); + edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container! + + // find at which index the second frame observes the point + Feature* ftr_frame2 = pt->findFrameRef(frame2); + e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth()); + optimizer.addEdge(e); + edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2)); + } + + // Optimization + double init_error, final_error; + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error); + + // Update Keyframe Positions + frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix(); + frame1->T_f_w_.translation() = v_frame1->estimate().translation(); + frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix(); + frame2->T_f_w_.translation() = v_frame2->estimate().translation(); + + // Update Mappoint Positions + for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + (*it)->point->pos_ = (*it)->point->v_pt_->estimate(); + (*it)->point->v_pt_ = NULL; + } + + // Find Mappoints with too large reprojection error + const double reproj_thresh_squared = reproj_thresh*reproj_thresh; + size_t n_incorrect_edges = 0; + for(list::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e) + if(it_e->edge->chi2() > reproj_thresh_squared) + { + if(it_e->feature->point != NULL) + { + map->safeDeletePoint(it_e->feature->point); + it_e->feature->point = NULL; + } + ++n_incorrect_edges; + } + + printf("2-View BA: Wrong edges = %zu\n", n_incorrect_edges); +} + +void localBA( + Frame* center_kf, + set* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error) +{ + + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + set mps; + list neib_kfs; + size_t v_id = 0; + size_t n_mps = 0; + size_t n_fix_kfs = 0; + size_t n_var_kfs = 1; + size_t n_edges = 0; + n_incorrect_edges_1 = 0; + n_incorrect_edges_2 = 0; + + // Add all core keyframes + for(set::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf) + { + g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false); + (*it_kf)->v_kf_ = v_kf; + ++n_var_kfs; + assert(optimizer.addVertex(v_kf)); + + // all points that the core keyframes observe are also optimized: + for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt) + if((*it_pt)->point != NULL) + mps.insert((*it_pt)->point); + } + + // Now go throug all the points and add a measurement. Add a fixed neighbour + // Keyframe if it is not in the set of core kfs + double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2(); + double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2(); + double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1; + for(set::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt) + { + // Create point vertex + g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false); + (*it_pt)->v_pt_ = v_pt; + assert(optimizer.addVertex(v_pt)); + ++n_mps; + + // Add edges + list::iterator it_obs=(*it_pt)->obs_.begin(); + while(it_obs!=(*it_pt)->obs_.end()) + { + Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_)); + + if((*it_obs)->frame->v_kf_ == NULL) + { + // frame does not have a vertex yet -> it belongs to the neib kfs and + // is fixed. create one: + g2oFrameSE3* v_kf = createG2oFrameSE3((*it_obs)->frame, v_id++, true); + (*it_obs)->frame->v_kf_ = v_kf; + ++n_fix_kfs; + assert(optimizer.addVertex(v_kf)); + neib_kfs.push_back((*it_obs)->frame); + } + + // create edge + g2oEdgeSE3* e = createG2oEdgeSE3((*it_obs)->frame->v_kf_, v_pt, + vk::project2d((*it_obs)->f), + true, + reproj_thresh_2*Config::lobaRobustHuberWidth(), + 1.0 / (1<<(*it_obs)->level)); + assert(optimizer.addEdge(e)); + edges.push_back(EdgeContainerSE3(e, (*it_obs)->frame, *it_obs)); + ++n_edges; + ++it_obs; + } + } + + // structure only + g2o::StructureOnlySolver<3> structure_only_ba; + g2o::OptimizableGraph::VertexContainer points; + for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) + { + g2o::OptimizableGraph::Vertex* v = static_cast(it->second); + if (v->dimension() == 3 && v->edges().size() >= 2) + points.push_back(v); + } + structure_only_ba.calc(points, 10); + + // Optimization + if(Config::lobaNumIter() > 0) + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + + // Update Keyframes + for(set::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it) + { + (*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(), + (*it)->v_kf_->estimate().translation()); + (*it)->v_kf_ = NULL; + } + + for(list::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it) + (*it)->v_kf_ = NULL; + + // Update Mappoints + for(set::iterator it = mps.begin(); it != mps.end(); ++it) + { + (*it)->pos_ = (*it)->v_pt_->estimate(); + (*it)->v_pt_ = NULL; + } + + // Remove Measurements with too large reprojection error + double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2; + for(list::iterator it = edges.begin(); it != edges.end(); ++it) + { + if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<feature_->level)) + { + map->removePtFrameRef(it->frame, it->feature); + ++n_incorrect_edges_2; + } + } + + // TODO: delete points and edges! + init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2(); + final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2(); +} + +void globalBA(Map* map) +{ + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + list< pair > incorrect_edges; + + // Go through all Keyframes + size_t v_id = 0; + double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2(); + double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2(); + reproj_thresh_1_squared *= reproj_thresh_1_squared; + for(list::iterator it_kf = map->keyframes_.begin(); + it_kf != map->keyframes_.end(); ++it_kf) + { + // New Keyframe Vertex + g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false); + (*it_kf)->v_kf_ = v_kf; + optimizer.addVertex(v_kf); + for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) + { + // for each keyframe add edges to all observed mapoints + Point* mp = (*it_ftr)->point; + if(mp == NULL) + continue; + g2oPoint* v_mp = mp->v_pt_; + if(v_mp == NULL) + { + // mappoint-vertex doesn't exist yet. create a new one: + v_mp = createG2oPoint(mp->pos_, v_id++, false); + mp->v_pt_ = v_mp; + optimizer.addVertex(v_mp); + } + + // Due to merging of mappoints it is possible that references in kfs suddenly + // have a very large reprojection error which may result in distorted results. + Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_)); + if(error.squaredNorm() > reproj_thresh_1_squared) + incorrect_edges.push_back(pair(*it_kf, *it_ftr)); + else + { + g2oEdgeSE3* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f), + true, + reproj_thresh_2*Config::lobaRobustHuberWidth()); + + edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr)); + optimizer.addEdge(e); + } + } + } + + // Optimization + double init_error=0.0, final_error=0.0; + if(Config::lobaNumIter() > 0) + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + + // Update Keyframe and MapPoint Positions + for(list::iterator it_kf = map->keyframes_.begin(); + it_kf != map->keyframes_.end(); ++it_kf) + { + (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(), + (*it_kf)->v_kf_->estimate().translation()); + (*it_kf)->v_kf_ = NULL; + for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) + { + Point* mp = (*it_ftr)->point; + if(mp == NULL) + continue; + if(mp->v_pt_ == NULL) + continue; // mp was updated before + mp->pos_ = mp->v_pt_->estimate(); + mp->v_pt_ = NULL; + } + } + + // Remove Measurements with too large reprojection error + for(list< pair >::iterator it=incorrect_edges.begin(); + it!=incorrect_edges.end(); ++it) + map->removePtFrameRef(it->first.get(), it->second); + + double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2; + for(list::iterator it = edges.begin(); it != edges.end(); ++it) + { + if(it->edge->chi2() > reproj_thresh_2_squared) + { + map->removePtFrameRef(it->frame, it->feature); + } + } +} + +void setupG2o(g2o::SparseOptimizer * optimizer) +{ + // TODO: What's happening with all this HEAP stuff? Memory Leak? + optimizer->setVerbose(false); + +#if SCHUR_TRICK + // solver + g2o::BlockSolver_6_3::LinearSolverType* linearSolver; + linearSolver = new g2o::LinearSolverCholmod(); + //linearSolver = new g2o::LinearSolverCSparse(); + + g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +#else + g2o::BlockSolverX::LinearSolverType * linearSolver; + linearSolver = new g2o::LinearSolverCholmod(); + //linearSolver = new g2o::LinearSolverCSparse(); + g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +#endif + + solver->setMaxTrialsAfterFailure(5); + optimizer->setAlgorithm(solver); + + // setup camera + g2o::CameraParameters * cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.); + cam_params->setId(0); + if (!optimizer->addParameter(cam_params)) { + assert(false); + } +} + +void +runSparseBAOptimizer(g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, double& final_error) +{ + optimizer->initializeOptimization(); + optimizer->computeActiveErrors(); + init_error = optimizer->activeChi2(); + optimizer->optimize(num_iter); + final_error = optimizer->activeChi2(); +} + +g2oFrameSE3* +createG2oFrameSE3(Frame* frame, size_t id, bool fixed) +{ + g2oFrameSE3* v = new g2oFrameSE3(); + v->setId(id); + v->setFixed(fixed); + v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation())); + return v; +} + +g2oPoint* +createG2oPoint(Vector3d pos, + size_t id, + bool fixed) +{ + g2oPoint* v = new g2oPoint(); + v->setId(id); +#if SCHUR_TRICK + v->setMarginalized(true); +#endif + v->setFixed(fixed); + v->setEstimate(pos); + return v; +} + +g2oEdgeSE3* +createG2oEdgeSE3( g2oFrameSE3* v_frame, + g2oPoint* v_point, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight) +{ + g2oEdgeSE3* e = new g2oEdgeSE3(); + e->setVertex(0, dynamic_cast(v_point)); + e->setVertex(1, dynamic_cast(v_frame)); + e->setMeasurement(f_up); + e->information() = weight * Eigen::Matrix2d::Identity(2,2); + g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber(); // TODO: memory leak + rk->setDelta(huber_width); + e->setRobustKernel(rk); + e->setParameterId(0, 0); //old: e->setId(v_point->id()); + return e; +} + +} // namespace ba +} // namespace svo diff --git a/CPU_version/svo/src/config.cpp b/CPU_version/svo/src/config.cpp new file mode 100644 index 0000000..e8475fe --- /dev/null +++ b/CPU_version/svo/src/config.cpp @@ -0,0 +1,102 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifdef SVO_USE_ROS +#include +#endif +#include + +namespace svo { + +Config::Config() : +#ifdef SVO_USE_ROS + trace_name(vk::getParam("svo/trace_name", "svo")), + trace_dir(vk::getParam("svo/trace_dir", "/tmp")), + n_pyr_levels(vk::getParam("svo/n_pyr_levels", 3)), + use_imu(vk::getParam("svo/use_imu", false)), + core_n_kfs(vk::getParam("svo/core_n_kfs", 3)), + map_scale(vk::getParam("svo/map_scale", 1.0)), + grid_size(vk::getParam("svo/grid_size", 30)), + init_min_disparity(vk::getParam("svo/init_min_disparity", 50.0)), + init_min_tracked(vk::getParam("svo/init_min_tracked", 50)), + init_min_inliers(vk::getParam("svo/init_min_inliers", 40)), + klt_max_level(vk::getParam("svo/klt_max_level", 4)), + klt_min_level(vk::getParam("svo/klt_min_level", 2)), + reproj_thresh(vk::getParam("svo/reproj_thresh", 2.0)), + poseoptim_thresh(vk::getParam("svo/poseoptim_thresh", 2.0)), + poseoptim_num_iter(vk::getParam("svo/poseoptim_num_iter", 10)), + structureoptim_max_pts(vk::getParam("svo/structureoptim_max_pts", 20)), + structureoptim_num_iter(vk::getParam("svo/structureoptim_num_iter", 5)), + loba_thresh(vk::getParam("svo/loba_thresh", 2.0)), + loba_robust_huber_width(vk::getParam("svo/loba_robust_huber_width", 1.0)), + loba_num_iter(vk::getParam("svo/loba_num_iter", 0)), + kfselect_mindist(vk::getParam("svo/kfselect_mindist", 0.12)), + triang_min_corner_score(vk::getParam("svo/triang_min_corner_score", 20.0)), + triang_half_patch_size(vk::getParam("svo/triang_half_patch_size", 4)), + subpix_n_iter(vk::getParam("svo/subpix_n_iter", 10)), + max_n_kfs(vk::getParam("svo/max_n_kfs", 10)), + img_imu_delay(vk::getParam("svo/img_imu_delay", 0.0)), + max_fts(vk::getParam("svo/max_fts", 120)), + quality_min_fts(vk::getParam("svo/quality_min_fts", 50)), + quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)), + ACC_noise(vk::getParam("svo/ACC_noise", 1e-4)), + GYO_noise(vk::getParam("svo/GYO_noise", 1e-4)), + iuc(vk::getParam("svo/IUC", 1e-1)), + abc(vk::getParam("svo/ABC", 1e-1)), + gbc(vk::getParam("svo/GBC", 1e-1)), + ebp(vk::getParam("svo/EBP", 1e-2)), + nrp(vk::getParam("svo/NRP", 1e-5)) +#else + trace_name("svo"), + trace_dir("/tmp"), + n_pyr_levels(3), + use_imu(false), + core_n_kfs(3), + map_scale(1.0), + grid_size(25), + init_min_disparity(50.0), + init_min_tracked(50), + init_min_inliers(40), + klt_max_level(4), + klt_min_level(2), + reproj_thresh(2.0), + poseoptim_thresh(2.0), + poseoptim_num_iter(10), + structureoptim_max_pts(20), + structureoptim_num_iter(5), + loba_thresh(2.0), + loba_robust_huber_width(1.0), + loba_num_iter(0), + kfselect_mindist(0.12), + triang_min_corner_score(20.0), + triang_half_patch_size(4), + subpix_n_iter(10), + max_n_kfs(0), + img_imu_delay(0.0), + max_fts(120), + quality_min_fts(50), + quality_max_drop_fts(40) +#endif +{} + +Config& Config::getInstance() +{ + static Config instance; // Instantiated on first use and guaranteed to be destroyed + return instance; +} + +} // namespace svo + diff --git a/CPU_version/svo/src/depth_filter.cpp b/CPU_version/svo/src/depth_filter.cpp new file mode 100644 index 0000000..db24408 --- /dev/null +++ b/CPU_version/svo/src/depth_filter.cpp @@ -0,0 +1,352 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Seed::batch_counter = 0; +int Seed::seed_counter = 0; + +Seed::Seed(Feature* ftr, float depth_mean, float depth_min) : + batch_id(batch_counter), + id(seed_counter++), + ftr(ftr), + a(10), + b(10), + mu(1.0/depth_mean), + z_range(1.0/depth_min), + sigma2(z_range*z_range/36) +{} + +DepthFilter::DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb) : + feature_detector_(feature_detector), + seed_converged_cb_(seed_converged_cb), + seeds_updating_halt_(false), + thread_(NULL), + new_keyframe_set_(false), + new_keyframe_min_depth_(0.0), + new_keyframe_mean_depth_(0.0) +{} + +DepthFilter::~DepthFilter() +{ + stopThread(); + SVO_INFO_STREAM("DepthFilter destructed."); +} + +void DepthFilter::startThread() +{ + thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this); +} + +void DepthFilter::stopThread() +{ + SVO_INFO_STREAM("DepthFilter stop thread invoked."); + if(thread_ != NULL) + { + SVO_INFO_STREAM("DepthFilter interrupt and join thread... "); + seeds_updating_halt_ = true; + thread_->interrupt(); + thread_->join(); + thread_ = NULL; + } +} + +void DepthFilter::addFrame(FramePtr frame) +{ + if(thread_ != NULL) + { + { + lock_t lock(frame_queue_mut_); + if(frame_queue_.size() > 2) + frame_queue_.pop(); + frame_queue_.push(frame); + } + seeds_updating_halt_ = false; + frame_queue_cond_.notify_one(); + } + else + updateSeeds(frame); +} + +void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min) +{ + new_keyframe_min_depth_ = depth_min; + new_keyframe_mean_depth_ = depth_mean; + if(thread_ != NULL) + { + new_keyframe_ = frame; + new_keyframe_set_ = true; + seeds_updating_halt_ = true; + frame_queue_cond_.notify_one(); + } + else + initializeSeeds(frame); +} + +void DepthFilter::initializeSeeds(FramePtr frame) +{ + Features new_features; + feature_detector_->setExistingFeatures(frame->fts_); + feature_detector_->detect(frame.get(), frame->img_pyr_, + Config::triangMinCornerScore(), new_features); + + // initialize a seed for every new feature + seeds_updating_halt_ = true; + lock_t lock(seeds_mut_); // by locking the updateSeeds function stops + ++Seed::batch_counter; + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_)); + }); + + if(options_.verbose) + SVO_INFO_STREAM("DepthFilter: Initialized "<::iterator it=seeds_.begin(); + size_t n_removed = 0; + while(it!=seeds_.end()) + { + if(it->ftr->frame == frame.get()) + { + it = seeds_.erase(it); + ++n_removed; + } + else + ++it; + } + seeds_updating_halt_ = false; +} + +void DepthFilter::reset() +{ + seeds_updating_halt_ = true; + { + lock_t lock(seeds_mut_); + seeds_.clear(); + } + lock_t lock(); + while(!frame_queue_.empty()) + frame_queue_.pop(); + seeds_updating_halt_ = false; + + if(options_.verbose) + SVO_INFO_STREAM("DepthFilter: RESET."); +} + +void DepthFilter::updateSeedsLoop() +{ + while(!boost::this_thread::interruption_requested()) + { + FramePtr frame; + { + lock_t lock(frame_queue_mut_); + while(frame_queue_.empty() && new_keyframe_set_ == false) + frame_queue_cond_.wait(lock); + if(new_keyframe_set_) + { + new_keyframe_set_ = false; + seeds_updating_halt_ = false; + clearFrameQueue(); + frame = new_keyframe_; + } + else + { + frame = frame_queue_.front(); + frame_queue_.pop(); + } + } + updateSeeds(frame); + if(frame->isKeyframe()) + initializeSeeds(frame); + } +} + +void DepthFilter::updateSeeds(FramePtr frame) +{ + // update only a limited number of seeds, because we don't have time to do it + // for all the seeds in every frame! + size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size(); + lock_t lock(seeds_mut_); + std::list::iterator it=seeds_.begin(); + + const double focal_length = frame->cam_->errorMultiplier2(); + double px_noise = 1.0; + double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz) + + while( it!=seeds_.end()) + { + // set this value true when seeds updating should be interrupted + if(seeds_updating_halt_) + return; + + // check if seed is not already too old + if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) { + it = seeds_.erase(it); + continue; + } + + // check if point is visible in the current image + SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse(); + const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); + if(xyz_f.z() < 0.0) { + ++it; // behind the camera + continue; + } + if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast())) { + ++it; // point does not project in image + continue; + } + + // we are using inverse depth coordinates + float z_inv_min = it->mu + sqrt(it->sigma2); + float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f); + double z; + if(!matcher_.findEpipolarMatchDirect( + *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z)) + { + it->b++; // increase outlier probability when no match was found + ++it; + ++n_failed_matches; + continue; + } + + // compute tau + double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle); + double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau)); + + // update the estimate + updateSeed(1./z, tau_inverse*tau_inverse, &*it); + ++n_updates; + + if(frame->isKeyframe()) + { + // The feature detector should not initialize new seeds close to this location + feature_detector_->setGridOccpuancy(matcher_.px_cur_); + } + + // if the seed has converged, we initialize a new candidate point and remove the seed + if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh) + { + assert(it->ftr->point == NULL); // TODO this should not happen anymore + Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu))); + Point* point = new Point(xyz_world, it->ftr); + it->ftr->point = point; + /* FIXME it is not threadsafe to add a feature to the frame here. + if(frame->isKeyframe()) + { + Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_); + ftr->point = point; + point->addFrameRef(ftr); + frame->addFeature(ftr); + it->ftr->frame->addFeature(it->ftr); + } + else + */ + { + seed_converged_cb_(point, it->sigma2); // put in candidate list + } + it = seeds_.erase(it); + } + else if(isnan(z_inv_min)) + { + SVO_WARN_STREAM("z_min is NaN"); + it = seeds_.erase(it); + } + else + ++it; + } +} + +void DepthFilter::clearFrameQueue() +{ + while(!frame_queue_.empty()) + frame_queue_.pop(); +} + +void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list& seeds) +{ + lock_t lock(seeds_mut_); + for(std::list::iterator it=seeds_.begin(); it!=seeds_.end(); ++it) + { + if (it->ftr->frame == frame.get()) + seeds.push_back(*it); + } +} + +void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) +{ + float norm_scale = sqrt(seed->sigma2 + tau2); + if(std::isnan(norm_scale)) + return; + boost::math::normal_distribution nd(seed->mu, norm_scale); + float s2 = 1./(1./seed->sigma2 + 1./tau2); + float m = s2*(seed->mu/seed->sigma2 + x/tau2); + float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x); + float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range; + float normalization_constant = C1 + C2; + C1 /= normalization_constant; + C2 /= normalization_constant; + float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.); + float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.)) + + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f)); + + // update parameters + float mu_new = C1*m+C2*seed->mu; + seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new; + seed->mu = mu_new; + seed->a = (e-f)/(f-e/f); + seed->b = seed->a*(1.0f-f)/f; +} + +double DepthFilter::computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle) +{ + Vector3d t(T_ref_cur.translation()); + Vector3d a = f*z-t; + double t_norm = t.norm(); + double a_norm = a.norm(); + double alpha = acos(f.dot(t)/t_norm); // dot product + double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product + double beta_plus = beta + px_error_angle; + double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI + double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines + return (z_plus - z); // tau +} + +} // namespace svo diff --git a/CPU_version/svo/src/feature_alignment.cpp b/CPU_version/svo/src/feature_alignment.cpp new file mode 100644 index 0000000..faa4027 --- /dev/null +++ b/CPU_version/svo/src/feature_alignment.cpp @@ -0,0 +1,601 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifdef __SSE2__ +#include +#endif +#ifdef __ARM_NEON__ +#include +#endif +#include + +namespace svo { +namespace feature_alignment { + +#define SUBPIX_VERBOSE 0 + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv) +{ + const int halfpatch_size_ = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dv[patch_area]; + Matrix2f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size+2; + float* it_dv = ref_patch_dv; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dv = ref_patch_dv; + float new_chi2 = 0.0; + Vector2f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + + chi2 = new_chi2; + update = Hinv * Jres; + u += update[0]*dir[0]; + v += update[0]*dir[1]; + mean_diff += update[1]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] + << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd) +{ +#ifdef __ARM_NEON__ + if(!no_simd) + return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); +#endif + + const int halfpatch_size_ = 4; + const int patch_size_ = 8; + const int patch_area_ = 64; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_]; + float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_]; + Matrix3f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size_+2; + float* it_dx = ref_patch_dx; + float* it_dy = ref_patch_dy; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dx = ref_patch_dx; + float* it_ref_dy = ref_patch_dy; +// float new_chi2 = 0.0; + Vector3f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + chi2 = new_chi2; +*/ + update = Hinv * Jres; + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] +// << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +#define DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n)) // rounds to closest integer and descales + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate) +{ + // TODO: This function should not be used as the alignment is not robust to illumination changes! + const int halfpatch_size = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + const int W_BITS = 14; + + // compute derivative of template and prepare inverse compositional + int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area]; + int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area]; + + // compute gradient and hessian + const int ref_step = patch_size+2; + int16_t* it_dx = ref_patch_dx; + int16_t* it_dy = ref_patch_dy; + float A11=0, A12=0, A22=0; + for(int y=0; y(it[1]) - it[-1]; + int16_t dy = static_cast(it[ref_step]) - it[-ref_step]; + *it_dx = dx; + *it_dy = dy; // we are missing a factor 1/2 + A11 += static_cast(dx*dx); // we are missing a factor 1/4 + A12 += static_cast(dx*dy); + A22 += static_cast(dy*dy); + } + } + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.03*0.03; + const int cur_step = cur_img.step.p[0]; + const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 + float chi2 = 0; + float update_u = 0, update_v = 0; + + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + float new_chi2 = 0.0; + +#ifdef __SSE2__ + // compute bilinear interpolation weights + int wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS)); + int wTR = static_cast(subpix_x * (1.0f-subpix_y)*(1 << W_BITS)); + int wBL = static_cast((1.0f-subpix_x)*subpix_y*(1 << W_BITS)); + int wBR = (1 << W_BITS) - wTL - wTR - wBL; + + __m128i qw0 = _mm_set1_epi32(wTL + (wTR << 16)); // Sets the 4 signed 32-bit integer values to [wTL, wTR]. + __m128i qw1 = _mm_set1_epi32(wBL + (wBR << 16)); + __m128i z = _mm_setzero_si128(); + __m128 qb0 = _mm_setzero_ps(); // 4 floats + __m128 qb1 = _mm_setzero_ps(); // 4 floats + __m128i qdelta = _mm_set1_epi32(1 << (W_BITS-1)); + for(int y=0; y(it[1] - it[-1]); + *it_dy = static_cast(it[ref_step] - it[-ref_step]); // divide by 2 missing + Vector3f J(*it_dx, *it_dy, 1.0f); + H += J*J.transpose(); + } + } + Matrix3f Hinv = H.inverse(); + float mean_diff = 0.0; + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.03*0.03; + const int cur_step = cur_img.step.p[0]; + Vector3f update; + Vector3f Jres; + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + +#ifdef __ARM_NEON__ + const int SHIFT_BITS = 7; + const uint16_t wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1<(subpix_x*(1.0f-subpix_y)*(1<((1.0f-subpix_x)*subpix_y*(1<((1 << SHIFT_BITS) - wTL - wTR - wBL); + + // initialize result to zero + int32x4_t vb1 = vdupq_n_s32(0); + int32x4_t vb2 = vdupq_n_s32(0); + int16x8_t vmean_diff = vdupq_n_s16( (int16_t) (mean_diff+0.5) ); + int16x8_t vres_sum = vdupq_n_s16(0); + for(int y=0; y Vr[i] := Va[i] + Vb[i] * Vc[i] + // int32x4_t vmlal_s16(int32x4_t a, int16x4_t b, int16x4_t c); // VMLAL.S16 q0,d0,d0 + int16x8_t grad = vld1q_s16(ref_patch_dx + y*patch_size); + vb1 = vmlal_s16(vb1, vget_low_s16(grad), vget_low_s16(res)); + vb1 = vmlal_s16(vb1, vget_high_s16(grad), vget_high_s16(res)); + + grad = vld1q_s16(ref_patch_dy + y*patch_size); + vb2 = vmlal_s16(vb2, vget_low_s16(grad), vget_low_s16(res)); + vb2 = vmlal_s16(vb2, vget_high_s16(grad), vget_high_s16(res)); + } + + // finally, sum results of vb1, vb2 and vres_sum + int32x2_t tmp; + tmp = vpadd_s32(vget_low_s32(vb1), vget_high_s32(vb1)); + Jres[0] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + tmp = vpadd_s32(vget_low_s32(vb2), vget_high_s32(vb2)); + Jres[1] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + int32x4_t vres_sum1 = vpaddlq_s16(vres_sum); + tmp = vpadd_s32(vget_low_s32(vres_sum1), vget_high_s32(vres_sum1)); + Jres[2] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); +#endif + + update = Hinv * Jres * 2; // * 2 to compensate because above, we did not compute the derivative correctly + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +} // namespace feature_alignment +} // namespace svo diff --git a/CPU_version/svo/src/feature_detection.cpp b/CPU_version/svo/src/feature_detection.cpp new file mode 100644 index 0000000..98183f8 --- /dev/null +++ b/CPU_version/svo/src/feature_detection.cpp @@ -0,0 +1,118 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include + +namespace svo { +namespace feature_detection { + +AbstractDetector::AbstractDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels) : + cell_size_(cell_size), + n_pyr_levels_(n_pyr_levels), + grid_n_cols_(ceil(static_cast(img_width)/cell_size_)), + grid_n_rows_(ceil(static_cast(img_height)/cell_size_)), + grid_occupancy_(grid_n_cols_*grid_n_rows_, false) +{} + +void AbstractDetector::resetGrid() +{ + std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false); +} + +void AbstractDetector::setExistingFeatures(const Features& fts) +{ + std::for_each(fts.begin(), fts.end(), [&](Feature* i){ + grid_occupancy_.at( + static_cast(i->px[1]/cell_size_)*grid_n_cols_ + + static_cast(i->px[0]/cell_size_)) = true; + }); +} + +void AbstractDetector::setGridOccpuancy(const Vector2d& px) +{ + grid_occupancy_.at( + static_cast(px[1]/cell_size_)*grid_n_cols_ + + static_cast(px[0]/cell_size_)) = true; +} + +FastDetector::FastDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels) : + AbstractDetector(img_width, img_height, cell_size, n_pyr_levels) +{} + +void FastDetector::detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts) +{ + Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f)); + for(int L=0; L fast_corners; +#if __SSE2__ + fast::fast_corner_detect_10_sse2( + (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, + img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#elif HAVE_FAST_NEON + fast::fast_corner_detect_9_neon( + (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, + img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#else + fast::fast_corner_detect_10( + (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, + img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#endif + vector scores, nm_corners; + fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores); + fast::fast_nonmax_3x3(fast_corners, scores, nm_corners); + + for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it) + { + fast::fast_xy& xy = fast_corners.at(*it); + const int k = static_cast((xy.y*scale)/cell_size_)*grid_n_cols_ + + static_cast((xy.x*scale)/cell_size_); + if(grid_occupancy_[k]) + continue; + const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y); + if(score > corners.at(k).score) + corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f); + } + } + + // Create feature for every corner that has high enough corner score + std::for_each(corners.begin(), corners.end(), [&](Corner& c) { + if(c.score > detection_threshold) + fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level)); + }); + + resetGrid(); +} + +} // namespace feature_detection +} // namespace svo + diff --git a/CPU_version/svo/src/frame.cpp b/CPU_version/svo/src/frame.cpp new file mode 100644 index 0000000..44ddab6 --- /dev/null +++ b/CPU_version/svo/src/frame.cpp @@ -0,0 +1,190 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Frame::frame_counter_ = 0; + +Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) : + id_(frame_counter_++), + timestamp_(timestamp), + cam_(cam), + key_pts_(5), + is_keyframe_(false), + v_kf_(NULL) +{ + initFrame(img); +} + +Frame::~Frame() +{ + std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;}); +} + +void Frame::initFrame(const cv::Mat& img) +{ + // check image + if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height()) + throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale"); + + // Set keypoints to NULL + std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; }); + + // Build Image Pyramid + frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_); +} + +void Frame::setKeyframe() +{ + is_keyframe_ = true; + setKeyPoints(); +} + +void Frame::addFeature(Feature* ftr) +{ + fts_.push_back(ftr); +} + +void Frame::setKeyPoints() +{ + for(size_t i = 0; i < 5; ++i) + if(key_pts_[i] != NULL) + if(key_pts_[i]->point == NULL) + key_pts_[i] = NULL; + + std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); }); +} + +void Frame::checkKeyPoints(Feature* ftr) +{ + const int cu = cam_->width()/2; + const int cv = cam_->height()/2; + + // center pixel + if(key_pts_[0] == NULL) + key_pts_[0] = ftr; + else if(std::max(std::fabs(ftr->px[0]-cu), std::fabs(ftr->px[1]-cv)) + < std::max(std::fabs(key_pts_[0]->px[0]-cu), std::fabs(key_pts_[0]->px[1]-cv))) + key_pts_[0] = ftr; + + if(ftr->px[0] >= cu && ftr->px[1] >= cv) + { + if(key_pts_[1] == NULL) + key_pts_[1] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[1]-cv)) + key_pts_[1] = ftr; + } + if(ftr->px[0] >= cu && ftr->px[1] < cv) + { + if(key_pts_[2] == NULL) + key_pts_[2] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[1]-cv)) + key_pts_[2] = ftr; + } + if(ftr->px[0] < cv && ftr->px[1] < cv) + { + if(key_pts_[3] == NULL) + key_pts_[3] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[1]-cv)) + key_pts_[3] = ftr; + } + if(ftr->px[0] < cv && ftr->px[1] >= cv) + { + if(key_pts_[4] == NULL) + key_pts_[4] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[4]->px[0]-cu) * (key_pts_[4]->px[1]-cv)) + key_pts_[4] = ftr; + } +} + +void Frame::removeKeyPoint(Feature* ftr) +{ + bool found = false; + std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){ + if(i == ftr) { + i = NULL; + found = true; + } + }); + if(found) + setKeyPoints(); +} + +bool Frame::isVisible(const Vector3d& xyz_w) const +{ + Vector3d xyz_f = T_f_w_*xyz_w; + if(xyz_f.z() < 0.0) + return false; // point is behind the camera + Vector2d px = f2c(xyz_f); + if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height()) + return true; + return false; +} + + +/// Utility functions for the Frame class +namespace frame_utils { + +void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr) +{ + pyr.resize(n_levels); + pyr[0] = img_level_0; + for(int i=1; i depth_vec; + depth_vec.reserve(frame.fts_.size()); + depth_min = std::numeric_limits::max(); + for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it) + { + if((*it)->point != NULL) + { + const double z = frame.w2f((*it)->point->pos_).z(); + depth_vec.push_back(z); + depth_min = fmin(z, depth_min); + } + } + if(depth_vec.empty()) + { + SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!"); + return false; + } + depth_mean = vk::getMedian(depth_vec); + return true; +} + +} // namespace frame_utils +} // namespace svo diff --git a/CPU_version/svo/src/frame_handler_base.cpp b/CPU_version/svo/src/frame_handler_base.cpp new file mode 100644 index 0000000..175ee01 --- /dev/null +++ b/CPU_version/svo/src/frame_handler_base.cpp @@ -0,0 +1,199 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo +{ + +// definition of global and static variables which were declared in the header +#ifdef SVO_TRACE +vk::PerformanceMonitor* g_permon = NULL; +#endif + +FrameHandlerBase::FrameHandlerBase() : + stage_(STAGE_PAUSED), + set_reset_(false), + set_start_(false), + acc_frame_timings_(10), + acc_num_obs_(10), + num_obs_last_(0), + tracking_quality_(TRACKING_INSUFFICIENT) +{ +#ifdef SVO_TRACE + // Initialize Performance Monitor + g_permon = new vk::PerformanceMonitor(); + g_permon->addTimer("pyramid_creation"); + g_permon->addTimer("sparse_img_align"); + g_permon->addTimer("reproject"); + g_permon->addTimer("reproject_kfs"); + g_permon->addTimer("reproject_candidates"); + g_permon->addTimer("feature_align"); + g_permon->addTimer("pose_optimizer"); + g_permon->addTimer("point_optimizer"); + g_permon->addTimer("local_ba"); + g_permon->addTimer("tot_time"); + g_permon->addLog("timestamp"); + g_permon->addLog("img_align_n_tracked"); + g_permon->addLog("repr_n_mps"); + g_permon->addLog("repr_n_new_references"); + g_permon->addLog("sfba_thresh"); + g_permon->addLog("sfba_error_init"); + g_permon->addLog("sfba_error_final"); + g_permon->addLog("sfba_n_edges_final"); + g_permon->addLog("loba_n_erredges_init"); + g_permon->addLog("loba_n_erredges_fin"); + g_permon->addLog("loba_err_init"); + g_permon->addLog("loba_err_fin"); + g_permon->addLog("n_candidates"); + g_permon->addLog("dropout"); + g_permon->init(Config::traceName(), Config::traceDir()); +#endif + + SVO_INFO_STREAM("SVO initialized"); +} + +FrameHandlerBase::~FrameHandlerBase() +{ + SVO_INFO_STREAM("SVO destructor invoked"); +#ifdef SVO_TRACE + delete g_permon; +#endif +} + +bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp) +{ + if(set_start_) + { + resetAll(); + stage_ = STAGE_FIRST_FRAME; + } + + if(stage_ == STAGE_PAUSED) + return false; + + SVO_LOG(timestamp); + SVO_DEBUG_STREAM("New Frame"); + SVO_START_TIMER("tot_time"); + timer_.start(); + + // some cleanup from last iteration, can't do before because of visualization + map_.emptyTrash(); + return true; +} + +int FrameHandlerBase::finishFrameProcessingCommon( + const size_t update_id, + const UpdateResult dropout, + const size_t num_observations) +{ + SVO_DEBUG_STREAM("Frame: "<writeToFile(); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + size_t n_candidates = map_.point_candidates_.candidates_.size(); + SVO_LOG(n_candidates); + } +#endif + + if(dropout == RESULT_FAILURE && + (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING )) + { + stage_ = STAGE_RELOCALIZING; + tracking_quality_ = TRACKING_INSUFFICIENT; + } + else if (dropout == RESULT_FAILURE) + resetAll(); + if(set_reset_) + resetAll(); + + return 0; +} + +void FrameHandlerBase::resetCommon() +{ + map_.reset(); + stage_ = STAGE_PAUSED; + set_reset_ = false; + set_start_ = false; + tracking_quality_ = TRACKING_INSUFFICIENT; + num_obs_last_ = 0; + SVO_INFO_STREAM("RESET"); +} + +void FrameHandlerBase::setTrackingQuality(const size_t num_observations) +{ + tracking_quality_ = TRACKING_GOOD; + if(num_observations < Config::qualityMinFts()) + { + SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!"); + tracking_quality_ = TRACKING_INSUFFICIENT; + } + const int feature_drop = static_cast(std::min(num_obs_last_, Config::maxFts())) - num_observations; + if(feature_drop > Config::qualityMaxFtsDrop()) + { + SVO_WARN_STREAM("Lost "<< feature_drop <<" features!"); + tracking_quality_ = TRACKING_INSUFFICIENT; + } +} + +bool ptLastOptimComparator(Point* lhs, Point* rhs) +{ + return (lhs->last_structure_optim_ < rhs->last_structure_optim_); +} + +void FrameHandlerBase::optimizeStructure( + FramePtr frame, + size_t max_n_pts, + int max_iter) +{ + deque pts; + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point != NULL) + pts.push_back((*it)->point); + } + max_n_pts = min(max_n_pts, pts.size()); + nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); + for(deque::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it) + { + (*it)->optimize(max_iter); + (*it)->last_structure_optim_ = frame->id_; + } +} + + +} // namespace svo diff --git a/CPU_version/svo/src/frame_handler_mono.cpp b/CPU_version/svo/src/frame_handler_mono.cpp new file mode 100644 index 0000000..799cb8c --- /dev/null +++ b/CPU_version/svo/src/frame_handler_mono.cpp @@ -0,0 +1,307 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef USE_BUNDLE_ADJUSTMENT +#include +#endif + +namespace svo { + +FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) : + FrameHandlerBase(), + cam_(cam), + reprojector_(cam_, map_), + depth_filter_(NULL) +{ + initialize(); + imu_integPtr_=std::make_unique(SE_init); +} + +void FrameHandlerMono::initialize() +{ + feature_detection::DetectorPtr feature_detector( + new feature_detection::FastDetector( + cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels())); + DepthFilter::callback_t depth_filter_cb = boost::bind( + &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2); + depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb); + depth_filter_->startThread(); +} + +FrameHandlerMono::~FrameHandlerMono() +{ + delete depth_filter_; +} + +void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp) +{ + if(!startFrameProcessingCommon(timestamp)) + return; + + // some cleanup from last iteration, can't do before because of visualization + core_kfs_.clear(); + overlap_kfs_.clear(); + + // create new frame + new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + + // process frame + UpdateResult res = RESULT_FAILURE; + if(stage_ == STAGE_DEFAULT_FRAME) + res = processFrame(); + else if(stage_ == STAGE_SECOND_FRAME) + res = processSecondFrame(); + else if(stage_ == STAGE_FIRST_FRAME) + res = processFirstFrame(); + else if(stage_ == STAGE_RELOCALIZING) + res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()), + map_.getClosestKeyframe(last_frame_)); + + // set last frame + last_frame_ = new_frame_; + new_frame_.reset(); + // finish processing + finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); +} + +FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() +{ + new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()); + if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE) + return RESULT_NO_KEYFRAME; + new_frame_->setKeyframe(); + map_.addKeyframe(new_frame_); + stage_ = STAGE_SECOND_FRAME; + SVO_INFO_STREAM("Init: Selected first frame."); + return RESULT_IS_KEYFRAME; +} + +FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() +{ + initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_); + if(res == initialization::FAILURE) + return RESULT_FAILURE; + else if(res == initialization::NO_KEYFRAME) + return RESULT_NO_KEYFRAME; + + // two-frame bundle adjustment +#ifdef USE_BUNDLE_ADJUSTMENT + ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_); +#endif + + new_frame_->setKeyframe(); + double depth_mean, depth_min; + frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + + // add frame to map + map_.addKeyframe(new_frame_); + stage_ = STAGE_DEFAULT_FRAME; + klt_homography_init_.reset(); + SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map."); + return RESULT_IS_KEYFRAME; +} + +FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() +{ + // Set initial pose TODO use prior + new_frame_->T_f_w_ = last_frame_->T_f_w_; + + // sparse image align + SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), + 30, SparseImgAlign::GaussNewton, false, false); + img_align.run(last_frame_, new_frame_); + // map reprojection & feature alignment + reprojector_.reprojectMap(new_frame_, overlap_kfs_); + std::cerr<<"Reprojection Map:\t nPoints = "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps + tracking_quality_ = TRACKING_INSUFFICIENT; + return RESULT_FAILURE; + } + + // pose optimization subject to change for adding IMU + size_t sfba_n_edges_final; + if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh())) + return RESULT_FAILURE; + + + std::cerr<<"Reprojected points after opmization: "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps + return RESULT_FAILURE; + } + double depth_mean, depth_min; + frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD) + { + depth_filter_->addFrame(new_frame_); + return RESULT_NO_KEYFRAME; + } + new_frame_->setKeyframe(); + + // new keyframe selected + for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it) + if((*it)->point != NULL) + (*it)->point->addFrameRef(*it); + map_.point_candidates_.addCandidatePointToFrame(new_frame_); + + // optional bundle adjustment +#ifdef USE_BUNDLE_ADJUSTMENT + if(Config::lobaNumIter() > 0) + { + SVO_START_TIMER("local_ba"); + setCoreKfs(Config::coreNKfs()); + size_t loba_n_erredges_init, loba_n_erredges_fin; + double loba_err_init, loba_err_fin; + ba::localBA(new_frame_.get(), &core_kfs_, &map_, + loba_n_erredges_init, loba_n_erredges_fin, + loba_err_init, loba_err_fin); + SVO_STOP_TIMER("local_ba"); + SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin); + SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + + // if limited number of keyframes, remove the one furthest apart + if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs()) + { + FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos()); + depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better + map_.safeDeleteFrame(furthest_frame); + } + + // add keyframe to map + map_.addKeyframe(new_frame_); + std::cerr<<"Svo pipline was finished\n"; + return RESULT_IS_KEYFRAME; +} + +FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame( + const SE3& T_cur_ref, + FramePtr ref_keyframe) +{ + if(ref_keyframe == nullptr) + { + return RESULT_FAILURE; + } + SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), + 30, SparseImgAlign::GaussNewton, false, false); + size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_); + usleep(2000); + if(img_align_n_tracked > Config::qualityMinFts()) + { + SE3 T_f_w_last = last_frame_->T_f_w_; + last_frame_ = ref_keyframe; + FrameHandlerMono::UpdateResult res = processFrame(); + if(res != RESULT_FAILURE) + { + stage_ = STAGE_DEFAULT_FRAME; + } + else + new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose + return res; + } + return RESULT_FAILURE; +} + +bool FrameHandlerMono::relocalizeFrameAtPose( + const int keyframe_id, + const SE3& T_f_kf, + const cv::Mat& img, + const double timestamp) +{ + FramePtr ref_keyframe; + if(!map_.getKeyframeById(keyframe_id, ref_keyframe)) + return false; + new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe); + if(res != RESULT_FAILURE) { + last_frame_ = new_frame_; + return true; + } + return false; +} + +void FrameHandlerMono::resetAll() +{ + resetCommon(); + last_frame_.reset(); + new_frame_.reset(); + core_kfs_.clear(); + overlap_kfs_.clear(); + depth_filter_->reset(); +} + +void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame) +{ + resetAll(); + last_frame_ = first_frame; + last_frame_->setKeyframe(); + map_.addKeyframe(last_frame_); + stage_ = STAGE_DEFAULT_FRAME; +} + +bool FrameHandlerMono::needNewKf(double scene_depth_mean) +{ + for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it) + { + Vector3d relpos = new_frame_->w2f(it->first->pos()); + if(fabs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() && + fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist()*0.8 && + fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist()*1.3) + return false; + } + return true; +} + +void FrameHandlerMono::setCoreKfs(size_t n_closest) +{ + size_t n = min(n_closest, overlap_kfs_.size()-1); + std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(), + boost::bind(&pair::second, _1) > + boost::bind(&pair::second, _2)); + std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair& i){ core_kfs_.insert(i.first); }); +} + +} // namespace svo diff --git a/CPU_version/svo/src/imu_integration.cpp b/CPU_version/svo/src/imu_integration.cpp new file mode 100644 index 0000000..a66223c --- /dev/null +++ b/CPU_version/svo/src/imu_integration.cpp @@ -0,0 +1,153 @@ +// +// Created by gfuse on 4/9/21. +// +//#define NDEBUG +#include "svo/imu_integration.h" +#include +#include +#include "svo/feature.h" +#include "svo/point.h" +#include "svo/svoVisionFactor.h" +#include +#include +#include +#include + +Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ + graphPtr=std::make_shared(); + valuesPtr=std::make_shared(); + gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0); + parameterPtr = boost::shared_ptr(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z()))); + parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame + parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous + parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous + parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous + parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC(); // acc bias in continuous + parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC(); // gyro bias in continuous + parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP(); // error in the bias used for preintegration + imu_biasPtr=std::make_shared(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8)); + preintegratedPtr = std::make_shared(parameterPtr,*imu_biasPtr); + ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection + optimizerParamPtr=std::make_shared(); + optimizerParamPtr->relinearizeThreshold = 0.1; + optimizerParamPtr->relinearizeSkip = 1; + //optimizerParamPtr->evaluateNonlinearError=true; + assert(preintegratedPtr); +} +Imu_Integration::~Imu_Integration(){ + +} +bool Imu_Integration::update(double* imu){ + if(syn)return false; + if(imu_factor_id<1)return false; + auto in=std::chrono::steady_clock::now(); + if(std::chrono::duration(in-t_1).count()>20.0){ + t_1=in; + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005); + }else{ + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration(in-t_1).count()); + t_1=in; + ++imu_n; + } + return true; + +} +bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.calculateEstimate(); + // Overwrite the beginning of the preintegration for the next step. + statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); + imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + ++imu_factor_id; + syn=false; + new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); + new_frame->Cov_=optimizer.marginalCovariance(P(imu_factor_id-1)); + graphPtr->resize(0); + valuesPtr->clear(); + valuesPtr->insert(P(imu_factor_id-1),statePtr->pose()); + valuesPtr->insert(V(imu_factor_id-1),statePtr->v()); + valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr); + graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),optimizer.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1))); + return true; +} +bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.optimize(); + // Overwrite the beginning of the preintegration for the next step. + statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); + imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + ++imu_factor_id; + syn=false; + gtsam::Marginals marginals(*graphPtr,result); + new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); + new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1)); + graphPtr->resize(0); + valuesPtr->clear(); + valuesPtr->insert(P(imu_factor_id-1),statePtr->pose()); + valuesPtr->insert(V(imu_factor_id-1),statePtr->v()); + valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr); + graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1))); + return true; +} +bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size_t& num_obs,const double reproj_thresh){ + usleep(5000); + syn=true; + if(imu_factor_id<1){ + statePtr=std::make_shared(gtsam::Pose3(new_frame->T_f_w_.matrix()), + gtsam::Vector3(1e-12,1e-12,1e-12)); + valuesPtr->insert(P(0),statePtr->pose()); + valuesPtr->insert(V(0),statePtr->v()); + valuesPtr->insert(B(0),*imu_biasPtr); + graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished())); + graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); + graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); + }else{ + if(imu_n<1)return false; + auto preint_imu_combined =dynamic_cast(*preintegratedPtr); + valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix())); + valuesPtr->insert(V(imu_factor_id), statePtr->v()); + valuesPtr->insert(B(imu_factor_id), *imu_biasPtr); + gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id), + preint_imu_combined); + graphPtr->emplace_shared(imu_factor); + imu_n=0; + } + + num_obs=0; + for(auto&& f:new_frame->fts_){ + if(f->point==NULL)continue; + graphPtr->emplace_shared(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id)); + ++num_obs; + } + // Now optimize. + //gtsam::ISAM2 optimizer(*optimizerParamPtr); + //optimizer.update(*graphPtr, *valuesPtr); + // Each call to iSAM2 update(*) performs one iteration of the iterative + // nonlinear solver. If accuracy is desired at the expense of time, + // update(*) can be called additional times to perform multiple optimizer + // iterations every step. + //optimizer.update(); + // optimizer.print(); + gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr); + + reset(optimizer,new_frame); + // Remove Measurements with too large reprojection error + double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2(); + for(auto&& f:new_frame->fts_) + { + if(f->point == NULL) + continue; + Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<level); + if(e.norm() > reproj_thresh_scaled) + { + // we don't need to delete a reference in the point since it was not created yet + f->point = NULL; + --num_obs; + } + } + return true; +} diff --git a/CPU_version/svo/src/initialization.cpp b/CPU_version/svo/src/initialization.cpp new file mode 100644 index 0000000..571a0bc --- /dev/null +++ b/CPU_version/svo/src/initialization.cpp @@ -0,0 +1,199 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace initialization { + +InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) +{ + reset(); + detectFeatures(frame_ref, px_ref_, f_ref_); + if(px_ref_.size() < 100) + { + SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment."); + return FAILURE; + } + frame_ref_ = frame_ref; + px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end()); + return SUCCESS; +} + +InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) +{ + trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_); + SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features"); + + if(disparities_.size() < Config::initMinTracked()) + return FAILURE; + + double disparity = vk::getMedian(disparities_); + SVO_INFO_STREAM("Init: KLT "<cam_->errorMultiplier2(), Config::poseOptimThresh(), + inliers_, xyz_in_cur_, T_cur_from_ref_); + SVO_INFO_STREAM("Init: Homography RANSAC "< depth_vec; + for(size_t i=0; iT_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_; + frame_cur->T_f_w_.translation() = + -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); + + // For each inlier create 3D point and add feature in both frames + SE3 T_world_cur = frame_cur->T_f_w_.inverse(); + for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) + { + Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y); + Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y); + if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) + { + Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); + Point* new_point = new Point(pos); + + Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0)); + frame_cur->addFeature(ftr_cur); + new_point->addFrameRef(ftr_cur); + + Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0)); + frame_ref_->addFeature(ftr_ref); + new_point->addFrameRef(ftr_ref); + } + } + return SUCCESS; +} + +void KltHomographyInit::reset() +{ + px_cur_.clear(); + frame_ref_.reset(); +} + +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec) +{ + Features new_features; + feature_detection::FastDetector detector( + frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels()); + detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); + + // now for all maximum corners, initialize a new seed + px_vec.clear(); px_vec.reserve(new_features.size()); + f_vec.clear(); f_vec.reserve(new_features.size()); + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1])); + f_vec.push_back(ftr->f); + delete ftr; + }); +} + +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities) +{ + const double klt_win_size = 30.0; + const int klt_max_iter = 30; + const double klt_eps = 0.001; + vector status; + vector error; + vector min_eig_vec; + cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps); + cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0], + px_ref, px_cur, + status, error, + cv::Size2i(klt_win_size, klt_win_size), + 4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW); + + vector::iterator px_ref_it = px_ref.begin(); + vector::iterator px_cur_it = px_cur.begin(); + vector::iterator f_ref_it = f_ref.begin(); + f_cur.clear(); f_cur.reserve(px_cur.size()); + disparities.clear(); disparities.reserve(px_cur.size()); + for(size_t i=0; px_ref_it != px_ref.end(); ++i) + { + if(!status[i]) + { + px_ref_it = px_ref.erase(px_ref_it); + px_cur_it = px_cur.erase(px_cur_it); + f_ref_it = f_ref.erase(f_ref_it); + continue; + } + f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y)); + disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm()); + ++px_ref_it; + ++px_cur_it; + ++f_ref_it; + } +} + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE3& T_cur_from_ref) +{ + vector uv_ref(f_ref.size()); + vector uv_cur(f_cur.size()); + for(size_t i=0, i_max=f_ref.size(); i outliers; + vk::computeInliers(f_cur, f_ref, + Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(), + reprojection_threshold, focal_length, + xyz_in_cur, inliers, outliers); + T_cur_from_ref = Homography.T_c2_from_c1; +} + + +} // namespace initialization +} // namespace svo diff --git a/CPU_version/svo/src/map.cpp b/CPU_version/svo/src/map.cpp new file mode 100644 index 0000000..2ecad1c --- /dev/null +++ b/CPU_version/svo/src/map.cpp @@ -0,0 +1,368 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include + +namespace svo { + +Map::Map() {} + +Map::~Map() +{ + reset(); + SVO_INFO_STREAM("Map destructed"); +} + +void Map::reset() +{ + keyframes_.clear(); + point_candidates_.reset(); + emptyTrash(); +} + +bool Map::safeDeleteFrame(FramePtr frame) +{ + bool found = false; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + if(*it == frame) + { + std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){ + removePtFrameRef(it->get(), ftr); + }); + keyframes_.erase(it); + found = true; + break; + } + } + + point_candidates_.removeFrameCandidates(frame); + + if(found) + return true; + + SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there."); + return false; +} + +void Map::removePtFrameRef(Frame* frame, Feature* ftr) +{ + if(ftr->point == NULL) + return; // mappoint may have been deleted in a previous ref. removal + Point* pt = ftr->point; + ftr->point = NULL; + if(pt->obs_.size() <= 2) + { + // If the references list of mappoint has only size=2, delete mappoint + safeDeletePoint(pt); + return; + } + pt->deleteFrameRef(frame); // Remove reference from map_point + frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe +} + +void Map::safeDeletePoint(Point* pt) +{ + // Delete references to mappoints in all keyframes + std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){ + ftr->point=NULL; + ftr->frame->removeKeyPoint(ftr); + }); + pt->obs_.clear(); + + // Delete mappoint + deletePoint(pt); +} + +void Map::deletePoint(Point* pt) +{ + pt->type_ = Point::TYPE_DELETED; + trash_points_.push_back(pt); +} + +void Map::addKeyframe(FramePtr new_keyframe) +{ + keyframes_.push_back(new_keyframe); +} + +void Map::getCloseKeyframes( + const FramePtr& frame, + std::list< std::pair >& close_kfs) const +{ + for(auto kf : keyframes_) + { + // check if kf has overlaping field of view with frame, use therefore KeyPoints + for(auto keypoint : kf->key_pts_) + { + if(keypoint == nullptr) + continue; + + if(frame->isVisible(keypoint->point->pos_)) + { + close_kfs.push_back( + std::make_pair( + kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm())); + break; // this keyframe has an overlapping field of view -> add to close_kfs + } + } + } +} + +FramePtr Map::getClosestKeyframe(const FramePtr& frame) const +{ + list< pair > close_kfs; + getCloseKeyframes(frame, close_kfs); + if(close_kfs.empty()) + { + return nullptr; + } + + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + if(close_kfs.front().first != frame) + return close_kfs.front().first; + close_kfs.pop_front(); + return close_kfs.front().first; +} + +FramePtr Map::getFurthestKeyframe(const Vector3d& pos) const +{ + FramePtr furthest_kf; + double maxdist = 0.0; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + double dist = ((*it)->pos()-pos).norm(); + if(dist > maxdist) { + maxdist = dist; + furthest_kf = *it; + } + } + return furthest_kf; +} + +bool Map::getKeyframeById(const int id, FramePtr& frame) const +{ + bool found = false; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + if((*it)->id_ == id) { + found = true; + frame = *it; + break; + } + return found; +} + +void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s) +{ + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + Vector3d pos = s*R*(*it)->pos() + t; + Matrix3d rot = R*(*it)->T_f_w_.rotation_matrix().inverse(); + (*it)->T_f_w_ = SE3(rot, pos).inverse(); + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if((*ftr)->point->last_published_ts_ == -1000) + continue; + (*ftr)->point->last_published_ts_ = -1000; + (*ftr)->point->pos_ = s*R*(*ftr)->point->pos_ + t; + } + } +} + +void Map::emptyTrash() +{ + std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){ + delete pt; + pt=NULL; + }); + trash_points_.clear(); + point_candidates_.emptyTrash(); +} + +MapPointCandidates::MapPointCandidates() +{} + +MapPointCandidates::~MapPointCandidates() +{ + reset(); +} + +void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2) +{ + point->type_ = Point::TYPE_CANDIDATE; + boost::unique_lock lock(mut_); + candidates_.push_back(PointCandidate(point, point->obs_.front())); +} + +void MapPointCandidates::addCandidatePointToFrame(FramePtr frame) +{ + boost::unique_lock lock(mut_); + PointCandidateList::iterator it=candidates_.begin(); + while(it != candidates_.end()) + { + if(it->first->obs_.front()->frame == frame.get()) + { + // insert feature in the frame + it->first->type_ = Point::TYPE_UNKNOWN; + it->first->n_failed_reproj_ = 0; + it->second->frame->addFeature(it->second); + it = candidates_.erase(it); + } + else + ++it; + } +} + +bool MapPointCandidates::deleteCandidatePoint(Point* point) +{ + boost::unique_lock lock(mut_); + for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it) + { + if(it->first == point) + { + deleteCandidate(*it); + candidates_.erase(it); + return true; + } + } + return false; +} + +void MapPointCandidates::removeFrameCandidates(FramePtr frame) +{ + boost::unique_lock lock(mut_); + auto it=candidates_.begin(); + while(it!=candidates_.end()) + { + if(it->second->frame == frame.get()) + { + deleteCandidate(*it); + it = candidates_.erase(it); + } + else + ++it; + } +} + +void MapPointCandidates::reset() +{ + boost::unique_lock lock(mut_); + std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){ + delete c.first; + delete c.second; + }); + candidates_.clear(); +} + +void MapPointCandidates::deleteCandidate(PointCandidate& c) +{ + // camera-rig: another frame might still be pointing to the candidate point + // therefore, we can't delete it right now. + delete c.second; c.second=NULL; + c.first->type_ = Point::TYPE_DELETED; + trash_points_.push_back(c.first); +} + +void MapPointCandidates::emptyTrash() +{ + std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){ + delete p; p=NULL; + }); + trash_points_.clear(); +} + +namespace map_debug { + +void mapValidation(Map* map, int id) +{ + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + frameValidation(it->get(), id); +} + +void frameValidation(Frame* frame, int id) +{ + for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point==NULL) + continue; + + if((*it)->point->type_ == Point::TYPE_DELETED) + printf("ERROR DataValidation %i: Referenced point was deleted.\n", id); + + if(!(*it)->point->findFrameRef(frame)) + printf("ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n", id); + + pointValidation((*it)->point, id); + } + for(auto it=frame->key_pts_.begin(); it!=frame->key_pts_.end(); ++it) + if(*it != NULL) + if((*it)->point == NULL) + printf("ERROR DataValidation %i: KeyPoints not correct!\n", id); +} + +void pointValidation(Point* point, int id) +{ + for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it) + { + bool found=false; + for(auto it_ftr=(*it)->frame->fts_.begin(); it_ftr!=(*it)->frame->fts_.end(); ++it_ftr) + if((*it_ftr)->point == point) { + found=true; break; + } + if(!found) + printf("ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n", id, point->id_, (*it)->frame->id_, (int) point->type_); + } +} + +void mapStatistics(Map* map) +{ + // compute average number of features which each frame observes + size_t n_pt_obs(0); + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + n_pt_obs += (*it)->nObs(); + printf("\n\nMap Statistics: Frame avg. point obs = %f\n", (float) n_pt_obs/map->size()); + + // compute average number of observations that each point has + size_t n_frame_obs(0); + size_t n_pts(0); + std::set points; + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + { + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if(points.insert((*ftr)->point).second) { + ++n_pts; + n_frame_obs += (*ftr)->point->nRefs(); + } + } + } + printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts); +} + +} // namespace map_debug +} // namespace svo diff --git a/CPU_version/svo/src/matcher.cpp b/CPU_version/svo/src/matcher.cpp new file mode 100644 index 0000000..38a89fb --- /dev/null +++ b/CPU_version/svo/src/matcher.cpp @@ -0,0 +1,323 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +namespace warp { + +void getWarpMatrixAffine( + const vk::AbstractCamera& cam_ref, + const vk::AbstractCamera& cam_cur, + const Vector2d& px_ref, + const Vector3d& f_ref, + const double depth_ref, + const SE3& T_cur_ref, + const int level_ref, + Matrix2d& A_cur_ref) +{ + // Compute affine warp matrix A_ref_cur + const int halfpatch_size = 5; + const Vector3d xyz_ref(f_ref*depth_ref); + Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1< 3.0 && search_level < max_level) + { + search_level += 1; + D *= 0.25; + } + return search_level; +} + +void warpAffine( + const Matrix2d& A_cur_ref, + const cv::Mat& img_ref, + const Vector2d& px_ref, + const int level_ref, + const int search_level, + const int halfpatch_size, + uint8_t* patch) +{ + const int patch_size = halfpatch_size*2 ; + const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + if(isnan(A_ref_cur(0,0))) + { + printf("Affine warp is NaN, probably camera has no translation\n"); // TODO + return; + } + + // Perform the warp on a larger patch. + uint8_t* patch_ptr = patch; + const Vector2f px_ref_pyr = px_ref.cast() / (1<=img_ref.cols-1 || px[1]>=img_ref.rows-1) + *patch_ptr = 0; + else + *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]); + } + } +} + +} // namespace warp + +bool depthFromTriangulation( + const SE3& T_search_ref, + const Vector3d& f_ref, + const Vector3d& f_cur, + double& depth) +{ + Matrix A; A << T_search_ref.rotation_matrix() * f_ref, f_cur; + const Matrix2d AtA = A.transpose()*A; + if(AtA.determinant() < 0.000001) + return false; + const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation(); + depth = fabs(depth2[0]); + return true; +} + +void Matcher::createPatchFromPatchWithBorder() +{ + uint8_t* ref_patch_ptr = patch_; + for(int y=1; yframe->cam_->isInFrame( + ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) + return false; + + // warp affine + warp::getWarpMatrixAffine( + *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, + (ref_ftr_->frame->pos() - pt.pos_).norm(), + cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_); + search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); + warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_); + createPatchFromPatchWithBorder(); + + // px_cur should be set + Vector2d px_scaled(px_cur/(1<type == Feature::EDGELET) + { + Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); + dir_cur.normalize(); + success = feature_alignment::align1D( + cur_frame.img_pyr_[search_level_], dir_cur.cast(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + } + else + { + success = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + } + px_cur = px_scaled * (1<world2cam(A)); + Vector2d px_B(cur_frame.cam_->world2cam(B)); + epi_length_ = (px_A-px_B).norm() / (1<().normalized(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + else + res = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + if(res) + { + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + return true; + } + return false; + } + + size_t n_steps = epi_length_/0.7; // one step per pixel + Vector2d step = epi_dir_/n_steps; + + if(n_steps > options_.max_epi_search_steps) + { + printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n", + n_steps, epi_length_, d_min, d_max); + return false; + } + + // for matching, precompute sum and sum2 of warped reference patch + int pixel_sum = 0; + int pixel_sum_square = 0; + PatchScore patch_score(patch_); + + // now we sample along the epipolar line + Vector2d uv = B-step; + Vector2i last_checked_pxi(0,0); + ++n_steps; + for(size_t i=0; iworld2cam(uv)); + Vector2i pxi(px[0]/(1<isInFrame(pxi, patch_size_, search_level_)) + continue; + + // TODO interpolation would probably be a good idea + uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data + + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols + + (pxi[0]-halfpatch_size_); + int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols); + + if(zmssd < zmssd_best) { + zmssd_best = zmssd; + uv_best = uv; + } + } + + if(zmssd_best < PatchScore::threshold()) + { + if(options_.subpix_refinement) + { + px_cur_ = cur_frame.cam_->world2cam(uv_best); + Vector2d px_scaled(px_cur_/(1<().normalized(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + else + res = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + if(res) + { + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + return true; + } + return false; + } + px_cur_ = cur_frame.cam_->world2cam(uv_best); + if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) + return true; + } + return false; +} + +} // namespace svo diff --git a/CPU_version/svo/src/point.cpp b/CPU_version/svo/src/point.cpp new file mode 100644 index 0000000..2e45e06 --- /dev/null +++ b/CPU_version/svo/src/point.cpp @@ -0,0 +1,179 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include + +namespace svo { + +int Point::point_counter_ = 0; + +Point::Point(const Vector3d& pos) : + id_(point_counter_++), + pos_(pos), + normal_set_(false), + n_obs_(0), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{} + +Point::Point(const Vector3d& pos, Feature* ftr) : + id_(point_counter_++), + pos_(pos), + normal_set_(false), + n_obs_(1), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{ + obs_.push_front(ftr); +} + +Point::~Point() +{} + +void Point::addFrameRef(Feature* ftr) +{ + obs_.push_front(ftr); + ++n_obs_; +} + +Feature* Point::findFrameRef(Frame* frame) +{ + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + if((*it)->frame == frame) + return *it; + return NULL; // no keyframe found +} + +bool Point::deleteFrameRef(Frame* frame) +{ + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + { + if((*it)->frame == frame) + { + obs_.erase(it); + return true; + } + } + return false; +} + +void Point::initNormal() +{ + assert(!obs_.empty()); + const Feature* ftr = obs_.back(); + assert(ftr->frame != NULL); + normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f); + normal_information_ = DiagonalMatrix(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0); + normal_set_ = true; +} + +bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const +{ + // TODO: get frame with same point of view AND same pyramid level! + Vector3d obs_dir(framepos - pos_); obs_dir.normalize(); + auto min_it=obs_.begin(); + double min_cos_angle = 0; + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + { + Vector3d dir((*it)->frame->pos() - pos_); dir.normalize(); + double cos_angle = obs_dir.dot(dir); + if(cos_angle > min_cos_angle) + { + min_cos_angle = cos_angle; + min_it = it; + } + } + ftr = *min_it; + if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless + return false; + return true; +} + +void Point::optimize(const size_t n_iter) +{ + Vector3d old_point = pos_; + double chi2 = 0.0; + Matrix3d A; + Vector3d b; + + for(size_t i=0; iframe->T_f_w_ * pos_); + Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J); + const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f)); + new_chi2 += e.squaredNorm(); + A.noalias() += J.transpose() * J; + b.noalias() -= J.transpose() * e; + } + + // solve linear system + const Vector3d dp(A.ldlt().solve(b)); + + // check if error increased + if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0])) + { +#ifdef POINT_OPTIMIZER_DEBUG + cout << "it " << i + << "\t FAILURE \t new_chi2 = " << new_chi2 << endl; +#endif + pos_ = old_point; // roll-back + break; + } + + // update the model + Vector3d new_point = pos_ + dp; + old_point = pos_; + pos_ = new_point; + chi2 = new_chi2; +#ifdef POINT_OPTIMIZER_DEBUG + cout << "it " << i + << "\t Success \t new_chi2 = " << new_chi2 + << "\t norm(b) = " << vk::norm_max(b) + << endl; +#endif + + // stop when converged + if(vk::norm_max(dp) <= EPS) + break; + } +#ifdef POINT_OPTIMIZER_DEBUG + cout << endl; +#endif +} + +} // namespace svo diff --git a/CPU_version/svo/src/pose_optimizer.cpp b/CPU_version/svo/src/pose_optimizer.cpp new file mode 100644 index 0000000..1c491b0 --- /dev/null +++ b/CPU_version/svo/src/pose_optimizer.cpp @@ -0,0 +1,165 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs) +{ + // init + double chi2(0.0); + vector chi2_vec_init, chi2_vec_final; + vk::robust_cost::TukeyWeightFunction weight_function; + SE3 T_old(frame->T_f_w_); + Matrix6d A; + Vector6d b; + + // compute the scale of the error for robust estimation + std::vector errors; errors.reserve(frame->fts_.size()); + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + //Reprojection error + Vector2d e = vk::project2d((*it)->f) + - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); + e *= 1.0 / (1<<(*it)->level); + errors.push_back(e.norm()); + } + if(errors.empty()) + return; + vk::robust_cost::MADScaleEstimator scale_estimator; + estimated_scale = scale_estimator.compute(errors); + + num_obs = errors.size(); + chi2_vec_init.reserve(num_obs); + chi2_vec_final.reserve(num_obs); + double scale = estimated_scale; + for(size_t iter=0; itercam_->errorMultiplier2(); + + b.setZero(); + A.setZero(); + double new_chi2(0.0); + + // compute residual + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Matrix26d J; + Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_); + Frame::jacobian_xyz2uv(xyz_f, J); + Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f); + double sqrt_inv_cov = 1.0 / (1<<(*it)->level); + e *= sqrt_inv_cov; + if(iter == 0) + chi2_vec_init.push_back(e.squaredNorm()); // just for debug + J *= sqrt_inv_cov; + double weight = weight_function.value(e.norm()/scale); + A.noalias() += J.transpose()*J*weight; + b.noalias() -= J.transpose()*e*weight; + new_chi2 += e.squaredNorm()*weight; + } + + // solve linear system + const Vector6d dT(A.ldlt().solve(b)); + + // check if error increased + if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0])) + { + if(verbose) + std::cout << "it " << iter + << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl; + frame->T_f_w_ = T_old; // roll-back + break; + } + + // update the model + SE3 T_new = SE3::exp(dT)*frame->T_f_w_; + T_old = frame->T_f_w_; + frame->T_f_w_ = T_new; + chi2 = new_chi2; + if(verbose) + std::cout << "it " << iter + << "\t Success \t new_chi2 = " << new_chi2 + << "\t norm(dT) = " << vk::norm_max(dT) << std::endl; + + // stop when converged + if(vk::norm_max(dT) <= EPS) + break; + } + + // Set covariance as inverse information matrix. Optimistic estimator! + const double pixel_variance=1.0; + frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse(); + + // Remove Measurements with too large reprojection error + double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2(); + size_t n_deleted_refs = 0; + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); + double sqrt_inv_cov = 1.0 / (1<<(*it)->level); + e *= sqrt_inv_cov; + chi2_vec_final.push_back(e.squaredNorm()); + if(e.norm() > reproj_thresh_scaled) + { + // we don't need to delete a reference in the point since it was not created yet + (*it)->point = NULL; + ++n_deleted_refs; + } + } + + error_init=0.0; + error_final=0.0; + if(!chi2_vec_init.empty()) + error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2(); + if(!chi2_vec_final.empty()) + error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2(); + + estimated_scale *= frame->cam_->errorMultiplier2(); + if(verbose) + std::cout << "n deleted obs = " << n_deleted_refs + << "\t scale = " << estimated_scale + << "\t error init = " << error_init + << "\t error end = " << error_final << std::endl; + num_obs -= n_deleted_refs; +} + +} // namespace pose_optimizer +} // namespace svo diff --git a/CPU_version/svo/src/reprojector.cpp b/CPU_version/svo/src/reprojector.cpp new file mode 100644 index 0000000..81a7835 --- /dev/null +++ b/CPU_version/svo/src/reprojector.cpp @@ -0,0 +1,219 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) : + map_(map) +{ + initializeGrid(cam); +} + +Reprojector::~Reprojector() +{ + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; }); +} + +void Reprojector::initializeGrid(vk::AbstractCamera* cam) +{ + grid_.cell_size = Config::gridSize(); + grid_.grid_n_cols = ceil(static_cast(cam->width())/grid_.cell_size); + grid_.grid_n_rows = ceil(static_cast(cam->height())/grid_.cell_size); + grid_.cells.resize(grid_.grid_n_cols*grid_.grid_n_rows); + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell*& c){ c = new Cell; }); + grid_.cell_order.resize(grid_.cells.size()); + for(size_t i=0; iclear(); }); +} + +void Reprojector::reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs) +{ + resetGrid(); + + // Identify those Keyframes which share a common field of view. + SVO_START_TIMER("reproject_kfs"); + list< pair > close_kfs; + map_.getCloseKeyframes(frame, close_kfs); + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + // Reproject all mappoints of the closest N kfs with overlap. We only store + // in which grid cell the points fall. + size_t n = 0; + overlap_kfs.reserve(options_.max_n_kfs); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + overlap_kfs.push_back(pair(ref_frame,0)); + + // Try to reproject each mappoint that the other KF observes + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + // check if the feature has a mappoint assigned + if((*it_ftr)->point == NULL) + continue; + + // make sure we project a point only once + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + if(reprojectPoint(frame, (*it_ftr)->point)) + overlap_kfs.back().second++; + } + } + SVO_STOP_TIMER("reproject_kfs"); + + // Now project all point candidates + SVO_START_TIMER("reproject_candidates"); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + auto it=map_.point_candidates_.candidates_.begin(); + while(it!=map_.point_candidates_.candidates_.end()) + { + if(!reprojectPoint(frame, it->first)) + { + it->first->n_failed_reproj_ += 3; + if(it->first->n_failed_reproj_ > 30) + { + map_.point_candidates_.deleteCandidate(*it); + it = map_.point_candidates_.candidates_.erase(it); + continue; + } + } + ++it; + } + } // unlock the mutex when out of scope + SVO_STOP_TIMER("reproject_candidates"); + + // Now we go through each grid cell and select one point to match. + // At the end, we should have at maximum one reprojected point per cell. + SVO_START_TIMER("feature_align"); + for(size_t i=0; i (size_t) Config::maxFts()) + break; + } + SVO_STOP_TIMER("feature_align"); +} + +bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) +{ + if(lhs.pt->type_ > rhs.pt->type_) + return true; + return false; +} + +bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) +{ + cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); + Cell::iterator it=cell.begin(); + while(it!=cell.end()) + { + ++n_trials_; + + if(it->pt->type_ == Point::TYPE_DELETED) + { + it = cell.erase(it); + continue; + } + + bool found_match = true; + if(options_.find_match_direct) + found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); + if(!found_match) + { + it->pt->n_failed_reproj_++; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) + map_.safeDeletePoint(it->pt); + if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) + map_.point_candidates_.deleteCandidatePoint(it->pt); + it = cell.erase(it); + continue; + } + it->pt->n_succeeded_reproj_++; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) + it->pt->type_ = Point::TYPE_GOOD; + + Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); + frame->addFeature(new_feature); + + // Here we add a reference in the feature to the 3D point, the other way + // round is only done if this frame is selected as keyframe. + new_feature->point = it->pt; + + if(matcher_.ref_ftr_->type == Feature::EDGELET) + { + new_feature->type = Feature::EDGELET; + new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad; + new_feature->grad.normalize(); + } + + // If the keyframe is selected and we reproject the rest, we don't have to + // check this point anymore. + it = cell.erase(it); + + // Maximum one point per cell. + return true; + } + return false; +} + +bool Reprojector::reprojectPoint(FramePtr frame, Point* point) +{ + Vector2d px(frame->w2c(point->pos_)); + if(frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher + { + const int k = static_cast(px[1]/grid_.cell_size)*grid_.grid_n_cols + + static_cast(px[0]/grid_.cell_size); + grid_.cells.at(k)->push_back(Candidate(point, px)); + return true; + } + return false; +} + +} // namespace svo diff --git a/CPU_version/svo/src/sparse_img_align.cpp b/CPU_version/svo/src/sparse_img_align.cpp new file mode 100644 index 0000000..ce1570b --- /dev/null +++ b/CPU_version/svo/src/sparse_img_align.cpp @@ -0,0 +1,274 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +SparseImgAlign::SparseImgAlign( + int max_level, int min_level, int n_iter, + Method method, bool display, bool verbose) : + display_(display), + max_level_(max_level), + min_level_(min_level) +{ + n_iter_ = n_iter; + n_iter_init_ = n_iter_; + method_ = method; + verbose_ = verbose; + eps_ = 0.000001; +} + +size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) +{ + reset(); + + if(ref_frame->fts_.empty()) + { + SVO_WARN_STREAM("SparseImgAlign: no features to track!"); + return 0; + } + + ref_frame_ = ref_frame; + cur_frame_ = cur_frame; + ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); + jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); + visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? + + SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse()); + + for(level_=max_level_; level_>=min_level_; --level_) + { + mu_ = 0.1; + jacobian_cache_.setZero(); + have_ref_patch_cache_ = false; + if(verbose_) + printf("\nPYRAMID LEVEL %i\n---------------\n", level_); + optimize(T_cur_from_ref); + } + cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_; + + + + return n_meas_/patch_area_; +} + +Matrix SparseImgAlign::getFisherInformation() +{ + double sigma_i_sq = 5e-4*255*255; // image noise + Matrix I = H_/sigma_i_sq; + return I; +} + +void SparseImgAlign::precomputeReferencePatches() +{ + const int border = patch_halfsize_+1; + const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_); + const int stride = ref_img.cols; + const float scale = 1.0f/(1<pos(); + const double focal_length = ref_frame_->cam_->errorMultiplier2(); + size_t feature_counter = 0; + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end(); + it!=ite; ++it, ++feature_counter, ++visiblity_it) + { + // check if reference with patch size is within image + const float u_ref = (*it)->px[0]*scale; + const float v_ref = (*it)->px[1]*scale; + const int u_ref_i = floorf(u_ref); + const int v_ref_i = floorf(v_ref); + if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows) + continue; + *visiblity_it = true; + + // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!! + const double depth(((*it)->point->pos_ - ref_pos).norm()); + const Vector3d xyz_ref((*it)->f*depth); + + // evaluate projection jacobian + Matrix frame_jac; + Frame::jacobian_xyz2uv(xyz_ref, frame_jac); + + // compute bilateral interpolation weights for reference image + const float subpix_u_ref = u_ref-u_ref_i; + const float subpix_v_ref = v_ref-v_ref_i; + const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref); + const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + size_t pixel_counter = 0; + float* cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + for(int y=0; yimg_pyr_.at(level_); + + if(linearize_system && display_) + resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0)); + + if(have_ref_patch_cache_ == false) + precomputeReferencePatches(); + + // compute the weights on the first iteration + std::vector errors; + if(compute_weight_scale) + errors.reserve(visible_fts_.size()); + const int stride = cur_img.cols; + const int border = patch_halfsize_+1; + const float scale = 1.0f/(1<pos()); + float chi2 = 0.0; + size_t feature_counter = 0; // is used to compute the index of the cached jacobian + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end(); + ++it, ++feature_counter, ++visiblity_it) + { + // check if feature is within image + if(!*visiblity_it) + continue; + + // compute pixel location in cur img + const double depth = ((*it)->point->pos_ - ref_pos).norm(); + const Vector3d xyz_ref((*it)->f*depth); + const Vector3d xyz_cur(T_cur_from_ref * xyz_ref); + const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast() * scale); + const float u_cur = uv_cur_pyr[0]; + const float v_cur = uv_cur_pyr[1]; + const int u_cur_i = floorf(u_cur); + const int v_cur_i = floorf(v_cur); + + // check if projection is within the image + if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows) + continue; + + // compute bilateral interpolation weights for the current image + const float subpix_u_cur = u_cur-u_cur_i; + const float subpix_v_cur = v_cur-v_cur_i; + const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur); + const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur); + const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur; + const float w_cur_br = subpix_u_cur * subpix_v_cur; + float* ref_patch_cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + size_t pixel_counter = 0; // is used to compute the index of the cached jacobian + for(int y=0; yvalue(res/scale_); + } + + chi2 += res*res*weight; + n_meas_++; + + if(linearize_system) + { + // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error) + const Vector6d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter)); + H_.noalias() += J*J.transpose()*weight; + Jres_.noalias() -= J*res*weight; + if(display_) + resimg_.at((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0; + } + } + } + } + + // compute the weights on the first iteration + if(compute_weight_scale && iter_ == 0) + scale_ = scale_estimator_->compute(errors); + + return chi2/n_meas_; +} + +int SparseImgAlign::solve() +{ + x_ = H_.ldlt().solve(Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void SparseImgAlign::update( + const ModelType& T_curold_from_ref, + ModelType& T_curnew_from_ref) +{ + T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_); +} + +void SparseImgAlign::startIteration() +{} + +void SparseImgAlign::finishIteration() +{ + if(display_) + { + cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*10); + cv::waitKey(0); + } +} + +} // namespace svo + diff --git a/CPU_version/svo/test/README.md b/CPU_version/svo/test/README.md new file mode 100644 index 0000000..c70f832 --- /dev/null +++ b/CPU_version/svo/test/README.md @@ -0,0 +1,24 @@ +#### Download Test Data + +Create a folder where you will store all datasets, e.g.: + + mkdir Datasets + +In your `.bashrc` script, create a new environment variable pointing to the dataset folder: + + export SVO_DATASET_DIR=${HOME}/Datasets + +Source your new `.bashrc` script, go to the new dataset folder and download the test data: + + source ~/.bashrc + cd ${SVO_DATASET_DIR} + wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz + +You can then run the tests, e.g.: + + rosrun svo test_pipeline + +or if you are not using ROS: + + cd svo/bin + ./test_pipeline diff --git a/CPU_version/svo/test/benchmark.csv b/CPU_version/svo/test/benchmark.csv new file mode 100644 index 0000000..a3831e4 --- /dev/null +++ b/CPU_version/svo/test/benchmark.csv @@ -0,0 +1,307 @@ +local_ba,point_optimizer,pose_optimizer,pyramid_creation,reproject,sparse_img_align,tot_time,dropout,img_align_n_tracked,loba_err_fin,loba_err_init,loba_n_erredges_fin,loba_n_erredges_init,n_candidates,repr_n_mps,repr_n_new_references,sfba_error_final,sfba_error_init,sfba_n_edges_final,sfba_thresh +0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.005536000000000,1.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000326000000000,0.000000000000000,0.000000000000000,0.016564000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000189000000000,0.000000000000000,0.000000000000000,0.016754000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000133000000000,0.000000000000000,0.000000000000000,0.017082000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000136000000000,0.000000000000000,0.000000000000000,0.023259000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000120000000000,0.000000000000000,0.000000000000000,0.029114000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.032538000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000123000000000,0.000000000000000,0.000000000000000,0.031375000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000137000000000,0.000000000000000,0.000000000000000,0.029995000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000119000000000,0.000000000000000,0.000000000000000,0.030888000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000461000000000,0.000000000000000,0.000000000000000,0.036835000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000159000000000,0.000000000000000,0.000000000000000,0.090994000000000,1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000036000000000,0.000143000000000,0.000117000000000,0.000836000000000,0.001616000000000,0.002862000000000,0.000000000000000,279.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,148.000000000000000,121.000000000000000,0.170175036288445,0.209622100274678,121.000000000000000,0.310240723182540 +0.000000000000000,0.000025000000000,0.000180000000000,0.000123000000000,0.000817000000000,0.000802000000000,0.001981000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,151.000000000000000,121.000000000000000,0.200086249066279,0.226929968835210,119.000000000000000,0.335856340084193 +0.000000000000000,0.000043000000000,0.000247000000000,0.000202000000000,0.000836000000000,0.000882000000000,0.002289000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,150.000000000000000,121.000000000000000,0.220021653137765,0.250579787207819,120.000000000000000,0.370858080741496 +0.000000000000000,0.000027000000000,0.000173000000000,0.000125000000000,0.000796000000000,0.000863000000000,0.002018000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,153.000000000000000,121.000000000000000,0.241933923583207,0.315090761433966,119.000000000000000,0.466334367626374 +0.000000000000000,0.000025000000000,0.000171000000000,0.000123000000000,0.000752000000000,0.000893000000000,0.002012000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,145.000000000000000,121.000000000000000,0.272951476803891,0.278442112643762,118.000000000000000,0.412094340922640 +0.000000000000000,0.000058000000000,0.000180000000000,0.000213000000000,0.001057000000000,0.000834000000000,0.002415000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,176.000000000000000,121.000000000000000,0.295685268713144,0.345204254249458,120.000000000000000,0.510902319377397 +0.000000000000000,0.000029000000000,0.000104000000000,0.000125000000000,0.000868000000000,0.000845000000000,0.002006000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,154.000000000000000,121.000000000000000,0.280726297104857,0.351937246921774,119.000000000000000,0.520867118388044 +0.000000000000000,0.000030000000000,0.000173000000000,0.000117000000000,0.000872000000000,0.000829000000000,0.002054000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,160.000000000000000,121.000000000000000,0.299712702632539,0.325868379793256,119.000000000000000,0.482285213145440 +0.000000000000000,0.000030000000000,0.000135000000000,0.000312000000000,0.001611000000000,0.000955000000000,0.003094000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,157.000000000000000,121.000000000000000,0.266414418920375,0.325723952270968,121.000000000000000,0.482071463677943 +0.000000000000000,0.000019000000000,0.000134000000000,0.000120000000000,0.000807000000000,0.000746000000000,0.001858000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,52.000000000000000,146.000000000000000,121.000000000000000,0.268513470166789,0.275002624777888,120.000000000000000,0.407003878905728 +0.000000000000000,0.000023000000000,0.000116000000000,0.000137000000000,0.000918000000000,0.000845000000000,0.002086000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,76.000000000000000,141.000000000000000,121.000000000000000,0.291779184224341,0.283057831418736,119.000000000000000,0.418925579815900 +0.000000000000000,0.000020000000000,0.000174000000000,0.000139000000000,0.000841000000000,0.000842000000000,0.002050000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,97.000000000000000,146.000000000000000,121.000000000000000,0.284752630550729,0.313033048850307,118.000000000000000,0.463288905244113 +0.000000000000000,0.000018000000000,0.000125000000000,0.000116000000000,0.000918000000000,0.000737000000000,0.001957000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,108.000000000000000,164.000000000000000,121.000000000000000,0.266984542461140,0.310809088449218,117.000000000000000,0.459997446489587 +0.000000000000000,0.000025000000000,0.000187000000000,0.000215000000000,0.001123000000000,0.000838000000000,0.002412000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,172.000000000000000,121.000000000000000,0.281472742741066,0.318106456171594,118.000000000000000,0.470797581856187 +0.000000000000000,0.000032000000000,0.000104000000000,0.000133000000000,0.000982000000000,0.001074000000000,0.002360000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,169.000000000000000,121.000000000000000,0.259578544711984,0.296051216412790,118.000000000000000,0.438155829408007 +0.000000000000000,0.000026000000000,0.000173000000000,0.000114000000000,0.000975000000000,0.000678000000000,0.001991000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,87.000000000000000,169.000000000000000,121.000000000000000,0.260854021690335,0.463271429083291,119.000000000000000,0.685641725628487 +0.000000000000000,0.000020000000000,0.000231000000000,0.000213000000000,0.001255000000000,0.000764000000000,0.002547000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,100.000000000000000,162.000000000000000,121.000000000000000,0.224857460689169,0.241819176684415,120.000000000000000,0.357892398925400 +0.000000000000000,0.000016000000000,0.000174000000000,0.000123000000000,0.000964000000000,0.000761000000000,0.002072000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,177.000000000000000,163.000000000000000,121.000000000000000,0.232802572049831,0.307670939545318,121.000000000000000,0.455352982415734 +0.000000000000000,0.000019000000000,0.000171000000000,0.000117000000000,0.000954000000000,0.000685000000000,0.001989000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,215.000000000000000,163.000000000000000,121.000000000000000,0.223503314093231,0.245376537152124,121.000000000000000,0.363157262109869 +0.000000000000000,0.000022000000000,0.000153000000000,0.000321000000000,0.001789000000000,0.000999000000000,0.003323000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,236.000000000000000,160.000000000000000,121.000000000000000,0.238679756461930,0.260245176933572,121.000000000000000,0.385162868349003 +0.000000000000000,0.000017000000000,0.000103000000000,0.000121000000000,0.000950000000000,0.000690000000000,0.001925000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,253.000000000000000,156.000000000000000,121.000000000000000,0.240028083468930,0.347317483304434,119.000000000000000,0.514029864789977 +0.000000000000000,0.000026000000000,0.000155000000000,0.000141000000000,0.001421000000000,0.000834000000000,0.002605000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,225.000000000000000,203.000000000000000,121.000000000000000,0.237050818956117,0.342187690832661,118.000000000000000,0.506437791010399 +0.000000000000000,0.000031000000000,0.000105000000000,0.000133000000000,0.001151000000000,0.000787000000000,0.002244000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,224.000000000000000,192.000000000000000,121.000000000000000,0.228256287503905,0.255917399607126,118.000000000000000,0.378757763709815 +0.000000000000000,0.000029000000000,0.000173000000000,0.000116000000000,0.001046000000000,0.000670000000000,0.002067000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,237.000000000000000,174.000000000000000,121.000000000000000,0.253024524323249,0.270830988595337,117.000000000000000,0.400829860236131 +0.000000000000000,0.000032000000000,0.000206000000000,0.000201000000000,0.001159000000000,0.000767000000000,0.002415000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,243.000000000000000,168.000000000000000,121.000000000000000,0.255837803512081,0.309767343777557,117.000000000000000,0.458455710853014 +0.000000000000000,0.000029000000000,0.000102000000000,0.000134000000000,0.001078000000000,0.000785000000000,0.002160000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,247.000000000000000,176.000000000000000,121.000000000000000,0.242039161090674,0.295753112981567,120.000000000000000,0.437714633120047 +0.000000000000000,0.000018000000000,0.000174000000000,0.000117000000000,0.001043000000000,0.000946000000000,0.002334000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,284.000000000000000,173.000000000000000,121.000000000000000,0.234035474669157,0.262962395172133,120.000000000000000,0.389184330309739 +0.000000000000000,0.000042000000000,0.000251000000000,0.000203000000000,0.001816000000000,0.000807000000000,0.003189000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,323.000000000000000,168.000000000000000,121.000000000000000,0.243963461663248,0.254676688230514,118.000000000000000,0.376921484440742 +0.000000000000000,0.000022000000000,0.000128000000000,0.000121000000000,0.001084000000000,0.000899000000000,0.002287000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,346.000000000000000,164.000000000000000,121.000000000000000,0.237498671286056,0.242318617577444,120.000000000000000,0.358631550548166 +0.000000000000000,0.000042000000000,0.000198000000000,0.000115000000000,0.001158000000000,0.000720000000000,0.002269000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,174.000000000000000,121.000000000000000,0.231204372046851,0.310626801011824,119.000000000000000,0.459727694560518 +0.000000000000000,0.000020000000000,0.000128000000000,0.000280000000000,0.000998000000000,0.000883000000000,0.002349000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,371.000000000000000,154.000000000000000,121.000000000000000,0.226372262196515,0.240389813013775,118.000000000000000,0.355776916048274 +0.000000000000000,0.000020000000000,0.000125000000000,0.000116000000000,0.001040000000000,0.000676000000000,0.002025000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,383.000000000000000,163.000000000000000,121.000000000000000,0.243430360339485,0.289701992216838,116.000000000000000,0.428758939836188 +0.000000000000000,0.000028000000000,0.000186000000000,0.000201000000000,0.001357000000000,0.000862000000000,0.002663000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,189.000000000000000,121.000000000000000,0.238491256129826,0.289513389727138,119.000000000000000,0.428479837315611 +0.000000000000000,0.000024000000000,0.000104000000000,0.000131000000000,0.001197000000000,0.000689000000000,0.002178000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,187.000000000000000,121.000000000000000,0.253535521704090,0.299150905814168,119.000000000000000,0.442743330689269 +0.000000000000000,0.000013000000000,0.000128000000000,0.000142000000000,0.001157000000000,0.000751000000000,0.002226000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,182.000000000000000,121.000000000000000,0.242747326189240,0.331002657945821,119.000000000000000,0.489883908154680 +0.000000000000000,0.000041000000000,0.000225000000000,0.000214000000000,0.001614000000000,0.000892000000000,0.003073000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,420.000000000000000,166.000000000000000,121.000000000000000,0.214060199250618,0.291833988602564,120.000000000000000,0.431914284304601 +0.000000000000000,0.000016000000000,0.000113000000000,0.000126000000000,0.001256000000000,0.000871000000000,0.002419000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,467.000000000000000,189.000000000000000,121.000000000000000,0.191542639509251,0.219467558857437,120.000000000000000,0.324811977376153 +0.000000000000000,0.000022000000000,0.000110000000000,0.000116000000000,0.001163000000000,0.000737000000000,0.002178000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,491.000000000000000,176.000000000000000,121.000000000000000,0.237287779136815,0.290385733638894,119.000000000000000,0.429770876012296 +0.000000000000000,0.000016000000000,0.000108000000000,0.000532000000000,0.001507000000000,0.001176000000000,0.003390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,503.000000000000000,195.000000000000000,121.000000000000000,0.255418654779892,0.271366103996528,119.000000000000000,0.401621830064983 +0.000000000000000,0.000023000000000,0.000128000000000,0.000118000000000,0.001247000000000,0.000704000000000,0.002255000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,519.000000000000000,192.000000000000000,121.000000000000000,0.243354932726649,0.322133765000893,118.000000000000000,0.476758002689501 +0.000000000000000,0.000028000000000,0.000117000000000,0.000194000000000,0.001487000000000,0.000763000000000,0.002640000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,537.000000000000000,182.000000000000000,121.000000000000000,0.256839812012575,0.290001379744665,120.000000000000000,0.429202006242450 +0.000000000000000,0.000021000000000,0.000192000000000,0.000135000000000,0.001268000000000,0.000737000000000,0.002390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,551.000000000000000,186.000000000000000,121.000000000000000,0.234921062453291,0.278307687859489,121.000000000000000,0.411895400770342 +0.000000000000000,0.000023000000000,0.000110000000000,0.000128000000000,0.001205000000000,0.000853000000000,0.002375000000000,1.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,566.000000000000000,182.000000000000000,121.000000000000000,0.236213436253286,0.243545081705577,120.000000000000000,0.360446727806674 +0.000000000000000,0.000033000000000,0.000148000000000,0.000221000000000,0.001810000000000,0.000752000000000,0.003004000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,194.000000000000000,121.000000000000000,0.256770574347429,0.281629144927868,120.000000000000000,0.416811158357270 +0.000000000000000,0.000018000000000,0.000107000000000,0.000166000000000,0.001283000000000,0.000754000000000,0.002364000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,193.000000000000000,121.000000000000000,0.238569415669078,0.245006057597332,120.000000000000000,0.362608988913209 +0.000000000000000,0.000026000000000,0.000199000000000,0.000126000000000,0.001415000000000,0.000739000000000,0.002538000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,525.000000000000000,207.000000000000000,121.000000000000000,0.272522903927536,0.292694935292295,120.000000000000000,0.433188542480310 +0.000000000000000,0.000024000000000,0.000152000000000,0.000385000000000,0.001329000000000,0.000772000000000,0.002715000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,532.000000000000000,193.000000000000000,121.000000000000000,0.300157389608362,0.352279544787609,121.000000000000000,0.521373718272822 +0.000000000000000,0.000025000000000,0.000103000000000,0.000124000000000,0.001300000000000,0.000765000000000,0.002352000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,542.000000000000000,192.000000000000000,121.000000000000000,0.262925192895899,0.296136504354747,121.000000000000000,0.438282037121495 +0.000000000000000,0.000027000000000,0.000159000000000,0.000227000000000,0.001454000000000,0.000792000000000,0.002702000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,565.000000000000000,171.000000000000000,121.000000000000000,0.281949830225302,0.308467076957178,121.000000000000000,0.456531258033208 +0.000000000000000,0.000027000000000,0.000177000000000,0.000130000000000,0.001440000000000,0.000817000000000,0.002620000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,608.000000000000000,201.000000000000000,121.000000000000000,0.262127192155801,0.344178587739890,120.000000000000000,0.509384339297628 +0.000000000000000,0.000031000000000,0.000173000000000,0.000122000000000,0.001320000000000,0.000744000000000,0.002429000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,636.000000000000000,196.000000000000000,121.000000000000000,0.243118415564597,0.307364154563702,120.000000000000000,0.454898948018352 +0.000000000000000,0.000031000000000,0.000237000000000,0.000391000000000,0.002601000000000,0.000982000000000,0.004294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,663.000000000000000,198.000000000000000,121.000000000000000,0.308234194177187,0.324201216218454,120.000000000000000,0.479817768949582 +0.000000000000000,0.000016000000000,0.000102000000000,0.000118000000000,0.001370000000000,0.000653000000000,0.002310000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,687.000000000000000,198.000000000000000,121.000000000000000,0.262171425081880,0.309537417467615,120.000000000000000,0.458115349322126 +0.000000000000000,0.000024000000000,0.000120000000000,0.000223000000000,0.001806000000000,0.000776000000000,0.002976000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,229.000000000000000,121.000000000000000,0.297772053023002,0.357203726783215,120.000000000000000,0.528661518750357 +0.000000000000000,0.000022000000000,0.000131000000000,0.000225000000000,0.001322000000000,0.000818000000000,0.002549000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,195.000000000000000,121.000000000000000,0.273836774721146,0.292258185663714,119.000000000000000,0.432542138616555 +0.000000000000000,0.000028000000000,0.000189000000000,0.000113000000000,0.001354000000000,0.000656000000000,0.002369000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,668.000000000000000,208.000000000000000,121.000000000000000,0.290291300745150,0.351263582277828,120.000000000000000,0.519870092614610 +0.000000000000000,0.000020000000000,0.000106000000000,0.000362000000000,0.002527000000000,0.000881000000000,0.003941000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,682.000000000000000,211.000000000000000,121.000000000000000,0.258959916330166,0.286061529228379,120.000000000000000,0.423371078465586 +0.000000000000000,0.000023000000000,0.000183000000000,0.000117000000000,0.001452000000000,0.000716000000000,0.002520000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,696.000000000000000,219.000000000000000,121.000000000000000,0.299843168031565,0.306288599781015,118.000000000000000,0.453307123537539 +0.000000000000000,0.000024000000000,0.000190000000000,0.000229000000000,0.001590000000000,0.000704000000000,0.002778000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,704.000000000000000,201.000000000000000,121.000000000000000,0.274895969986079,0.339774469390948,119.000000000000000,0.502866218860539 +0.000000000000000,0.000032000000000,0.000184000000000,0.000127000000000,0.001388000000000,0.000658000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,712.000000000000000,205.000000000000000,121.000000000000000,0.276261011279968,0.281429264300083,120.000000000000000,0.416515325859454 +0.000000000000000,0.000019000000000,0.000103000000000,0.000122000000000,0.001302000000000,0.000759000000000,0.002334000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,743.000000000000000,194.000000000000000,121.000000000000000,0.308426080941849,0.293337004748022,118.000000000000000,0.434138764752467 +0.000000000000000,0.000020000000000,0.000188000000000,0.000336000000000,0.002590000000000,0.000953000000000,0.004130000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,769.000000000000000,216.000000000000000,121.000000000000000,0.273447755201539,0.322902569141660,119.000000000000000,0.477895792772933 +0.000000000000000,0.000019000000000,0.000104000000000,0.000118000000000,0.001466000000000,0.000743000000000,0.002480000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,791.000000000000000,204.000000000000000,121.000000000000000,0.288081750238067,0.251959938318303,120.000000000000000,0.372900704820467 +0.000000000000000,0.000024000000000,0.000118000000000,0.000231000000000,0.001575000000000,0.000986000000000,0.003013000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,190.000000000000000,121.000000000000000,0.282457846343284,0.276454604943847,117.000000000000000,0.409152821737331 +0.000000000000000,0.000016000000000,0.000210000000000,0.000129000000000,0.001436000000000,0.000881000000000,0.002693000000000,0.000000000000000,111.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,208.000000000000000,121.000000000000000,0.294182859046983,0.317841061067694,121.000000000000000,0.470404755925379 +0.000000000000000,0.000015000000000,0.000142000000000,0.000184000000000,0.001424000000000,0.000955000000000,0.002755000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,194.000000000000000,121.000000000000000,0.243748783908456,0.304183797141307,119.000000000000000,0.450192011884479 +0.000000000000000,0.000014000000000,0.000107000000000,0.000318000000000,0.001624000000000,0.000893000000000,0.002992000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,220.000000000000000,121.000000000000000,0.265012454662619,0.270354981784829,119.000000000000000,0.400125381617550 +0.000000000000000,0.000023000000000,0.000213000000000,0.000128000000000,0.001340000000000,0.000949000000000,0.002681000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,811.000000000000000,196.000000000000000,121.000000000000000,0.253755971722852,0.269982133879470,119.000000000000000,0.399573570359238 +0.000000000000000,0.000038000000000,0.000249000000000,0.000219000000000,0.001747000000000,0.000847000000000,0.003214000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,217.000000000000000,121.000000000000000,0.242158110541114,0.277566291448718,118.000000000000000,0.410798121492822 +0.000000000000000,0.000022000000000,0.000200000000000,0.000131000000000,0.001465000000000,0.000761000000000,0.002608000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,826.000000000000000,208.000000000000000,121.000000000000000,0.247025005586580,0.299927107710462,119.000000000000000,0.443892088763821 +0.000000000000000,0.000028000000000,0.000129000000000,0.000115000000000,0.001507000000000,0.000767000000000,0.002582000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,835.000000000000000,206.000000000000000,121.000000000000000,0.251694282278063,0.262688495423133,119.000000000000000,0.388778969531936 +0.000000000000000,0.000018000000000,0.000176000000000,0.000303000000000,0.001569000000000,0.000763000000000,0.002865000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,858.000000000000000,208.000000000000000,121.000000000000000,0.204259521447942,0.239471125876921,121.000000000000000,0.354417264227415 +0.000000000000000,0.000027000000000,0.000197000000000,0.000119000000000,0.001520000000000,0.000717000000000,0.002613000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,182.000000000000000,121.000000000000000,0.288688133937508,0.321107040561571,121.000000000000000,0.475238455753857 +0.000000000000000,0.000074000000000,0.000308000000000,0.000231000000000,0.002302000000000,0.000817000000000,0.004009000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,895.000000000000000,169.000000000000000,121.000000000000000,0.226466756684178,0.283662806855079,120.000000000000000,0.419820962132456 +0.000000000000000,0.000032000000000,0.000120000000000,0.000146000000000,0.001779000000000,0.000719000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,201.000000000000000,121.000000000000000,0.248964133477333,0.301424345755280,121.000000000000000,0.446108027319984 +0.000000000000000,0.000031000000000,0.000209000000000,0.000347000000000,0.002379000000000,0.001268000000000,0.004286000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,881.000000000000000,197.000000000000000,121.000000000000000,0.197089987231914,0.215315450958625,121.000000000000000,0.318666844452755 +0.000000000000000,0.000022000000000,0.000183000000000,0.000138000000000,0.001523000000000,0.000904000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,889.000000000000000,208.000000000000000,121.000000000000000,0.211913610123421,0.217730864556872,121.000000000000000,0.322241701945577 +0.000000000000000,0.000023000000000,0.000188000000000,0.000135000000000,0.001539000000000,0.001010000000000,0.002927000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,191.000000000000000,121.000000000000000,0.223781553150169,0.264534665335954,119.000000000000000,0.391511288186158 +0.000000000000000,0.000024000000000,0.000111000000000,0.000561000000000,0.002319000000000,0.001083000000000,0.004153000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,959.000000000000000,191.000000000000000,121.000000000000000,0.192480342616366,0.242812113218322,120.000000000000000,0.359361932832411 +0.000000000000000,0.000023000000000,0.000113000000000,0.000129000000000,0.001473000000000,0.000780000000000,0.002555000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1010.000000000000000,186.000000000000000,121.000000000000000,0.226239188570666,0.246580462159093,121.000000000000000,0.364939080501376 +0.000000000000000,0.000025000000000,0.000147000000000,0.000235000000000,0.001877000000000,0.001081000000000,0.003415000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1042.000000000000000,191.000000000000000,121.000000000000000,0.213010810373553,0.226959616128893,121.000000000000000,0.335900212048673 +0.000000000000000,0.000019000000000,0.000180000000000,0.000123000000000,0.001581000000000,0.000797000000000,0.002856000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1066.000000000000000,197.000000000000000,121.000000000000000,0.236500218717681,0.308339991149568,118.000000000000000,0.456343184784554 +0.000000000000000,0.000020000000000,0.000176000000000,0.000124000000000,0.001670000000000,0.001020000000000,0.003039000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1045.000000000000000,222.000000000000000,121.000000000000000,0.203403550946325,0.322649020817046,119.000000000000000,0.477520556062907 +0.000000000000000,0.000021000000000,0.000102000000000,0.000120000000000,0.001506000000000,0.000747000000000,0.002537000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,208.000000000000000,121.000000000000000,0.217457541731394,0.242874100604298,119.000000000000000,0.359453694988533 +0.000000000000000,0.000014000000000,0.000102000000000,0.000114000000000,0.001438000000000,0.000775000000000,0.002481000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,202.000000000000000,121.000000000000000,0.237066555963892,0.363698547046353,120.000000000000000,0.538273825733024 +0.000000000000000,0.000028000000000,0.000138000000000,0.000130000000000,0.001867000000000,0.000905000000000,0.003190000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1051.000000000000000,204.000000000000000,121.000000000000000,0.204004037194742,0.217771792656962,121.000000000000000,0.322302253343558 +0.000000000000000,0.000016000000000,0.000108000000000,0.000146000000000,0.001730000000000,0.000807000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1092.000000000000000,216.000000000000000,121.000000000000000,0.189498788663577,0.218460412109291,119.000000000000000,0.323321417274184 +0.000000000000000,0.000021000000000,0.000142000000000,0.000136000000000,0.001571000000000,0.000950000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1150.000000000000000,195.000000000000000,121.000000000000000,0.234911243099688,0.229840840010708,121.000000000000000,0.340164460854319 +0.000000000000000,0.000023000000000,0.000178000000000,0.000327000000000,0.001509000000000,0.001060000000000,0.003143000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1188.000000000000000,188.000000000000000,121.000000000000000,0.238938910296195,0.242239031677996,119.000000000000000,0.358513758367035 +0.000000000000000,0.000026000000000,0.000127000000000,0.000121000000000,0.001413000000000,0.000746000000000,0.002666000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,181.000000000000000,121.000000000000000,0.231069770120829,0.351821144360750,121.000000000000000,0.520695269679250 +0.000000000000000,0.000013000000000,0.000123000000000,0.000156000000000,0.002291000000000,0.000810000000000,0.003493000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,223.000000000000000,121.000000000000000,0.232050436410629,0.243086413240903,121.000000000000000,0.359767900135068 +0.000000000000000,0.000017000000000,0.000183000000000,0.000122000000000,0.001660000000000,0.000828000000000,0.002856000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1126.000000000000000,205.000000000000000,121.000000000000000,0.217672407670591,0.232176089084970,120.000000000000000,0.343620616378729 +0.000000000000000,0.000025000000000,0.000104000000000,0.000126000000000,0.001640000000000,0.000956000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1122.000000000000000,225.000000000000000,121.000000000000000,0.210874531577464,0.231177462797771,120.000000000000000,0.342142641667492 +0.000000000000000,0.000011000000000,0.000104000000000,0.000149000000000,0.001579000000000,0.000928000000000,0.002811000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1124.000000000000000,218.000000000000000,121.000000000000000,0.199068638688048,0.231930228681434,121.000000000000000,0.343256752009726 +0.000000000000000,0.000016000000000,0.000105000000000,0.000114000000000,0.001529000000000,0.000835000000000,0.002636000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,217.000000000000000,121.000000000000000,0.199461859922069,0.201973412653913,121.000000000000000,0.298920644384362 +0.000000000000000,0.000020000000000,0.000107000000000,0.000175000000000,0.002271000000000,0.000832000000000,0.003453000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1170.000000000000000,216.000000000000000,121.000000000000000,0.220686177070489,0.284068818474203,121.000000000000000,0.420421851360250 +0.000000000000000,0.000021000000000,0.000110000000000,0.000115000000000,0.001720000000000,0.000987000000000,0.002996000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1207.000000000000000,220.000000000000000,121.000000000000000,0.211157109658996,0.207022193542394,121.000000000000000,0.306392853689555 +0.000000000000000,0.000019000000000,0.000128000000000,0.000124000000000,0.001660000000000,0.000836000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,230.000000000000000,121.000000000000000,0.227298389602018,0.273505984323976,121.000000000000000,0.404788875408716 +0.000000000000000,0.000019000000000,0.000103000000000,0.000135000000000,0.001696000000000,0.000751000000000,0.002919000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,235.000000000000000,121.000000000000000,0.230641563123264,0.341660758731761,121.000000000000000,0.505657951678639 +0.000000000000000,0.000015000000000,0.000177000000000,0.000130000000000,0.001556000000000,0.000919000000000,0.002864000000000,0.000000000000000,106.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1118.000000000000000,225.000000000000000,121.000000000000000,0.192303276106424,0.284766422639249,121.000000000000000,0.421454308293938 +0.000000000000000,0.000013000000000,0.000138000000000,0.000178000000000,0.002041000000000,0.000992000000000,0.003412000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1112.000000000000000,223.000000000000000,121.000000000000000,0.206634709237684,0.414844586254086,120.000000000000000,0.613969997701391 +0.000000000000000,0.000015000000000,0.000134000000000,0.000127000000000,0.001905000000000,0.000764000000000,0.002995000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1116.000000000000000,248.000000000000000,121.000000000000000,0.217282816286385,0.386388750278064,121.000000000000000,0.571855388443357 +0.000000000000000,0.000016000000000,0.000128000000000,0.000127000000000,0.001825000000000,0.000925000000000,0.003066000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1115.000000000000000,245.000000000000000,121.000000000000000,0.205232081617916,0.260414398266005,121.000000000000000,0.385413312570202 +0.000000000000000,0.000020000000000,0.000107000000000,0.000141000000000,0.001807000000000,0.000923000000000,0.003060000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1141.000000000000000,234.000000000000000,121.000000000000000,0.215575249362573,0.253408200953891,120.000000000000000,0.375044163656510 +0.000000000000000,0.000019000000000,0.000128000000000,0.000120000000000,0.001568000000000,0.000884000000000,0.002762000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1162.000000000000000,189.000000000000000,121.000000000000000,0.263382144346756,0.344167879309156,121.000000000000000,0.509368468563933 +0.000000000000000,0.000023000000000,0.000131000000000,0.000373000000000,0.001757000000000,0.001182000000000,0.003515000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1191.000000000000000,211.000000000000000,121.000000000000000,0.211945557067404,0.302128504397490,121.000000000000000,0.447150188651345 +0.000000000000000,0.000025000000000,0.000154000000000,0.000124000000000,0.001844000000000,0.000803000000000,0.003004000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,227.000000000000000,121.000000000000000,0.202381916559221,0.390013881008811,120.000000000000000,0.577220555675650 +0.000000000000000,0.000015000000000,0.000109000000000,0.000129000000000,0.001708000000000,0.000890000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1231.000000000000000,213.000000000000000,121.000000000000000,0.214162701863959,0.216971594768139,120.000000000000000,0.321117988293146 +0.000000000000000,0.000019000000000,0.000106000000000,0.000141000000000,0.001738000000000,0.000788000000000,0.002834000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,223.000000000000000,121.000000000000000,0.199426583525084,0.257295226754736,121.000000000000000,0.380796925542741 +0.000000000000000,0.000015000000000,0.000104000000000,0.000127000000000,0.001749000000000,0.000817000000000,0.002854000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,237.000000000000000,121.000000000000000,0.197980287517504,0.261753748672962,121.000000000000000,0.387395536882405 +0.000000000000000,0.000022000000000,0.000120000000000,0.000159000000000,0.002300000000000,0.000963000000000,0.003628000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,228.000000000000000,121.000000000000000,0.236498759179881,0.240201532654298,121.000000000000000,0.355498268421338 +0.000000000000000,0.000017000000000,0.000181000000000,0.000128000000000,0.001959000000000,0.000872000000000,0.003382000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,248.000000000000000,121.000000000000000,0.184287569106640,0.201686577196243,121.000000000000000,0.298496127529902 +0.000000000000000,0.000013000000000,0.000178000000000,0.000136000000000,0.001684000000000,0.000852000000000,0.002943000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1105.000000000000000,230.000000000000000,121.000000000000000,0.190585604418642,0.200521952748998,120.000000000000000,0.296772510252565 +0.000000000000000,0.000019000000000,0.000106000000000,0.000143000000000,0.001668000000000,0.000981000000000,0.002955000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1104.000000000000000,218.000000000000000,121.000000000000000,0.200527511479873,0.209064190915326,120.000000000000000,0.309415015408653 +0.000000000000000,0.000014000000000,0.000104000000000,0.000123000000000,0.001517000000000,0.000849000000000,0.002647000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,201.000000000000000,121.000000000000000,0.227018266784565,0.269186250934732,121.000000000000000,0.398395648547930 +0.000000000000000,0.000017000000000,0.000178000000000,0.000348000000000,0.001620000000000,0.001141000000000,0.003354000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,206.000000000000000,121.000000000000000,0.206456021778136,0.213075158345683,121.000000000000000,0.315351251063391 +0.000000000000000,0.000026000000000,0.000108000000000,0.000150000000000,0.001619000000000,0.000821000000000,0.002770000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,197.000000000000000,121.000000000000000,0.195769620795380,0.235699009656539,120.000000000000000,0.348834556747284 +0.000000000000000,0.000018000000000,0.000178000000000,0.000127000000000,0.001593000000000,0.000826000000000,0.002776000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,206.000000000000000,121.000000000000000,0.202443810598108,0.259322628022743,121.000000000000000,0.383797505085764 +0.000000000000000,0.000018000000000,0.000105000000000,0.000140000000000,0.001539000000000,0.001030000000000,0.002870000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1255.000000000000000,195.000000000000000,121.000000000000000,0.234795060265796,0.251596569636394,120.000000000000000,0.372362919449417 +0.000000000000000,0.000017000000000,0.000175000000000,0.000125000000000,0.001584000000000,0.000833000000000,0.002923000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1267.000000000000000,221.000000000000000,121.000000000000000,0.207163879614002,0.272216719799699,121.000000000000000,0.402880748216245 +0.000000000000000,0.000017000000000,0.000133000000000,0.000153000000000,0.002442000000000,0.000831000000000,0.003670000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,216.000000000000000,121.000000000000000,0.170524915168911,0.199849692332816,121.000000000000000,0.295777531500515 +0.000000000000000,0.000013000000000,0.000157000000000,0.000149000000000,0.001748000000000,0.000942000000000,0.003057000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,234.000000000000000,121.000000000000000,0.189221335752284,0.197743002271810,121.000000000000000,0.292659639941872 +0.000000000000000,0.000020000000000,0.000178000000000,0.000125000000000,0.001721000000000,0.000742000000000,0.002827000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1176.000000000000000,225.000000000000000,121.000000000000000,0.198900978356214,0.245389821232497,119.000000000000000,0.363176948895773 +0.000000000000000,0.000013000000000,0.000176000000000,0.000145000000000,0.001697000000000,0.000855000000000,0.002929000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,222.000000000000000,121.000000000000000,0.196051895442564,0.262432687056220,121.000000000000000,0.388400396935210 +0.000000000000000,0.000015000000000,0.000176000000000,0.000128000000000,0.001658000000000,0.000736000000000,0.002754000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1264.000000000000000,222.000000000000000,121.000000000000000,0.188130722176495,0.226906201366918,120.000000000000000,0.335821186914497 +0.000000000000000,0.000015000000000,0.000114000000000,0.000128000000000,0.001732000000000,0.000994000000000,0.003025000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1280.000000000000000,221.000000000000000,121.000000000000000,0.195205519657246,0.208004184809569,119.000000000000000,0.307846188328575 +0.000000000000000,0.000020000000000,0.000195000000000,0.000126000000000,0.002006000000000,0.000791000000000,0.003194000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1295.000000000000000,262.000000000000000,121.000000000000000,0.204929539974912,0.216820480192029,119.000000000000000,0.320894306981155 +0.000000000000000,0.000017000000000,0.000176000000000,0.000139000000000,0.001546000000000,0.000896000000000,0.002812000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,192.000000000000000,121.000000000000000,0.226704780607630,0.257512342665443,120.000000000000000,0.381118257356317 +0.000000000000000,0.000011000000000,0.000105000000000,0.000299000000000,0.001761000000000,0.001102000000000,0.003328000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,234.000000000000000,121.000000000000000,0.206579202251784,0.240623999885611,121.000000000000000,0.356123519730790 +0.000000000000000,0.000020000000000,0.000103000000000,0.000130000000000,0.001498000000000,0.000871000000000,0.002766000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1312.000000000000000,207.000000000000000,121.000000000000000,0.208586912880041,0.214158703806542,121.000000000000000,0.316954877506999 +0.000000000000000,0.000017000000000,0.000176000000000,0.000374000000000,0.001665000000000,0.000976000000000,0.003373000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1139.000000000000000,223.000000000000000,121.000000000000000,0.209543770412094,0.213150938854858,121.000000000000000,0.315463382345879 +0.000000000000000,0.000021000000000,0.000157000000000,0.000124000000000,0.001652000000000,0.000947000000000,0.002947000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1137.000000000000000,212.000000000000000,121.000000000000000,0.214888927283406,0.261239649375707,120.000000000000000,0.386634701996087 +0.000000000000000,0.000018000000000,0.000104000000000,0.000124000000000,0.001479000000000,0.000897000000000,0.002661000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,188.000000000000000,121.000000000000000,0.205526459876717,0.194046988139053,121.000000000000000,0.287189543909738 +0.000000000000000,0.000016000000000,0.000175000000000,0.000142000000000,0.001509000000000,0.000744000000000,0.002626000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,190.000000000000000,121.000000000000000,0.193638964960760,0.209501566643394,121.000000000000000,0.310062329059690 +0.000000000000000,0.000022000000000,0.000103000000000,0.000125000000000,0.001561000000000,0.000725000000000,0.002572000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,218.000000000000000,121.000000000000000,0.192895828207804,0.213686959411965,121.000000000000000,0.316256716855654 +0.000000000000000,0.000023000000000,0.000184000000000,0.000162000000000,0.001899000000000,0.000812000000000,0.003140000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1258.000000000000000,187.000000000000000,121.000000000000000,0.178433372351327,0.196124969530122,120.000000000000000,0.290264953456693 +0.000000000000000,0.000016000000000,0.000134000000000,0.000140000000000,0.001663000000000,0.000876000000000,0.002874000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1274.000000000000000,203.000000000000000,121.000000000000000,0.204379056805537,0.211513754780143,119.000000000000000,0.313040366095618 +0.000000000000000,0.000020000000000,0.000176000000000,0.000130000000000,0.001587000000000,0.000869000000000,0.002823000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1284.000000000000000,206.000000000000000,121.000000000000000,0.194394953473234,0.214318086264921,121.000000000000000,0.317190765131688 +0.000000000000000,0.000017000000000,0.000107000000000,0.000148000000000,0.001575000000000,0.000805000000000,0.002693000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1286.000000000000000,205.000000000000000,121.000000000000000,0.192692252511560,0.217301832678175,121.000000000000000,0.321606720966583 +0.000000000000000,0.000020000000000,0.000173000000000,0.000124000000000,0.001507000000000,0.000785000000000,0.002649000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,210.000000000000000,121.000000000000000,0.204881580167579,0.231890569564824,121.000000000000000,0.343198045458178 +0.000000000000000,0.000041000000000,0.000220000000000,0.000144000000000,0.001900000000000,0.000655000000000,0.003038000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,206.000000000000000,121.000000000000000,0.174115401194611,0.223973720218107,121.000000000000000,0.331481122502030 +0.000000000000000,0.000043000000000,0.000182000000000,0.000145000000000,0.001457000000000,0.000728000000000,0.002734000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,191.000000000000000,121.000000000000000,0.194538477548438,0.187220557909554,121.000000000000000,0.277086431960381 +0.000000000000000,0.000020000000000,0.000182000000000,0.000129000000000,0.001451000000000,0.000813000000000,0.002657000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1148.000000000000000,207.000000000000000,121.000000000000000,0.209547468410092,0.205381330935579,119.000000000000000,0.303964378715603 +0.000000000000000,0.000037000000000,0.000255000000000,0.000211000000000,0.002943000000000,0.001198000000000,0.004740000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1146.000000000000000,192.000000000000000,121.000000000000000,0.194112090334278,0.213834860708738,121.000000000000000,0.316475596512546 +0.000000000000000,0.000021000000000,0.000119000000000,0.000128000000000,0.001482000000000,0.000836000000000,0.002622000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1196.000000000000000,188.000000000000000,121.000000000000000,0.194707145965396,0.200711676435348,120.000000000000000,0.297053280716492 +0.000000000000000,0.000018000000000,0.000128000000000,0.000353000000000,0.001530000000000,0.001123000000000,0.003198000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1222.000000000000000,182.000000000000000,121.000000000000000,0.165297929068422,0.223118899091734,120.000000000000000,0.330215962199139 +0.000000000000000,0.000019000000000,0.000133000000000,0.000124000000000,0.001554000000000,0.001016000000000,0.002890000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,187.000000000000000,121.000000000000000,0.189732092265728,0.231665076279646,121.000000000000000,0.342864305156952 +0.000000000000000,0.000018000000000,0.000195000000000,0.000130000000000,0.001387000000000,0.000871000000000,0.002639000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,167.000000000000000,121.000000000000000,0.173396135722069,0.185374303680201,121.000000000000000,0.274353961674946 +0.000000000000000,0.000015000000000,0.000105000000000,0.000165000000000,0.001566000000000,0.001052000000000,0.002950000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1292.000000000000000,196.000000000000000,121.000000000000000,0.192868888286218,0.205034141945327,120.000000000000000,0.303450551076308 +0.000000000000000,0.000014000000000,0.000102000000000,0.000127000000000,0.001521000000000,0.000791000000000,0.002592000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1302.000000000000000,199.000000000000000,121.000000000000000,0.190528673137881,0.222906754530888,121.000000000000000,0.329902009771293 +0.000000000000000,0.000021000000000,0.000215000000000,0.000167000000000,0.001934000000000,0.000805000000000,0.003204000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.202526348712512,0.259278841345545,120.000000000000000,0.383732682741932 +0.000000000000000,0.000022000000000,0.000156000000000,0.000126000000000,0.001696000000000,0.000788000000000,0.002834000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.196451913404442,0.201069768524068,120.000000000000000,0.297583282351909 +0.000000000000000,0.000025000000000,0.000103000000000,0.000132000000000,0.001543000000000,0.000902000000000,0.002745000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1305.000000000000000,194.000000000000000,121.000000000000000,0.200871633124814,0.183908150343481,120.000000000000000,0.272184068463992 +0.000000000000000,0.000025000000000,0.000102000000000,0.000150000000000,0.001478000000000,0.000854000000000,0.002788000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1335.000000000000000,179.000000000000000,121.000000000000000,0.193823047570574,0.212329442302877,119.000000000000000,0.314247578003029 +0.000000000000000,0.000016000000000,0.000196000000000,0.000126000000000,0.001299000000000,0.000752000000000,0.002455000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1194.000000000000000,176.000000000000000,121.000000000000000,0.207923081779578,0.229508478265270,121.000000000000000,0.339672543925396 +0.000000000000000,0.000018000000000,0.000110000000000,0.000166000000000,0.001844000000000,0.000982000000000,0.003170000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,180.000000000000000,121.000000000000000,0.163342705330096,0.249575746286056,120.000000000000000,0.369372120119674 +0.000000000000000,0.000015000000000,0.000183000000000,0.000124000000000,0.001749000000000,0.000847000000000,0.002963000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,212.000000000000000,121.000000000000000,0.173881865983057,0.194395754595030,121.000000000000000,0.287705721832844 +0.000000000000000,0.000026000000000,0.000176000000000,0.000127000000000,0.001498000000000,0.000877000000000,0.002752000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1259.000000000000000,185.000000000000000,121.000000000000000,0.155523040470466,0.162766723518840,120.000000000000000,0.240894740082153 +0.000000000000000,0.000015000000000,0.000177000000000,0.000144000000000,0.001550000000000,0.000743000000000,0.002669000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,192.000000000000000,121.000000000000000,0.152934150143871,0.272911472202902,121.000000000000000,0.403908984747820 +0.000000000000000,0.000014000000000,0.000174000000000,0.000116000000000,0.001439000000000,0.000731000000000,0.002513000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1291.000000000000000,196.000000000000000,121.000000000000000,0.188113809380047,0.184618919088813,121.000000000000000,0.273236010008633 +0.000000000000000,0.000016000000000,0.000106000000000,0.000145000000000,0.002198000000000,0.000804000000000,0.003318000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1299.000000000000000,184.000000000000000,121.000000000000000,0.189391540723312,0.210953756830538,121.000000000000000,0.312211549881851 +0.000000000000000,0.000019000000000,0.000181000000000,0.000116000000000,0.001662000000000,0.000840000000000,0.002865000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1306.000000000000000,204.000000000000000,121.000000000000000,0.184488742976474,0.215925329354907,121.000000000000000,0.319569479795696 +0.000000000000000,0.000016000000000,0.000183000000000,0.000132000000000,0.001540000000000,0.000737000000000,0.002647000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1313.000000000000000,197.000000000000000,121.000000000000000,0.164667840588859,0.233194073999862,121.000000000000000,0.345127224117476 +0.000000000000000,0.000017000000000,0.000177000000000,0.000144000000000,0.001453000000000,0.000747000000000,0.002725000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1307.000000000000000,179.000000000000000,121.000000000000000,0.190996220324968,0.267097150771334,121.000000000000000,0.395303787014294 +0.000000000000000,0.000017000000000,0.000177000000000,0.000120000000000,0.001414000000000,0.000681000000000,0.002474000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1109.000000000000000,205.000000000000000,121.000000000000000,0.175048358387321,0.227273301247532,121.000000000000000,0.336364481552976 +0.000000000000000,0.000023000000000,0.000186000000000,0.000160000000000,0.001932000000000,0.000738000000000,0.003097000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1100.000000000000000,194.000000000000000,121.000000000000000,0.173557283673669,0.191658131610694,121.000000000000000,0.283654034716855 +0.000000000000000,0.000019000000000,0.000107000000000,0.000116000000000,0.001671000000000,0.000745000000000,0.002708000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,204.000000000000000,121.000000000000000,0.158568907261423,0.243359531069700,121.000000000000000,0.360172098406900 +0.000000000000000,0.000018000000000,0.000103000000000,0.000132000000000,0.001512000000000,0.000732000000000,0.002547000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1187.000000000000000,194.000000000000000,121.000000000000000,0.146749823653506,0.164413086505490,121.000000000000000,0.243331352597904 +0.000000000000000,0.000014000000000,0.000181000000000,0.000144000000000,0.001537000000000,0.000892000000000,0.002808000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,196.000000000000000,121.000000000000000,0.131283592229348,0.155956326816494,121.000000000000000,0.230815358417731 +0.000000000000000,0.000012000000000,0.000176000000000,0.000121000000000,0.001482000000000,0.000877000000000,0.002705000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,196.000000000000000,121.000000000000000,0.159033296053125,0.194395333842714,121.000000000000000,0.287705090036121 +0.000000000000000,0.000020000000000,0.000211000000000,0.000162000000000,0.001881000000000,0.000840000000000,0.003174000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1262.000000000000000,184.000000000000000,121.000000000000000,0.163845159646448,0.177604150932044,121.000000000000000,0.262854149344864 +0.000000000000000,0.000015000000000,0.000115000000000,0.000124000000000,0.001771000000000,0.000924000000000,0.002997000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1269.000000000000000,218.000000000000000,121.000000000000000,0.167741708685618,0.187775885781358,121.000000000000000,0.277908323682058 +0.000000000000000,0.000012000000000,0.000104000000000,0.000127000000000,0.001764000000000,0.000780000000000,0.002830000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,229.000000000000000,121.000000000000000,0.186347950475868,0.251813866360726,121.000000000000000,0.372684503981683 +0.000000000000000,0.000020000000000,0.000102000000000,0.000142000000000,0.001444000000000,0.000872000000000,0.002621000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1275.000000000000000,174.000000000000000,121.000000000000000,0.166303486197133,0.189747084068580,120.000000000000000,0.280825682960661 +0.000000000000000,0.000016000000000,0.000125000000000,0.000117000000000,0.001504000000000,0.000769000000000,0.002568000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,202.000000000000000,121.000000000000000,0.167769122437661,0.195787376215580,121.000000000000000,0.289765303335874 +0.000000000000000,0.000029000000000,0.000158000000000,0.000154000000000,0.001818000000000,0.000771000000000,0.003044000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1304.000000000000000,177.000000000000000,121.000000000000000,0.158463858246833,0.191255881763862,119.000000000000000,0.283058705300228 +0.000000000000000,0.000020000000000,0.000185000000000,0.000118000000000,0.001683000000000,0.000786000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1339.000000000000000,206.000000000000000,121.000000000000000,0.161183566619254,0.178028171732775,121.000000000000000,0.263481700394391 +0.000000000000000,0.000017000000000,0.000177000000000,0.000136000000000,0.001597000000000,0.000782000000000,0.002874000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1383.000000000000000,198.000000000000000,121.000000000000000,0.142916686874711,0.162123928033689,121.000000000000000,0.239943431119633 +0.000000000000000,0.000016000000000,0.000127000000000,0.000234000000000,0.001629000000000,0.000716000000000,0.002906000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,214.000000000000000,121.000000000000000,0.154355717762879,0.218609409653489,121.000000000000000,0.323541914330688 +0.000000000000000,0.000018000000000,0.000175000000000,0.000117000000000,0.001623000000000,0.000874000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,227.000000000000000,121.000000000000000,0.174558038196839,0.199124535422790,121.000000000000000,0.294704311042227 +0.000000000000000,0.000023000000000,0.000101000000000,0.000109000000000,0.001533000000000,0.000703000000000,0.002504000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1228.000000000000000,215.000000000000000,121.000000000000000,0.137995003938898,0.152632389051692,121.000000000000000,0.225895936409808 +0.000000000000000,0.000027000000000,0.000177000000000,0.000139000000000,0.001775000000000,0.000662000000000,0.002820000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,237.000000000000000,121.000000000000000,0.135541904251767,0.167668386817417,121.000000000000000,0.248149206964557 +0.000000000000000,0.000022000000000,0.000181000000000,0.000307000000000,0.001635000000000,0.001033000000000,0.003237000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,200.000000000000000,121.000000000000000,0.163536557849499,0.206044880204936,121.000000000000000,0.304946443542624 +0.000000000000000,0.000013000000000,0.000197000000000,0.000138000000000,0.001693000000000,0.000665000000000,0.002750000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,224.000000000000000,121.000000000000000,0.153373223673703,0.199555553830275,120.000000000000000,0.295342223558018 +0.000000000000000,0.000022000000000,0.000130000000000,0.000127000000000,0.001747000000000,0.000744000000000,0.002816000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,234.000000000000000,121.000000000000000,0.139032943637894,0.186670354479152,121.000000000000000,0.276272121799386 +0.000000000000000,0.000023000000000,0.000189000000000,0.000117000000000,0.001436000000000,0.000905000000000,0.002707000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,191.000000000000000,121.000000000000000,0.139072087006321,0.170137357542569,121.000000000000000,0.251803291766507 +0.000000000000000,0.000019000000000,0.000189000000000,0.000134000000000,0.001636000000000,0.001043000000000,0.003059000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,205.000000000000000,121.000000000000000,0.147221191440148,0.219839242753776,121.000000000000000,0.325362095419246 +0.000000000000000,0.000026000000000,0.000136000000000,0.000141000000000,0.001640000000000,0.000834000000000,0.002814000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,198.000000000000000,121.000000000000000,0.134535729635055,0.177361976863745,120.000000000000000,0.262495743699553 +0.000000000000000,0.000024000000000,0.000188000000000,0.000137000000000,0.001576000000000,0.000999000000000,0.002968000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1224.000000000000000,202.000000000000000,121.000000000000000,0.150440594097383,0.170822892372034,120.000000000000000,0.252817881488855 +0.000000000000000,0.000018000000000,0.000128000000000,0.000123000000000,0.001595000000000,0.000906000000000,0.002923000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,211.000000000000000,121.000000000000000,0.161157776486683,0.176850558454008,121.000000000000000,0.261738825952921 +0.000000000000000,0.000023000000000,0.000171000000000,0.000136000000000,0.001546000000000,0.000836000000000,0.002772000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1094.000000000000000,211.000000000000000,121.000000000000000,0.135044752661059,0.176661372259443,121.000000000000000,0.261458838916931 +0.000000000000000,0.000016000000000,0.000180000000000,0.000129000000000,0.001692000000000,0.000849000000000,0.002919000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1089.000000000000000,203.000000000000000,121.000000000000000,0.143949354754192,0.167668782751111,120.000000000000000,0.248149788217542 +0.000000000000000,0.000017000000000,0.000108000000000,0.000139000000000,0.001568000000000,0.000704000000000,0.002578000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1166.000000000000000,200.000000000000000,121.000000000000000,0.155160670749007,0.231930876186467,120.000000000000000,0.343257712340746 +0.000000000000000,0.000035000000000,0.000227000000000,0.000132000000000,0.001664000000000,0.000790000000000,0.002968000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,195.000000000000000,121.000000000000000,0.133676069237326,0.147626375091364,120.000000000000000,0.218487058408179 +0.000000000000000,0.000015000000000,0.000128000000000,0.000120000000000,0.001551000000000,0.000732000000000,0.002590000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,204.000000000000000,121.000000000000000,0.122400626475427,0.146223124911287,121.000000000000000,0.216410216218663 +0.000000000000000,0.000020000000000,0.000245000000000,0.000118000000000,0.001618000000000,0.000770000000000,0.002815000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,219.000000000000000,121.000000000000000,0.137303784430655,0.150577933818061,121.000000000000000,0.222855351498252 +0.000000000000000,0.000016000000000,0.000214000000000,0.000131000000000,0.001570000000000,0.000888000000000,0.002875000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1239.000000000000000,183.000000000000000,121.000000000000000,0.131464957645487,0.148026075188344,121.000000000000000,0.219078597044465 +0.000000000000000,0.000018000000000,0.000103000000000,0.000139000000000,0.001556000000000,0.000929000000000,0.002796000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,197.000000000000000,121.000000000000000,0.128347170794260,0.131002024287506,121.000000000000000,0.193882998604601 +0.000000000000000,0.000034000000000,0.000281000000000,0.000139000000000,0.001662000000000,0.000725000000000,0.002956000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,194.000000000000000,121.000000000000000,0.162024538919116,0.207094523301695,121.000000000000000,0.306499905326386 +0.000000000000000,0.000019000000000,0.000198000000000,0.000119000000000,0.001621000000000,0.000844000000000,0.002973000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,215.000000000000000,121.000000000000000,0.153235488057770,0.262436341625619,121.000000000000000,0.388405779843294 +0.000000000000000,0.000021000000000,0.000198000000000,0.000131000000000,0.001561000000000,0.000847000000000,0.002828000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1054.000000000000000,201.000000000000000,121.000000000000000,0.157919685289792,0.167838875425546,121.000000000000000,0.248401521304057 +0.000000000000000,0.000015000000000,0.000170000000000,0.000130000000000,0.001648000000000,0.000666000000000,0.002683000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,211.000000000000000,121.000000000000000,0.142424009465490,0.209332762567431,120.000000000000000,0.309812491363346 +0.000000000000000,0.000020000000000,0.000127000000000,0.000139000000000,0.001528000000000,0.000726000000000,0.002588000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,207.000000000000000,121.000000000000000,0.140603779602412,0.159626649619649,121.000000000000000,0.236247445558979 +0.000000000000000,0.000019000000000,0.000194000000000,0.000143000000000,0.001999000000000,0.000624000000000,0.003025000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,205.000000000000000,121.000000000000000,0.147458908627836,0.237281094131071,121.000000000000000,0.351176020676474 +0.000000000000000,0.000022000000000,0.000175000000000,0.000126000000000,0.001454000000000,0.000837000000000,0.002656000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1106.000000000000000,198.000000000000000,121.000000000000000,0.125862132363062,0.146166756283214,120.000000000000000,0.216326793779291 +0.000000000000000,0.000018000000000,0.000233000000000,0.000119000000000,0.001470000000000,0.000923000000000,0.002807000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1145.000000000000000,188.000000000000000,121.000000000000000,0.148866800739627,0.175671536734951,121.000000000000000,0.259993879218198 +0.000000000000000,0.000021000000000,0.000205000000000,0.000128000000000,0.001543000000000,0.000761000000000,0.002708000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1156.000000000000000,186.000000000000000,121.000000000000000,0.127848678819928,0.170790549426913,121.000000000000000,0.252770016569082 +0.000000000000000,0.000016000000000,0.000176000000000,0.000145000000000,0.001515000000000,0.000719000000000,0.002758000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1165.000000000000000,189.000000000000000,121.000000000000000,0.145912191034524,0.246210134411681,121.000000000000000,0.364391034751536 +0.000000000000000,0.000017000000000,0.000137000000000,0.000142000000000,0.001535000000000,0.000625000000000,0.002544000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1007.000000000000000,197.000000000000000,121.000000000000000,0.155415123408675,0.250107101898265,121.000000000000000,0.370158504865490 +0.000000000000000,0.000018000000000,0.000187000000000,0.000129000000000,0.001485000000000,0.000745000000000,0.002609000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,997.000000000000000,196.000000000000000,121.000000000000000,0.156330994449520,0.195977327324711,121.000000000000000,0.290046452877835 +0.000000000000000,0.000027000000000,0.000216000000000,0.000122000000000,0.001463000000000,0.000715000000000,0.002603000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,994.000000000000000,202.000000000000000,121.000000000000000,0.148181823678736,0.163960783045368,121.000000000000000,0.242661951333412 +0.000000000000000,0.000021000000000,0.000148000000000,0.000131000000000,0.001726000000000,0.000788000000000,0.002871000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,990.000000000000000,214.000000000000000,121.000000000000000,0.159041550968813,0.290710356872846,121.000000000000000,0.430251294240852 +0.000000000000000,0.000014000000000,0.000109000000000,0.000146000000000,0.001782000000000,0.000832000000000,0.002935000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,237.000000000000000,121.000000000000000,0.144598980992546,0.166027958406800,121.000000000000000,0.245721363787328 +0.000000000000000,0.000033000000000,0.000225000000000,0.000141000000000,0.001562000000000,0.000724000000000,0.002797000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,200.000000000000000,121.000000000000000,0.138993500529387,0.196990361228138,121.000000000000000,0.291545731774589 +0.000000000000000,0.000018000000000,0.000102000000000,0.000130000000000,0.001577000000000,0.000768000000000,0.002776000000000,1.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1025.000000000000000,218.000000000000000,121.000000000000000,0.159928213604770,0.189984919538914,121.000000000000000,0.281177669551262 +0.000000000000000,0.000012000000000,0.000247000000000,0.000124000000000,0.001584000000000,0.000607000000000,0.002648000000000,0.000000000000000,108.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,232.000000000000000,121.000000000000000,0.169367654114597,0.221725877325533,121.000000000000000,0.328154308402856 +0.000000000000000,0.000029000000000,0.000128000000000,0.000128000000000,0.001642000000000,0.000782000000000,0.002760000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,211.000000000000000,121.000000000000000,0.147968518377667,0.154122927463339,121.000000000000000,0.228101943121478 +0.000000000000000,0.000016000000000,0.000176000000000,0.000142000000000,0.001527000000000,0.000917000000000,0.002821000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,946.000000000000000,207.000000000000000,121.000000000000000,0.137541055415491,0.157761641471241,121.000000000000000,0.233487226761447 +0.000000000000000,0.000032000000000,0.000147000000000,0.000136000000000,0.001548000000000,0.000751000000000,0.002712000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,954.000000000000000,199.000000000000000,121.000000000000000,0.162415187446687,0.197580421079148,121.000000000000000,0.292419026477691 +0.000000000000000,0.000019000000000,0.000178000000000,0.000129000000000,0.001473000000000,0.000728000000000,0.002570000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,952.000000000000000,191.000000000000000,121.000000000000000,0.171041015417160,0.194915117263576,121.000000000000000,0.288474365726664 +0.000000000000000,0.000029000000000,0.000197000000000,0.000115000000000,0.001354000000000,0.000809000000000,0.002551000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,965.000000000000000,175.000000000000000,121.000000000000000,0.154222763807780,0.285544163762687,120.000000000000000,0.422605366108563 +0.000000000000000,0.000019000000000,0.000139000000000,0.000125000000000,0.001460000000000,0.000814000000000,0.002600000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,968.000000000000000,171.000000000000000,121.000000000000000,0.150595603214115,0.221852768512487,121.000000000000000,0.328342103660952 +0.000000000000000,0.000013000000000,0.000104000000000,0.000134000000000,0.001356000000000,0.000752000000000,0.002398000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,996.000000000000000,168.000000000000000,121.000000000000000,0.147430237486138,0.148159285827304,121.000000000000000,0.219275742894069 +0.000000000000000,0.000020000000000,0.000193000000000,0.000142000000000,0.001953000000000,0.000947000000000,0.003301000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1030.000000000000000,177.000000000000000,121.000000000000000,0.145798815037739,0.159663663944367,121.000000000000000,0.236302234970841 +0.000000000000000,0.000015000000000,0.000103000000000,0.000115000000000,0.001290000000000,0.000699000000000,0.002255000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,172.000000000000000,121.000000000000000,0.142196014156506,0.187147109561421,121.000000000000000,0.276977737652069 +0.000000000000000,0.000024000000000,0.000160000000000,0.000150000000000,0.001811000000000,0.000777000000000,0.003271000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1069.000000000000000,202.000000000000000,121.000000000000000,0.139609449101900,0.165271225439012,121.000000000000000,0.244601415643369 +0.000000000000000,0.000018000000000,0.000179000000000,0.000151000000000,0.001532000000000,0.000737000000000,0.002693000000000,0.000000000000000,97.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,203.000000000000000,121.000000000000000,0.147947477270869,0.179555832175793,121.000000000000000,0.265742648149137 +0.000000000000000,0.000019000000000,0.000177000000000,0.000125000000000,0.001512000000000,0.000717000000000,0.002588000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,212.000000000000000,121.000000000000000,0.147575394987053,0.165266294276773,121.000000000000000,0.244594112073245 +0.000000000000000,0.000016000000000,0.000106000000000,0.000137000000000,0.001490000000000,0.000826000000000,0.002618000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,915.000000000000000,203.000000000000000,121.000000000000000,0.154152939014589,0.159181033056470,121.000000000000000,0.235587925595243 +0.000000000000000,0.000013000000000,0.000177000000000,0.000117000000000,0.001355000000000,0.000751000000000,0.002448000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,921.000000000000000,192.000000000000000,121.000000000000000,0.150692609588339,0.183809895950044,121.000000000000000,0.272038654130109 +0.000000000000000,0.000023000000000,0.000221000000000,0.000167000000000,0.001790000000000,0.000948000000000,0.003199000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,932.000000000000000,170.000000000000000,121.000000000000000,0.149963553230709,0.203273297272569,121.000000000000000,0.300844465407444 +0.000000000000000,0.000026000000000,0.000151000000000,0.000122000000000,0.001357000000000,0.000723000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,961.000000000000000,168.000000000000000,121.000000000000000,0.142178288778563,0.151033799800819,121.000000000000000,0.223530034583352 +0.000000000000000,0.000018000000000,0.000155000000000,0.000123000000000,0.001394000000000,0.000766000000000,0.002492000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,986.000000000000000,182.000000000000000,121.000000000000000,0.141207634148801,0.172667793318101,121.000000000000000,0.255548330024774 +0.000000000000000,0.000018000000000,0.000174000000000,0.000143000000000,0.001389000000000,0.000790000000000,0.002550000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1023.000000000000000,171.000000000000000,121.000000000000000,0.140177814193211,0.162740001476768,121.000000000000000,0.240855214879131 +0.000000000000000,0.000021000000000,0.000173000000000,0.000122000000000,0.001280000000000,0.000796000000000,0.002581000000000,1.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1049.000000000000000,171.000000000000000,121.000000000000000,0.138041804635574,0.175894893269503,121.000000000000000,0.260324460535807 +0.000000000000000,0.000040000000000,0.000240000000000,0.000156000000000,0.001610000000000,0.000905000000000,0.003078000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,181.000000000000000,121.000000000000000,0.128640393991057,0.132619727567557,121.000000000000000,0.196277204924269 +0.000000000000000,0.000016000000000,0.000181000000000,0.000117000000000,0.001424000000000,0.000760000000000,0.002543000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,886.000000000000000,172.000000000000000,121.000000000000000,0.138352531751683,0.243902625004102,121.000000000000000,0.360975895470416 +0.000000000000000,0.000020000000000,0.000178000000000,0.000131000000000,0.001344000000000,0.000965000000000,0.002674000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,904.000000000000000,177.000000000000000,121.000000000000000,0.132849298892330,0.165943140041726,121.000000000000000,0.245595863686171 +0.000000000000000,0.000018000000000,0.000129000000000,0.000145000000000,0.001297000000000,0.000878000000000,0.002506000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,916.000000000000000,164.000000000000000,121.000000000000000,0.139100399764650,0.179464095411716,121.000000000000000,0.265606862397320 +0.000000000000000,0.000015000000000,0.000175000000000,0.000119000000000,0.001188000000000,0.000717000000000,0.002249000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,926.000000000000000,158.000000000000000,121.000000000000000,0.124508577433483,0.232688205210390,121.000000000000000,0.344378545000118 +0.000000000000000,0.000041000000000,0.000173000000000,0.000189000000000,0.001590000000000,0.000722000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,950.000000000000000,167.000000000000000,121.000000000000000,0.124364211160562,0.198989442345874,121.000000000000000,0.294504385287040 +0.000000000000000,0.000022000000000,0.000180000000000,0.000121000000000,0.001454000000000,0.000762000000000,0.002586000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,981.000000000000000,174.000000000000000,121.000000000000000,0.121986273502660,0.181985786779231,121.000000000000000,0.269338961458690 +0.000000000000000,0.000022000000000,0.000175000000000,0.000128000000000,0.001283000000000,0.000822000000000,0.002468000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1006.000000000000000,154.000000000000000,121.000000000000000,0.120933033868538,0.208757551950541,121.000000000000000,0.308961183186222 +0.000000000000000,0.000026000000000,0.000175000000000,0.000143000000000,0.001286000000000,0.000813000000000,0.002485000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1029.000000000000000,160.000000000000000,121.000000000000000,0.109269904153490,0.130322654376541,121.000000000000000,0.192877532026941 +0.000000000000000,0.000023000000000,0.000175000000000,0.000120000000000,0.001254000000000,0.000637000000000,0.002411000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,160.000000000000000,121.000000000000000,0.126897062957379,0.186841162125259,121.000000000000000,0.276524916304396 +0.000000000000000,0.000021000000000,0.000140000000000,0.000122000000000,0.001471000000000,0.000691000000000,0.002520000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,891.000000000000000,190.000000000000000,121.000000000000000,0.112694197899670,0.185521960041402,121.000000000000000,0.274572512797542 +0.000000000000000,0.000020000000000,0.000176000000000,0.000199000000000,0.001263000000000,0.000879000000000,0.002581000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,888.000000000000000,161.000000000000000,121.000000000000000,0.099793871260750,0.108940668612828,121.000000000000000,0.161232198827340 +0.000000000000000,0.000018000000000,0.000103000000000,0.000137000000000,0.001309000000000,0.000726000000000,0.002333000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,901.000000000000000,171.000000000000000,121.000000000000000,0.091727983876207,0.161347683449736,121.000000000000000,0.238794571957605 +0.000000000000000,0.000018000000000,0.000175000000000,0.000143000000000,0.001336000000000,0.000860000000000,0.002571000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,909.000000000000000,175.000000000000000,121.000000000000000,0.124196463571817,0.147059251648374,121.000000000000000,0.217647703825064 +0.000000000000000,0.000019000000000,0.000174000000000,0.000125000000000,0.001265000000000,0.000845000000000,0.002464000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,175.000000000000000,121.000000000000000,0.114265472855587,0.148521989180687,121.000000000000000,0.219812542662230 +0.000000000000000,0.000030000000000,0.000158000000000,0.000151000000000,0.001613000000000,0.000756000000000,0.002826000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,943.000000000000000,170.000000000000000,121.000000000000000,0.092242007337942,0.147927216635033,121.000000000000000,0.218932272923301 +0.000000000000000,0.000017000000000,0.000106000000000,0.000120000000000,0.001308000000000,0.001030000000000,0.002622000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,970.000000000000000,159.000000000000000,121.000000000000000,0.106941604326635,0.142972503313410,121.000000000000000,0.211599311615969 +0.000000000000000,0.000021000000000,0.000127000000000,0.000130000000000,0.001305000000000,0.000748000000000,0.002368000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,164.000000000000000,121.000000000000000,0.114683633251297,0.167145956283516,121.000000000000000,0.247376014134327 +0.000000000000000,0.000023000000000,0.000135000000000,0.000297000000000,0.001353000000000,0.000968000000000,0.002824000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1014.000000000000000,170.000000000000000,121.000000000000000,0.103049615014965,0.141054167934770,121.000000000000000,0.208760168771737 +0.000000000000000,0.000020000000000,0.000101000000000,0.000125000000000,0.001114000000000,0.000766000000000,0.002163000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1033.000000000000000,149.000000000000000,121.000000000000000,0.088395574941732,0.099895628586074,121.000000000000000,0.147845538218435 +0.000000000000000,0.000023000000000,0.000190000000000,0.000110000000000,0.001226000000000,0.000774000000000,0.002371000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,162.000000000000000,121.000000000000000,0.098469632616480,0.121321004607324,121.000000000000000,0.179555087237014 +0.000000000000000,0.000022000000000,0.000180000000000,0.000138000000000,0.001374000000000,0.001109000000000,0.002867000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1058.000000000000000,162.000000000000000,121.000000000000000,0.106165831669812,0.125397650867987,120.000000000000000,0.185588518499696 +0.000000000000000,0.000022000000000,0.000177000000000,0.000138000000000,0.001328000000000,0.000617000000000,0.002320000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1063.000000000000000,154.000000000000000,121.000000000000000,0.100439787680905,0.214221605374360,121.000000000000000,0.317047979072175 +0.000000000000000,0.000019000000000,0.000114000000000,0.000131000000000,0.001262000000000,0.000730000000000,0.002294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1073.000000000000000,151.000000000000000,121.000000000000000,0.101286833254115,0.171921771357668,121.000000000000000,0.254444227342640 +0.000000000000000,0.000018000000000,0.000111000000000,0.000120000000000,0.001321000000000,0.000814000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1075.000000000000000,159.000000000000000,121.000000000000000,0.100562747826060,0.189141833234101,121.000000000000000,0.279929921566071 +0.000000000000000,0.000020000000000,0.000137000000000,0.000115000000000,0.001291000000000,0.000697000000000,0.002305000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1102.000000000000000,170.000000000000000,121.000000000000000,0.108513981039663,0.171230581801412,121.000000000000000,0.253421272631674 +0.000000000000000,0.000023000000000,0.000142000000000,0.000126000000000,0.001300000000000,0.000836000000000,0.002476000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1133.000000000000000,146.000000000000000,121.000000000000000,0.093525447667265,0.136448459114485,121.000000000000000,0.201943713921807 +0.000000000000000,0.000018000000000,0.000175000000000,0.000137000000000,0.001338000000000,0.000746000000000,0.002454000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1153.000000000000000,167.000000000000000,121.000000000000000,0.098830315127647,0.135984610426414,121.000000000000000,0.201257228873912 +0.000000000000000,0.000016000000000,0.000153000000000,0.000133000000000,0.001342000000000,0.000846000000000,0.002529000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1169.000000000000000,168.000000000000000,121.000000000000000,0.104911664860530,0.116528427363267,121.000000000000000,0.172462069689336 +0.000000000000000,0.000016000000000,0.000177000000000,0.000153000000000,0.001420000000000,0.001057000000000,0.002862000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,171.000000000000000,121.000000000000000,0.112357270101045,0.155883824635645,121.000000000000000,0.230708054062211 +0.000000000000000,0.000019000000000,0.000183000000000,0.000118000000000,0.001173000000000,0.000692000000000,0.002218000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1205.000000000000000,160.000000000000000,121.000000000000000,0.114134537418471,0.211839504506888,121.000000000000000,0.313522477539393 +0.000000000000000,0.000022000000000,0.000144000000000,0.000340000000000,0.002673000000000,0.001050000000000,0.004300000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1210.000000000000000,169.000000000000000,121.000000000000000,0.118247530316238,0.220707314595291,121.000000000000000,0.326646841420566 +0.000000000000000,0.000021000000000,0.000121000000000,0.000119000000000,0.001312000000000,0.000708000000000,0.002323000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,159.000000000000000,121.000000000000000,0.108077994964789,0.189119078021565,121.000000000000000,0.279896234164773 +0.000000000000000,0.000022000000000,0.000131000000000,0.000170000000000,0.001575000000000,0.000930000000000,0.002873000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,161.000000000000000,121.000000000000000,0.111392372838571,0.157100432720142,121.000000000000000,0.232508649452340 +0.000000000000000,0.000021000000000,0.000129000000000,0.000146000000000,0.001357000000000,0.001069000000000,0.002758000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,169.000000000000000,121.000000000000000,0.115843692072703,0.148606734789863,121.000000000000000,0.219937966947781 +0.000000000000000,0.000018000000000,0.000196000000000,0.000246000000000,0.003021000000000,0.001245000000000,0.004778000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1237.000000000000000,170.000000000000000,121.000000000000000,0.110486981424295,0.140524548316829,121.000000000000000,0.207976323848749 +0.000000000000000,0.000019000000000,0.000176000000000,0.000120000000000,0.001319000000000,0.000781000000000,0.002451000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,166.000000000000000,121.000000000000000,0.116858932300554,0.142202739311708,121.000000000000000,0.210460043128204 +0.000000000000000,0.000023000000000,0.000194000000000,0.000117000000000,0.001352000000000,0.000677000000000,0.002411000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1229.000000000000000,165.000000000000000,121.000000000000000,0.110055898076008,0.160160155663640,121.000000000000000,0.237037039832150 +0.000000000000000,0.000019000000000,0.000128000000000,0.000129000000000,0.001375000000000,0.000783000000000,0.002473000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,170.000000000000000,121.000000000000000,0.126041915882071,0.160651736154074,121.000000000000000,0.237764591667073 +0.000000000000000,0.000024000000000,0.000199000000000,0.000203000000000,0.001799000000000,0.000878000000000,0.003158000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,182.000000000000000,121.000000000000000,0.131395777815393,0.222014190834195,121.000000000000000,0.328581023909914 +0.000000000000000,0.000019000000000,0.000103000000000,0.000125000000000,0.001415000000000,0.000879000000000,0.002580000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,183.000000000000000,121.000000000000000,0.135634727713905,0.187089754575309,121.000000000000000,0.276892849444298 +0.000000000000000,0.000017000000000,0.000101000000000,0.000119000000000,0.001263000000000,0.000774000000000,0.002307000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,171.000000000000000,121.000000000000000,0.140713971719655,0.131521683676958,121.000000000000000,0.194652084756127 +0.000000000000000,0.000016000000000,0.000127000000000,0.000147000000000,0.001449000000000,0.000744000000000,0.002523000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1225.000000000000000,178.000000000000000,121.000000000000000,0.139061294677762,0.203192935815326,121.000000000000000,0.300725561264080 +0.000000000000000,0.000018000000000,0.000191000000000,0.000120000000000,0.001241000000000,0.000897000000000,0.002515000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,165.000000000000000,121.000000000000000,0.137698280788293,0.173674353597993,121.000000000000000,0.257038030883200 +0.000000000000000,0.000018000000000,0.000177000000000,0.000127000000000,0.001350000000000,0.000790000000000,0.002651000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,165.000000000000000,121.000000000000000,0.156706447381789,0.199509549579289,121.000000000000000,0.295274141143093 +0.000000000000000,0.000016000000000,0.000153000000000,0.000147000000000,0.001501000000000,0.000734000000000,0.002643000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1046.000000000000000,206.000000000000000,121.000000000000000,0.145254964747455,0.192623491876607,121.000000000000000,0.285082754555528 +0.000000000000000,0.000017000000000,0.000175000000000,0.000117000000000,0.001356000000000,0.000838000000000,0.002539000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1044.000000000000000,191.000000000000000,121.000000000000000,0.154344725976771,0.284210388757305,121.000000000000000,0.420631380425634 +0.000000000000000,0.000021000000000,0.000118000000000,0.000127000000000,0.001456000000000,0.000725000000000,0.002492000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,193.000000000000000,121.000000000000000,0.154361389589804,0.236960337646421,121.000000000000000,0.350701313890299 +0.000000000000000,0.000018000000000,0.000108000000000,0.000128000000000,0.001548000000000,0.000868000000000,0.002721000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1036.000000000000000,190.000000000000000,121.000000000000000,0.144847037730729,0.155773166896081,121.000000000000000,0.230544292351477 +0.000000000000000,0.000018000000000,0.000129000000000,0.000124000000000,0.001383000000000,0.000975000000000,0.002669000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1027.000000000000000,183.000000000000000,121.000000000000000,0.145841805810321,0.158516249676071,121.000000000000000,0.234604041193658 +0.000000000000000,0.000028000000000,0.000216000000000,0.000131000000000,0.001615000000000,0.000765000000000,0.002878000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1020.000000000000000,192.000000000000000,121.000000000000000,0.144426676201549,0.156437471874113,121.000000000000000,0.231527443868863 +0.000000000000000,0.000021000000000,0.000153000000000,0.000120000000000,0.001406000000000,0.000771000000000,0.002509000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1015.000000000000000,188.000000000000000,121.000000000000000,0.147869059454709,0.168692675134760,120.000000000000000,0.249665165294811 +0.000000000000000,0.000021000000000,0.000170000000000,0.000115000000000,0.001425000000000,0.000608000000000,0.002395000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1012.000000000000000,187.000000000000000,121.000000000000000,0.168987279193652,0.289264692226178,121.000000000000000,0.428111727272626 +0.000000000000000,0.000017000000000,0.000135000000000,0.000127000000000,0.001615000000000,0.000791000000000,0.002734000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1011.000000000000000,192.000000000000000,121.000000000000000,0.170578883496370,0.166353065557405,121.000000000000000,0.246202540171952 +0.000000000000000,0.000019000000000,0.000178000000000,0.000136000000000,0.001520000000000,0.000741000000000,0.002636000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1005.000000000000000,203.000000000000000,121.000000000000000,0.156640038600278,0.207055055847171,121.000000000000000,0.306441476765396 +0.000000000000000,0.000033000000000,0.000177000000000,0.000135000000000,0.001641000000000,0.000861000000000,0.003288000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,999.000000000000000,186.000000000000000,121.000000000000000,0.143274685502394,0.179257874186524,121.000000000000000,0.265301654036128 +0.000000000000000,0.000015000000000,0.000129000000000,0.000142000000000,0.001443000000000,0.000740000000000,0.002555000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,202.000000000000000,121.000000000000000,0.156881076812533,0.218447877056405,121.000000000000000,0.323302867722382 +0.000000000000000,0.000036000000000,0.000177000000000,0.000151000000000,0.001530000000000,0.000725000000000,0.002741000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,820.000000000000000,168.000000000000000,121.000000000000000,0.155951030020751,0.213023655758012,121.000000000000000,0.315275031106667 +0.000000000000000,0.000030000000000,0.000196000000000,0.000128000000000,0.001426000000000,0.000831000000000,0.002660000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,816.000000000000000 \ No newline at end of file diff --git a/CPU_version/svo/test/data/.gitignore b/CPU_version/svo/test/data/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/CPU_version/svo/test/data/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/CPU_version/svo/test/results/.gitignore b/CPU_version/svo/test/results/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/CPU_version/svo/test/results/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/CPU_version/svo/test/test_depth_filter.cpp b/CPU_version/svo/test/test_depth_filter.cpp new file mode 100644 index 0000000..4d22493 --- /dev/null +++ b/CPU_version/svo/test/test_depth_filter.cpp @@ -0,0 +1,206 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +struct ConvergedSeed { + int x_, y_; + double depth_, error_; + ConvergedSeed(int x, int y, double depth, double error) : + x_(x), y_(y), depth_(depth), error_(error) + {} +}; + +/// DepthFilter Test-Fixture +class DepthFilterTest { + public: + DepthFilterTest(); + virtual ~DepthFilterTest(); + void depthFilterCb(svo::Point* point, double depth_sigma2); + void testReconstruction(std::string dataset_dir, std::string experiment_name); + + std::list results_; + std::vector errors_; + size_t n_converged_seeds_; + vk::PinholeCamera* cam_; + svo::DepthFilter* depth_filter_; + svo::FramePtr frame_ref_; + svo::FramePtr frame_cur_; + cv::Mat depth_ref_; +}; + +DepthFilterTest::DepthFilterTest() : + n_converged_seeds_(0), + cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)), + depth_filter_(NULL) +{ + errors_.reserve(1000); +} + +DepthFilterTest::~DepthFilterTest() +{ + delete cam_; +} + +void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2) +{ + double depth = (frame_ref_->pos() - point->pos_).norm(); + double error = fabs(depth - depth_ref_.at(point->obs_.front()->px[1], + point->obs_.front()->px[0])); + results_.push_back(ConvergedSeed( + point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error)); + errors_.push_back(error); + delete point->obs_.front(); + delete point; + ++n_converged_seeds_; +} + +void DepthFilterTest::testReconstruction( + std::string dataset_dir, + std::string experiment_name) +{ + vk::FileReader sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + std::cout << "RUN EXPERIMENT: read " << sequence.size() << " dataset entries." << std::endl; + std::vector::iterator it = sequence.begin(); + vk::Timer t; + std::list n_converged_per_iteration; + + svo::feature_detection::DetectorPtr feature_detector( + new svo::feature_detection::FastDetector( + cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels())); + svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2); + depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb); + depth_filter_->options_.verbose = true; + + for(int i=0; it != sequence.end() && i<20; ++it, ++i) + { + std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png"); + printf("reading image: '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + Sophus::SE3 T_w_f(it->q_, it->t_); + if(i == 0) + { + // create reference frame and load ground truth depthmap + frame_ref_ = boost::make_shared(cam_, img, 0.0); + frame_ref_->T_f_w_ = T_w_f.inverse(); + depth_filter_->addKeyframe(frame_ref_, 2, 0.5); + vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_); + continue; + } + + n_converged_seeds_ = 0; + frame_cur_ = boost::make_shared(cam_, img, 0.0); + frame_cur_->T_f_w_ = T_w_f.inverse(); + depth_filter_->addFrame(frame_cur_); + n_converged_per_iteration.push_back(n_converged_seeds_); + } + printf("Experiment '%s' took %f ms\n", experiment_name.c_str(), t.stop()*1000); + + // compute mean, median and variance of error in converged area + { + printf("# converged: \t %zu (ref: 287)\n", errors_.size()); + double sum_error = 0; + std::for_each(errors_.begin(), errors_.end(), [&](double& e){sum_error+=e;}); + printf("mean error: \t %f cm (ref: 0.080357)\n", sum_error*100/errors_.size()); + std::vector::iterator it = errors_.begin()+0.5*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("50-percentile: \t %f cm (ref: 0.062042)\n", *it*100); + it = errors_.begin()+0.8*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("80-percentile: \t %f cm (ref: 0.124526)\n", *it*100); + it = errors_.begin()+0.95*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("95-percentile: \t %f cm (ref: 0.200417)\n", *it*100); + } + + // trace error + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt"); + std::ofstream ofs(trace_name.c_str()); + for(std::list::iterator i=results_.begin(); i!=results_.end(); ++i) + ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl; + ofs.close(); + + // trace convergence rate + trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt"; + ofs.open(trace_name.c_str()); + for(std::list::iterator it=n_converged_per_iteration.begin(); + it!=n_converged_per_iteration.end(); ++it) + ofs << *it << std::endl; + ofs.close(); + + // write ply file for pointcloud visualization in Meshlab + trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply"; + ofs.open(trace_name.c_str()); + ofs << "ply" << std::endl + << "format ascii 1.0" << std::endl + << "element vertex " << results_.size() << std::endl + << "property float x" << std::endl + << "property float y" << std::endl + << "property float z" << std::endl + << "property uchar blue" << std::endl + << "property uchar green" << std::endl + << "property uchar red" << std::endl + << "end_header" << std::endl; + + for(std::list::iterator i=results_.begin(); i!=results_.end(); ++i) + { + cv::Vec3b c = frame_ref_->img_pyr_[0].at(i->y_, i->x_); + Eigen::Vector3d p = cam_->cam2world(i->x_, i->y_)*i->depth_; + ofs << p[0] << " " << p[1] << " " << p[2] << " " + << (int) c[0] << " " << (int) c[1] << " " << (int) c[2] << std::endl; + } +} + +} // namespace + +int main(int argc, char** argv) +{ + DepthFilterTest test; + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string experiment_name("sin2_tex2_h1_v8_d"); + test.testReconstruction(dataset_dir, experiment_name); + return 0; +} diff --git a/CPU_version/svo/test/test_feature_alignment.cpp b/CPU_version/svo/test/test_feature_alignment.cpp new file mode 100644 index 0000000..dac8911 --- /dev/null +++ b/CPU_version/svo/test/test_feature_alignment.cpp @@ -0,0 +1,124 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include "test_utils.h" +#if __ARM_NEON__ +#include +#endif + +using namespace Eigen; + +void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px, + cv::Mat& ref_patch_with_border) +{ + // compute interpolation weights + const int u_r = floor(px[0]); + const int v_r = floor(px[1]); + const float subpix_u = px[0]-u_r; + const float subpix_v = px[1]-v_r; + const float wTL = (1.0-subpix_u)*(1.0-subpix_v); + const float wTR = subpix_u * (1.0-subpix_v); + const float wBL = (1.0-subpix_u)*subpix_v; + const float wBR = subpix_u * subpix_v; + + // loop through search_patch, interpolate + ref_patch_with_border = cv::Mat(10, 10, CV_8UC1); + uint8_t* patch_ptr = ref_patch_with_border.data; + const int stride = img.step.p[0]; + for(int y=0; y<10; ++y) + { + uint8_t* img_ptr = (uint8_t*) img.data + (v_r+y-4-1)*stride + u_r-4-1; + for(int x=0; x<10; ++x, ++patch_ptr, ++img_ptr) + *patch_ptr = wTL*img_ptr[0] + wTR*img_ptr[1] + wBL*img_ptr[stride] + wBR*img_ptr[stride+1]; + } +} + +int main(int argc, char **argv) +{ + + std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(img.type() == CV_8UC1); + + // TODO: test on corner/gradient features! + Vector2d px_true(130.2,120.3); + + // create reference patch with border + cv::Mat ref_patch_with_border; + generateRefPatchNoWarpInterpolate(img, px_true, ref_patch_with_border); + + // create reference patch, aligned + uint8_t* ref_patch = vk::aligned_mem::aligned_alloc(64, 16); + uint8_t* ref_patch_ptr = ref_patch; + for(int y=1; y<9; ++y) + { + uint8_t* ref_patch_border_ptr = ref_patch_with_border.data + y*10 + 1; + for(int x=0; x<8; ++x, ++ref_patch_border_ptr, ++ref_patch_ptr) + *ref_patch_ptr = *ref_patch_border_ptr; + } + + Vector2d px_est, px_error(-1.1, -0.8); + double h_inv; + vk::Timer t; + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + Vector2f dir = px_error.normalized().cast(); + svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv); + } + Vector2d e = px_est-px_true; + printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.982000ms, 0.000033px) \n", t.stop()*1000, e.norm()); + + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D(img, ref_patch_with_border.data, ref_patch, 3, px_est, true); + } + e = px_est-px_true; + printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.306000ms, 0.015102px)\n", t.stop()*1000, e.norm()); + +#ifdef __SSE2__ + // Note, this KLT implementation is not invariant to illuminatino changes! + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D_SSE2(img, ref_patch_with_border.data, ref_patch, 3, px_est); + } + e = px_est-px_true; + printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.460000ms, 0.021881px)\n", t.stop()*1000, e.norm()); +#endif + +#ifdef __ARM_NEON__ + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D_NEON(img, ref_patch_with_border.data, ref_patch, 3, px_est); + } + e = px_est-px_true; + printf("1000Xalign 2D NEON %fms, error = %fpx \t (ref Odroid-U2: ?, ?)\n", t.stop()*1000, e.norm()); +#endif + + return 0; +} diff --git a/CPU_version/svo/test/test_feature_detection.cpp b/CPU_version/svo/test/test_feature_detection.cpp new file mode 100644 index 0000000..3ba2b2f --- /dev/null +++ b/CPU_version/svo/test/test_feature_detection.cpp @@ -0,0 +1,74 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +using namespace Eigen; +using namespace std; + +void testCornerDetector() +{ + std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(img.type() == CV_8UC1 && !img.empty()); + + vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092); + svo::FramePtr frame(new svo::Frame(cam, img, 0.0)); + + // Corner detection + vk::Timer t; + svo::Features fts; + svo::feature_detection::FastDetector fast_detector( + img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels()); + for(int i=0; i<100; ++i) + { + fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts); + } + printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 7.166360ms, 40000)\n", t.stop()*10, fts.size()); + printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n"); + cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3); + cv::cvtColor(img, img_rgb, CV_GRAY2RGB); + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ + cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1); + }); + cv::imshow("ref_img", img_rgb); + cv::waitKey(0); + + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; }); +} + +} // namespace + + +int main(int argc, char **argv) +{ + testCornerDetector(); + return 0; +} diff --git a/CPU_version/svo/test/test_matcher.cpp b/CPU_version/svo/test/test_matcher.cpp new file mode 100644 index 0000000..0b65cb9 --- /dev/null +++ b/CPU_version/svo/test/test_matcher.cpp @@ -0,0 +1,200 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +/// Matcher Test-Fixture +class MatcherTest { + public: + MatcherTest() + { + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + + // load images + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string img_name(dataset_dir+"/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img_ref(cv::imread(img_name, 0)); + img_name = std::string(dataset_dir+"/img/frame_000006_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img_cur(cv::imread(img_name, 0)); + assert(!img_ref.empty() && !img_cur.empty()); + + // create frame + frame_ref_ = new svo::Frame(cam_, img_ref, 1.0); + frame_cur_ = new svo::Frame(cam_, img_cur, 2.0); + ref_ftr_ = new svo::Feature(frame_ref_, Eigen::Vector2d(300, 260), 0); + + // set poses + Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000); + Eigen::Vector3d t_w_cur(0.5673, 0.5641, 2.0000); + Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0); + Eigen::Quaterniond q_w_cur(0.0, 0.8235, 0.2130, 0.0); + frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse(); + frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur, t_w_cur).inverse(); + + // load ground-truth depth + vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000002_0.depth", *cam_, depth_ref_); + } + + virtual ~MatcherTest() + { + delete cam_; + delete frame_ref_; + delete frame_cur_; + delete ref_ftr_; + } + + // Individual tests + void testEpipolarSearchFullImg(); + void testWarpAffine(); + + // Objects declared here can be used by all tests + vk::PinholeCamera* cam_; + svo::Frame* frame_ref_; + svo::Frame* frame_cur_; + svo::Feature* ref_ftr_; + cv::Mat depth_ref_; +}; + +void MatcherTest::testEpipolarSearchFullImg() +{ + svo::Matcher matcher; + cv::Mat depthmap(frame_ref_->cam_->height(), frame_ref_->cam_->width(), CV_32FC1, cv::Scalar(0)); + cv::Mat error_map(depthmap.rows, depthmap.cols, CV_32FC1, cv::Scalar(0)); + cv::Mat mask(depthmap.rows, depthmap.cols, CV_8UC1, cv::Scalar(1)); + double depth_range = 0.8; + size_t n_converged = 0; + double sum_error = 0; + std::vector errors; errors.reserve(depthmap.cols*depthmap.rows); + for(int y=4; y(y, x); + double depth_estimate; + int res = matcher.findEpipolarMatchDirect( + *frame_ref_, *frame_cur_, ftr, depth_gt, fmax(depth_gt-depth_range,0.0), + depth_gt+depth_range, depth_estimate); + if(res) + { + depthmap.at(y,x) = depth_estimate; + error_map.at(y,x) = fabsf(depth_estimate-depth_gt); + ++n_converged; + sum_error += error_map.at(y,x); + errors.push_back(error_map.at(y,x)); + } + else + mask.at(y,x) = 0; + } + } + + // compute mean, median and variance of error in converged area + printf("n converged: \t %zu mm (ref: 216114)\n", n_converged); + printf("mean error: \t %f mm (ref: 0.410084)\n", sum_error*100/n_converged); + std::vector::iterator it = errors.begin()+0.5*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("50-percentile: \t %f mm (ref: 0.083203)\n", *it*100); + it = errors.begin()+0.8*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("80-percentile: \t %f mm (ref: 0.161824)\n", *it*100); + it = errors.begin()+0.95*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("95-percentile: \t %f mm (ref: 0.263539)\n", *it*100); + + // save results to file + std::ofstream output_stream; + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string output_filename(trace_dir + "/depthmap.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float)); + output_stream.close(); + + output_filename.assign(trace_dir + "/depthmap_error.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float)); + output_stream.close(); + + output_filename.assign(trace_dir + "/depthmap_mask.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t)); + output_stream.close(); +} + +void MatcherTest::testWarpAffine() +{ + const int halfpatch_size = 15; + const int patch_size=halfpatch_size*2; + svo::Matcher matcher; + double depth = 1.0; + + Sophus::SE3 T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse()); + Eigen::Vector2d px_cur(frame_cur_->cam_->world2cam(T_cur_ref*(ref_ftr_->f*depth))); + + // compute reference patch + cv::Mat ref_patch(matcher.halfpatch_size_+1, matcher.halfpatch_size_+1, + CV_8U, matcher.patch_with_border_); + + Eigen::Matrix2d A_cur_ref; + svo::warp::getWarpMatrixAffine( + *cam_, *cam_, ref_ftr_->px, ref_ftr_->f, 1.0, T_cur_ref, ref_ftr_->level, A_cur_ref); + int level_cur = svo::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1); + svo::warp::warpAffine( + A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, level_cur, matcher.halfpatch_size_+1, matcher.patch_with_border_); + + // copy current patch + Eigen::Vector2i pxi(px_cur[0]/(1<img_pyr_.at(level_cur)(cv::Range(pxi[1]-halfpatch_size, + pxi[1]+halfpatch_size), + cv::Range(pxi[0]-halfpatch_size, + pxi[0]+halfpatch_size)).copyTo(cur_patch); + + // display results + cv::circle(frame_cur_->img_pyr_[level_cur], + cv::Point2f(pxi[0], pxi[1]), + patch_size, cv::Scalar(255), 2); + cv::circle(frame_ref_->img_pyr_[ref_ftr_->level], + cv::Point2f(ref_ftr_->px[0]/(1<level), ref_ftr_->px[1]/(1<level)), + patch_size, cv::Scalar(255), 2); + cv::imshow("cur_patch", cur_patch); + cv::imshow("ref_patch", ref_patch); + cv::imshow("ref_img", frame_ref_->img_pyr_[ref_ftr_->level]); + cv::imshow("cur_img", frame_cur_->img_pyr_[level_cur]); + cv::waitKey(0); +} + +} // namespace + +int main(int argc, char** argv) +{ + MatcherTest test; + test.testEpipolarSearchFullImg(); +// test.testWarpAffine(); + return 0; +} diff --git a/CPU_version/svo/test/test_pipeline.cpp b/CPU_version/svo/test/test_pipeline.cpp new file mode 100644 index 0000000..ede3b25 --- /dev/null +++ b/CPU_version/svo/test/test_pipeline.cpp @@ -0,0 +1,98 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace svo { + +class BenchmarkNode +{ + vk::AbstractCamera* cam_; + svo::FrameHandlerMono* vo_; + +public: + BenchmarkNode(); + ~BenchmarkNode(); + void runFromFolder(); +}; + +BenchmarkNode::BenchmarkNode() +{ + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + vo_ = new svo::FrameHandlerMono(cam_); + vo_->start(); +} + +BenchmarkNode::~BenchmarkNode() +{ + delete vo_; + delete cam_; +} + +void BenchmarkNode::runFromFolder() +{ + for(int img_id = 2; img_id < 188; ++img_id) + { + // load image + std::stringstream ss; + ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_" + << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png"; + if(img_id == 2) + std::cout << "reading image " << ss.str() << std::endl; + cv::Mat img(cv::imread(ss.str().c_str(), 0)); + assert(!img.empty()); + + // process frame + vo_->addImage(img, 0.01*img_id); + + // display tracking quality + if(vo_->lastFrame() != NULL) + { + std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" + << "#Features: " << vo_->lastNumObservations() << " \t" + << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n"; + + // access the pose of the camera via vo_->lastFrame()->T_f_w_. + } + } +} + +} // namespace svo + +int main(int argc, char** argv) +{ + { + svo::BenchmarkNode benchmark; + benchmark.runFromFolder(); + } + printf("BenchmarkNode finished.\n"); + return 0; +} + diff --git a/CPU_version/svo/test/test_pose_optimizer.cpp b/CPU_version/svo/test/test_pose_optimizer.cpp new file mode 100644 index 0000000..59e0b46 --- /dev/null +++ b/CPU_version/svo/test/test_pose_optimizer.cpp @@ -0,0 +1,110 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +using namespace svo; +using namespace Eigen; + +/// Pose Optimizer Test-Fixture +class PoseOptimizerTest { + public: + PoseOptimizerTest() + { + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + + // load image + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string img_name(dataset_dir+"/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + // create frame + frame_.reset(new svo::Frame(cam_, img, 1.0)); + + // set pose + Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000); + Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0); + frame_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse(); + + // load ground-truth depth + vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000002_0.depth", *cam_, depthmap_); + + // detect features + feature_detection::FastDetector detector( + cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()); + detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_); + size_t n_fts = 0; + std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){ + Point* point(new Point(frame_->f2w(i->f*depthmap_.at(i->px[1], i->px[0])), i)); + i->point = point; + ++n_fts; + }); + printf("Added %zu features to frame.\n", n_fts); + } + + virtual ~PoseOptimizerTest() + { + delete cam_; + } + + // Individual tests + void test(const Vector3d& pose_disturbance, double pixel_sigma2); + + // Objects declared here can be used by all tests + vk::PinholeCamera* cam_; + FramePtr frame_; + cv::Mat depthmap_; +}; + +void PoseOptimizerTest::test(const Vector3d& pose_disturbance, double pixel_sigma2) +{ + printf("Add %f px noise to each observation\n", pixel_sigma2); + for(Features::iterator it=frame_->fts_.begin(); it!=frame_->fts_.end(); ++it) + { + (*it)->px += Vector2d(vk::Sample::gaussian(pixel_sigma2), vk::Sample::gaussian(pixel_sigma2)); + (*it)->f = frame_->c2f((*it)->px); + } + frame_->T_f_w_ = frame_->T_f_w_*SE3(Matrix3d::Identity(), pose_disturbance); + double estimated_scale, error_init, error_final; + size_t num_obs; + pose_optimizer::optimizeGaussNewton( + Config::reprojThresh(), 10, true, frame_, + estimated_scale, error_init, error_final, num_obs); +} + +} // namespace + +int main(int argc, char** argv) +{ + PoseOptimizerTest test; + test.test(Eigen::Vector3d(0.2, 0.2, 0.2), 1.0); + return 0; +} diff --git a/CPU_version/svo/test/test_sparse_img_align.cpp b/CPU_version/svo/test/test_sparse_img_align.cpp new file mode 100644 index 0000000..1312da0 --- /dev/null +++ b/CPU_version/svo/test/test_sparse_img_align.cpp @@ -0,0 +1,163 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +/// SparseImgAlign Test-Fixture +class SparseImgAlignTest { + public: + + SparseImgAlignTest(); + virtual ~SparseImgAlignTest(); + void testSequence( + const std::string& dataset_dir, + const std::string& experiment_name, + svo::feature_detection::AbstractDetector* feature_detector); + + vk::PinholeCamera* cam_; + svo::FramePtr frame_ref_; + svo::FramePtr frame_cur_; +}; + +SparseImgAlignTest::SparseImgAlignTest() : + cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)) +{} + +SparseImgAlignTest::~SparseImgAlignTest() +{} + +void SparseImgAlignTest::testSequence( + const std::string& dataset_dir, + const std::string& experiment_name, + svo::feature_detection::AbstractDetector* feature_detector) +{ + vk::FileReader sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + printf("RUN EXPERIMENT: read %zu dataset entries.\n", sequence.size()); + std::vector::iterator iter = sequence.begin(); + std::list translation_error; + + Sophus::SE3 T_prev_w, T_prevgt_w; + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt"); + std::ofstream ofs(trace_name.c_str()); + for(int i=0; iter != sequence.end() && i<30; ++iter, ++i) + { + // load img + // IMPORTANT: We need to flip the image because the Povray dataset has a + // negative focal length in y direction which we didn't set. + std::string img_name(dataset_dir+"/img/" + iter->image_name_ + "_0.png"); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + // load pose + Sophus::SE3 T_w_gt(iter->q_, iter->t_); + Sophus::SE3 T_gt_w = T_w_gt.inverse(); // ground-truth + + if(i==0) + { + // set reference frame + frame_ref_.reset(new svo::Frame(cam_, img, 0.0)); + frame_ref_->T_f_w_ = T_gt_w; + + // load ground-truth depth + cv::Mat depthmap; + vk::blender_utils::loadBlenderDepthmap( + dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap); + + // extract features, generate features with 3D points + feature_detector->detect( + frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_); + std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) { + Eigen::Vector3d pt_pos_cur = i->f*depthmap.at(i->px[1], i->px[0]); + Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur; + svo::Point* pt = new svo::Point(pt_pos_w, i); + i->point = pt; + }); + + printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs()); + T_prev_w = frame_ref_->T_f_w_; + T_prevgt_w = T_gt_w; + continue; + } + + frame_cur_.reset(new svo::Frame(cam_, img, 0.0)); + //frame_cur_->T_f_w_ = frame_ref_->T_f_w_; // start at reference frame + frame_cur_->T_f_w_ = T_prev_w; // start at last frame + + // run image align + vk::Timer t; + svo::SparseImgAlign img_align(svo::Config::kltMaxLevel(), svo::Config::kltMinLevel(), + 30, svo::SparseImgAlign::GaussNewton, false, false); + img_align.run(frame_ref_, frame_cur_); + + // compute error + Sophus::SE3 T_f_gt = frame_cur_->T_f_w_ * T_gt_w.inverse(); + translation_error.push_back(T_f_gt.translation().norm()); + printf("[%3.i] time = %f ms \t |t| = %f \t translation error = %f\n", + i, t.stop()*1000, (frame_ref_->T_f_w_*T_gt_w.inverse()).translation().norm(), + translation_error.back()); + + // save old pose for next iteration + T_prev_w = frame_cur_->T_f_w_; + T_prevgt_w = T_gt_w; + + ofs << frame_cur_->T_f_w_.inverse().translation().transpose() << " " + << T_gt_w.inverse().translation().transpose() << std::endl; + } + ofs.close(); + + // trace error + trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt"; + ofs.open(trace_name.c_str()); + for(std::list::iterator it=translation_error.begin(); it!=translation_error.end(); ++it) + ofs << *it << std::endl; + ofs.close(); +} + +} // namespace + + +int main(int argc, char** argv) +{ + std::string experiment_name("flying_room_1_rig_1_fast_minlev0"); + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + svo::Config::triangMinCornerScore() = 20; + svo::Config::kltMinLevel() = 0; + SparseImgAlignTest test; + svo::feature_detection::FastDetector detector( + test.cam_->width(), test.cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()); + test.testSequence(dataset_dir, experiment_name, &detector); + return 0; +} diff --git a/CPU_version/svo/test/test_utils.h b/CPU_version/svo/test/test_utils.h new file mode 100644 index 0000000..2759fe0 --- /dev/null +++ b/CPU_version/svo/test/test_utils.h @@ -0,0 +1,57 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef TEST_UTILS_H_ +#define TEST_UTILS_H_ + +#include +#include // for getenv +#ifdef SVO_USE_ROS +# include +# include +#endif + +namespace svo { +namespace test_utils { + +std::string getDatasetDir() +{ + const char* env_dir = std::getenv("SVO_DATASET_DIR"); +#ifdef SVO_USE_ROS + std::string dataset_dir(ros::package::getPath("svo")+"/test/data"); + if(env_dir != NULL) + dataset_dir = std::string(env_dir); + return dataset_dir; +#else + return std::string(env_dir); +#endif +} + +std::string getTraceDir() +{ +#ifdef SVO_USE_ROS + std::string default_dir(ros::package::getPath("svo")+"/test/results"); + return vk::getParam("svo/trace_dir", default_dir); +#else + return "/tmp"; +#endif +} + +} // namespace test_utils +} // namespace svo + + +#endif // TEST_UTILS_H_ diff --git a/CPU_version/svo_analysis/.gitignore b/CPU_version/svo_analysis/.gitignore new file mode 100644 index 0000000..7e99e36 --- /dev/null +++ b/CPU_version/svo_analysis/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/CPU_version/svo_analysis/CMakeLists.txt b/CPU_version/svo_analysis/CMakeLists.txt new file mode 100644 index 0000000..53ea7dc --- /dev/null +++ b/CPU_version/svo_analysis/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8.3) +project(svo_analysis) + +find_package(catkin REQUIRED COMPONENTS rospy) +include_directories(${catkin_INCLUDE_DIRS}) + +catkin_python_setup() +#catkin_package(CATKIN_DEPENDS sensor_msgs) \ No newline at end of file diff --git a/CPU_version/svo_analysis/README.md b/CPU_version/svo_analysis/README.md new file mode 100644 index 0000000..b3201b1 --- /dev/null +++ b/CPU_version/svo_analysis/README.md @@ -0,0 +1,27 @@ +## SVO Analysis + +#### Download Test Data + +Create a folder where you will store all datasets, e.g.: + + mkdir Datasets + +In your `.bashrc` script, create a new environment variable pointing to the dataset folder: + + export SVO_DATASET_DIR=${HOME}/Datasets + +Source your new `.bashrc` script, go to the new dataset folder and download the test data: + + source ~/.bashrc + cd ${SVO_DATASET_DIR} + wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz + +#### Run Benchmark + +In the _CMakeLists.txt_ of SVO you need to set the `TRACE` flag to `TRUE` and recompile SVO. +Afterwards run + + rosrun svo_analysis benchmark.py sin2_tex2_h1_v8_d + +The script will create a new folder in `svo_analysis/results/`, run svo on the dataset and save the tracefile in this folder and at the end it will run some scripts to generate plots from the tracefile. + diff --git a/CPU_version/svo_analysis/__init__.py b/CPU_version/svo_analysis/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/CPU_version/svo_analysis/comparisons/speed.yaml b/CPU_version/svo_analysis/comparisons/speed.yaml new file mode 100644 index 0000000..2f706d4 --- /dev/null +++ b/CPU_version/svo_analysis/comparisons/speed.yaml @@ -0,0 +1,14 @@ +comparison_name: speed +experiment_sets: +- label: 'old' + experiments: + - 20140415_1028_svo_ros_synthetic_speed_1404150920_plane_straight_speed4 + - 20140415_1028_svo_ros_synthetic_speed_1404150928_plane_straight_speed5 + - 20140415_1028_svo_ros_synthetic_speed_1404150935_plane_straight_speed6 + - 20140415_1028_svo_ros_synthetic_speed_1404150940_plane_straight_speed7 +- label: 'new' + experiments: + - 20140513_0949_svo_ros_synthetic_speed_1404150920_plane_straight_speed4 + - 20140513_0949_svo_ros_synthetic_speed_1404150928_plane_straight_speed5 + - 20140513_0949_svo_ros_synthetic_speed_1404150935_plane_straight_speed6 + - 20140513_0949_svo_ros_synthetic_speed_1404150940_plane_straight_speed7 \ No newline at end of file diff --git a/CPU_version/svo_analysis/comparisons/textures.yaml b/CPU_version/svo_analysis/comparisons/textures.yaml new file mode 100644 index 0000000..7279f6e --- /dev/null +++ b/CPU_version/svo_analysis/comparisons/textures.yaml @@ -0,0 +1,22 @@ +comparison_name: textures +experiment_sets: +- label: 'T1' + experiments: + - 20140516_1449_svo_ros_synthetic_all_str_tex1_h1_v6_d + - 20140516_1449_svo_ros_synthetic_all_str_tex1_h1_v9_d + - 20140516_1449_svo_ros_synthetic_all_str_tex1_h1_v12_d +- label: 'T2' + experiments: + - 20140516_1449_svo_ros_synthetic_all_str_tex2_h1_v6_d + - 20140516_1449_svo_ros_synthetic_all_str_tex2_h1_v9_d + - 20140516_1449_svo_ros_synthetic_all_str_tex2_h1_v12_d +- label: 'T3' + experiments: + - 20140516_1449_svo_ros_synthetic_all_str_tex3_h1_v6_d + - 20140516_1449_svo_ros_synthetic_all_str_tex3_h1_v9_d + - 20140516_1449_svo_ros_synthetic_all_str_tex3_h1_v12_d +# - label: 'T4' +# experiments: +# - 20140516_1449_svo_ros_synthetic_all_str_tex4_h1_v6_d +# - 20140516_1449_svo_ros_synthetic_all_str_tex4_h1_v9_d +# - 20140516_1449_svo_ros_synthetic_all_str_tex4_h1_v12_d \ No newline at end of file diff --git a/CPU_version/svo_analysis/experiments/.gitignore b/CPU_version/svo_analysis/experiments/.gitignore new file mode 100644 index 0000000..5b74354 --- /dev/null +++ b/CPU_version/svo_analysis/experiments/.gitignore @@ -0,0 +1,2 @@ +*.yaml +!.gitignore diff --git a/CPU_version/svo_analysis/package.xml b/CPU_version/svo_analysis/package.xml new file mode 100644 index 0000000..dcc5a62 --- /dev/null +++ b/CPU_version/svo_analysis/package.xml @@ -0,0 +1,19 @@ + + + svo_analysis + 0.0.1 + + Python scripts to analyse performance of SVO. + + Christian Forster + Christian Forster + BSD + catkin + + rospy + + python-rospkg + python-yaml + rospy + + \ No newline at end of file diff --git a/CPU_version/svo_analysis/results/.gitignore b/CPU_version/svo_analysis/results/.gitignore new file mode 100644 index 0000000..c1aefd2 --- /dev/null +++ b/CPU_version/svo_analysis/results/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore diff --git a/CPU_version/svo_analysis/scripts/__init__.py b/CPU_version/svo_analysis/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/CPU_version/svo_analysis/scripts/benchmark.py b/CPU_version/svo_analysis/scripts/benchmark.py new file mode 100755 index 0000000..a3733ea --- /dev/null +++ b/CPU_version/svo_analysis/scripts/benchmark.py @@ -0,0 +1,111 @@ +#!/usr/bin/python +""" +Created on Sat Aug 10 18:21:47 2013 + +@author: Christian Forster +""" + +import os +import yaml +import rospkg +import argparse +import time +import vikit_py.cpu_info as cpu_info +import vikit_py.ros_node as ros_node +import evaluate +import shutil + +def run_experiment(dataset, params): + # load dataset parameters + params['dataset_directory'] = os.path.join(os.environ['SVO_DATASET_DIR'], dataset) + if not os.path.exists(params['dataset_directory']): + raise Exception("Provided dataset folder does not exist.") + dataset_params_file = os.path.join(params['dataset_directory'], 'dataset_params.yaml') + dataset_params = yaml.load(open(dataset_params_file,'r')) + + # load algorithm parameters + algo_params_file = os.path.join(rospkg.RosPack().get_path('svo_ros'), + 'param', params['param_settings']+'.yaml') + algo_params = yaml.load(open(algo_params_file,'r')) + + # combine all parameters + if 'rig_size' not in dataset_params or dataset_params['rig_size'] == 1: + params = dict(params.items() + algo_params.items() + dataset_params['cam0'].items()) + else: + params = dict(params.items() + algo_params.items() + dataset_params.items()) + if 'dataset_is_blender' in dataset_params: + params['dataset_is_blender'] = dataset_params['dataset_is_blender'] + else: + params['dataset_is_blender'] = False + if 'dataset_first_frame' in dataset_params: + params['dataset_first_frame'] = dataset_params['dataset_first_frame'] + else: + params['dataset_first_frame'] = 0 + + # dump experiment params to file and copy the other parameter files: + params_dump_file = os.path.join(params['trace_dir'],'params.yaml') + with open(params_dump_file,'w') as outfile: + outfile.write(yaml.dump(params, default_flow_style=False)) + shutil.copyfile(dataset_params_file, + os.path.join(params['trace_dir'], 'dataset_params.yaml')) + + # copy the groundtruth trajectory to the trace dir for later evaluation + if params['dataset_is_blender']: + shutil.copyfile(os.path.join(params['dataset_directory'], 'trajectory_nominal.txt'), + os.path.join(params['trace_dir'], 'groundtruth_matched.txt')) + else: + shutil.copyfile(os.path.join(params['dataset_directory'], 'groundtruth_matched.txt'), + os.path.join(params['trace_dir'], 'groundtruth_matched.txt')) + + # execute ros node + node = ros_node.RosNode(args.version, args.executable) + node.run(params) + +if __name__=="__main__": + + # parse command line + parser = argparse.ArgumentParser(description=''' + Runs SVO with the dataset and parameters specified in the provided experiment file. + ''') + parser.add_argument('experiment_file', help='experiment file in svo_analysis/experiments folder') + parser.add_argument('--evaluate', help='evaluate tracefile after running SVO', action='store_true') + parser.add_argument('--version', help='version of svo to evaluate', default='svo_ros') + parser.add_argument('--executable', help='the executable to be called', default='benchmark') + args = parser.parse_args() + + # load experiment parameters + args.experiment_file = args.experiment_file.replace('.yaml','') + experiment_params_file = os.path.join(rospkg.RosPack().get_path('svo_analysis'), + 'experiments', args.experiment_file+'.yaml') + if os.path.exists(experiment_params_file): + experiment_params = yaml.load(open(experiment_params_file, 'r')) + else: + print('experiment file does not exist. run with default setttings') + experiment_params = dict({'experiment_label':'default','param_settings':'vo_fast', + 'datasets':list([args.experiment_file])}) + params = dict() + params['experiment_label'] = experiment_params['experiment_label'] + params['param_settings'] = experiment_params['param_settings'] + params['time'] = time.strftime("%Y%m%d_%H%M", time.localtime()) + params['platform'] = cpu_info.get_cpu_info() + params['trace_name'] = 'trace' + + trace_dirs = list() + for dataset in experiment_params['datasets']: + params['experiment_name'] = params['time']+'_'+args.version+'_'+args.experiment_file+'_'+dataset + params['trace_dir'] = os.path.join(rospkg.RosPack().get_path('svo_analysis'), + 'results', params['experiment_name']) + if not os.path.exists(params['trace_dir']): + os.makedirs(params['trace_dir']) + trace_dirs.append(params['trace_dir']) + run_experiment(dataset, params) + + # TODO: check if it is a synthetic dataset + for trace in trace_dirs: + evaluate.evaluate_dataset(trace) + + + + + + diff --git a/CPU_version/svo_analysis/scripts/compare_results.py b/CPU_version/svo_analysis/scripts/compare_results.py new file mode 100755 index 0000000..5605fe3 --- /dev/null +++ b/CPU_version/svo_analysis/scripts/compare_results.py @@ -0,0 +1,150 @@ +#!/usr/bin/python + +import os +import sys +import time +import rospkg +import numpy as np +import matplotlib.pyplot as plt +import yaml +import argparse +from matplotlib import rc + +# tell matplotlib to use latex font +rc('font',**{'family':'serif','serif':['Cardo']}) +rc('text', usetex=True) + +from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes +from mpl_toolkits.axes_grid1.inset_locator import mark_inset + +def plot_trajectory(ax, filename, label, color, linewidth): + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + trajectory = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) + ax.plot(trajectory[:,1], trajectory[:,2], label=label, color=color, linewidth=linewidth) + +def compare_results(experiments, results_dir, comparison_dir, + plot_scale_drift = False): + + # ------------------------------------------------------------------------------ + # position error + fig_poserr = plt.figure(figsize=(8,6)) + ax_poserr_x = fig_poserr.add_subplot(311, ylabel='x-error [m]') + ax_poserr_y = fig_poserr.add_subplot(312, ylabel='y-error [m]') + ax_poserr_z = fig_poserr.add_subplot(313, ylabel='z-error [m]', xlabel='time [s]') + + for exp in experiments: + + # load dataset parameters + params_stream = open(os.path.join(results_dir, exp, 'params.yaml')) + params = yaml.load(params_stream) + + # plot translation error + trans_error = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt')) + trans_error[:,0] = trans_error[:,0]-trans_error[0,0] + ax_poserr_x.plot(trans_error[:,0], trans_error[:,1], label=params['experiment_label']) + ax_poserr_y.plot(trans_error[:,0], trans_error[:,2]) + ax_poserr_z.plot(trans_error[:,0], trans_error[:,3]) + + ax_poserr_x.set_xlim([0, trans_error[-1,0]+4]) + ax_poserr_y.set_xlim([0, trans_error[-1,0]+4]) + ax_poserr_z.set_xlim([0, trans_error[-1,0]+4]) + ax_poserr_x.legend(bbox_to_anchor=[0, 0], loc='lower left', ncol=3) + ax_poserr_x.grid() + ax_poserr_y.grid() + ax_poserr_z.grid() + fig_poserr.tight_layout() + fig_poserr.savefig(os.path.join(comparison_dir, 'translation_error.pdf')) + + # ------------------------------------------------------------------------------ + # orientation error + fig_roterr = plt.figure(figsize=(8,6)) + ax_roterr_r = fig_roterr.add_subplot(311, ylabel='roll-error [rad]') + ax_roterr_p = fig_roterr.add_subplot(312, ylabel='pitch-error [rad]') + ax_roterr_y = fig_roterr.add_subplot(313, ylabel='yaw-error [rad]', xlabel='time [s]') + + for exp in experiments: + + # load dataset parameters + params_stream = open(os.path.join(results_dir, exp, 'params.yaml')) + params = yaml.load(params_stream) + + # plot translation error + rot_error = np.loadtxt(os.path.join(results_dir, exp, 'orientation_error.txt')) + rot_error[:,0] = rot_error[:,0]-rot_error[0,0] + ax_roterr_r.plot(rot_error[:,0], rot_error[:,3], label=params['experiment_label']) + ax_roterr_p.plot(rot_error[:,0], rot_error[:,2]) + ax_roterr_y.plot(rot_error[:,0], rot_error[:,1]) + + ax_roterr_r.set_xlim([0, rot_error[-1,0]+4]) + ax_roterr_p.set_xlim([0, rot_error[-1,0]+4]) + ax_roterr_y.set_xlim([0, rot_error[-1,0]+4]) + ax_roterr_r.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3) + ax_roterr_r.grid() + ax_roterr_p.grid() + ax_roterr_y.grid() + fig_roterr.tight_layout() + fig_roterr.savefig(os.path.join(comparison_dir, 'orientation_error.pdf')) + + # ------------------------------------------------------------------------------ + # scale error + if plot_scale_drift: + fig_scale = plt.figure(figsize=(8,2.5)) + ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]') + + for exp in experiments: + + # load dataset parameters + params = yaml.load(open(os.path.join(results_dir, exp, 'params.yaml'))) + + # plot translation error + scale_drift = open(os.path.join(results_dir, exp, 'scale_drift.txt')) + scale_drift[:,0] = scale_drift[:,0]-scale_drift[0,0] + ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label']) + + ax_scale.set_xlim([0, rot_error[-1,0]+4]) + ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3) + ax_scale.grid() + fig_scale.tight_layout() + fig_scale.savefig(os.path.join(comparison_dir, 'scale_drift.pdf')) + + # ------------------------------------------------------------------------------ + # trajectory + +# fig_traj = plt.figure(figsize=(8,4.8)) +# ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6]) +# +# plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5) +# plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1) +# plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1) +# +# mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b') +# plt.draw() +# plt.show() +# ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3) +# ax_traj.grid() +# fig_traj.tight_layout() +# fig_traj.savefig('../results/trajectory_asl.pdf') + + +if __name__ == '__main__': + + default_name = time.strftime("%Y%m%d_%H%M", time.localtime())+'_comparison' + parser = argparse.ArgumentParser(description='Compare results.') + parser.add_argument('result_directories', nargs='+', help='list of result directories to compare') + parser.add_argument('--name', help='name of the comparison', default=default_name) + args = parser.parse_args() + + # create folder for comparison results + results_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 'results') + comparison_dir = os.path.join(results_dir, args.name) + if not os.path.exists(comparison_dir): + os.makedirs(comparison_dir) + + # run comparison + compare_results(args.result_directories, results_dir, comparison_dir) + + + + diff --git a/CPU_version/svo_analysis/scripts/comparison.py b/CPU_version/svo_analysis/scripts/comparison.py new file mode 100755 index 0000000..a502ad1 --- /dev/null +++ b/CPU_version/svo_analysis/scripts/comparison.py @@ -0,0 +1,159 @@ +#!/usr/bin/python + +import os +import sys +import time +import rospkg +import numpy as np +import matplotlib.pyplot as plt +import yaml +import argparse +import svo_analysis.analyse_depth as analyse_depth +from matplotlib import rc + +# tell matplotlib to use latex font +rc('font',**{'family':'serif','serif':['Cardo']}) +rc('text', usetex=True) + +from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes +from mpl_toolkits.axes_grid1.inset_locator import mark_inset + +def plot_trajectory(ax, filename, label, color, linewidth): + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + trajectory = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) + ax.plot(trajectory[:,1], trajectory[:,2], label=label, color=color, linewidth=linewidth) + +def save_figure(fig, name, directory): + fig.tight_layout() + fig.savefig(os.path.join(directory, name+'.pdf'), bbox_inches="tight") + +def distances_along_trajectory(traj): + keys = traj.keys() + keys.sort() + motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)] + distances = [0] + sum = 0 + for t in motion: + sum += compute_distance(t) + distances.append(sum) + return distances + +def get_distance_from_start(gt): + distances = np.diff(gt[:,1:4],axis=0) + distances = np.sqrt(np.sum(np.multiply(distances,distances),1)) + distances = np.cumsum(distances) + distances = np.concatenate(([0], distances)) + return distances + +def compare_results(comp_params, results_dir, comparison_dir): + print('run comparison: '+comp_params['comparison_name']) + + line_styles = ['-','--',':'] + line_colors = ['b','g','r','m','c'] + + # ------------------------------------------------------------------------- + # plot trajectory: + fig = plt.figure(figsize=(6,2)) + ax = fig.add_subplot(111, xlabel='x [m]', ylabel='y [m]') + for exp_set in comp_params['experiment_sets']: + print('processing experiment set: ' + exp_set['label']) + + for i, exp in enumerate(exp_set['experiments']): + data = np.loadtxt(os.path.join(results_dir, exp, 'traj_estimate.txt')) + if i == 0: + base_plot, = ax.plot(data[:,1], data[:,2], label=exp_set['label'], linestyle=line_styles[np.mod(i, len(line_styles))]) + else: + ax.plot(data[:,1], data[:,2], color=base_plot.get_color(), linestyle= line_styles[np.mod(i, len(line_styles))]) + ax.legend(loc='upper left', ncol=3) + save_figure(fig, 'trajectory', comparison_dir) + + # ------------------------------------------------------------------------- + # plot translation error: + fig = plt.figure(figsize=(6,2)) + ax = fig.add_subplot(111, xlabel='distance [m]', ylabel='translation drift [mm]') + for exp_set in comp_params['experiment_sets']: + print('processing experiment set: ' + exp_set['label']) + + for i, exp in enumerate(exp_set['experiments']): + gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) + distances = get_distance_from_start(gt) + data = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt')) + e = np.sqrt(np.sum(np.multiply(data[:,1:4],data[:,1:4]),1)) + + if np.shape(e)[0] > np.shape(distances)[0]: + print('WARNING: estimate has more measurement than groundtruth: ' + +str(np.shape(e)[0]-np.shape(distances)[0])) + e = e[0:np.shape(distances)[0]] + + distances = distances[0:np.shape(e)[0]] + + print '--' + print np.shape(e) + print np.shape(distances) + + if i == 0: + base_plot, = ax.plot(distances, e*1000, label=exp_set['label'], linestyle= line_styles[np.mod(i, len(line_styles))]) + else: + ax.plot(distances, e*1000, color=base_plot.get_color(), linestyle= line_styles[np.mod(i, len(line_styles))]) + + ax.legend(loc='upper left', ncol=3) + save_figure(fig, 'translation_error', comparison_dir) + + # ------------------------------------------------------------------------- + # plot depth estimation error: + fig = plt.figure(figsize=(6,5)) + ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]') + for k, exp_set in enumerate(comp_params['experiment_sets']): + print('plot depth error for experiment: ' + exp) + exp = exp_set['experiments'][0] + gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) + x_axis_data = get_distance_from_start(gt) + analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax, + x_axis_data[1:], line_colors[k], exp_set['label']) + ax.legend(loc='upper left', ncol=3) + save_figure(fig, 'depth_error_textures', comparison_dir) + + fig = plt.figure(figsize=(6,5)) + ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]') + exp_set = comp_params['experiment_sets'][0] + for i, exp in enumerate(exp_set['experiments']): + print('plot depth error for speed: ' + exp) + gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) + x_axis_data = get_distance_from_start(gt) + params = yaml.load(open(os.path.join(results_dir, exp, 'dataset_params.yaml'))) + analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax, + x_axis_data[1:], line_colors[i], + str(params['trajectory_modifiers']['speed'])+' m/s') + ax.legend(loc='upper left', ncol=3) + save_figure(fig, 'depth_error_speed', comparison_dir) + +if __name__ == '__main__': + + parser = argparse.ArgumentParser(description='Compare results of a VO pipeline.') + parser.add_argument('comparison_file', help='A YAML file that contains the details of the comparison') + args = parser.parse_args() + + # load comparison file + args.experiment_file = args.comparison_file.replace('.yaml','') + comparison_params_file = os.path.join(rospkg.RosPack().get_path('svo_analysis'), + 'comparisons', args.comparison_file+'.yaml') + if os.path.exists(comparison_params_file): + comp_params = yaml.load(open(comparison_params_file, 'r')) + else: + raise Exception("Provided comparison file does not exist.") + + # create folder for comparison results + comparison_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), + 'comparisons', comp_params['comparison_name']) + if not os.path.exists(comparison_dir): + os.makedirs(comparison_dir) + + # folder where the results of previous experiments are saved + results_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 'results') + + # run comparison + compare_results(comp_params, results_dir, comparison_dir) + + diff --git a/CPU_version/svo_analysis/scripts/evaluate.py b/CPU_version/svo_analysis/scripts/evaluate.py new file mode 100755 index 0000000..726544c --- /dev/null +++ b/CPU_version/svo_analysis/scripts/evaluate.py @@ -0,0 +1,58 @@ +#!/usr/bin/python +""" +Created on Sat Aug 10 18:21:47 2013 + +@author: Christian Forster +""" + +import os +import csv +import numpy as np +import rospkg +import argparse +import yaml +import svo_analysis.analyse_logs as analyse_logs +import svo_analysis.analyse_timing as analyse_timing +import svo_analysis.analyse_trajectory as analyse_trajectory +import svo_analysis.analyse_depth as analyse_depth + +def evaluate_dataset(trace_dir): + + # read tracefile + data = csv.reader(open(os.path.join(trace_dir, 'trace.csv'))) + fields = data.next() + D = dict() + for field in fields: + D[field] = list() + + # fill dictionary with column values + for row in data: + for (field, value) in zip(fields, row): + D[field].append(float(value)) + + # change dictionary values from list to numpy array for easier manipulation + for field, value in D.items(): + D[field] = np.array(D[field]) + + # generate plots + analyse_logs.analyse_logs(D, trace_dir) + analyse_timing.analyse_timing(D, trace_dir) + analyse_trajectory.analyse_trajectory(trace_dir) + + param = yaml.load(open(os.path.join(trace_dir, 'dataset_params.yaml'), 'r')) + if param['dataset_is_blender']: + analyse_depth.analyse_depth(trace_dir) + analyse_depth.analyse_depth_over_time(trace_dir) + + +if __name__=="__main__": + parser = argparse.ArgumentParser(description=''' + Evaluates tracefiles of SVO and generates the plots. + ''') + parser.add_argument('experiment_name', help='directory name of the tracefiles') + args = parser.parse_args() + + trace_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), + 'results', + args.experiment_name) + evaluate_dataset(trace_dir) \ No newline at end of file diff --git a/CPU_version/svo_analysis/setup.py b/CPU_version/svo_analysis/setup.py new file mode 100644 index 0000000..0d94673 --- /dev/null +++ b/CPU_version/svo_analysis/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['svo_analysis'], + package_dir={'': 'src'}, + install_requires=['rospkg', 'yaml'], + ) + +setup(**d) \ No newline at end of file diff --git a/CPU_version/svo_analysis/src/__init__.py b/CPU_version/svo_analysis/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/CPU_version/svo_analysis/src/svo_analysis/__init__.py b/CPU_version/svo_analysis/src/svo_analysis/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/CPU_version/svo_analysis/src/svo_analysis/analyse_dataset.py b/CPU_version/svo_analysis/src/svo_analysis/analyse_dataset.py new file mode 100644 index 0000000..53a2501 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/analyse_dataset.py @@ -0,0 +1,41 @@ +# -*- coding: utf-8 -*- + +import associate +import numpy as np +import matplotlib.pyplot as plt +import yaml + +def loadDataset(filename): + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) + return D + +dataset_dir = '/home/cforster/Datasets/SlamBenchmark/px4_r2' +trajectory_data = dataset_dir+'/groundtruth.txt' +stepsize = 10 + +# load dataset +data = loadDataset(trajectory_data) +n = data.shape[0] +steps = np.arange(0,n,stepsize) + +# compute trajectory length +last_pos = data[0,1:4] +trajectory_length = 0 + +for i in steps[1:]: + new_pos = data[i,1:4] + trajectory_length += np.linalg.norm(new_pos-last_pos) + last_pos = new_pos + + +print 'trajectory lenght = ' + str(trajectory_length) + 'm' +print 'height mean = ' + str(np.mean(data[:,3])) + 'm' +print 'height median = ' + str(np.median(data[:,3])) + 'm' +print 'height std = ' + str(np.std(data[:,3])) + 'm' +print 'duration = ' + str(data[-1,0]-data[0,0]) + 's' +print 'speed = ' + str(trajectory_length/(data[-1,0]-data[0,0])) + 'm/s' + + diff --git a/CPU_version/svo_analysis/src/svo_analysis/analyse_depth.py b/CPU_version/svo_analysis/src/svo_analysis/analyse_depth.py new file mode 100755 index 0000000..8a21b53 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/analyse_depth.py @@ -0,0 +1,66 @@ +#!/usr/bin/python + +import os +import argparse +import numpy as np +import matplotlib.pyplot as plt +from matplotlib import rc + +rc('font',**{'family':'serif','serif':['Cardo']}) +rc('text', usetex=True) + +def precision_plot(ax, errors, max_error, color, linestyle): + number_of_error_ranges = 500 + error_step = max_error / number_of_error_ranges + precision_array = np.zeros(number_of_error_ranges) + error_array = np.zeros(number_of_error_ranges) + n_measurements = len(errors) + for error_ind in range(0, number_of_error_ranges): + precision_array[error_ind] = np.sum(errors < error_step*error_ind) / float(n_measurements) * 100.0 + error_array[error_ind] = error_step*error_ind + plot_obj, = ax.plot(error_array, precision_array, label='error', linestyle=linestyle, color=color) + return plot_obj + +def analyse_depth(results_dir): + D = np.loadtxt(os.path.join(results_dir, 'depth_error.txt'), delimiter=' ') + D = D[D[:,0] > 1,:] + + fig = plt.figure(figsize=(6,5)) + ax = fig.add_subplot(111, xlabel='depth error [m]', ylabel='Precision [\%]') + errors = np.abs(D[:,1]) + precision_plot(ax, errors, 0.05, 'r', '-') + fig.tight_layout() + fig.savefig(os.path.join(results_dir,'depth_error.pdf'), bbox_inches="tight") + + +def plot_depth_over_time(results_dir, ax, x_axis_data, color, label=''): + D = np.loadtxt(os.path.join(results_dir, 'depth_error.txt'), delimiter=' ') + D = D[D[:,0] > 1,:] + idxs = np.unique(D[:,0]) + percentile_10 = np.zeros(len(idxs)) + percentile_50 = np.zeros(len(idxs)) + percentile_90 = np.zeros(len(idxs)) + for i in range(len(idxs)): + errors = np.abs(D[D[:,0]==idxs[i],1]) + percentile_10[i] = np.percentile(errors, 10) + percentile_50[i] = np.percentile(errors, 50) + percentile_90[i] = np.percentile(errors, 90) + if len(x_axis_data) == 0: + x_axis_data = idxs + print np.shape(x_axis_data) + print np.shape(idxs) + print '--' + ax.plot(x_axis_data, percentile_50, linewidth=2, color=color, label=label) + ax.plot(x_axis_data, percentile_10, linewidth=0.5, color=color, alpha=0.5) + ax.plot(x_axis_data, percentile_90, linewidth=0.5, color=color, alpha=0.5) + ax.fill_between(x_axis_data, percentile_10, percentile_90, color=color, alpha=0.1) + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Analyse depth estimate') + parser.add_argument('results_dir', help='folder with the results') + args = parser.parse_args() + analyse_depth(args.results_dir) + + fig = plt.figure(figsize=(6,5)) + ax = fig.add_subplot(111, xlabel='Measurement', ylabel='Scale-Drift') + plot_depth_over_time(args.results_dir, [], ax) \ No newline at end of file diff --git a/CPU_version/svo_analysis/src/svo_analysis/analyse_logs.py b/CPU_version/svo_analysis/src/svo_analysis/analyse_logs.py new file mode 100755 index 0000000..7dc2acc --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/analyse_logs.py @@ -0,0 +1,74 @@ +#!/usr/bin/python + +import os +import yaml +import numpy as np +import matplotlib.pyplot as plt + +def analyse_logs(D, trace_dir): + + # identify measurements which result from normal frames and which from keyframes + is_kf = np.argwhere( (D['dropout'] == 1) & (D['repr_n_mps'] >= 0)) + is_frame = np.argwhere(D['repr_n_mps'] >= 0) + is_nokf = np.argwhere( (D['dropout'] == 0) & (D['repr_n_mps'] >= 0)) + + # set initial time to zero + D['timestamp'] = D['timestamp'] - D['timestamp'][0] + + # ---------------------------------------------------------------------------- + # plot number of reprojected points + mean_n_reproj_points = np.mean(D['repr_n_mps'][is_frame]); + mean_n_reproj_matches = np.mean(D['repr_n_new_references'][is_frame]); + mean_n_edges_final = np.mean(D['sfba_n_edges_final'][is_frame]); + + fig = plt.figure(figsize=(8,3)) + ax = fig.add_subplot(111, xlabel='time [s]') + ax.plot(D['timestamp'][is_frame], D['repr_n_mps'][is_frame], 'r-', + label='Reprojected Points, avg = %.2f'%mean_n_reproj_points) + ax.plot(D['timestamp'][is_frame], D['repr_n_new_references'][is_frame], 'b-', + label='Feature Matches, avg = %.2f'%mean_n_reproj_matches) + ax.plot(D['timestamp'][is_frame], D['sfba_n_edges_final'][is_frame], 'g-', + label='Points after Optimization, avg = %.2f'%mean_n_edges_final) + ax.set_ylim(bottom=0) + ax.legend(loc='lower right') + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'num_reprojected.pdf'), bbox_inches="tight") + + # ---------------------------------------------------------------------------- + # plot median error before and after pose-optimzation and bundle adjustment + init_error_avg = np.mean(D['sfba_error_init'][is_frame]) + opt1_avg = np.mean(D['sfba_error_final'][is_frame]) + + fig = plt.figure(figsize=(8,2)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='error [px]') + ax.plot(D['timestamp'][is_frame], D['sfba_error_init'][is_frame], 'r-', label='Initial error') + ax.plot(D['timestamp'][is_frame], D['sfba_error_final'][is_frame], 'b-', label='Final error') + ax.legend(ncol=2) + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'reprojection_error.pdf'), bbox_inches="tight") + print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg) + + # ---------------------------------------------------------------------------- + # plot number of candidate points + fig = plt.figure(figsize=(8,3)) + ax = fig.add_subplot(111, xlabel='time [s]') + ax.plot(D['timestamp'][is_frame], D['n_candidates'][is_frame], 'r-', label='Candidate Points') + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'candidate_points.pdf'), bbox_inches="tight") + + # ---------------------------------------------------------------------------- + # plot number of candidate points + fig = plt.figure(figsize=(8,2)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='px') + ax.plot(D['timestamp'][is_frame], D['sfba_thresh'][is_frame], 'r-', label='Threshold') + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'optimization_thresh.pdf'), bbox_inches="tight") + + # ---------------------------------------------------------------------------- + # write other statistics to file + stat = {'num_frames': len(is_frame), + 'num_kfs': len(is_kf), + 'reproj_error_avg_improvement': float(init_error_avg - opt1_avg)} + with open(os.path.join(trace_dir,'dataset_stats.yaml'),'w') as outfile: + outfile.write(yaml.dump(stat, default_flow_style=False)) + diff --git a/CPU_version/svo_analysis/src/svo_analysis/analyse_timing.py b/CPU_version/svo_analysis/src/svo_analysis/analyse_timing.py new file mode 100755 index 0000000..0dfc1bc --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/analyse_timing.py @@ -0,0 +1,68 @@ +#!/usr/bin/python + +import os +import numpy as np +import matplotlib.pyplot as plt + +def analyse_timing(D, trace_dir): + + # identify measurements which result from normal frames and which from keyframes + is_frame = np.argwhere(D['repr_n_mps'] >= 0) + n_frames = len(is_frame) + + # set initial time to zero + D['timestamp'] = D['timestamp'] - D['timestamp'][0] + + # ---------------------------------------------------------------------------- + # plot total time for frame processing + avg_time = np.mean(D['tot_time'][is_frame])*1000; + fig = plt.figure(figsize=(8, 3)) + ax = fig.add_subplot(111, ylabel='processing time [ms]', xlabel='time [s]') + ax.plot(D['timestamp'][is_frame], D['tot_time'][is_frame]*1000, 'g-', label='total time [ms]') + ax.plot(D['timestamp'][is_frame], np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time})) + ax.legend() + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'timing.pdf'), bbox_inches="tight") + + # ---------------------------------------------------------------------------- + # plot boxplot + fig = plt.figure(figsize=(6,2)) + ax = fig.add_subplot(111, xlabel='Processing time [ms]') + ax.boxplot([ + D['tot_time'][is_frame]*1000, +# D['t_local_ba'][is_kf]*1000, + D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000, + D['reproject'][is_frame]*1000, + D['sparse_img_align'][is_frame]*1000, + D['pyramid_creation'][is_frame]*1000 + ], 0,'', vert=0) + + boxplot_labels = [ + r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['tot_time'][is_frame]*1000), +# 'Local BA (KF only): %.2fms ' % np.median(D['local_ba'][is_kf]*1000), + 'Refinement: %2.2fms' % np.median(D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000), + 'Feature Alignment: %2.2fms' % np.median(D['reproject'][is_frame]*1000), + 'Sparse Image Alignment: %2.2fms' % np.median(D['sparse_img_align'][is_frame]*1000), + 'Pyramid Creation: %2.2fms' % np.median(D['pyramid_creation'][is_frame]*1000) ] + + ax.set_yticks(np.arange(len(boxplot_labels))+1) + ax.set_yticklabels(boxplot_labels) + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'timing_boxplot.pdf'), bbox_inches="tight") + + # ---------------------------------------------------------------------------- + # plot boxplot reprojection + fig = plt.figure(figsize=(6,2)) + ax = fig.add_subplot(111, xlabel='Processing time [ms]') + ax.boxplot([ D['reproject'][is_frame]*1000, + D['feature_align'][is_frame]*1000, + D['reproject_candidates'][is_frame]*1000, + D['reproject_kfs'][is_frame]*1000 ], 0, '', vert=0) + boxplot_labels = [r'\textbf{Total Reprojection: %2.2fms}' % np.median(D['reproject'][is_frame]*1000), + 'Feature Alignment: %2.2fms' % np.median(D['feature_align'][is_frame]*1000), + 'Reproject Candidates: %2.2fms' % np.median(D['reproject_candidates'][is_frame]*1000), + 'Reproject Keyframes: %2.2fms' % np.median(D['reproject_kfs'][is_frame]*1000) ] + ax.set_yticks(np.arange(len(boxplot_labels))+1) + ax.set_yticklabels(boxplot_labels) + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'timing_reprojection.pdf'), bbox_inches="tight") \ No newline at end of file diff --git a/CPU_version/svo_analysis/src/svo_analysis/analyse_trajectory.py b/CPU_version/svo_analysis/src/svo_analysis/analyse_trajectory.py new file mode 100755 index 0000000..c2c93de --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/analyse_trajectory.py @@ -0,0 +1,185 @@ +#!/usr/bin/python + +import os +import yaml +import argparse +import numpy as np +import matplotlib.pyplot as plt +import svo_analysis.tum_benchmark_tools.associate as associate +import vikit_py.transformations as transformations +import vikit_py.align_trajectory as align_trajectory +from matplotlib import rc +rc('font',**{'family':'serif','serif':['Cardo']}) +rc('text', usetex=True) + +def plot_translation_error(timestamps, translation_error, results_dir): + fig = plt.figure(figsize=(8, 2.5)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4]) + ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x') + ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y') + ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z') + ax.legend() + fig.tight_layout() + fig.savefig(results_dir+'/translation_error.pdf') + +def plot_rotation_error(timestamps, rotation_error, results_dir): + fig = plt.figure(figsize=(8, 2.5)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4]) + ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw') + ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch') + ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll') + ax.legend() + fig.tight_layout() + fig.savefig(results_dir+'/orientation_error.pdf') + +def analyse_synthetic_trajectory(results_dir): + data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt')) + timestamps = data[:,0] + translation_error = data[:,1:4] + plot_translation_error(timestamps, translation_error, results_dir) + + # plot orientation error + data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt')) + timestamps = data[:,0] + orientation_error = data[:,1:4] + plot_rotation_error(timestamps, orientation_error, results_dir) + +def analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames = 200): + print('loading hand-eye-calib') + T_cm_quat = np.array([params['hand_eye_calib']['Tcm_qx'], + params['hand_eye_calib']['Tcm_qy'], + params['hand_eye_calib']['Tcm_qz'], + params['hand_eye_calib']['Tcm_qw']]) + T_cm_tran = np.array([params['hand_eye_calib']['Tcm_tx'], + params['hand_eye_calib']['Tcm_ty'], + params['hand_eye_calib']['Tcm_tz']]) + T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran) + T_mc = transformations.inverse_matrix(T_cm) + + t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay']) + + # align Sim3 to get scale + print('align Sim3 using '+str(n_align_frames)+' first frames.') + scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:]) + print 'scale = '+str(scale) + + # get trafo between (v)ision and (o)ptitrack frame + print q_gt[0,:] + print p_gt[0,:] + T_om = get_rigid_body_trafo(q_gt[0,:], p_gt[0,:]) + T_vc = get_rigid_body_trafo(q_es[0,:], scale*p_es[0,:]) + T_cv = transformations.inverse_matrix(T_vc) + T_ov = np.dot(T_om, np.dot(T_mc, T_cv)) + print 'T_ov = ' + str(T_ov) + + # apply transformation to estimated trajectory + q_es_aligned = np.zeros(np.shape(q_es)) + rpy_es_aligned = np.zeros(np.shape(p_es)) + rpy_gt = np.zeros(np.shape(p_es)) + p_es_aligned = np.zeros(np.shape(p_es)) + for i in range(np.shape(p_es)[0]): + T_vc = get_rigid_body_trafo(q_es[i,:],p_es[i,:]) + T_vc[0:3,3] *= scale + T_om = np.dot(T_ov, np.dot(T_vc, T_cm)) + p_es_aligned[i,:] = T_om[0:3,3] + q_es_aligned[i,:] = transformations.quaternion_from_matrix(T_om) + rpy_es_aligned[i,:] = transformations.euler_from_quaternion(q_es_aligned[i,:], 'rzyx') + rpy_gt[i,:] = transformations.euler_from_quaternion(q_gt[i,:], 'rzyx') + + # plot position error (drift) + translation_error = (p_gt-p_es_aligned) + plot_translation_error(t_es, translation_error, results_dir) + + # plot orientation error (drift) + orientation_error = (rpy_gt - rpy_es_aligned) + plot_rotation_error(t_es, orientation_error, results_dir) + + # plot scale drift + motion_gt = np.diff(p_gt, 0) + motion_es = np.diff(p_es_aligned, 0) + dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1)) + dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1)) + fig = plt.figure(figsize=(8,2.5)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]', xlim=[0,t_es[-1]+4]) + scale_drift = np.divide(dist_es,dist_gt)*100-100 + ax.plot(t_es, scale_drift, 'b-') + fig.tight_layout() + fig.savefig(results_dir+'/scale_drift.pdf') + + # plot trajectory + fig = plt.figure() + ax = fig.add_subplot(111, title='trajectory', aspect='equal', xlabel='x [m]', ylabel='y [m]') + ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'b-', label='estimate') + ax.plot(p_gt[:,0], p_gt[:,1], 'r-', label='groundtruth') + ax.plot(p_es_aligned[0:n_align_frames,0], p_es_aligned[0:n_align_frames,1], 'g-', linewidth=2, label='aligned') + ax.legend() + fig.tight_layout() + fig.savefig(results_dir+'/trajectory.pdf') + +def analyse_trajectory(results_dir, n_align_frames = 200, use_hand_eye_calib = True): + params = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r')) + if params['dataset_is_blender']: + analyse_synthetic_trajectory(results_dir) + elif use_hand_eye_calib: + analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames) + else: + t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay']) + scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:]) + p_es_aligned = np.zeros(np.shape(p_es)) + for i in range(np.shape(p_es)[0]): + p_es_aligned[i,:] = scale*rot.dot(p_es[i,:]) + trans + # plot position error (drift) + translation_error = (p_gt-p_es_aligned) + plot_translation_error(t_es, translation_error, results_dir) + +def get_rigid_body_trafo(quat,trans): + T = transformations.quaternion_matrix(quat) + T[0:3,3] = trans + return T + +def load_dataset(results_dir, cam_delay): + print('loading dataset in '+results_dir) + print('cam_delay = '+str(cam_delay)) + + data_gt = open(os.path.join(results_dir, 'groundtruth.txt')).read() + lines = data_gt.replace(","," ").replace("\t"," ").split("\n") + data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]) + #data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if i != 1 and v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]) + data_gt = [(float(l[0]),l[1:]) for l in data_gt] + data_gt = dict(data_gt) + + data_es = open(os.path.join(results_dir, 'traj_estimate.txt')).read() + lines = data_es.replace(","," ").replace("\t"," ").split("\n") + data_es = np.array([[np.float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]) + data_es = [(float(l[0]),l[1:]) for l in data_es] + data_es = dict(data_es) + + matches = associate.associate(data_gt, data_es, -cam_delay, 0.02) + p_gt = np.array([[np.float(value) for value in data_gt[a][0:3]] for a,b in matches]) + q_gt = np.array([[np.float(value) for value in data_gt[a][3:7]] for a,b in matches]) + p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a,b in matches]) + q_es = np.array([[np.float(value) for value in data_es[b][3:7]] for a,b in matches]) + t_gt = np.array([np.float(a) for a,b in matches]) + t_es = np.array([np.float(b) for a,b in matches]) + + # set start time to zero + start_time = min(t_es[0], t_gt[0]) + t_es -= start_time + t_gt -= start_time + return t_es, p_es, q_es, t_gt, p_gt, q_gt + +if __name__ == '__main__': + + # parse command line + parser = argparse.ArgumentParser(description=''' + Analyse trajectory + ''') + parser.add_argument('results_dir', help='folder with the results') + parser.add_argument('--use_hand_eye_calib', help='', action='store_true') + parser.add_argument('--n_align_frames', help='', default=200) + args = parser.parse_args() + + print('analyse trajectory for dataset: '+str(args.results_dir)) + analyse_trajectory(args.results_dir, + n_align_frames = int(args.n_align_frames), + use_hand_eye_calib = args.use_hand_eye_calib) \ No newline at end of file diff --git a/CPU_version/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py b/CPU_version/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py new file mode 100644 index 0000000..d0cc240 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py @@ -0,0 +1,60 @@ +#!/usr/bin/python + + +import numpy as np +import matplotlib.pyplot as plt +import transformations +from scipy import signal + +save = True +data_filename = '/home/cforster/Datasets/SlamBenchmark/asl_vicon_d2/groundtruth.txt' +filtered_data_filename = '/home/cforster/Datasets/SlamBenchmark/asl_vicon_d2/groundtruth_filtered.txt' +file = open(data_filename) +data = file.read() +lines = data.replace(","," ").replace("\t"," ").split("\n") +D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) + +n = np.shape(D)[0] +rpy = np.empty([n,3]) +for i in range(n): + quat = D[i,4:8] + rpy[i,:] = transformations.euler_from_quaternion(quat, axes='sxyz') + +# filter rpy +f_sensor = 200.0; # sampling frequency in hz +f_cut = 15.0; # cutoff frequency in hz +b,a = signal.butter(5,f_cut/(f_sensor/2)); +print a +print b + +rpy_filt = np.empty([n,3]) +rpy_filt[:,0] = signal.filtfilt(b, a, rpy[:,0]) +rpy_filt[:,1] = signal.filtfilt(b, a, rpy[:,1]) +rpy_filt[:,2] = signal.filtfilt(b, a, rpy[:,2]) + +fig = plt.figure() +ax = fig.add_subplot(111, title='orientation filtered') +ax.plot(rpy[:,0], 'r-') +ax.plot(rpy[:,1], 'g-') +ax.plot(rpy[:,2], 'b-') +ax.plot(rpy_filt[:,0], 'k-', linewidth=2) +ax.plot(rpy_filt[:,1], 'k-', linewidth=2) +ax.plot(rpy_filt[:,2], 'k-', linewidth=2) + +fig = plt.figure() +ax = fig.add_subplot(111, title='position') +ax.plot(D[:,1], 'r') +ax.plot(D[:,2], 'g') +ax.plot(D[:,3], 'b') + +fig = plt.figure() +ax = fig.add_subplot(111, title='trajectory from top') +ax.plot(D[:,1], D[:,2]) + +if save: + f = open(filtered_data_filename,'w') + for i in range(np.shape(D)[0]): + quat = transformations.quaternion_from_euler(rpy_filt[i,0], rpy_filt[i,1], rpy_filt[i,2], axes='sxyz') + f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (D[i,0], D[i,1], D[i,2], D[i,3], quat[0], quat[1], quat[2], quat[3])) + + f.close() diff --git a/CPU_version/svo_analysis/src/svo_analysis/hand_eye_calib.py b/CPU_version/svo_analysis/src/svo_analysis/hand_eye_calib.py new file mode 100644 index 0000000..a54b4f8 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/hand_eye_calib.py @@ -0,0 +1,227 @@ +# -*- coding: utf-8 -*- +""" +Created on Sun Aug 4 15:47:55 2013 + +@author: cforster +""" +import os +import yaml +import numpy as np +import svo_analysis.tum_benchmark_tools.associate as associate +import vikit_py.align_trajectory as align_trajectory +import vikit_py.transformations as transformations +import matplotlib.pyplot as plt + +# user config +display = True +dataset_dir = '/home/cforster/catkin_ws/src/rpg_svo/svo_analysis/results/rpg_circle_1' +n_measurements = 400 +n_align_sim3 = 600 +delta = 50 + +# load dataset parameters +params = yaml.load(open(os.path.join(dataset_dir, 'dataset_params.yaml'))) + +# set trajectory groundtruth and estimate file +traj_groundtruth = os.path.join(dataset_dir, 'groundtruth.txt') +traj_estimate = os.path.join(dataset_dir,'traj_estimate.txt') + +# load data +data_gt = associate.read_file_list(traj_groundtruth) +data_es = associate.read_file_list(traj_estimate) + +# select matches +offset = -params['cam_delay'] +print ('offset = '+str(offset)) +matches = associate.associate(data_gt, data_es, offset, 0.02) +#matches = matches[500:] +p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches]) +q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches]) +p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches]) +q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches]) + +# -------------------------------------------------------------------------------- +# align Sim3 to get scale +scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_sim3,:], p_es[0:n_align_sim3,:]) + +#model_aligned = s * R * model + t +#alignment_error = model_aligned - data +#t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0] + +p_es_aligned = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans +p_es = scale*p_es + + +print 's='+str(scale) +print 't='+str(trans) +print 'R='+str(rot) + +# plot sim3 aligned trajectory +fig = plt.figure() +ax = fig.add_subplot(111, aspect='equal') +ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'r-', label='estimate', alpha=0.2) +ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth', alpha=0.2) +ax.plot(p_es_aligned[:n_align_sim3,0], p_es_aligned[:n_align_sim3,1], 'g-', label='aligned', linewidth=2) +ax.plot(p_gt[:n_align_sim3,0], p_gt[:n_align_sim3,1], 'm-', label='aligned', linewidth=2) +ax.legend() +for (x1,y1,z1),(x2,y2,z2) in zip(p_es_aligned[:n_align_sim3:10],p_gt[:n_align_sim3:10]): + ax.plot([x1,x2],[y1,y2],'-',color="red") + +fig = plt.figure() +ax = fig.add_subplot(111) +ax.plot(p_gt[:,0], 'r-') +ax.plot(p_gt[:,1], 'g-') +ax.plot(p_gt[:,2], 'b-') +ax.plot(p_es_aligned[:,0], 'r--') +ax.plot(p_es_aligned[:,1], 'g--') +ax.plot(p_es_aligned[:,2], 'b--') + + +# -------------------------------------------------------------------------------- +# hand-eye-calib + +# select random measurements +I = np.array(np.random.rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0] +R,b = align_trajectory.hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True) + +print 'quat = ' + str(transformations.quaternion_from_matrix(transformations.convert_3x3_to_4x4(R))) +print 'b = ' + str(b) + +rpy_es = np.zeros([q_es.shape[0]-1, 3]) +rpy_gt = np.zeros([q_gt.shape[0]-1, 3]) +t_gt = np.zeros([q_es.shape[0]-1,3]) +t_es = np.zeros([q_es.shape[0]-1,3]) + +for i in range(delta,np.shape(q_es)[0]): + A1 = transformations.quaternion_matrix(q_es[i-delta,:])[:3,:3] + A2 = transformations.quaternion_matrix(q_es[i,:])[:3,:3] + A = np.dot(A1.transpose(), A2) + B1 = transformations.quaternion_matrix(q_gt[i-delta,:])[:3,:3] + B2 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3] + B = np.dot(B1.transpose(), B2) + B_es = np.dot(np.transpose(R), np.dot(A, R)) + rpy_gt[i-delta,:] = transformations.euler_from_matrix(B, 'rzyx') + rpy_es[i-delta,:] = transformations.euler_from_matrix(B_es, 'rzyx') + t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:])) + t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:])) + t_gt[i-delta,:] = t_B + t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0]) + +alignment_error = (t_gt-t_es) +error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1)) +I_accurate = np.argwhere(error < np.percentile(error, 90))[:,0] + +if display: + plt.figure() + plt.plot(rpy_es[:,0], 'r-', label='es roll') + plt.plot(rpy_es[:,1], 'g-', label='es pitch') + plt.plot(rpy_es[:,2], 'b-', label='es yaw') + plt.plot(rpy_gt[:,0], 'r--', label='gt roll') + plt.plot(rpy_gt[:,1], 'g--', label='gt pitch') + plt.plot(rpy_gt[:,2], 'b--', label='gt yaw') + plt.legend() + plt.figure() + plt.plot(t_gt[:,0], 'r-', label='gt x') + plt.plot(t_gt[:,1], 'g-', label='gt y') + plt.plot(t_gt[:,2], 'b-', label='gt z') + plt.plot(t_es[:,0], 'r--', label='es x') + plt.plot(t_es[:,1], 'g--', label='es y') + plt.plot(t_es[:,2], 'b--', label='es z') + plt.legend() + plt.figure() + plt.plot(error,'-g') + + +print 'e_rms = ' + str(np.sqrt(np.dot(error,error) / len(error))) +print 'e_mean = ' + str(np.mean(error)) +print 'e_median = ' + str(np.median(error)) +print 'e_std = ' + str(np.std(error)) + + +# now sample again from the filtered list: +N = 10 +display = False +n_acc_meas = I_accurate.size +n_measurements = 500 + +#for i in range(5): +# print '-------------------------------------' +# i0 = np.array(rand(n_measurements,1)*np.shape(I_accurate)[0], dtype=int)[:,0] +# i1 = np.minimum(i0+N, n_acc_meas-1) +# I = np.empty(i0.size*2, dtype=int) +# I[0::2] = I_accurate[i0] +# I[1::2] = I_accurate[i1] +# R,b = handEyeCalib(q_gt[I,:], q_es[I,:], p_gt[I,:], p_es[I,:], True) +# print 'quat = ' + str(ru.dcm2quat(R)) +# print 'b = ' + str(b) +# rpy_es = np.zeros([q_es.shape[0]-1, 3]) +# rpy_gt = np.zeros([q_gt.shape[0]-1, 3]) +# t_gt = np.zeros([q_es.shape[0]-1,3]) +# t_es = np.zeros([q_es.shape[0]-1,3]) +# +# delta = 10 +# for i in range(delta,np.shape(q_es)[0]): +# A1 = ru.quat2dcm(q_es[i-delta,:]) +# A2 = ru.quat2dcm(q_es[i,:]) +# A = np.dot(A1.transpose(), A2) +# B1 = ru.quat2dcm(q_gt[i-delta,:]) +# B2 = ru.quat2dcm(q_gt[i,:]) +# B = np.dot(B1.transpose(), B2) +# B_es = np.dot(np.transpose(R), np.dot(A, R)) +# rpy_gt[i-delta,:] = ru.dcm2rpy(B) +# rpy_es[i-delta,:] = ru.dcm2rpy(B_es) +# t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:])) +# t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:])) +# t_gt[i-delta,:] = t_B +# t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0]) +# alignment_error = (t_gt-t_es) +# error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1)) +# +# if display: +# plt.figure() +# plt.plot(rpy_es[:,0], 'r-', label='es roll') +# plt.plot(rpy_es[:,1], 'g-', label='es pitch') +# plt.plot(rpy_es[:,2], 'b-', label='es yaw') +# plt.plot(rpy_gt[:,0], 'r--', label='gt roll') +# plt.plot(rpy_gt[:,1], 'g--', label='gt pitch') +# plt.plot(rpy_gt[:,2], 'b--', label='gt yaw') +# plt.legend() +# plt.figure() +# plt.plot(t_gt[:,0], 'r-', label='es x') +# plt.plot(t_gt[:,1], 'g-', label='es y') +# plt.plot(t_gt[:,2], 'b-', label='es z') +# plt.plot(t_es[:,0], 'r--', label='gt x') +# plt.plot(t_es[:,1], 'g--', label='gt y') +# plt.plot(t_es[:,2], 'b--', label='gt z') +# plt.legend() +# plt.figure() +# plt.plot(error,'-g') +# +# print 'e_rms = ' + str(np.sqrt(np.dot(error,error) / len(error))) +# print 'e_mean = ' + str(np.mean(error)) +# print 'e_median = ' + str(np.median(error)) +# print 'e_std = ' + str(np.std(error)) + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py new file mode 100644 index 0000000..633f866 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py @@ -0,0 +1,2 @@ +# -*- coding: utf-8 -*- + diff --git a/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py new file mode 100755 index 0000000..361cdd5 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py @@ -0,0 +1,130 @@ +#!/usr/bin/python +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Juergen Sturm, TUM +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of TUM 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 OWNER 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. +# +# Requirements: +# sudo apt-get install python-argparse + +""" +The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images. + +For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches. +""" + +import argparse +import sys +import os +import numpy + + +def read_file_list(filename): + """ + Reads a trajectory from a text file. + + File format: + The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) + and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. + + Input: + filename -- File name + + Output: + dict -- dictionary of (stamp,data) tuples + + """ + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] + list = [(float(l[0]),l[1:]) for l in list if len(l)>1] + return dict(list) + +def associate(first_list, second_list,offset,max_difference): + """ + Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim + to find the closest match for every input tuple. + + Input: + first_list -- first dictionary of (stamp,data) tuples + second_list -- second dictionary of (stamp,data) tuples + offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) + max_difference -- search radius for candidate generation + + Output: + matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) + + """ + first_keys = first_list.keys() + second_keys = second_list.keys() + potential_matches = [(abs(a - (b + offset)), a, b) + for a in first_keys + for b in second_keys + if abs(a - (b + offset)) < max_difference] + potential_matches.sort() + matches = [] + for diff, a, b in potential_matches: + if a in first_keys and b in second_keys: + first_keys.remove(a) + second_keys.remove(b) + matches.append((a, b)) + + matches.sort() + return matches + +if __name__ == '__main__': + + # parse command line + parser = argparse.ArgumentParser(description=''' + This script takes two data files with timestamps and associates them + ''') + parser.add_argument('first_file', help='estimated trajectory (format: timestamp data)') + parser.add_argument('second_file', help='groundtruth (format: timestamp data)') + parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) + parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) + args = parser.parse_args() + + first_list = read_file_list(args.first_file) + second_list = read_file_list(args.second_file) + + matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) + + filename = 'groundtruth_matched.txt' + associates_file = open(filename, 'w') + for a,b in matches: + img = first_list[a] + pos = second_list[b] + associates_file.write('%.6f %s %s %s %s %s %s %s %s\n' % + (a, img[0], + pos[0], pos[1], pos[2], + pos[3], pos[4], pos[5], pos[6])) + print('Wrote matches to file: ' + filename) + diff --git a/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_ate.py b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_ate.py new file mode 100755 index 0000000..a8b2ca9 --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_ate.py @@ -0,0 +1,194 @@ +#!/usr/bin/python +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Juergen Sturm, TUM +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of TUM 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 OWNER 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. +# +# Requirements: +# sudo apt-get install python-argparse + +""" +This script computes the absolute trajectory error from the ground truth +trajectory and the estimated trajectory. +""" + +import sys +import numpy +import argparse +import associate + +def align(model,data): + """Align two trajectories using the method of Horn (closed-form). + + Input: + model -- first trajectory (3xn) + data -- second trajectory (3xn) + + Output: + rot -- rotation matrix (3x3) + trans -- translation vector (3x1) + trans_error -- translational error per point (1xn) + + """ + numpy.set_printoptions(precision=3,suppress=True) + model_zerocentered = model - model.mean(1) + data_zerocentered = data - data.mean(1) + + W = numpy.zeros( (3,3) ) + for column in range(model.shape[1]): + W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column]) + U,d,Vh = numpy.linalg.linalg.svd(W.transpose()) + S = numpy.matrix(numpy.identity( 3 )) + if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0): + S[2,2] = -1 + rot = U*S*Vh + trans = data.mean(1) - rot * model.mean(1) + + model_aligned = rot * model + trans + alignment_error = model_aligned - data + + trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0] + + return rot,trans,trans_error + +def plot_traj(ax,stamps,traj,style,color,label): + """ + Plot a trajectory using matplotlib. + + Input: + ax -- the plot + stamps -- time stamps (1xn) + traj -- trajectory (3xn) + style -- line style + color -- line color + label -- plot legend + + """ + stamps.sort() + interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])]) + x = [] + y = [] + last = stamps[0] + for i in range(len(stamps)): + if stamps[i]-last < 2*interval: + x.append(traj[i][0]) + y.append(traj[i][1]) + elif len(x)>0: + ax.plot(x,y,style,color=color,label=label) + label="" + x=[] + y=[] + last= stamps[i] + if len(x)>0: + ax.plot(x,y,style,color=color,label=label) + + +if __name__=="__main__": + # parse command line + parser = argparse.ArgumentParser(description=''' + This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. + ''') + parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)') + parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)') + parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) + parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0) + parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) + parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)') + parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)') + parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)') + parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true') + args = parser.parse_args() + + first_list = associate.read_file_list(args.first_file) + second_list = associate.read_file_list(args.second_file) + + matches = associate.associate(first_list, second_list, float(args.offset), float(args.max_difference)) + if len(matches)<2: + sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?") + + + first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose() + second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose() + rot,trans,trans_error = align(second_xyz,first_xyz) + + second_xyz_aligned = rot * second_xyz + trans + + first_stamps = first_list.keys() + first_stamps.sort() + first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose() + + second_stamps = second_list.keys() + second_stamps.sort() + second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose() + second_xyz_full_aligned = rot * second_xyz_full + trans + + if args.verbose: + print "compared_pose_pairs %d pairs"%(len(trans_error)) + + print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) + print "absolute_translational_error.mean %f m"%numpy.mean(trans_error) + print "absolute_translational_error.median %f m"%numpy.median(trans_error) + print "absolute_translational_error.std %f m"%numpy.std(trans_error) + print "absolute_translational_error.min %f m"%numpy.min(trans_error) + print "absolute_translational_error.max %f m"%numpy.max(trans_error) + else: + print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) + + if args.save_associations: + file = open(args.save_associations,"w") + file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)])) + file.close() + + if args.save: + file = open(args.save,"w") + file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)])) + file.close() + + if args.plot: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + + fig = plt.figure() + ax = fig.add_subplot(111) + plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth") + plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") + + label="difference" + for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A): + ax.plot([x1,x2],[y1,y2],'-',color="red",label=label) + label="" + + ax.legend() + + ax.set_xlabel('x [m]') + ax.set_ylabel('y [m]') + plt.savefig(args.plot,dpi=90) + diff --git a/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_rpe.py b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_rpe.py new file mode 100755 index 0000000..fb42e4a --- /dev/null +++ b/CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_rpe.py @@ -0,0 +1,382 @@ +#!/usr/bin/python +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Juergen Sturm, TUM +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of TUM 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 OWNER 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. + +""" +This script computes the relative pose error from the ground truth trajectory +and the estimated trajectory. +""" + +import argparse +import random +import numpy +import sys + +_EPS = numpy.finfo(float).eps * 4.0 + +def transform44(l): + """ + Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. + + Input: + l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where + (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion. + + Output: + matrix -- 4x4 homogeneous transformation matrix + """ + t = l[1:4] + q = numpy.array(l[4:8], dtype=numpy.float64, copy=True) + nq = numpy.dot(q, q) + if nq < _EPS: + return numpy.array(( + ( 1.0, 0.0, 0.0, t[0]) + ( 0.0, 1.0, 0.0, t[1]) + ( 0.0, 0.0, 1.0, t[2]) + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + q *= numpy.sqrt(2.0 / nq) + q = numpy.outer(q, q) + return numpy.array(( + (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], t[0]), + ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], t[1]), + ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], t[2]), + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + +def read_trajectory(filename, matrix=True): + """ + Read a trajectory from a text file. + + Input: + filename -- file to be read + matrix -- convert poses to 4x4 matrices + + Output: + dictionary of stamped 3D poses + """ + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + list = [[float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] + list_ok = [] + for i,l in enumerate(list): + if l[4:8]==[0,0,0,0]: + continue + isnan = False + for v in l: + if numpy.isnan(v): + isnan = True + break + if isnan: + sys.stderr.write("Warning: line %d of file '%s' has NaNs, skipping line\n"%(i,filename)) + continue + list_ok.append(l) + if matrix : + traj = dict([(l[0],transform44(l[0:])) for l in list_ok]) + else: + traj = dict([(l[0],l[1:8]) for l in list_ok]) + return traj + +def find_closest_index(L,t): + """ + Find the index of the closest value in a list. + + Input: + L -- the list + t -- value to be found + + Output: + index of the closest element + """ + beginning = 0 + difference = abs(L[0] - t) + best = 0 + end = len(L) + while beginning < end: + middle = int((end+beginning)/2) + if abs(L[middle] - t) < difference: + difference = abs(L[middle] - t) + best = middle + if t == L[middle]: + return middle + elif L[middle] > t: + end = middle + else: + beginning = middle + 1 + return best + +def ominus(a,b): + """ + Compute the relative 3D transformation between a and b. + + Input: + a -- first pose (homogeneous 4x4 matrix) + b -- second pose (homogeneous 4x4 matrix) + + Output: + Relative 3D transformation from a to b. + """ + return numpy.dot(numpy.linalg.inv(a),b) + +def scale(a,scalar): + """ + Scale the translational components of a 4x4 homogeneous matrix by a scale factor. + """ + return numpy.array( + [[a[0,0], a[0,1], a[0,2], a[0,3]*scalar], + [a[1,0], a[1,1], a[1,2], a[1,3]*scalar], + [a[2,0], a[2,1], a[2,2], a[2,3]*scalar], + [a[3,0], a[3,1], a[3,2], a[3,3]]] + ) + +def compute_distance(transform): + """ + Compute the distance of the translational component of a 4x4 homogeneous matrix. + """ + return numpy.linalg.norm(transform[0:3,3]) + +def compute_angle(transform): + """ + Compute the rotation angle from a 4x4 homogeneous matrix. + """ + # an invitation to 3-d vision, p 27 + return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) )) + +def distances_along_trajectory(traj): + """ + Compute the translational distances along a trajectory. + """ + keys = traj.keys() + keys.sort() + motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)] + distances = [0] + sum = 0 + for t in motion: + sum += compute_distance(t) + distances.append(sum) + return distances + +def rotations_along_trajectory(traj,scale): + """ + Compute the angular rotations along a trajectory. + """ + keys = traj.keys() + keys.sort() + motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)] + distances = [0] + sum = 0 + for t in motion: + sum += compute_angle(t)*scale + distances.append(sum) + return distances + + +def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00): + """ + Compute the relative pose error between two trajectories. + + Input: + traj_gt -- the first trajectory (ground truth) + traj_est -- the second trajectory (estimated trajectory) + param_max_pairs -- number of relative poses to be evaluated + param_fixed_delta -- false: evaluate over all possible pairs + true: only evaluate over pairs with a given distance (delta) + param_delta -- distance between the evaluated pairs + param_delta_unit -- unit for comparison: + "s": seconds + "m": meters + "rad": radians + "deg": degrees + "f": frames + param_offset -- time offset between two trajectories (to model the delay) + param_scale -- scale to be applied to the second trajectory + + Output: + list of compared poses and the resulting translation and rotation error + """ + stamps_gt = list(traj_gt.keys()) + stamps_est = list(traj_est.keys()) + stamps_gt.sort() + stamps_est.sort() + + stamps_est_return = [] + for t_est in stamps_est: + t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)] + t_est_return = stamps_est[find_closest_index(stamps_est,t_gt - param_offset)] + t_gt_return = stamps_gt[find_closest_index(stamps_gt,t_est_return + param_offset)] + if not t_est_return in stamps_est_return: + stamps_est_return.append(t_est_return) + if(len(stamps_est_return)<2): + raise Exception("Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?") + + if param_delta_unit=="s": + index_est = list(traj_est.keys()) + index_est.sort() + elif param_delta_unit=="m": + index_est = distances_along_trajectory(traj_est) + elif param_delta_unit=="rad": + index_est = rotations_along_trajectory(traj_est,1) + elif param_delta_unit=="deg": + index_est = rotations_along_trajectory(traj_est,180/numpy.pi) + elif param_delta_unit=="f": + index_est = range(len(traj_est)) + else: + raise Exception("Unknown unit for delta: '%s'"%param_delta_unit) + + if not param_fixed_delta: + if(param_max_pairs==0 or len(traj_est)param_max_pairs): + pairs = random.sample(pairs,param_max_pairs) + + gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])]) + gt_max_time_difference = 2*gt_interval + + result = [] + for i,j in pairs: + stamp_est_0 = stamps_est[i] + stamp_est_1 = stamps_est[j] + + stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ] + stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ] + + if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference or + abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference): + continue + + error44 = ominus( scale( + ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale), + ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) ) + + trans = compute_distance(error44) + rot = compute_angle(error44) + + result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot]) + + if len(result)<2: + raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!") + + return result + +def percentile(seq,q): + """ + Return the q-percentile of a list + """ + seq_sorted = list(seq) + seq_sorted.sort() + return seq_sorted[int((len(seq_sorted)-1)*q)] + +if __name__ == '__main__': + random.seed(0) + + parser = argparse.ArgumentParser(description=''' + This script computes the relative pose error from the ground truth trajectory and the estimated trajectory. + ''') + parser.add_argument('groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")') + parser.add_argument('estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")') + parser.add_argument('--max_pairs', help='maximum number of pose comparisons (default: 10000, set to zero to disable downsampling)', default=10000) + parser.add_argument('--fixed_delta', help='only consider pose pairs that have a distance of delta delta_unit (e.g., for evaluating the drift per second/meter/radian)', action='store_true') + parser.add_argument('--delta', help='delta for evaluation (default: 1.0)',default=1.0) + parser.add_argument('--delta_unit', help='unit of delta (options: \'s\' for seconds, \'m\' for meters, \'rad\' for radians, \'f\' for frames; default: \'s\')',default='s') + parser.add_argument('--offset', help='time offset between ground-truth and estimated trajectory (default: 0.0)',default=0.0) + parser.add_argument('--scale', help='scaling factor for the estimated trajectory (default: 1.0)',default=1.0) + parser.add_argument('--save', help='text file to which the evaluation will be saved (format: stamp_est0 stamp_est1 stamp_gt0 stamp_gt1 trans_error rot_error)') + parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)') + parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', action='store_true') + args = parser.parse_args() + + if args.plot and not args.fixed_delta: + sys.exit("The '--plot' option can only be used in combination with '--fixed_delta'") + + traj_gt = read_trajectory(args.groundtruth_file) + traj_est = read_trajectory(args.estimated_file) + + result = evaluate_trajectory(traj_gt, + traj_est, + int(args.max_pairs), + args.fixed_delta, + float(args.delta), + args.delta_unit, + float(args.offset), + float(args.scale)) + + stamps = numpy.array(result)[:,0] + trans_error = numpy.array(result)[:,4] + rot_error = numpy.array(result)[:,5] + + if args.save: + f = open(args.save,"w") + f.write("\n".join([" ".join(["%f"%v for v in line]) for line in result])) + f.close() + + if args.verbose: + print "compared_pose_pairs %d pairs"%(len(trans_error)) + + print "translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) + print "translational_error.mean %f m"%numpy.mean(trans_error) + print "translational_error.median %f m"%numpy.median(trans_error) + print "translational_error.std %f m"%numpy.std(trans_error) + print "translational_error.min %f m"%numpy.min(trans_error) + print "translational_error.max %f m"%numpy.max(trans_error) + + print "rotational_error.rmse %f deg"%(numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi) + print "rotational_error.mean %f deg"%(numpy.mean(rot_error) * 180.0 / numpy.pi) + print "rotational_error.median %f deg"%numpy.median(rot_error) + print "rotational_error.std %f deg"%(numpy.std(rot_error) * 180.0 / numpy.pi) + print "rotational_error.min %f deg"%(numpy.min(rot_error) * 180.0 / numpy.pi) + print "rotational_error.max %f deg"%(numpy.max(rot_error) * 180.0 / numpy.pi) + else: + print numpy.mean(trans_error) + + if args.plot: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + import matplotlib.pylab as pylab + fig = plt.figure() + ax = fig.add_subplot(111) + ax.plot(stamps - stamps[0],trans_error,'-',color="blue") + #ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red") + ax.set_xlabel('time [s]') + ax.set_ylabel('translational error [m]') + plt.savefig(args.plot,dpi=300) + + diff --git a/CPU_version/svo_msgs/.idea/.gitignore b/CPU_version/svo_msgs/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/CPU_version/svo_msgs/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/CPU_version/svo_msgs/.idea/misc.xml b/CPU_version/svo_msgs/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/CPU_version/svo_msgs/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/CPU_version/svo_msgs/.idea/modules.xml b/CPU_version/svo_msgs/.idea/modules.xml new file mode 100644 index 0000000..668cc83 --- /dev/null +++ b/CPU_version/svo_msgs/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/CPU_version/svo_msgs/.idea/svo_msgs.iml b/CPU_version/svo_msgs/.idea/svo_msgs.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/CPU_version/svo_msgs/.idea/svo_msgs.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/CPU_version/svo_msgs/.idea/vcs.xml b/CPU_version/svo_msgs/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/CPU_version/svo_msgs/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/CPU_version/svo_msgs/CMakeLists.txt b/CPU_version/svo_msgs/CMakeLists.txt new file mode 100644 index 0000000..6d1a371 --- /dev/null +++ b/CPU_version/svo_msgs/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8.3) +project(svo_msgs) + +# search for everything we need to build the messages, dont forget the message_generation +find_package(catkin REQUIRED COMPONENTS + message_generation + geometry_msgs + sensor_msgs +) + + + +# notify catkin to look at the previously found msg files +add_message_files( + FILES + Info.msg +) + +# build the header files from the msg files, and notify catkin about the dependencies +generate_messages( + DEPENDENCIES + geometry_msgs + sensor_msgs +) + +# export the dependencis of this package for who ever depends on us +catkin_package( + CATKIN_DEPENDS message_runtime geometry_msgs +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) \ No newline at end of file diff --git a/CPU_version/svo_msgs/msg/DenseInput.msg b/CPU_version/svo_msgs/msg/DenseInput.msg new file mode 100644 index 0000000..d36dee8 --- /dev/null +++ b/CPU_version/svo_msgs/msg/DenseInput.msg @@ -0,0 +1,6 @@ +Header header +uint32 frame_id +geometry_msgs/Pose pose +sensor_msgs/Image image +float32 min_depth +float32 max_depth \ No newline at end of file diff --git a/CPU_version/svo_msgs/msg/Feature.msg b/CPU_version/svo_msgs/msg/Feature.msg new file mode 100644 index 0000000..2972c32 --- /dev/null +++ b/CPU_version/svo_msgs/msg/Feature.msg @@ -0,0 +1,5 @@ +float32 u +float32 v +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/CPU_version/svo_msgs/msg/Info.msg b/CPU_version/svo_msgs/msg/Info.msg new file mode 100644 index 0000000..b962e1b --- /dev/null +++ b/CPU_version/svo_msgs/msg/Info.msg @@ -0,0 +1,6 @@ +Header header +float32 processing_time +int32[] keyframes +int32 num_matches +int32 tracking_quality +int32 stage \ No newline at end of file diff --git a/CPU_version/svo_msgs/msg/NbvTrajectory.msg b/CPU_version/svo_msgs/msg/NbvTrajectory.msg new file mode 100644 index 0000000..03f0792 --- /dev/null +++ b/CPU_version/svo_msgs/msg/NbvTrajectory.msg @@ -0,0 +1,2 @@ +Header header +geometry_msgs/Pose[] trajectory \ No newline at end of file diff --git a/CPU_version/svo_msgs/package.xml b/CPU_version/svo_msgs/package.xml new file mode 100644 index 0000000..b122e2d --- /dev/null +++ b/CPU_version/svo_msgs/package.xml @@ -0,0 +1,19 @@ + + + svo_msgs + 0.0.0 + Custom messages for svo + Christian Forster + BSD + catkin + + message_generation + geometry_msgs + sensor_msgs + + message_runtime + geometry_msgs + sensor_msgs + + + \ No newline at end of file diff --git a/CPU_version/vio_svo/.idea/.gitignore b/CPU_version/vio_svo/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/CPU_version/vio_svo/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/CPU_version/vio_svo/.idea/misc.xml b/CPU_version/vio_svo/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/CPU_version/vio_svo/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/CPU_version/vio_svo/.idea/modules.xml b/CPU_version/vio_svo/.idea/modules.xml new file mode 100644 index 0000000..eb3cacf --- /dev/null +++ b/CPU_version/vio_svo/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/CPU_version/vio_svo/.idea/svo_ros.iml b/CPU_version/vio_svo/.idea/svo_ros.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/CPU_version/vio_svo/.idea/svo_ros.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/CPU_version/vio_svo/.idea/vcs.xml b/CPU_version/vio_svo/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/CPU_version/vio_svo/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/CPU_version/vio_svo/CMakeLists.txt b/CPU_version/vio_svo/CMakeLists.txt new file mode 100644 index 0000000..7fdf637 --- /dev/null +++ b/CPU_version/vio_svo/CMakeLists.txt @@ -0,0 +1,90 @@ +SET(PROJECT_NAME vio_svo) +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_VERBOSE_MAKEFILE OFF) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") + +# Set build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +ENDIF() +IF(CMAKE_COMPILER_IS_GNUCC) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ENDIF() +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Add catkin and required ROS packages +FIND_PACKAGE(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + cmake_modules + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + svo + vikit_common + vikit_ros +) + +# Add plain cmake packages +FIND_PACKAGE(OpenCV 3 REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(Boost REQUIRED COMPONENTS thread) +FIND_PACKAGE(GTSAM REQUIRED) + +# notify catkin to look at the previously found msg files +add_message_files( + FILES + Info.msg +) + +# build the header files from the msg files, and notify catkin about the dependencies +generate_messages( + DEPENDENCIES + geometry_msgs + sensor_msgs +) +# Describe catkin Project +catkin_package( + DEPENDS Eigen OpenCV Sophus Boost + CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo vikit_common vikit_ros geometry_msgs + INCLUDE_DIRS include + LIBRARIES svo_visualizer +) + +# Include dirs +INCLUDE_DIRECTORIES( + include + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries +LIST(APPEND LINK_LIBS + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +# Create visualizer library +ADD_LIBRARY(svo_visualizer src/visualizer.cpp) +TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS} gtsam) + +# Create Executables +ADD_EXECUTABLE(vo src/vo_node.cpp) +TARGET_LINK_LIBRARIES(vo svo_visualizer ${LINK_LIBS}) diff --git a/CPU_version/vio_svo/include/vio_svo/visualizer.h b/CPU_version/vio_svo/include/vio_svo/visualizer.h new file mode 100644 index 0000000..1b730ee --- /dev/null +++ b/CPU_version/vio_svo/include/vio_svo/visualizer.h @@ -0,0 +1,73 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_VISUALIZER_H_ +#define SVO_VISUALIZER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Point; +class Map; +class FrameHandlerMono; + +typedef boost::shared_ptr FramePtr; + +/// This class bundles all functions to publish visualisation messages. +class Visualizer +{ +public: + ros::NodeHandle pnh_; + size_t trace_id_; + ros::Publisher pub_frames_; + ros::Publisher pub_points_; + ros::Publisher pub_pose_; + ros::Publisher pub_info_; + tf::TransformBroadcaster br_; + SE3 T_world_from_vision_; + + Visualizer(); + + ~Visualizer() {}; + + void publishMinimal( + const cv::Mat& img, + const FramePtr& frame, + const FrameHandlerMono& slam, + const double timestamp); + + void visualizeMarkers( + const FramePtr& frame, + const set& core_kfs, + const Map& map); + + void removeDeletedPts(const Map& map); + +}; + +} // end namespace svo + +#endif /* SVO_VISUALIZER_H_ */ diff --git a/CPU_version/vio_svo/launch/Px30.launch b/CPU_version/vio_svo/launch/Px30.launch new file mode 100644 index 0000000..7e3e73c --- /dev/null +++ b/CPU_version/vio_svo/launch/Px30.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/CPU_version/vio_svo/launch/live.launch b/CPU_version/vio_svo/launch/live.launch new file mode 100644 index 0000000..7cd0ad8 --- /dev/null +++ b/CPU_version/vio_svo/launch/live.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/CPU_version/vio_svo/launch/test.launch b/CPU_version/vio_svo/launch/test.launch new file mode 100644 index 0000000..4bcd965 --- /dev/null +++ b/CPU_version/vio_svo/launch/test.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/CPU_version/vio_svo/launch/unity.launch b/CPU_version/vio_svo/launch/unity.launch new file mode 100644 index 0000000..ea34997 --- /dev/null +++ b/CPU_version/vio_svo/launch/unity.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/CPU_version/vio_svo/msg/Info.msg b/CPU_version/vio_svo/msg/Info.msg new file mode 100644 index 0000000..b962e1b --- /dev/null +++ b/CPU_version/vio_svo/msg/Info.msg @@ -0,0 +1,6 @@ +Header header +float32 processing_time +int32[] keyframes +int32 num_matches +int32 tracking_quality +int32 stage \ No newline at end of file diff --git a/CPU_version/vio_svo/package.xml b/CPU_version/vio_svo/package.xml new file mode 100644 index 0000000..c63372b --- /dev/null +++ b/CPU_version/vio_svo/package.xml @@ -0,0 +1,43 @@ + + + vio_svo + 0.1.0 + + Monocular Visual Odometry - ROS Nodes + + Christian Forster + Majid Geravand + GPLv3 + + + catkin + + + roscpp + cmake_modules + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + svo + vikit_common + vikit_ros + geometry_msgs + + + roscpp + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + svo + vikit_common + vikit_ros + geometry_msgs + \ No newline at end of file diff --git a/CPU_version/vio_svo/param/camera_atan.yaml b/CPU_version/vio_svo/param/camera_atan.yaml new file mode 100644 index 0000000..e3c625c --- /dev/null +++ b/CPU_version/vio_svo/param/camera_atan.yaml @@ -0,0 +1,8 @@ +cam_model: ATAN +cam_width: 752 +cam_height: 480 +cam_fx: 0.509326 +cam_fy: 0.796651 +cam_cx: 0.45905 +cam_cy: 0.510056 +cam_d0: 0.9320 \ No newline at end of file diff --git a/CPU_version/vio_svo/param/camera_pinhole.yaml b/CPU_version/vio_svo/param/camera_pinhole.yaml new file mode 100644 index 0000000..506a856 --- /dev/null +++ b/CPU_version/vio_svo/param/camera_pinhole.yaml @@ -0,0 +1,11 @@ +cam_model: Pinhole +cam_width: 640 +cam_height: 480 +cam_fx: 789.860721 +cam_fy: 790.097440 +cam_cx: 305.323524 +cam_cy: 260.464072 +cam_d0: 0.027470 +cam_d1: -0.051518 +cam_d2: 0.004375 +cam_d3: -0.004028 \ No newline at end of file diff --git a/CPU_version/vio_svo/param/px30.yaml b/CPU_version/vio_svo/param/px30.yaml new file mode 100644 index 0000000..10fa932 --- /dev/null +++ b/CPU_version/vio_svo/param/px30.yaml @@ -0,0 +1,8 @@ +cam_model: ATAN +cam_width: 640 +cam_height: 480 +cam_fx: 0.463632 +cam_fy: 0.823661 +cam_cx: 0.497465 +cam_cy: 0.498443 +cam_d0: 1.01687 \ No newline at end of file diff --git a/CPU_version/vio_svo/param/unity.yaml b/CPU_version/vio_svo/param/unity.yaml new file mode 100644 index 0000000..023f378 --- /dev/null +++ b/CPU_version/vio_svo/param/unity.yaml @@ -0,0 +1,8 @@ +cam_model: ATAN +cam_width: 1024 +cam_height: 720 +cam_fx: 1.98248 +cam_fy: 2.68212 +cam_cx: 0.502524 +cam_cy: 0.497744 +cam_d0: -0.000499999 \ No newline at end of file diff --git a/CPU_version/vio_svo/param/vo_accurate.yaml b/CPU_version/vio_svo/param/vo_accurate.yaml new file mode 100644 index 0000000..8446875 --- /dev/null +++ b/CPU_version/vio_svo/param/vo_accurate.yaml @@ -0,0 +1,4 @@ +grid_size: 20 +max_n_kfs: 50 +loba_num_iter: 10 +max_fts: 200 \ No newline at end of file diff --git a/CPU_version/vio_svo/param/vo_fast.yaml b/CPU_version/vio_svo/param/vo_fast.yaml new file mode 100644 index 0000000..e33b18a --- /dev/null +++ b/CPU_version/vio_svo/param/vo_fast.yaml @@ -0,0 +1,13 @@ +svo: + grid_size: 30 #Feature grid size of a cell in [px]. + max_n_kfs: 10 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. + loba_num_iter: 0 #Number of iterations in the local bundle adjustment. + quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. + quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad. + ACC_noise: 0.00001 #acc white noise in continuous + GYO_noise: 0.00001 #gyro white noise in continuous + IUC: 0.1 #integration uncertainty continuous + ABC: 0.1 #acc bias in continuous + GBC: 0.1 #gyro bias in continuous + EBP: 0.1 #error in the bias used for preintegration + NRP: 0.0000001 #noise used for projection in preintegration diff --git a/CPU_version/vio_svo/rqt.perspective b/CPU_version/vio_svo/rqt.perspective new file mode 100644 index 0000000..66dd7ef --- /dev/null +++ b/CPU_version/vio_svo/rqt.perspective @@ -0,0 +1,293 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_rviz/RViz': [1], u'rqt_reconfigure/Param': [1], u'rqt_image_view/ImageView': [1], u'svo_rqt/Svo': [1]}" + } + }, + "groups": { + "plugin__rqt_rviz__RViz__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_robot_steering__RobotSteering__1": { + "keys": {}, + "groups": { + "dock_widget__RobotSteeringUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "topic": { + "type": "repr", + "repr": "u'/cmd_vel'" + }, + "vx_min": { + "type": "repr", + "repr": "u'-1'" + }, + "vw_min": { + "type": "repr", + "repr": "u'-3'" + }, + "vw_max": { + "type": "repr", + "repr": "u'3'" + }, + "vx_max": { + "type": "repr", + "repr": "u'1'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_reconfigure__Param__1": { + "keys": {}, + "groups": { + "dock_widget___plugincontainer_top_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000c00000006401000000060100000001')", + "pretty-print": " d " + }, + "_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000013e0000006401000000090100000002')", + "pretty-print": " > d " + } + }, + "groups": {} + } + } + }, + "plugin__rqt_image_view__ImageView__1": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "dynamic_range": { + "type": "repr", + "repr": "False" + }, + "max_range": { + "type": "repr", + "repr": "10.0" + }, + "topic": { + "type": "repr", + "repr": "u'/svo/image'" + }, + "zoom1": { + "type": "repr", + "repr": "False" + } + }, + "groups": {} + } + } + }, + "plugin__svo_rqt__Svo__1": { + "keys": {}, + "groups": { + "dock_widget__SvoWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "dock_widget__SvoUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_shell__Shell__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "shell_type": { + "type": "repr", + "repr": "u'0'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_console__Console__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "show_highlighted_only": { + "type": "repr", + "repr": "u'false'" + }, + "message_limit": { + "type": "repr", + "repr": "u'20000'" + }, + "exclude_filters": { + "type": "repr", + "repr": "u'severity'" + }, + "table_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000004e000000ed01000000090100000002')", + "pretty-print": " N " + }, + "highlight_filters": { + "type": "repr", + "repr": "u'message'" + }, + "paused": { + "type": "repr", + "repr": "u'false'" + }, + "settings_exist": { + "type": "repr", + "repr": "u'true'" + }, + "filter_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000720000007201000000090100000002')", + "pretty-print": " r r " + } + }, + "groups": { + "exclude_filter_0": { + "keys": { + "enabled": { + "type": "repr", + "repr": "u'true'" + }, + "itemlist": { + "type": "repr", + "repr": "u''" + } + }, + "groups": {} + }, + "highlight_filter_0": { + "keys": { + "regex": { + "type": "repr", + "repr": "u'false'" + }, + "text": { + "type": "repr", + "repr": "u''" + }, + "enabled": { + "type": "repr", + "repr": "u'true'" + } + }, + "groups": {} + } + } + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000007a9fffffffc00001187000005a7000007b1000000180000117f0000059f000000000000')", + "pretty-print": " " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000009cf00000559fc0100000005fb0000002c00730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f005500690100000000000003890000000000000000fb0000006a007200710074005f0072006f0062006f0074005f007300740065006500720069006e0067005f005f0052006f0062006f0074005300740065006500720069006e0067005f005f0031005f005f0052006f0062006f0074005300740065006500720069006e00670055006901000000000000020b0000000000000000fc00000000000003530000022e00fffffffc0200000004fb0000003400730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f0057006900640067006500740100000019000000a30000009900fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000000c3000003b50000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000c2000002700000005100fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f00770069006400670065007401000003380000023a0000010f00fffffffb00000026007200710074005f007200760069007a005f005f005200560069007a005f005f0031005f005f010000035900000676000000a700fffffffb00000040007200710074005f007300680065006c006c005f005f005300680065006c006c005f005f0031005f005f0058005400650072006d005700690064006700650074010000091c000000b30000000000000000000009cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "pretty-print": " ,svo_rqt__Svo__1__SvoUi jrqt_robot_steering__RobotSteering__1__RobotSteeringUi S . p Q 8 : Y v " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/CPU_version/vio_svo/rviz_config.rviz b/CPU_version/vio_svo/rviz_config.rviz new file mode 100644 index 0000000..95d7c0e --- /dev/null +++ b/CPU_version/vio_svo/rviz_config.rviz @@ -0,0 +1,153 @@ +Panels: + - Class: rviz/Displays + Help Height: 108 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /Marker1 + - /Marker2 + - /Marker2/Namespaces1 + - /Image1 + Splitter Ratio: 0.5 + Tree Height: 680 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /svo/keyframes + Name: Marker + Namespaces: + cams: true + kfs: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /svo/points + Name: Marker + Namespaces: + pts: true + trajectory: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /svo/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: /world + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.48911 + Focal Point: + X: 0.777511 + Y: 0.689927 + Z: -0.445833 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.125204 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.45215 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1416 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002cc00000542fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000355000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000383000001e70000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006fd0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2511 + X: 1962 + Y: 24 diff --git a/CPU_version/vio_svo/src/visualizer.cpp b/CPU_version/vio_svo/src/visualizer.cpp new file mode 100644 index 0000000..901f9ef --- /dev/null +++ b/CPU_version/vio_svo/src/visualizer.cpp @@ -0,0 +1,127 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Visualizer:: +Visualizer() : + pnh_("~"), + trace_id_(0), + T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()) +{ + // Init ROS Marker Publishers + pub_frames_ = pnh_.advertise("keyframes", 10); + pub_points_ = pnh_.advertise("points", 1000); + pub_pose_ = pnh_.advertise("pose",10); + pub_info_ = pnh_.advertise("info", 10); +} + +void Visualizer::publishMinimal( + const cv::Mat& img, + const FramePtr& frame, + const FrameHandlerMono& slam, + const double timestamp) +{ + ++trace_id_; + std_msgs::Header header_msg; + header_msg.frame_id = "/cam"; + header_msg.seq = trace_id_; + header_msg.stamp = ros::Time(timestamp); + + // publish info msg. + if(pub_info_.getNumSubscribers() > 0) + { + vio_svo::Info msg_info; + msg_info.header = header_msg; + msg_info.processing_time = slam.lastProcessingTime(); + msg_info.keyframes.reserve(slam.map().keyframes_.size()); + for(list::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it) + msg_info.keyframes.push_back((*it)->id_); + msg_info.stage = static_cast(slam.stage()); + msg_info.tracking_quality = static_cast(slam.trackingQuality()); + if(frame != NULL) + msg_info.num_matches = slam.lastNumObservations(); + else + msg_info.num_matches = 0; + pub_info_.publish(msg_info); + } + if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME) + { + Quaterniond q; + Vector3d p; + Matrix Cov; + // publish cam in world frame (Estimated odometry in the worls frame) + SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera + q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose()); + p = T_world_from_cam.translation(); + Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj(); + geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped); + msg_pose->header = header_msg; + msg_pose->pose.pose.position.x = p[0]; + msg_pose->pose.pose.position.y = p[1]; + msg_pose->pose.pose.position.z = p[2]; + msg_pose->pose.pose.orientation.x = q.x(); + msg_pose->pose.pose.orientation.y = q.y(); + msg_pose->pose.pose.orientation.z = q.z(); + msg_pose->pose.pose.orientation.w = q.w(); + for(size_t i=0; i<36; ++i) + msg_pose->pose.covariance[i] = Cov(i%6, i/6); + pub_pose_.publish(msg_pose); + } +} + +void Visualizer::visualizeMarkers( + const FramePtr& frame, + const set& core_kfs, + const Map& map) +{ + if(frame == NULL) + return; + + vk::output_helper::publishTfTransform( + frame->T_f_w_*T_world_from_vision_.inverse(), + ros::Time(frame->timestamp_), "cam_pos", "world", br_); + + if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0) + { + vk::output_helper::publishPointMarker( + pub_points_, T_world_from_vision_*frame->pos(), "trajectory", + ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5)); + removeDeletedPts(map); + } +} + +void Visualizer::removeDeletedPts(const Map& map) +{ + if(pub_points_.getNumSubscribers() > 0) + { + for(list::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it) + vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d()); + } +} + +} // end namespace svo diff --git a/CPU_version/vio_svo/src/vo_node.cpp b/CPU_version/vio_svo/src/vo_node.cpp new file mode 100644 index 0000000..9779f3e --- /dev/null +++ b/CPU_version/vio_svo/src/vo_node.cpp @@ -0,0 +1,148 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace svo { + +/// SVO Interface +class VoNode +{ +public: + svo::FrameHandlerMono* vo_; + svo::Visualizer visualizer_; + bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices) + vk::AbstractCamera* cam_; + FILE* logtime; + bool quit_; + boost::thread* imu_th; + VoNode(); + ~VoNode(); + void imgCb(const sensor_msgs::ImageConstPtr& msg); + void imuCb(const sensor_msgs::ImuPtr& imu); + void imu(); +private: + double* imu_; +}; + +VoNode::VoNode() : + vo_(NULL), + publish_markers_(vk::getParam("vio/publish_markers", true)), + cam_(NULL), + quit_(false) +{ + // Create Camera + imu_=(double *)calloc(static_cast(6), sizeof(double)); + if(!vk::camera_loader::loadFromRosNs("vio", cam_)) + throw std::runtime_error("Camera model not correctly specified."); + // Get initial position and orientation + visualizer_.T_world_from_vision_ = Sophus::SE3( + vk::rpy2dcm(Vector3d(vk::getParam("vio/init_rx", 0.0), + vk::getParam("vio/init_ry", 0.0), + vk::getParam("vio/init_rz", 0.0))), + Eigen::Vector3d(vk::getParam("vio/init_tx", 0.0), + vk::getParam("vio/init_ty", 0.0), + vk::getParam("vio/init_tz", 0.0))); + // Init VO and start + vo_ = new svo::FrameHandlerMono(cam_,visualizer_.T_world_from_vision_); + vo_->start(); + imu_th=new boost::thread(&VoNode::imu,this); + logtime = fopen((std::string(getenv("HOME"))+"/time.txt").c_str(), "w+"); + +} + +VoNode::~VoNode() +{ + quit_=true; + fclose(logtime); + imu_th->join(); + delete imu_th; + delete logtime; + delete vo_; + delete cam_; +} + +void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) +{ + try { + auto start=std::chrono::steady_clock::now(); + cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; + vo_->addImage(img, msg->header.stamp.toSec()); + visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec()); + if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED) + visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); + if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED) + usleep(100000); + fprintf(logtime, "%s\n",std::to_string(std::chrono::duration(std::chrono::steady_clock::now()-start).count()).c_str()); + + } catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + } +} +void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { + double imu_in[6]; + imu_in[0] = 0.1*imu_[0]+0.9*imu->linear_acceleration.x; + imu_in[1] = 0.1*imu_[1]+0.9*imu->linear_acceleration.y; + imu_in[2] = 0.1*imu_[2]+0.9*imu->linear_acceleration.z; + imu_in[3] = 0.1*imu_[3]+0.9*imu->angular_velocity.x; + imu_in[4] = 0.1*imu_[4]+0.9*imu->angular_velocity.y; + imu_in[5] = 0.1*imu_[5]+0.9*imu->angular_velocity.z; + memcpy(imu_, imu_in, static_cast(6*sizeof(double))); + vo_->imu_integPtr_->update(imu_in); +} +void VoNode::imu(){ + ros::NodeHandle nh; + // subscribe to cam msgs + image_transport::ImageTransport it(nh); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 1, &svo::VoNode::imgCb, this); + // start processing callbacks + while(ros::ok() && !quit_) + { + ros::spinOnce(); + } + +} +} // namespace svo + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "vio"); + svo::VoNode vo_node; + ros::NodeHandle nh; + ros::Subscriber imu_sub=nh.subscribe(vk::getParam("vio/imu_topic", "imu/raw"),1,&svo::VoNode::imuCb, &vo_node); + // start processing callbacks + while(ros::ok() && !vo_node.quit_) + { + ros::spinOnce(); + usleep(1000); + // TODO check when last image was processed. when too long ago. publish warning that no msgs are received! + } + printf("SVO terminated.\n"); + return 0; +} diff --git a/GPU_version/README.md b/GPU_version/README.md new file mode 100755 index 0000000..7194fd5 --- /dev/null +++ b/GPU_version/README.md @@ -0,0 +1,69 @@ +SVO +=== + +This code implements a semi-direct monocular visual odometry pipeline which has been integrated with GTSAM to infused IMU as well. The feature extraction part is implemented in OpenCL to take advantage of GPU. + + +Paper svo: http://rpg.ifi.uzh.ch/docs/ICRA14_Forster.pdf +Paper Svo+GTSAM: https://arxiv.org/abs/1512.02363 + + +#### Disclaimer + +SVO has been tested under ROS Groovy, Hydro and Indigo with Ubuntu 12.04, 13.04 and 14.04. This is research code, any fitness for a particular purpose is disclaimed. + + +#### Licence + +The source code is released under a GPLv3 licence. A closed-source professional edition is available for commercial purposes. In this case, please contact the authors for further info. + + +#### Citing + +If you use SVO in an academic context, please cite the following publication: + + @inproceedings{Forster2014ICRA, + author = {Forster, Christian and Pizzoli, Matia and Scaramuzza, Davide}, + title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry}, + booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, + year = {2014} + } + + +#### Documentation +OpenCL documentation + +The API is documented here: http://uzh-rpg.github.io/rpg_svo/doc/ + +The GTSAM documentation https://gtsam.org/tutorials/ + +#### Instructions +Please be sure that you have the OpenCL driver installed + +First install gtsam library => https://github.com/borglab/gtsam + +Then follow the SVO instalation + +See the Wiki for more instructions. https://github.com/uzh-rpg/rpg_svo/wiki and https://gtsam.org/get_started/ + + +## Setting + +You can edit the algorithm parameters in the follwoing file, please look at the config file to underestand the meaning of the parameters (CPU_version/svo/include/svo/config.h) + +CPU_version/vio_svo/param/vo_fast.yaml + +# the GPU_version + +Dose not have dependency with GTSAM, the dependencies are: +1. OpenCV 3 +2. Eigen +3. Sophus https://github.com/strasdat/Sophus.git +4. Boost +5. OpenCL + +if make fails in the build of Sophus with the error lvalue required as left operand of assignment in so2.cpp, see this issue: https://github.com/uzh-rpg/rpg_svo/issues/237 + + + + diff --git a/GPU_version/vio/.idea/.gitignore b/GPU_version/vio/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/GPU_version/vio/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/GPU_version/vio/.idea/.name b/GPU_version/vio/.idea/.name new file mode 100644 index 0000000..19d75c0 --- /dev/null +++ b/GPU_version/vio/.idea/.name @@ -0,0 +1 @@ +vio \ No newline at end of file diff --git a/GPU_version/vio/.idea/misc.xml b/GPU_version/vio/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/GPU_version/vio/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/GPU_version/vio/.idea/modules.xml b/GPU_version/vio/.idea/modules.xml new file mode 100644 index 0000000..4c8cf2b --- /dev/null +++ b/GPU_version/vio/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/GPU_version/vio/.idea/svo.iml b/GPU_version/vio/.idea/svo.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/GPU_version/vio/.idea/svo.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/GPU_version/vio/.idea/vcs.xml b/GPU_version/vio/.idea/vcs.xml new file mode 100644 index 0000000..64713b8 --- /dev/null +++ b/GPU_version/vio/.idea/vcs.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt new file mode 100755 index 0000000..4644e2b --- /dev/null +++ b/GPU_version/vio/CMakeLists.txt @@ -0,0 +1,81 @@ +SET(PROJECT_NAME vio) +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) + +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_VERBOSE_MAKEFILE OFF) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") +set(CMAKE_C_STANDARD 17) +set(CMAKE_CXX_STANDARD 17) + +# Set definitions +ADD_DEFINITIONS(-DSVO_TRACE) +ADD_DEFINITIONS(-DSVO_USE_ROS) + + +# Set build flags, set ARM_ARCHITECTURE environment variable on Odroid +# Set build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +ENDIF() +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Add plain cmake packages +FIND_PACKAGE(OpenCV 3 REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system) +FIND_PACKAGE(OpenCL REQUIRED) + +set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}") +FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation) + +add_service_files(FILES + getOdom.srv + stop.srv + start.srv) + +generate_messages(DEPENDENCIES std_msgs) +catkin_package( + DEPENDS Eigen OpenCV Sophus Boost + CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime + INCLUDE_DIRS include +) +# Include dirs +INCLUDE_DIRECTORIES( + include + kernel + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${OpenCL_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries +LIST(APPEND LINK_LIBS + #gtsam + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + ${Boost_LIBRARIES} + ${OpenCL_LIBRARIES} + ${catkin_LIBRARIES} +) + +file(GLOB SRC CONFIGURE_DEPENDS "src/*.cpp") + + + +# Create svo library +ADD_EXECUTABLE(${PROJECT_NAME} ${SRC}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) +target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for) +ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\") + + diff --git a/GPU_version/vio/CMakeModules/FindEigen.cmake b/GPU_version/vio/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/GPU_version/vio/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# 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. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/GPU_version/vio/CMakeModules/FindG2O.cmake b/GPU_version/vio/CMakeModules/FindG2O.cmake new file mode 100644 index 0000000..655600f --- /dev/null +++ b/GPU_version/vio/CMakeModules/FindG2O.cmake @@ -0,0 +1,113 @@ +# Find the header files + +FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h + ${G2O_ROOT}/include + $ENV{G2O_ROOT}/include + $ENV{G2O_ROOT} + /usr/local/include + /usr/include + /opt/local/include + /sw/local/include + /sw/include + NO_DEFAULT_PATH + ) + +# Macro to unify finding both the debug and release versions of the +# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY +# macro. + +MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ${G2O_ROOT}/lib/Debug + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Debug + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ${G2O_ROOT}/lib/Release + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Release + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + IF(NOT ${MYLIBRARY}_DEBUG) + IF(MYLIBRARY) + SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) + ENDIF(MYLIBRARY) + ENDIF( NOT ${MYLIBRARY}_DEBUG) + +ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) + +# Find the core elements +FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) +FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) + +# Find the CLI library +FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) + +# Find the pluggable solvers +FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) +FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) +FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) +FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) +FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) +FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) + +# Find the predefined types +FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) +FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) +FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) +FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) + +# G2O solvers declared found if we found at least one solver +SET(G2O_SOLVERS_FOUND "NO") +IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + SET(G2O_SOLVERS_FOUND "YES") +ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + +# G2O itself declared found if we found the core libraries and at least one solver +SET(G2O_FOUND "NO") +IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) + SET(G2O_FOUND "YES") +ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) diff --git a/GPU_version/vio/Doxyfile b/GPU_version/vio/Doxyfile new file mode 100755 index 0000000..b1b4256 --- /dev/null +++ b/GPU_version/vio/Doxyfile @@ -0,0 +1,1782 @@ +# Doxyfile 1.7.6.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" "). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "svo" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = "Semi-Direct Visual Odometry" + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = "doc" + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = YES + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. +# This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = "src", "include/svo" + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = g2o +EXCLUDE_SYMBOLS += vk + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. +# If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. +# Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. +# The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. +# Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# style sheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = NO + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 4 + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = NO + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the +# mathjax.org site, so you can quickly see the result without installing +# MathJax, but it is strongly recommended to install a local copy of MathJax +# before deployment. + +MATHJAX_RELPATH = http://www.mathjax.org/mathjax + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvantages are that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = YES + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. +# This is useful +# if you want to understand what is going on. +# On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = YES + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES diff --git a/GPU_version/vio/doc/.gitignore b/GPU_version/vio/doc/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/GPU_version/vio/doc/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/GPU_version/vio/doc/notation.ipe b/GPU_version/vio/doc/notation.ipe new file mode 100644 index 0000000..f8c1efe --- /dev/null +++ b/GPU_version/vio/doc/notation.ipe @@ -0,0 +1,312 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +$_w\mathtt{p}$ +$\overrightarrow{\mathcal{F}}_w$ + +64 576 m +96 576 l + + +64 576 m +64 608 l + + +64 576 m +80 592 l + + +144 688 m +176 688 l + + +144 688 m +144 720 l + + +144 688 m +160 704 l + + +285.262 759.615 m +162.017 735.456 +129.232 628.801 s + + +$\mathtt{T}_{f,w}$ + +128.441 626.745 m +320.625 647.224 l + + +331.681 587.399 m +331.681 587.399 l + +$x$ +$z$ +$y$ + +256 736 m +224 704 l +320 704 l +352 736 l +h + + +285.262 759.265 m +322.176 651.119 l + +$\mathtt{px}$ + +285.612 758.915 m +315.723 669.632 l + +$\mathtt{f}$ +$\overrightarrow{\mathcal{F}}_f$ + +255.668 719.918 m +274.728 719.957 l + + +256.107 720.028 m +246.655 710.527 l + +$v$ +$u$ + +$\mathbf{p}$ +$_f\mathtt{p}$ + + diff --git a/GPU_version/vio/doc/notation.pdf b/GPU_version/vio/doc/notation.pdf new file mode 100644 index 0000000..3bc3fcd Binary files /dev/null and b/GPU_version/vio/doc/notation.pdf differ diff --git a/GPU_version/vio/doc/notation.png b/GPU_version/vio/doc/notation.png new file mode 100644 index 0000000..d613b70 Binary files /dev/null and b/GPU_version/vio/doc/notation.png differ diff --git a/GPU_version/vio/gtsam/imu_integration.cpp b/GPU_version/vio/gtsam/imu_integration.cpp new file mode 100755 index 0000000..d2e751b --- /dev/null +++ b/GPU_version/vio/gtsam/imu_integration.cpp @@ -0,0 +1,131 @@ +// +// Created by gfuse on 4/9/21. +// +//#define NDEBUG +#include "imu_integration.h" +#include +#include +#include "gpu_svo/feature.h" +#include "gpu_svo/point.h" +#include "gpu_svo/svoVisionFactor.h" +#include +#include +#include +#include +#include +#define debug 0 +Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ + graphPtr=std::make_shared(); + valuesPtr=std::make_shared(); + parameterPtr = boost::shared_ptr(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(0.0,0.0,0.0))); + parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous + parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous + parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous + parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC(); // acc bias in continuous + parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC(); // gyro bias in continuous + parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP(); // error in the bias used for preintegration + imu_biasPtr=std::make_shared(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8)); + preintegratedPtr = std::make_shared(parameterPtr,*imu_biasPtr); + ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection + optimizerParamPtr=std::make_shared(); + optimizerParamPtr->relinearizeThreshold = 0.1; + optimizerParamPtr->relinearizeSkip = 1; + optimizerParamPtr->evaluateNonlinearError=true; + optimizerParamPtr->enableDetailedResults=true; + if(debug)logtime = fopen((std::string(getenv("HOME"))+"/imu_preIntegrate.txt").c_str(), "w+"); + assert(preintegratedPtr); +} +Imu_Integration::~Imu_Integration(){ + +} +bool Imu_Integration::update(double* imu){ + if(syn)return false; + if(imu_factor_id<1)return false; + auto in=std::chrono::steady_clock::now(); + if(std::chrono::duration(in-t_1).count()>20.0){ + t_1=in; + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[2]-svo::Config::Teta_off(),0.0),0.0005); + }else{ + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[2]-svo::Config::Teta_off(),0.0),std::chrono::duration(in-t_1).count()); + t_1=in; + ++imu_n; + } + return true; + +} +bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.optimize(); + statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); + imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + ++imu_factor_id; + syn=false; + gtsam::Marginals marginals(*graphPtr,result); + if(debug)fprintf(logtime, "%d,%f,%f,%f,%f,\n", + imu_factor_id-1, + statePtr->pose().x(), + statePtr->pose().y(), + statePtr->pose().z(), + statePtr->pose().rotation().pitch()); + new_frame->T_f_w_=svo::SE2_5(statePtr->pose().x(),statePtr->pose().z(),statePtr->pose().rotation().pitch()); + new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1)); + graphPtr->resize(0); + valuesPtr->clear(); + valuesPtr->insert(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix())); + valuesPtr->insert(V(imu_factor_id-1),statePtr->v()); + valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr); + graphPtr->addPrior(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()),marginals.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1))); + return true; +} +bool Imu_Integration::preintegrate_predict(svo::SE2_5& frame) { + if(imu_n<1){ + syn= false; + return false; + }else{ + syn=true; + } + Integ_predict=preintegratedPtr->predict(*statePtr,*imu_biasPtr); + frame =svo::SE2_5(Integ_predict.pose().x(),Integ_predict.pose().z(),Integ_predict.pose().rotation().pitch()); + return true; +} +void Imu_Integration::reset() { + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + imu_n=0; + syn=false; +} +bool Imu_Integration::predict(boost::shared_ptr& new_frame, + boost::shared_ptr& last_frame, + const double cell_match){ + valuesPtr->insert(P(imu_factor_id), Integ_predict.pose()); + valuesPtr->insert(V(imu_factor_id), Integ_predict.v()); + valuesPtr->insert(B(imu_factor_id), *imu_biasPtr); + gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id), + *preintegratedPtr); + graphPtr->emplace_shared(imu_factor); + imu_n=0; + graphPtr->emplace_shared(gtsam::Pose3(new_frame->T_f_w_.T->matrix()), + gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(3) << 1.0, 1.0, 1.0).finished()/(cell_match+1)), + P(imu_factor_id)); + gtsam::LevenbergMarquardtParams params0; + params0.verbosityLM = gtsam::LevenbergMarquardtParams::SILENT; + params0.setlambdaFactor(10.0); + params0.setlambdaInitial(100); + params0.setlambdaUpperBound(1e10); + gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0); + reset(optimizer,new_frame); + return true; +} +bool Imu_Integration::init(boost::shared_ptr& new_frame){ + statePtr=std::make_shared(gtsam::Pose3(new_frame->T_f_w_.T->matrix()), + gtsam::Vector3(1e-9,1e-9,1e-9)); + valuesPtr->insert(P(0),statePtr->pose()); + valuesPtr->insert(V(0),statePtr->v()); + valuesPtr->insert(B(0),*imu_biasPtr); + graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-5, 1e-15, 1e-5, 1e-15, 1e-8, 1e-15).finished())); + graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); + graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); + ++imu_factor_id; + return true; +} \ No newline at end of file diff --git a/GPU_version/vio/gtsam/imu_integration.h b/GPU_version/vio/gtsam/imu_integration.h new file mode 100644 index 0000000..11eb514 --- /dev/null +++ b/GPU_version/vio/gtsam/imu_integration.h @@ -0,0 +1,50 @@ +// +// Created by gufe on 4/9/21. +// + +#ifndef SVO_IMU_INTEGRATION_H +#define SVO_IMU_INTEGRATION_H +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using gtsam::symbol_shorthand::P; // Pose3 (x,y,z,r,p,y) +using gtsam::symbol_shorthand::L; // Landmark + +class Imu_Integration{ +public: + Imu_Integration(Sophus::SE3& SE_init); + ~Imu_Integration(); + void reset(); + bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame); + bool update(double* imu= nullptr); + bool predict(boost::shared_ptr&, + boost::shared_ptr& last_frame ,const double match); + bool init(boost::shared_ptr&); + bool preintegrate_predict(svo::SE2_5& frame); +private: + FILE* logtime; + std::shared_ptr statePtr; + std::shared_ptr imu_biasPtr; + std::shared_ptr valuesPtr; + std::shared_ptr graphPtr; + boost::shared_ptr parameterPtr; + std::shared_ptr preintegratedPtr; + boost::shared_ptr ProjectNoisePtr; + std::chrono::steady_clock::time_point t_1; + std::uint32_t imu_factor_id=0; + uint imu_n=0; + std::shared_ptr optimizerParamPtr; + gtsam::NavState Integ_predict; + bool syn=false; +}; +#endif //SVO_IMU_INTEGRATION_H diff --git a/GPU_version/vio/gtsam/svoVisionFactor.h b/GPU_version/vio/gtsam/svoVisionFactor.h new file mode 100644 index 0000000..01ad6da --- /dev/null +++ b/GPU_version/vio/gtsam/svoVisionFactor.h @@ -0,0 +1,64 @@ +// +// Created by root on 4/13/21. +// + +#ifndef SVO_SVOVISIONFACTOR_H +#define SVO_SVOVISIONFACTOR_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactor: public gtsam::NoiseModelFactor1 { + + private: + // measurement information + gtsam::Pose3 pose; /// shared_ptr; + + /** + * Constructor + * @param pose Camera pose. + * @param pointKey is the index of the landmark + */ + VisionFactor(const gtsam::Pose3& pose, + const gtsam::SharedNoiseModel& H, + gtsam::Key poseKey) : + gtsam::NoiseModelFactor1(H, poseKey), pose(pose){} + virtual ~VisionFactor(){} + + /// error function + /// @param p the Estimated pose in Pose3 + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p + ,boost::optional H1 = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + Eigen::Matrix E_r=0.5*(p.rotation().matrix().transpose()*pose.rotation().matrix()-pose.rotation().matrix().transpose()*p.rotation().matrix()); + Vector3d e_t=p.translation()-pose.translation(); + if(H1)(*H1) = (gtsam::Matrix(3,6)<< 1,0,0,0,0,0, + 0,0,1,0,0,0, + 0,0,0,0,1,0).finished(); + return (gtsam::Matrix(3,1)<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/GPU_version/vio/gtsam/svoVisionFactorLandMark.h b/GPU_version/vio/gtsam/svoVisionFactorLandMark.h new file mode 100644 index 0000000..69ed0c8 --- /dev/null +++ b/GPU_version/vio/gtsam/svoVisionFactorLandMark.h @@ -0,0 +1,87 @@ +// +// Created by root on 4/13/21. +// + +#ifndef SVO_VisionFactorLandMark_H +#define SVO_VisionFactorLandMark_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactorLandMark: public gtsam::NoiseModelFactor3 { + + private: + // measurement information + gtsam::Point3 feature_new; /// shared_ptr; + + /** + * Constructor + * @param feature Unit-bearing vector of the feature. + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param poseKey is the index of the camera pose + * @param landmark 3D position of the landmark + * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param level Image pyramid level where feature was extracted. + */ + VisionFactorLandMark(const gtsam::Point3& feature_new,const gtsam::Point3& feature_ref, int level, + const gtsam::SharedNoiseModel& H, + gtsam::Key poseKey_ref, gtsam::Key poseKey_new,gtsam::Key landmarkKey ) : + gtsam::NoiseModelFactor3(H, poseKey_ref,poseKey_new,landmarkKey), feature_new(feature_new), + feature_ref(feature_ref),level(level){} + virtual ~VisionFactorLandMark(){} + + /// error function + /// @param p_new the Estimated pose in Pose3 + /// @param P_ref the Estimated point of landmark + /// @param l the Estimated point of landmark + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p_ref,const gtsam::Pose3& p_new, const gtsam::Point3& l, + boost::optional H1 = boost::none,boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + gtsam::Matrix26 J,J1; + gtsam::Matrix23 J2,J3; + + gtsam::Point3 point(p_ref*l); + svo::Frame::jacobian_xyz2uv(point, J); + svo::Frame::jacobian_l2uv(point,J2); + + gtsam::Point3 point2(p_new*l); + svo::Frame::jacobian_xyz2uv(point2, J1); + svo::Frame::jacobian_l2uv(point2,J3); + Vector2d e1=vk::project2d(feature_ref) - vk::project2d(point)*1.0 / (1<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactorLandMark(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/GPU_version/vio/gtsam/svoVisionFactorSmall.h b/GPU_version/vio/gtsam/svoVisionFactorSmall.h new file mode 100644 index 0000000..738bd95 --- /dev/null +++ b/GPU_version/vio/gtsam/svoVisionFactorSmall.h @@ -0,0 +1,70 @@ +// +// Created by root on 4/13/21. +// + +#ifndef SVO_SVOVISIONFACTOR_H +#define SVO_SVOVISIONFACTOR_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactor: public gtsam::NoiseModelFactor1 { + + private: + // measurement information + gtsam::Point3 feature; /// body_P_sensor_; ///< TODO The pose of the sensor in the body frame + int level; ///< Image pyramid level where feature was extracted. + + public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param feature Unit-bearing vector of the feature. + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param poseKey is the index of the camera pose + * @param landmark 3D position of the landmark + * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param level Image pyramid level where feature was extracted. + */ + VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp, + gtsam::Key poseKey,boost::optional body_P_sensor = boost::none) : + gtsam::NoiseModelFactor1(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){} + virtual ~VisionFactor(){} + + /// error function + /// @param p the Estimated pose in Pose3 + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional Hp = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + gtsam::Matrix26 J; + gtsam::Point3 point(p*landmark); + svo::Frame::jacobian_xyz2uv(point, J); + Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/GPU_version/vio/include/gpu_svo/abstract_camera.h b/GPU_version/vio/include/gpu_svo/abstract_camera.h new file mode 100644 index 0000000..5d643dc --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/abstract_camera.h @@ -0,0 +1,77 @@ +/* + * abstract_camera.h + * + * Created on: Jul 23, 2012 + * Author: cforster + */ + +#ifndef ABSTRACT_CAMERA_H_ +#define ABSTRACT_CAMERA_H_ + +#include + +namespace vk +{ + +using namespace std; +using namespace Eigen; + +class AbstractCamera +{ +protected: + + int width_; // TODO cannot be const because of omni-camera model + int height_; + +public: + + AbstractCamera() {}; // need this constructor for omni camera + AbstractCamera(int width, int height) : width_(width), height_(height) {}; + + virtual ~AbstractCamera() {}; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const double& x, const double& y) const = 0; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const Vector2d& px) const = 0; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const = 0; + + /// projects unit plane coordinates to camera coordinates + virtual Vector2d + world2cam(const Vector2d& uv) const = 0; + + virtual double + errorMultiplier2() const = 0; + + virtual double + errorMultiplier() const = 0; + + inline int width() const { return width_; } + + inline int height() const { return height_; } + + inline bool isInFrame(const Vector2i & obs, int boundary=0) const + { + if(obs[0]>=boundary && obs[0]=boundary && obs[1]= boundary && obs[0] < width()/(1<= boundary && obs[1] // memset +#include // memset +#include +#include + +namespace vk { +namespace aligned_mem { + + /// Check if the pointer is aligned to the specified byte granularity + inline bool + is_aligned8(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0x7) == 0; + } + + inline bool + is_aligned16(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0xF) == 0; + } + + template struct placement_delete + { + enum { Size = (1<= Size) { + placement_delete::free(buf+Size,M-Size); + placement_delete::destruct(buf); + } else { + placement_delete::free(buf, M); + } + } + }; + + template struct placement_delete + { + static inline void free(T*, size_t ) {} + }; + + inline void * aligned_alloc(size_t count, size_t alignment){ + void * mem = NULL; + assert(posix_memalign(&mem, alignment, count) == 0); + return mem; + } + + inline void aligned_free(void * memory) { + free(memory); + } + + template + inline T * aligned_alloc(size_t count, size_t alignment){ + void * data = aligned_alloc(sizeof(T)* count, alignment); + return new (data) T[count]; + } + + template + inline void aligned_free(T * memory, size_t count){ + placement_delete::free(memory, count); + aligned_free(memory); + } + + template inline void memfill(T* data, int n, const T val) + { + T* de = data + n; + for(;data < de; data++) + *data=val; + } + + template<> inline void memfill(unsigned char* data, int n, const unsigned char val) + { + memset(data, val, n); + } + + template<> inline void memfill(signed char* data, int n, const signed char val) + { + memset(data, val, n); + } + + template<> inline void memfill(char* data, int n, const char val) + { + memset(data, val, n); + } + + template + struct AlignedMem { + T* mem; + size_t count; + AlignedMem(size_t c) : count(c) { + mem = aligned_alloc(count, N); + } + ~AlignedMem() { + aligned_free(mem, count); + } + T* data() { return mem; } + const T* data() const { return mem; } + }; + +} // namespace aligned_mem +} // namespace vikit + + +#endif // VIKIT_ALIGNED_MEM_H_ diff --git a/GPU_version/vio/include/gpu_svo/atan_camera.h b/GPU_version/vio/include/gpu_svo/atan_camera.h new file mode 100644 index 0000000..49550b2 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/atan_camera.h @@ -0,0 +1,91 @@ +/* + * atan_camera.h + * + * Created on: Aug 21, 2012 + * Author: cforster + * + * This class implements the FOV distortion model of Deverneay and Faugeras, + * Straight lines have to be straight, 2001. + * + * The code is an implementation of the ATAN class in PTAM by Georg Klein using Eigen. + */ + +#ifndef ATAN_CAMERA_H_ +#define ATAN_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class ATANCamera : public AbstractCamera { + +private: + double fx_, fy_; //!< focal length + double fx_inv_, fy_inv_; //!< inverse focal length + double cx_, cy_; //!< projection center + double s_, s_inv_; //!< distortion model coeff + double tans_; //!< distortion model coeff + double tans_inv_; //!< distortion model coeff + bool distortion_; //!< use distortion model? + + //! Radial distortion transformation factor: returns ration of distorted / undistorted radius. + inline double rtrans_factor(double r) const + { + if(r < 0.001 || s_ == 0.0) + return 1.0; + else + return (s_inv_* atan(r * tans_) / r); + }; + + //! Inverse radial distortion: returns un-distorted radius from distorted. + inline double invrtrans(double r) const + { + if(s_ == 0.0) + return r; + return (tan(r * s_) * tans_inv_); + }; + +public: + + ATANCamera(double width, double height, double fx, double fy, double dx, double dy, double s); + + ~ATANCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + virtual double errorMultiplier2() const + { + return fx_; + } + + virtual double errorMultiplier() const + { + return 4*fx_*fy_; + } +}; + +} // end namespace vk + +#endif /* ATAN_CAMERA_H_ */ diff --git a/GPU_version/vio/include/gpu_svo/bundle_adjustment.h b/GPU_version/vio/include/gpu_svo/bundle_adjustment.h new file mode 100644 index 0000000..9eb8a94 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/bundle_adjustment.h @@ -0,0 +1,109 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_BUNDLE_ADJUSTMENT_H_ +#define SVO_BUNDLE_ADJUSTMENT_H_ + +#include + +namespace g2o { +class EdgeProjectXYZ2UV; +class SparseOptimizer; +class VertexSE3Expmap; +class VertexSBAPointXYZ; +} + +namespace svo { + +typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3; +typedef g2o::VertexSE3Expmap g2oFrameSE3; +typedef g2o::VertexSBAPointXYZ g2oPoint; + +class Frame; +class Point; +class Feature; +class Map; + +/// Local, global and 2-view bundle adjustment with g2o +namespace ba { + +/// Temporary container to hold the g2o edge with reference to frame and point. +struct EdgeContainerSE3 +{ + g2oEdgeSE3* edge; + Frame* frame; + Feature* feature; + bool is_deleted; + EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) : + edge(e), frame(frame), feature(feature), is_deleted(false) + {} +}; + +/// Optimize two camera frames and their observed 3D points. +/// Is used after initialization. +void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map); + +/// Local bundle adjustment. +/// Optimizes core_kfs and their observed map points while keeping the +/// neighbourhood fixed. +void localBA( + Frame* center_kf, + set* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error); + +/// Global bundle adjustment. +/// Optimizes the whole map. Is currently not used in SVO. +void globalBA(Map* map); + +/// Initialize g2o with solver type, optimization strategy and camera model. +void setupG2o(g2o::SparseOptimizer * optimizer); + +/// Run the optimization on the provided graph. +void runSparseBAOptimizer( + g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, + double& final_error); + +/// Create a g2o vertice from a keyframe object. +g2oFrameSE3* createG2oFrameSE3( + Frame* kf, + size_t id, + bool fixed); + +/// Creates a g2o vertice from a mappoint object. +g2oPoint* createG2oPoint( + Vector3d pos, + size_t id, + bool fixed); + +/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. +g2oEdgeSE3* createG2oEdgeSE3( + g2oFrameSE3* v_kf, + g2oPoint* v_mp, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight = 1); + +} // namespace ba +} // namespace svo + +#endif // SVO_BUNDLE_ADJUSTMENT_H_ diff --git a/GPU_version/vio/include/gpu_svo/camera_loader.h b/GPU_version/vio/include/gpu_svo/camera_loader.h new file mode 100644 index 0000000..e9d5a94 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/camera_loader.h @@ -0,0 +1,46 @@ +/* + * camera_loader.h + * + * Created on: Feb 11, 2014 + * Author: cforster + */ + +#ifndef VIKIT_CAMERA_LOADER_H_ +#define VIKIT_CAMERA_LOADER_H_ + +#include +#include +#include +#include + +namespace vk { +namespace camera_loader { + +/// Load from ROS Namespace +bool loadFromRosNs(const std::string& ns, vk::AbstractCamera*& cam) +{ + bool res = true; + std::string cam_model(getParam(ns+"/cam_model")); + if(cam_model == "ATAN") + { + cam = new vk::ATANCamera( + getParam(ns+"/cam_width"), + getParam(ns+"/cam_height"), + getParam(ns+"/cam_fx"), + getParam(ns+"/cam_fy"), + getParam(ns+"/cam_cx"), + getParam(ns+"/cam_cy"), + getParam(ns+"/cam_d0")); + } + else + { + cam = NULL; + res = false; + } + return res; +} + +} // namespace camera_loader +} // namespace vk + +#endif // VIKIT_CAMERA_LOADER_H_ diff --git a/GPU_version/vio/include/gpu_svo/cl.hpp b/GPU_version/vio/include/gpu_svo/cl.hpp new file mode 100644 index 0000000..6634f8c --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/cl.hpp @@ -0,0 +1,12947 @@ +/******************************************************************************* + * Copyright (c) 2008-2015 The Khronos Group Inc. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and/or associated documentation files (the + * "Materials"), to deal in the Materials without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Materials, and to + * permit persons to whom the Materials are 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 Materials. + * + * MODIFICATIONS TO THIS FILE MAY MEAN IT NO LONGER ACCURATELY REFLECTS + * KHRONOS STANDARDS. THE UNMODIFIED, NORMATIVE VERSIONS OF KHRONOS + * SPECIFICATIONS AND HEADER INFORMATION ARE LOCATED AT + * https://www.khronos.org/registry/ + * + * THE MATERIALS ARE 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 + * MATERIALS OR THE USE OR OTHER DEALINGS IN THE MATERIALS. + ******************************************************************************/ + +/*! \file + * + * \brief C++ bindings for OpenCL 1.0 (rev 48), OpenCL 1.1 (rev 33) and + * OpenCL 1.2 (rev 15) + * \author Benedict R. Gaster, Laurent Morichetti and Lee Howes + * + * Additions and fixes from: + * Brian Cole, March 3rd 2010 and April 2012 + * Matt Gruenke, April 2012. + * Bruce Merry, February 2013. + * Tom Deakin and Simon McIntosh-Smith, July 2013 + * + * \version 1.2.9 + * \date December 2015 + * + * Optional extension support + * + * cl + * cl_ext_device_fission + * #define USE_CL_DEVICE_FISSION + */ + +/*! \mainpage + * \section intro Introduction + * For many large applications C++ is the language of choice and so it seems + * reasonable to define C++ bindings for OpenCL. + * + * + * The interface is contained with a single C++ header file \em cl.hpp and all + * definitions are contained within the namespace \em cl. There is no additional + * requirement to include \em cl.h and to use either the C++ or original C + * bindings it is enough to simply include \em cl.hpp. + * + * The bindings themselves are lightweight and correspond closely to the + * underlying C API. Using the C++ bindings introduces no additional execution + * overhead. + * + * For detail documentation on the bindings see: + * + * The OpenCL C++ Wrapper API 1.2 (revision 09) + * http://www.khronos.org/registry/cl/specs/opencl-cplusplus-1.2.pdf + * + * \section example Example + * + * The following example shows a general use case for the C++ + * bindings, including support for the optional exception feature and + * also the supplied vector and string classes, see following sections for + * decriptions of these features. + * + * \code + * #define __CL_ENABLE_EXCEPTIONS + * + * #if defined(__APPLE__) || defined(__MACOSX) + * #include + * #else + * #include + * #endif + * #include + * #include + * #include + * + * const char * helloStr = "__kernel void " + * "hello(void) " + * "{ " + * " " + * "} "; + * + * int + * main(void) + * { + * cl_int err = CL_SUCCESS; + * try { + * + * std::vector platforms; + * cl::Platform::get(&platforms); + * if (platforms.size() == 0) { + * std::cout << "Platform size 0\n"; + * return -1; + * } + * + * cl_context_properties properties[] = + * { CL_CONTEXT_PLATFORM, (cl_context_properties)(platforms[0])(), 0}; + * cl::Context context(CL_DEVICE_TYPE_CPU, properties); + * + * std::vector devices = context.getInfo(); + * + * cl::Program::Sources source(1, + * std::make_pair(helloStr,strlen(helloStr))); + * cl::Program program_ = cl::Program(context, source); + * program_.build(devices); + * + * cl::Kernel kernel(program_, "hello", &err); + * + * cl::Event event; + * cl::CommandQueue queue(context, devices[0], 0, &err); + * queue.enqueueNDRangeKernel( + * kernel, + * cl::NullRange, + * cl::NDRange(4,4), + * cl::NullRange, + * NULL, + * &event); + * + * event.wait(); + * } + * catch (cl::Error err) { + * std::cerr + * << "ERROR: " + * << err.what() + * << "(" + * << err.err() + * << ")" + * << std::endl; + * } + * + * return EXIT_SUCCESS; + * } + * + * \endcode + * + */ +#ifndef CL_HPP_ +#define CL_HPP_ + +#ifdef _WIN32 + +#include + +#if defined(USE_DX_INTEROP) +#include +#include +#endif +#endif // _WIN32 + +#if defined(_MSC_VER) +#include +#endif // _MSC_VER + +// +#if defined(USE_CL_DEVICE_FISSION) +#include +#endif + +#if defined(__APPLE__) || defined(__MACOSX) +#include +#else +#include +#endif // !__APPLE__ + +#if (_MSC_VER >= 1700) || (__cplusplus >= 201103L) +#define CL_HPP_RVALUE_REFERENCES_SUPPORTED +#define CL_HPP_CPP11_ATOMICS_SUPPORTED +#include +#endif + +#if (__cplusplus >= 201103L) +#define CL_HPP_NOEXCEPT noexcept +#else +#define CL_HPP_NOEXCEPT +#endif + + +// To avoid accidentally taking ownership of core OpenCL types +// such as cl_kernel constructors are made explicit +// under OpenCL 1.2 +#if defined(CL_VERSION_1_2) && !defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) +#define __CL_EXPLICIT_CONSTRUCTORS explicit +#else // #if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) +#define __CL_EXPLICIT_CONSTRUCTORS +#endif // #if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + +// Define deprecated prefixes and suffixes to ensure compilation +// in case they are not pre-defined +#if !defined(CL_EXT_PREFIX__VERSION_1_1_DEPRECATED) +#define CL_EXT_PREFIX__VERSION_1_1_DEPRECATED +#endif // #if !defined(CL_EXT_PREFIX__VERSION_1_1_DEPRECATED) +#if !defined(CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED) +#define CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED +#endif // #if !defined(CL_EXT_PREFIX__VERSION_1_1_DEPRECATED) + +#if !defined(CL_CALLBACK) +#define CL_CALLBACK +#endif //CL_CALLBACK + +#include +#include +#include + +#if defined(__CL_ENABLE_EXCEPTIONS) +#include +#endif // #if defined(__CL_ENABLE_EXCEPTIONS) + +#if !defined(__NO_STD_VECTOR) +#include +#endif + +#if !defined(__NO_STD_STRING) +#include +#endif + +#if defined(__ANDROID__) || defined(linux) || defined(__APPLE__) || defined(__MACOSX) +#include +#endif // linux + +#include + + +/*! \namespace cl + * + * \brief The OpenCL C++ bindings are defined within this namespace. + * + */ +namespace cl { + +class Memory; + +/** + * Deprecated APIs for 1.2 + */ +#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) || (defined(CL_VERSION_1_1) && !defined(CL_VERSION_1_2)) +#define __INIT_CL_EXT_FCN_PTR(name) \ + if(!pfn_##name) { \ + pfn_##name = (PFN_##name) \ + clGetExtensionFunctionAddress(#name); \ + if(!pfn_##name) { \ + } \ + } +#endif // #if defined(CL_VERSION_1_1) + +#if defined(CL_VERSION_1_2) +#define __INIT_CL_EXT_FCN_PTR_PLATFORM(platform, name) \ + if(!pfn_##name) { \ + pfn_##name = (PFN_##name) \ + clGetExtensionFunctionAddressForPlatform(platform, #name); \ + if(!pfn_##name) { \ + } \ + } +#endif // #if defined(CL_VERSION_1_1) + +class Program; +class Device; +class Context; +class CommandQueue; +class Memory; +class Buffer; + +#if defined(__CL_ENABLE_EXCEPTIONS) +/*! \brief Exception class + * + * This may be thrown by API functions when __CL_ENABLE_EXCEPTIONS is defined. + */ +class Error : public std::exception +{ +private: + cl_int err_; + const char * errStr_; +public: + /*! \brief Create a new CL error exception for a given error code + * and corresponding message. + * + * \param err error code value. + * + * \param errStr a descriptive string that must remain in scope until + * handling of the exception has concluded. If set, it + * will be returned by what(). + */ + Error(cl_int err, const char * errStr = NULL) : err_(err), errStr_(errStr) + {} + + ~Error() throw() {} + + /*! \brief Get error string associated with exception + * + * \return A memory pointer to the error message string. + */ + virtual const char * what() const throw () + { + if (errStr_ == NULL) { + return "empty"; + } + else { + return errStr_; + } + } + + /*! \brief Get error code associated with exception + * + * \return The error code. + */ + cl_int err(void) const { return err_; } +}; + +#define __ERR_STR(x) #x +#else +#define __ERR_STR(x) NULL +#endif // __CL_ENABLE_EXCEPTIONS + + +namespace detail +{ +#if defined(__CL_ENABLE_EXCEPTIONS) +static inline cl_int errHandler ( + cl_int err, + const char * errStr = NULL) +{ + if (err != CL_SUCCESS) { + throw Error(err, errStr); + } + return err; +} +#else +static inline cl_int errHandler (cl_int err, const char * errStr = NULL) +{ + (void) errStr; // suppress unused variable warning + return err; +} +#endif // __CL_ENABLE_EXCEPTIONS +} + + + +//! \cond DOXYGEN_DETAIL +#if !defined(__CL_USER_OVERRIDE_ERROR_STRINGS) +#define __GET_DEVICE_INFO_ERR __ERR_STR(clGetDeviceInfo) +#define __GET_PLATFORM_INFO_ERR __ERR_STR(clGetPlatformInfo) +#define __GET_DEVICE_IDS_ERR __ERR_STR(clGetDeviceIDs) +#define __GET_PLATFORM_IDS_ERR __ERR_STR(clGetPlatformIDs) +#define __GET_CONTEXT_INFO_ERR __ERR_STR(clGetContextInfo) +#define __GET_EVENT_INFO_ERR __ERR_STR(clGetEventInfo) +#define __GET_EVENT_PROFILE_INFO_ERR __ERR_STR(clGetEventProfileInfo) +#define __GET_MEM_OBJECT_INFO_ERR __ERR_STR(clGetMemObjectInfo) +#define __GET_IMAGE_INFO_ERR __ERR_STR(clGetImageInfo) +#define __GET_SAMPLER_INFO_ERR __ERR_STR(clGetSamplerInfo) +#define __GET_KERNEL_INFO_ERR __ERR_STR(clGetKernelInfo) +#if defined(CL_VERSION_1_2) +#define __GET_KERNEL_ARG_INFO_ERR __ERR_STR(clGetKernelArgInfo) +#endif // #if defined(CL_VERSION_1_2) +#define __GET_KERNEL_WORK_GROUP_INFO_ERR __ERR_STR(clGetKernelWorkGroupInfo) +#define __GET_PROGRAM_INFO_ERR __ERR_STR(clGetProgramInfo) +#define __GET_PROGRAM_BUILD_INFO_ERR __ERR_STR(clGetProgramBuildInfo) +#define __GET_COMMAND_QUEUE_INFO_ERR __ERR_STR(clGetCommandQueueInfo) + +#define __CREATE_CONTEXT_ERR __ERR_STR(clCreateContext) +#define __CREATE_CONTEXT_FROM_TYPE_ERR __ERR_STR(clCreateContextFromType) +#define __GET_SUPPORTED_IMAGE_FORMATS_ERR __ERR_STR(clGetSupportedImageFormats) + +#define __CREATE_BUFFER_ERR __ERR_STR(clCreateBuffer) +#define __COPY_ERR __ERR_STR(cl::copy) +#define __CREATE_SUBBUFFER_ERR __ERR_STR(clCreateSubBuffer) +#define __CREATE_GL_BUFFER_ERR __ERR_STR(clCreateFromGLBuffer) +#define __CREATE_GL_RENDER_BUFFER_ERR __ERR_STR(clCreateFromGLBuffer) +#define __GET_GL_OBJECT_INFO_ERR __ERR_STR(clGetGLObjectInfo) +#if defined(CL_VERSION_1_2) +#define __CREATE_IMAGE_ERR __ERR_STR(clCreateImage) +#define __CREATE_GL_TEXTURE_ERR __ERR_STR(clCreateFromGLTexture) +#define __IMAGE_DIMENSION_ERR __ERR_STR(Incorrect image dimensions) +#endif // #if defined(CL_VERSION_1_2) +#define __CREATE_SAMPLER_ERR __ERR_STR(clCreateSampler) +#define __SET_MEM_OBJECT_DESTRUCTOR_CALLBACK_ERR __ERR_STR(clSetMemObjectDestructorCallback) + +#define __CREATE_USER_EVENT_ERR __ERR_STR(clCreateUserEvent) +#define __SET_USER_EVENT_STATUS_ERR __ERR_STR(clSetUserEventStatus) +#define __SET_EVENT_CALLBACK_ERR __ERR_STR(clSetEventCallback) +#define __WAIT_FOR_EVENTS_ERR __ERR_STR(clWaitForEvents) + +#define __CREATE_KERNEL_ERR __ERR_STR(clCreateKernel) +#define __SET_KERNEL_ARGS_ERR __ERR_STR(clSetKernelArg) +#define __CREATE_PROGRAM_WITH_SOURCE_ERR __ERR_STR(clCreateProgramWithSource) +#define __CREATE_PROGRAM_WITH_BINARY_ERR __ERR_STR(clCreateProgramWithBinary) +#if defined(CL_VERSION_1_2) +#define __CREATE_PROGRAM_WITH_BUILT_IN_KERNELS_ERR __ERR_STR(clCreateProgramWithBuiltInKernels) +#endif // #if defined(CL_VERSION_1_2) +#define __BUILD_PROGRAM_ERR __ERR_STR(clBuildProgram) +#if defined(CL_VERSION_1_2) +#define __COMPILE_PROGRAM_ERR __ERR_STR(clCompileProgram) +#define __LINK_PROGRAM_ERR __ERR_STR(clLinkProgram) +#endif // #if defined(CL_VERSION_1_2) +#define __CREATE_KERNELS_IN_PROGRAM_ERR __ERR_STR(clCreateKernelsInProgram) + +#define __CREATE_COMMAND_QUEUE_ERR __ERR_STR(clCreateCommandQueue) +#define __SET_COMMAND_QUEUE_PROPERTY_ERR __ERR_STR(clSetCommandQueueProperty) +#define __ENQUEUE_READ_BUFFER_ERR __ERR_STR(clEnqueueReadBuffer) +#define __ENQUEUE_READ_BUFFER_RECT_ERR __ERR_STR(clEnqueueReadBufferRect) +#define __ENQUEUE_WRITE_BUFFER_ERR __ERR_STR(clEnqueueWriteBuffer) +#define __ENQUEUE_WRITE_BUFFER_RECT_ERR __ERR_STR(clEnqueueWriteBufferRect) +#define __ENQEUE_COPY_BUFFER_ERR __ERR_STR(clEnqueueCopyBuffer) +#define __ENQEUE_COPY_BUFFER_RECT_ERR __ERR_STR(clEnqueueCopyBufferRect) +#define __ENQUEUE_FILL_BUFFER_ERR __ERR_STR(clEnqueueFillBuffer) +#define __ENQUEUE_READ_IMAGE_ERR __ERR_STR(clEnqueueReadImage) +#define __ENQUEUE_WRITE_IMAGE_ERR __ERR_STR(clEnqueueWriteImage) +#define __ENQUEUE_COPY_IMAGE_ERR __ERR_STR(clEnqueueCopyImage) +#define __ENQUEUE_FILL_IMAGE_ERR __ERR_STR(clEnqueueFillImage) +#define __ENQUEUE_COPY_IMAGE_TO_BUFFER_ERR __ERR_STR(clEnqueueCopyImageToBuffer) +#define __ENQUEUE_COPY_BUFFER_TO_IMAGE_ERR __ERR_STR(clEnqueueCopyBufferToImage) +#define __ENQUEUE_MAP_BUFFER_ERR __ERR_STR(clEnqueueMapBuffer) +#define __ENQUEUE_MAP_IMAGE_ERR __ERR_STR(clEnqueueMapImage) +#define __ENQUEUE_UNMAP_MEM_OBJECT_ERR __ERR_STR(clEnqueueUnMapMemObject) +#define __ENQUEUE_NDRANGE_KERNEL_ERR __ERR_STR(clEnqueueNDRangeKernel) +#define __ENQUEUE_TASK_ERR __ERR_STR(clEnqueueTask) +#define __ENQUEUE_NATIVE_KERNEL __ERR_STR(clEnqueueNativeKernel) +#if defined(CL_VERSION_1_2) +#define __ENQUEUE_MIGRATE_MEM_OBJECTS_ERR __ERR_STR(clEnqueueMigrateMemObjects) +#endif // #if defined(CL_VERSION_1_2) + +#define __ENQUEUE_ACQUIRE_GL_ERR __ERR_STR(clEnqueueAcquireGLObjects) +#define __ENQUEUE_RELEASE_GL_ERR __ERR_STR(clEnqueueReleaseGLObjects) + + +#define __RETAIN_ERR __ERR_STR(Retain Object) +#define __RELEASE_ERR __ERR_STR(Release Object) +#define __FLUSH_ERR __ERR_STR(clFlush) +#define __FINISH_ERR __ERR_STR(clFinish) +#define __VECTOR_CAPACITY_ERR __ERR_STR(Vector capacity error) + +/** + * CL 1.2 version that uses device fission. + */ +#if defined(CL_VERSION_1_2) +#define __CREATE_SUB_DEVICES __ERR_STR(clCreateSubDevices) +#else +#define __CREATE_SUB_DEVICES __ERR_STR(clCreateSubDevicesEXT) +#endif // #if defined(CL_VERSION_1_2) + +/** + * Deprecated APIs for 1.2 + */ +#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) || (defined(CL_VERSION_1_1) && !defined(CL_VERSION_1_2)) +#define __ENQUEUE_MARKER_ERR __ERR_STR(clEnqueueMarker) +#define __ENQUEUE_WAIT_FOR_EVENTS_ERR __ERR_STR(clEnqueueWaitForEvents) +#define __ENQUEUE_BARRIER_ERR __ERR_STR(clEnqueueBarrier) +#define __UNLOAD_COMPILER_ERR __ERR_STR(clUnloadCompiler) +#define __CREATE_GL_TEXTURE_2D_ERR __ERR_STR(clCreateFromGLTexture2D) +#define __CREATE_GL_TEXTURE_3D_ERR __ERR_STR(clCreateFromGLTexture3D) +#define __CREATE_IMAGE2D_ERR __ERR_STR(clCreateImage2D) +#define __CREATE_IMAGE3D_ERR __ERR_STR(clCreateImage3D) +#endif // #if defined(CL_VERSION_1_1) + +#endif // __CL_USER_OVERRIDE_ERROR_STRINGS +//! \endcond + +/** + * CL 1.2 marker and barrier commands + */ +#if defined(CL_VERSION_1_2) +#define __ENQUEUE_MARKER_WAIT_LIST_ERR __ERR_STR(clEnqueueMarkerWithWaitList) +#define __ENQUEUE_BARRIER_WAIT_LIST_ERR __ERR_STR(clEnqueueBarrierWithWaitList) +#endif // #if defined(CL_VERSION_1_2) + +#if !defined(__USE_DEV_STRING) && !defined(__NO_STD_STRING) +typedef std::string STRING_CLASS; +#elif !defined(__USE_DEV_STRING) + +/*! \class string + * \brief Simple string class, that provides a limited subset of std::string + * functionality but avoids many of the issues that come with that class. + + * \note Deprecated. Please use std::string as default or + * re-define the string class to match the std::string + * interface by defining STRING_CLASS + */ +class CL_EXT_PREFIX__VERSION_1_1_DEPRECATED string +{ +private: + ::size_t size_; + char * str_; +public: + //! \brief Constructs an empty string, allocating no memory. + string(void) : size_(0), str_(NULL) + { + } + + /*! \brief Constructs a string populated from an arbitrary value of + * specified size. + * + * An extra '\0' is added, in case none was contained in str. + * + * \param str the initial value of the string instance. Note that '\0' + * characters receive no special treatment. If NULL, + * the string is left empty, with a size of 0. + * + * \param size the number of characters to copy from str. + */ + string(const char * str, ::size_t size) : + size_(size), + str_(NULL) + { + if( size > 0 ) { + str_ = new char[size_+1]; + if (str_ != NULL) { + memcpy(str_, str, size_ * sizeof(char)); + str_[size_] = '\0'; + } + else { + size_ = 0; + } + } + } + + /*! \brief Constructs a string populated from a null-terminated value. + * + * \param str the null-terminated initial value of the string instance. + * If NULL, the string is left empty, with a size of 0. + */ + string(const char * str) : + size_(0), + str_(NULL) + { + if( str ) { + size_= ::strlen(str); + } + if( size_ > 0 ) { + str_ = new char[size_ + 1]; + if (str_ != NULL) { + memcpy(str_, str, (size_ + 1) * sizeof(char)); + } + } + } + + void resize( ::size_t n ) + { + if( size_ == n ) { + return; + } + if (n == 0) { + if( str_ ) { + delete [] str_; + } + str_ = NULL; + size_ = 0; + } + else { + char *newString = new char[n + 1]; + ::size_t copySize = n; + if( size_ < n ) { + copySize = size_; + } + size_ = n; + + if(str_) { + memcpy(newString, str_, (copySize + 1) * sizeof(char)); + } + if( copySize < size_ ) { + memset(newString + copySize, 0, size_ - copySize); + } + newString[size_] = '\0'; + + delete [] str_; + str_ = newString; + } + } + + const char& operator[] ( ::size_t pos ) const + { + return str_[pos]; + } + + char& operator[] ( ::size_t pos ) + { + return str_[pos]; + } + + /*! \brief Copies the value of another string to this one. + * + * \param rhs the string to copy. + * + * \returns a reference to the modified instance. + */ + string& operator=(const string& rhs) + { + if (this == &rhs) { + return *this; + } + + if( str_ != NULL ) { + delete [] str_; + str_ = NULL; + size_ = 0; + } + + if (rhs.size_ == 0 || rhs.str_ == NULL) { + str_ = NULL; + size_ = 0; + } + else { + str_ = new char[rhs.size_ + 1]; + size_ = rhs.size_; + + if (str_ != NULL) { + memcpy(str_, rhs.str_, (size_ + 1) * sizeof(char)); + } + else { + size_ = 0; + } + } + + return *this; + } + + /*! \brief Constructs a string by copying the value of another instance. + * + * \param rhs the string to copy. + */ + string(const string& rhs) : + size_(0), + str_(NULL) + { + *this = rhs; + } + + //! \brief Destructor - frees memory used to hold the current value. + ~string() + { + delete[] str_; + str_ = NULL; + } + + //! \brief Queries the length of the string, excluding any added '\0's. + ::size_t size(void) const { return size_; } + + //! \brief Queries the length of the string, excluding any added '\0's. + ::size_t length(void) const { return size(); } + + /*! \brief Returns a pointer to the private copy held by this instance, + * or "" if empty/unset. + */ + const char * c_str(void) const { return (str_) ? str_ : "";} +} CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED; +typedef cl::string STRING_CLASS; +#endif // #elif !defined(__USE_DEV_STRING) + +#if !defined(__USE_DEV_VECTOR) && !defined(__NO_STD_VECTOR) +#define VECTOR_CLASS std::vector +#elif !defined(__USE_DEV_VECTOR) +#define VECTOR_CLASS cl::vector + +#if !defined(__MAX_DEFAULT_VECTOR_SIZE) +#define __MAX_DEFAULT_VECTOR_SIZE 10 +#endif + +/*! \class vector + * \brief Fixed sized vector implementation that mirroring + * + * \note Deprecated. Please use std::vector as default or + * re-define the vector class to match the std::vector + * interface by defining VECTOR_CLASS + + * \note Not recommended for use with custom objects as + * current implementation will construct N elements + * + * std::vector functionality. + * \brief Fixed sized vector compatible with std::vector. + * + * \note + * This differs from std::vector<> not just in memory allocation, + * but also in terms of when members are constructed, destroyed, + * and assigned instead of being copy constructed. + * + * \param T type of element contained in the vector. + * + * \param N maximum size of the vector. + */ +template +class CL_EXT_PREFIX__VERSION_1_1_DEPRECATED vector +{ +private: + T data_[N]; + unsigned int size_; + +public: + //! \brief Constructs an empty vector with no memory allocated. + vector() : + size_(static_cast(0)) + {} + + //! \brief Deallocates the vector's memory and destroys all of its elements. + ~vector() + { + clear(); + } + + //! \brief Returns the number of elements currently contained. + unsigned int size(void) const + { + return size_; + } + + /*! \brief Empties the vector of all elements. + * \note + * This does not deallocate memory but will invoke destructors + * on contained elements. + */ + void clear() + { + while(!empty()) { + pop_back(); + } + } + + /*! \brief Appends an element after the last valid element. + * Calling this on a vector that has reached capacity will throw an + * exception if exceptions are enabled. + */ + void push_back (const T& x) + { + if (size() < N) { + new (&data_[size_]) T(x); + size_++; + } else { + detail::errHandler(CL_MEM_OBJECT_ALLOCATION_FAILURE, __VECTOR_CAPACITY_ERR); + } + } + + /*! \brief Removes the last valid element from the vector. + * Calling this on an empty vector will throw an exception + * if exceptions are enabled. + */ + void pop_back(void) + { + if (size_ != 0) { + --size_; + data_[size_].~T(); + } else { + detail::errHandler(CL_MEM_OBJECT_ALLOCATION_FAILURE, __VECTOR_CAPACITY_ERR); + } + } + + /*! \brief Constructs with a value copied from another. + * + * \param vec the vector to copy. + */ + vector(const vector& vec) : + size_(vec.size_) + { + if (size_ != 0) { + assign(vec.begin(), vec.end()); + } + } + + /*! \brief Constructs with a specified number of initial elements. + * + * \param size number of initial elements. + * + * \param val value of initial elements. + */ + vector(unsigned int size, const T& val = T()) : + size_(0) + { + for (unsigned int i = 0; i < size; i++) { + push_back(val); + } + } + + /*! \brief Overwrites the current content with that copied from another + * instance. + * + * \param rhs vector to copy. + * + * \returns a reference to this. + */ + vector& operator=(const vector& rhs) + { + if (this == &rhs) { + return *this; + } + + if (rhs.size_ != 0) { + assign(rhs.begin(), rhs.end()); + } else { + clear(); + } + + return *this; + } + + /*! \brief Tests equality against another instance. + * + * \param vec the vector against which to compare. + */ + bool operator==(vector &vec) + { + if (size() != vec.size()) { + return false; + } + + for( unsigned int i = 0; i < size(); ++i ) { + if( operator[](i) != vec[i] ) { + return false; + } + } + return true; + } + + //! \brief Conversion operator to T*. + operator T* () { return data_; } + + //! \brief Conversion operator to const T*. + operator const T* () const { return data_; } + + //! \brief Tests whether this instance has any elements. + bool empty (void) const + { + return size_==0; + } + + //! \brief Returns the maximum number of elements this instance can hold. + unsigned int max_size (void) const + { + return N; + } + + //! \brief Returns the maximum number of elements this instance can hold. + unsigned int capacity () const + { + return N; + } + + //! \brief Resizes the vector to the given size + void resize(unsigned int newSize, T fill = T()) + { + if (newSize > N) + { + detail::errHandler(CL_MEM_OBJECT_ALLOCATION_FAILURE, __VECTOR_CAPACITY_ERR); + } + else + { + while (size_ < newSize) + { + new (&data_[size_]) T(fill); + size_++; + } + while (size_ > newSize) + { + --size_; + data_[size_].~T(); + } + } + } + + /*! \brief Returns a reference to a given element. + * + * \param index which element to access. * + * \note + * The caller is responsible for ensuring index is >= 0 and < size(). + */ + T& operator[](int index) + { + return data_[index]; + } + + /*! \brief Returns a const reference to a given element. + * + * \param index which element to access. + * + * \note + * The caller is responsible for ensuring index is >= 0 and < size(). + */ + const T& operator[](int index) const + { + return data_[index]; + } + + /*! \brief Assigns elements of the vector based on a source iterator range. + * + * \param start Beginning iterator of source range + * \param end Enditerator of source range + * + * \note + * Will throw an exception if exceptions are enabled and size exceeded. + */ + template + void assign(I start, I end) + { + clear(); + while(start != end) { + push_back(*start); + start++; + } + } + + /*! \class iterator + * \brief Const iterator class for vectors + */ + class iterator + { + private: + const vector *vec_; + int index_; + + /** + * Internal iterator constructor to capture reference + * to the vector it iterates over rather than taking + * the vector by copy. + */ + iterator (const vector &vec, int index) : + vec_(&vec) + { + if( !vec.empty() ) { + index_ = index; + } else { + index_ = -1; + } + } + + public: + iterator(void) : + index_(-1), + vec_(NULL) + { + } + + iterator(const iterator& rhs) : + vec_(rhs.vec_), + index_(rhs.index_) + { + } + + ~iterator(void) {} + + static iterator begin(const cl::vector &vec) + { + iterator i(vec, 0); + + return i; + } + + static iterator end(const cl::vector &vec) + { + iterator i(vec, vec.size()); + + return i; + } + + bool operator==(iterator i) + { + return ((vec_ == i.vec_) && + (index_ == i.index_)); + } + + bool operator!=(iterator i) + { + return (!(*this==i)); + } + + iterator& operator++() + { + ++index_; + return *this; + } + + iterator operator++(int) + { + iterator retVal(*this); + ++index_; + return retVal; + } + + iterator& operator--() + { + --index_; + return *this; + } + + iterator operator--(int) + { + iterator retVal(*this); + --index_; + return retVal; + } + + const T& operator *() const + { + return (*vec_)[index_]; + } + }; + + iterator begin(void) + { + return iterator::begin(*this); + } + + iterator begin(void) const + { + return iterator::begin(*this); + } + + iterator end(void) + { + return iterator::end(*this); + } + + iterator end(void) const + { + return iterator::end(*this); + } + + T& front(void) + { + return data_[0]; + } + + T& back(void) + { + return data_[size_]; + } + + const T& front(void) const + { + return data_[0]; + } + + const T& back(void) const + { + return data_[size_-1]; + } +} CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED; +#endif // #if !defined(__USE_DEV_VECTOR) && !defined(__NO_STD_VECTOR) + + + + + +namespace detail { +#define __DEFAULT_NOT_INITIALIZED 1 +#define __DEFAULT_BEING_INITIALIZED 2 +#define __DEFAULT_INITIALIZED 4 + + /* + * Compare and exchange primitives are needed for handling of defaults + */ + +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED + inline int compare_exchange(std::atomic * dest, int exchange, int comparand) +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED + inline int compare_exchange(volatile int * dest, int exchange, int comparand) +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED + { +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED + std::atomic_compare_exchange_strong(dest, &comparand, exchange); + return comparand; +#elif _MSC_VER + return (int)(_InterlockedCompareExchange( + (volatile long*)dest, + (long)exchange, + (long)comparand)); +#else // !_MSC_VER && !CL_HPP_CPP11_ATOMICS_SUPPORTED + return (__sync_val_compare_and_swap( + dest, + comparand, + exchange)); +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED + } + + inline void fence() { +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED + std::atomic_thread_fence(std::memory_order_seq_cst); +#elif _MSC_VER // !CL_HPP_CPP11_ATOMICS_SUPPORTED + _ReadWriteBarrier(); +#else // !_MSC_VER && !CL_HPP_CPP11_ATOMICS_SUPPORTED + __sync_synchronize(); +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED + } +} // namespace detail + + +/*! \brief class used to interface between C++ and + * OpenCL C calls that require arrays of size_t values, whose + * size is known statically. + */ +template +class size_t +{ +private: + ::size_t data_[N]; + +public: + //! \brief Initialize size_t to all 0s + size_t() + { + for( int i = 0; i < N; ++i ) { + data_[i] = 0; + } + } + + ::size_t& operator[](int index) + { + return data_[index]; + } + + const ::size_t& operator[](int index) const + { + return data_[index]; + } + + //! \brief Conversion operator to T*. + operator ::size_t* () { return data_; } + + //! \brief Conversion operator to const T*. + operator const ::size_t* () const { return data_; } +}; + +namespace detail { + +// Generic getInfoHelper. The final parameter is used to guide overload +// resolution: the actual parameter passed is an int, which makes this +// a worse conversion sequence than a specialization that declares the +// parameter as an int. +template +inline cl_int getInfoHelper(Functor f, cl_uint name, T* param, long) +{ + return f(name, sizeof(T), param, NULL); +} + +// Specialized getInfoHelper for VECTOR_CLASS params +template +inline cl_int getInfoHelper(Func f, cl_uint name, VECTOR_CLASS* param, long) +{ + ::size_t required; + cl_int err = f(name, 0, NULL, &required); + if (err != CL_SUCCESS) { + return err; + } + + T* value = (T*) alloca(required); + err = f(name, required, value, NULL); + if (err != CL_SUCCESS) { + return err; + } + + param->assign(&value[0], &value[required/sizeof(T)]); + return CL_SUCCESS; +} + +/* Specialization for reference-counted types. This depends on the + * existence of Wrapper::cl_type, and none of the other types having the + * cl_type member. Note that simplify specifying the parameter as Wrapper + * does not work, because when using a derived type (e.g. Context) the generic + * template will provide a better match. + */ +template +inline cl_int getInfoHelper(Func f, cl_uint name, VECTOR_CLASS* param, int, typename T::cl_type = 0) +{ + ::size_t required; + cl_int err = f(name, 0, NULL, &required); + if (err != CL_SUCCESS) { + return err; + } + + typename T::cl_type * value = (typename T::cl_type *) alloca(required); + err = f(name, required, value, NULL); + if (err != CL_SUCCESS) { + return err; + } + + ::size_t elements = required / sizeof(typename T::cl_type); + param->assign(&value[0], &value[elements]); + for (::size_t i = 0; i < elements; i++) + { + if (value[i] != NULL) + { + err = (*param)[i].retain(); + if (err != CL_SUCCESS) { + return err; + } + } + } + return CL_SUCCESS; +} + +// Specialized for getInfo +template +inline cl_int getInfoHelper(Func f, cl_uint name, VECTOR_CLASS* param, int) +{ + cl_int err = f(name, param->size() * sizeof(char *), &(*param)[0], NULL); + + if (err != CL_SUCCESS) { + return err; + } + + return CL_SUCCESS; +} + +// Specialized GetInfoHelper for STRING_CLASS params +template +inline cl_int getInfoHelper(Func f, cl_uint name, STRING_CLASS* param, long) +{ +#if defined(__NO_STD_VECTOR) || defined(__NO_STD_STRING) + ::size_t required; + cl_int err = f(name, 0, NULL, &required); + if (err != CL_SUCCESS) { + return err; + } + + char* value = (char*)alloca(required); + err = f(name, required, value, NULL); + if (err != CL_SUCCESS) { + return err; + } + + *param = value; + return CL_SUCCESS; +#else + ::size_t required; + cl_int err = f(name, 0, NULL, &required); + if (err != CL_SUCCESS) { + return err; + } + + // std::string has a constant data member + // a char vector does not + VECTOR_CLASS value(required); + err = f(name, required, value.data(), NULL); + if (err != CL_SUCCESS) { + return err; + } + if (param) { + param->assign(value.begin(), value.end()); + } +#endif + return CL_SUCCESS; +} + +// Specialized GetInfoHelper for cl::size_t params +template +inline cl_int getInfoHelper(Func f, cl_uint name, size_t* param, long) +{ + ::size_t required; + cl_int err = f(name, 0, NULL, &required); + if (err != CL_SUCCESS) { + return err; + } + + ::size_t* value = (::size_t*) alloca(required); + err = f(name, required, value, NULL); + if (err != CL_SUCCESS) { + return err; + } + + for(int i = 0; i < N; ++i) { + (*param)[i] = value[i]; + } + + return CL_SUCCESS; +} + +template struct ReferenceHandler; + +/* Specialization for reference-counted types. This depends on the + * existence of Wrapper::cl_type, and none of the other types having the + * cl_type member. Note that simplify specifying the parameter as Wrapper + * does not work, because when using a derived type (e.g. Context) the generic + * template will provide a better match. + */ +template +inline cl_int getInfoHelper(Func f, cl_uint name, T* param, int, typename T::cl_type = 0) +{ + typename T::cl_type value; + cl_int err = f(name, sizeof(value), &value, NULL); + if (err != CL_SUCCESS) { + return err; + } + *param = value; + if (value != NULL) + { + err = param->retain(); + if (err != CL_SUCCESS) { + return err; + } + } + return CL_SUCCESS; +} + +#define __PARAM_NAME_INFO_1_0(F) \ + F(cl_platform_info, CL_PLATFORM_PROFILE, STRING_CLASS) \ + F(cl_platform_info, CL_PLATFORM_VERSION, STRING_CLASS) \ + F(cl_platform_info, CL_PLATFORM_NAME, STRING_CLASS) \ + F(cl_platform_info, CL_PLATFORM_VENDOR, STRING_CLASS) \ + F(cl_platform_info, CL_PLATFORM_EXTENSIONS, STRING_CLASS) \ + \ + F(cl_device_info, CL_DEVICE_TYPE, cl_device_type) \ + F(cl_device_info, CL_DEVICE_VENDOR_ID, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_COMPUTE_UNITS, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_WORK_GROUP_SIZE, ::size_t) \ + F(cl_device_info, CL_DEVICE_MAX_WORK_ITEM_SIZES, VECTOR_CLASS< ::size_t>) \ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_CHAR, cl_uint) \ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_SHORT, cl_uint) \ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_INT, cl_uint) \ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_LONG, cl_uint) \ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_FLOAT, cl_uint) \ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_CLOCK_FREQUENCY, cl_uint) \ + F(cl_device_info, CL_DEVICE_ADDRESS_BITS, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_READ_IMAGE_ARGS, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_WRITE_IMAGE_ARGS, cl_uint) \ + F(cl_device_info, CL_DEVICE_MAX_MEM_ALLOC_SIZE, cl_ulong) \ + F(cl_device_info, CL_DEVICE_IMAGE2D_MAX_WIDTH, ::size_t) \ + F(cl_device_info, CL_DEVICE_IMAGE2D_MAX_HEIGHT, ::size_t) \ + F(cl_device_info, CL_DEVICE_IMAGE3D_MAX_WIDTH, ::size_t) \ + F(cl_device_info, CL_DEVICE_IMAGE3D_MAX_HEIGHT, ::size_t) \ + F(cl_device_info, CL_DEVICE_IMAGE3D_MAX_DEPTH, ::size_t) \ + F(cl_device_info, CL_DEVICE_IMAGE_SUPPORT, cl_bool) \ + F(cl_device_info, CL_DEVICE_MAX_PARAMETER_SIZE, ::size_t) \ + F(cl_device_info, CL_DEVICE_MAX_SAMPLERS, cl_uint) \ + F(cl_device_info, CL_DEVICE_MEM_BASE_ADDR_ALIGN, cl_uint) \ + F(cl_device_info, CL_DEVICE_MIN_DATA_TYPE_ALIGN_SIZE, cl_uint) \ + F(cl_device_info, CL_DEVICE_SINGLE_FP_CONFIG, cl_device_fp_config) \ + F(cl_device_info, CL_DEVICE_GLOBAL_MEM_CACHE_TYPE, cl_device_mem_cache_type) \ + F(cl_device_info, CL_DEVICE_GLOBAL_MEM_CACHELINE_SIZE, cl_uint)\ + F(cl_device_info, CL_DEVICE_GLOBAL_MEM_CACHE_SIZE, cl_ulong) \ + F(cl_device_info, CL_DEVICE_GLOBAL_MEM_SIZE, cl_ulong) \ + F(cl_device_info, CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE, cl_ulong) \ + F(cl_device_info, CL_DEVICE_MAX_CONSTANT_ARGS, cl_uint) \ + F(cl_device_info, CL_DEVICE_LOCAL_MEM_TYPE, cl_device_local_mem_type) \ + F(cl_device_info, CL_DEVICE_LOCAL_MEM_SIZE, cl_ulong) \ + F(cl_device_info, CL_DEVICE_ERROR_CORRECTION_SUPPORT, cl_bool) \ + F(cl_device_info, CL_DEVICE_PROFILING_TIMER_RESOLUTION, ::size_t) \ + F(cl_device_info, CL_DEVICE_ENDIAN_LITTLE, cl_bool) \ + F(cl_device_info, CL_DEVICE_AVAILABLE, cl_bool) \ + F(cl_device_info, CL_DEVICE_COMPILER_AVAILABLE, cl_bool) \ + F(cl_device_info, CL_DEVICE_EXECUTION_CAPABILITIES, cl_device_exec_capabilities) \ + F(cl_device_info, CL_DEVICE_QUEUE_PROPERTIES, cl_command_queue_properties) \ + F(cl_device_info, CL_DEVICE_PLATFORM, cl_platform_id) \ + F(cl_device_info, CL_DEVICE_NAME, STRING_CLASS) \ + F(cl_device_info, CL_DEVICE_VENDOR, STRING_CLASS) \ + F(cl_device_info, CL_DRIVER_VERSION, STRING_CLASS) \ + F(cl_device_info, CL_DEVICE_PROFILE, STRING_CLASS) \ + F(cl_device_info, CL_DEVICE_VERSION, STRING_CLASS) \ + F(cl_device_info, CL_DEVICE_EXTENSIONS, STRING_CLASS) \ + \ + F(cl_context_info, CL_CONTEXT_REFERENCE_COUNT, cl_uint) \ + F(cl_context_info, CL_CONTEXT_DEVICES, VECTOR_CLASS) \ + F(cl_context_info, CL_CONTEXT_PROPERTIES, VECTOR_CLASS) \ + \ + F(cl_event_info, CL_EVENT_COMMAND_QUEUE, cl::CommandQueue) \ + F(cl_event_info, CL_EVENT_COMMAND_TYPE, cl_command_type) \ + F(cl_event_info, CL_EVENT_REFERENCE_COUNT, cl_uint) \ + F(cl_event_info, CL_EVENT_COMMAND_EXECUTION_STATUS, cl_int) \ + \ + F(cl_profiling_info, CL_PROFILING_COMMAND_QUEUED, cl_ulong) \ + F(cl_profiling_info, CL_PROFILING_COMMAND_SUBMIT, cl_ulong) \ + F(cl_profiling_info, CL_PROFILING_COMMAND_START, cl_ulong) \ + F(cl_profiling_info, CL_PROFILING_COMMAND_END, cl_ulong) \ + \ + F(cl_mem_info, CL_MEM_TYPE, cl_mem_object_type) \ + F(cl_mem_info, CL_MEM_FLAGS, cl_mem_flags) \ + F(cl_mem_info, CL_MEM_SIZE, ::size_t) \ + F(cl_mem_info, CL_MEM_HOST_PTR, void*) \ + F(cl_mem_info, CL_MEM_MAP_COUNT, cl_uint) \ + F(cl_mem_info, CL_MEM_REFERENCE_COUNT, cl_uint) \ + F(cl_mem_info, CL_MEM_CONTEXT, cl::Context) \ + \ + F(cl_image_info, CL_IMAGE_FORMAT, cl_image_format) \ + F(cl_image_info, CL_IMAGE_ELEMENT_SIZE, ::size_t) \ + F(cl_image_info, CL_IMAGE_ROW_PITCH, ::size_t) \ + F(cl_image_info, CL_IMAGE_SLICE_PITCH, ::size_t) \ + F(cl_image_info, CL_IMAGE_WIDTH, ::size_t) \ + F(cl_image_info, CL_IMAGE_HEIGHT, ::size_t) \ + F(cl_image_info, CL_IMAGE_DEPTH, ::size_t) \ + \ + F(cl_sampler_info, CL_SAMPLER_REFERENCE_COUNT, cl_uint) \ + F(cl_sampler_info, CL_SAMPLER_CONTEXT, cl::Context) \ + F(cl_sampler_info, CL_SAMPLER_NORMALIZED_COORDS, cl_bool) \ + F(cl_sampler_info, CL_SAMPLER_ADDRESSING_MODE, cl_addressing_mode) \ + F(cl_sampler_info, CL_SAMPLER_FILTER_MODE, cl_filter_mode) \ + \ + F(cl_program_info, CL_PROGRAM_REFERENCE_COUNT, cl_uint) \ + F(cl_program_info, CL_PROGRAM_CONTEXT, cl::Context) \ + F(cl_program_info, CL_PROGRAM_NUM_DEVICES, cl_uint) \ + F(cl_program_info, CL_PROGRAM_DEVICES, VECTOR_CLASS) \ + F(cl_program_info, CL_PROGRAM_SOURCE, STRING_CLASS) \ + F(cl_program_info, CL_PROGRAM_BINARY_SIZES, VECTOR_CLASS< ::size_t>) \ + F(cl_program_info, CL_PROGRAM_BINARIES, VECTOR_CLASS) \ + \ + F(cl_program_build_info, CL_PROGRAM_BUILD_STATUS, cl_build_status) \ + F(cl_program_build_info, CL_PROGRAM_BUILD_OPTIONS, STRING_CLASS) \ + F(cl_program_build_info, CL_PROGRAM_BUILD_LOG, STRING_CLASS) \ + \ + F(cl_kernel_info, CL_KERNEL_FUNCTION_NAME, STRING_CLASS) \ + F(cl_kernel_info, CL_KERNEL_NUM_ARGS, cl_uint) \ + F(cl_kernel_info, CL_KERNEL_REFERENCE_COUNT, cl_uint) \ + F(cl_kernel_info, CL_KERNEL_CONTEXT, cl::Context) \ + F(cl_kernel_info, CL_KERNEL_PROGRAM, cl::Program) \ + \ + F(cl_kernel_work_group_info, CL_KERNEL_WORK_GROUP_SIZE, ::size_t) \ + F(cl_kernel_work_group_info, CL_KERNEL_COMPILE_WORK_GROUP_SIZE, cl::size_t<3>) \ + F(cl_kernel_work_group_info, CL_KERNEL_LOCAL_MEM_SIZE, cl_ulong) \ + \ + F(cl_command_queue_info, CL_QUEUE_CONTEXT, cl::Context) \ + F(cl_command_queue_info, CL_QUEUE_DEVICE, cl::Device) \ + F(cl_command_queue_info, CL_QUEUE_REFERENCE_COUNT, cl_uint) \ + F(cl_command_queue_info, CL_QUEUE_PROPERTIES, cl_command_queue_properties) + +#if defined(CL_VERSION_1_1) +#define __PARAM_NAME_INFO_1_1(F) \ + F(cl_context_info, CL_CONTEXT_NUM_DEVICES, cl_uint)\ + F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_HALF, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_CHAR, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_SHORT, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_INT, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_LONG, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_FLOAT, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_DOUBLE, cl_uint) \ + F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_HALF, cl_uint) \ + F(cl_device_info, CL_DEVICE_DOUBLE_FP_CONFIG, cl_device_fp_config) \ + F(cl_device_info, CL_DEVICE_HALF_FP_CONFIG, cl_device_fp_config) \ + F(cl_device_info, CL_DEVICE_HOST_UNIFIED_MEMORY, cl_bool) \ + F(cl_device_info, CL_DEVICE_OPENCL_C_VERSION, STRING_CLASS) \ + \ + F(cl_mem_info, CL_MEM_ASSOCIATED_MEMOBJECT, cl::Memory) \ + F(cl_mem_info, CL_MEM_OFFSET, ::size_t) \ + \ + F(cl_kernel_work_group_info, CL_KERNEL_PREFERRED_WORK_GROUP_SIZE_MULTIPLE, ::size_t) \ + F(cl_kernel_work_group_info, CL_KERNEL_PRIVATE_MEM_SIZE, cl_ulong) \ + \ + F(cl_event_info, CL_EVENT_CONTEXT, cl::Context) +#endif // CL_VERSION_1_1 + + +#if defined(CL_VERSION_1_2) +#define __PARAM_NAME_INFO_1_2(F) \ + F(cl_image_info, CL_IMAGE_BUFFER, cl::Buffer) \ + \ + F(cl_program_info, CL_PROGRAM_NUM_KERNELS, ::size_t) \ + F(cl_program_info, CL_PROGRAM_KERNEL_NAMES, STRING_CLASS) \ + \ + F(cl_program_build_info, CL_PROGRAM_BINARY_TYPE, cl_program_binary_type) \ + \ + F(cl_kernel_info, CL_KERNEL_ATTRIBUTES, STRING_CLASS) \ + \ + F(cl_kernel_arg_info, CL_KERNEL_ARG_ADDRESS_QUALIFIER, cl_kernel_arg_address_qualifier) \ + F(cl_kernel_arg_info, CL_KERNEL_ARG_ACCESS_QUALIFIER, cl_kernel_arg_access_qualifier) \ + F(cl_kernel_arg_info, CL_KERNEL_ARG_TYPE_NAME, STRING_CLASS) \ + F(cl_kernel_arg_info, CL_KERNEL_ARG_NAME, STRING_CLASS) \ + F(cl_kernel_arg_info, CL_KERNEL_ARG_TYPE_QUALIFIER, cl_kernel_arg_type_qualifier) \ + \ + F(cl_device_info, CL_DEVICE_PARENT_DEVICE, cl_device_id) \ + F(cl_device_info, CL_DEVICE_PARTITION_PROPERTIES, VECTOR_CLASS) \ + F(cl_device_info, CL_DEVICE_PARTITION_TYPE, VECTOR_CLASS) \ + F(cl_device_info, CL_DEVICE_REFERENCE_COUNT, cl_uint) \ + F(cl_device_info, CL_DEVICE_PREFERRED_INTEROP_USER_SYNC, ::size_t) \ + F(cl_device_info, CL_DEVICE_PARTITION_AFFINITY_DOMAIN, cl_device_affinity_domain) \ + F(cl_device_info, CL_DEVICE_BUILT_IN_KERNELS, STRING_CLASS) +#endif // #if defined(CL_VERSION_1_2) + +#if defined(USE_CL_DEVICE_FISSION) +#define __PARAM_NAME_DEVICE_FISSION(F) \ + F(cl_device_info, CL_DEVICE_PARENT_DEVICE_EXT, cl_device_id) \ + F(cl_device_info, CL_DEVICE_PARTITION_TYPES_EXT, VECTOR_CLASS) \ + F(cl_device_info, CL_DEVICE_AFFINITY_DOMAINS_EXT, VECTOR_CLASS) \ + F(cl_device_info, CL_DEVICE_REFERENCE_COUNT_EXT , cl_uint) \ + F(cl_device_info, CL_DEVICE_PARTITION_STYLE_EXT, VECTOR_CLASS) +#endif // USE_CL_DEVICE_FISSION + +template +struct param_traits {}; + +#define __CL_DECLARE_PARAM_TRAITS(token, param_name, T) \ +struct token; \ +template<> \ +struct param_traits \ +{ \ + enum { value = param_name }; \ + typedef T param_type; \ +}; + +__PARAM_NAME_INFO_1_0(__CL_DECLARE_PARAM_TRAITS) +#if defined(CL_VERSION_1_1) +__PARAM_NAME_INFO_1_1(__CL_DECLARE_PARAM_TRAITS) +#endif // CL_VERSION_1_1 +#if defined(CL_VERSION_1_2) +__PARAM_NAME_INFO_1_2(__CL_DECLARE_PARAM_TRAITS) +#endif // CL_VERSION_1_1 + +#if defined(USE_CL_DEVICE_FISSION) +__PARAM_NAME_DEVICE_FISSION(__CL_DECLARE_PARAM_TRAITS); +#endif // USE_CL_DEVICE_FISSION + +#ifdef CL_PLATFORM_ICD_SUFFIX_KHR +__CL_DECLARE_PARAM_TRAITS(cl_platform_info, CL_PLATFORM_ICD_SUFFIX_KHR, STRING_CLASS) +#endif + +#ifdef CL_DEVICE_PROFILING_TIMER_OFFSET_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_PROFILING_TIMER_OFFSET_AMD, cl_ulong) +#endif + +#ifdef CL_DEVICE_GLOBAL_FREE_MEMORY_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_GLOBAL_FREE_MEMORY_AMD, VECTOR_CLASS< ::size_t>) +#endif +#ifdef CL_DEVICE_SIMD_PER_COMPUTE_UNIT_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_SIMD_PER_COMPUTE_UNIT_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_SIMD_WIDTH_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_SIMD_WIDTH_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_SIMD_INSTRUCTION_WIDTH_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_SIMD_INSTRUCTION_WIDTH_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_WAVEFRONT_WIDTH_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_WAVEFRONT_WIDTH_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_GLOBAL_MEM_CHANNELS_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_GLOBAL_MEM_CHANNELS_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_GLOBAL_MEM_CHANNEL_BANKS_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_GLOBAL_MEM_CHANNEL_BANKS_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_GLOBAL_MEM_CHANNEL_BANK_WIDTH_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_GLOBAL_MEM_CHANNEL_BANK_WIDTH_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_LOCAL_MEM_SIZE_PER_COMPUTE_UNIT_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_LOCAL_MEM_SIZE_PER_COMPUTE_UNIT_AMD, cl_uint) +#endif +#ifdef CL_DEVICE_LOCAL_MEM_BANKS_AMD +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_LOCAL_MEM_BANKS_AMD, cl_uint) +#endif + +#ifdef CL_DEVICE_COMPUTE_CAPABILITY_MAJOR_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_COMPUTE_CAPABILITY_MAJOR_NV, cl_uint) +#endif +#ifdef CL_DEVICE_COMPUTE_CAPABILITY_MINOR_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_COMPUTE_CAPABILITY_MINOR_NV, cl_uint) +#endif +#ifdef CL_DEVICE_REGISTERS_PER_BLOCK_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_REGISTERS_PER_BLOCK_NV, cl_uint) +#endif +#ifdef CL_DEVICE_WARP_SIZE_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_WARP_SIZE_NV, cl_uint) +#endif +#ifdef CL_DEVICE_GPU_OVERLAP_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_GPU_OVERLAP_NV, cl_bool) +#endif +#ifdef CL_DEVICE_KERNEL_EXEC_TIMEOUT_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_KERNEL_EXEC_TIMEOUT_NV, cl_bool) +#endif +#ifdef CL_DEVICE_INTEGRATED_MEMORY_NV +__CL_DECLARE_PARAM_TRAITS(cl_device_info, CL_DEVICE_INTEGRATED_MEMORY_NV, cl_bool) +#endif + +// Convenience functions + +template +inline cl_int +getInfo(Func f, cl_uint name, T* param) +{ + return getInfoHelper(f, name, param, 0); +} + +template +struct GetInfoFunctor0 +{ + Func f_; const Arg0& arg0_; + cl_int operator ()( + cl_uint param, ::size_t size, void* value, ::size_t* size_ret) + { return f_(arg0_, param, size, value, size_ret); } +}; + +template +struct GetInfoFunctor1 +{ + Func f_; const Arg0& arg0_; const Arg1& arg1_; + cl_int operator ()( + cl_uint param, ::size_t size, void* value, ::size_t* size_ret) + { return f_(arg0_, arg1_, param, size, value, size_ret); } +}; + +template +inline cl_int +getInfo(Func f, const Arg0& arg0, cl_uint name, T* param) +{ + GetInfoFunctor0 f0 = { f, arg0 }; + return getInfoHelper(f0, name, param, 0); +} + +template +inline cl_int +getInfo(Func f, const Arg0& arg0, const Arg1& arg1, cl_uint name, T* param) +{ + GetInfoFunctor1 f0 = { f, arg0, arg1 }; + return getInfoHelper(f0, name, param, 0); +} + +template +struct ReferenceHandler +{ }; + +#if defined(CL_VERSION_1_2) +/** + * OpenCL 1.2 devices do have retain/release. + */ +template <> +struct ReferenceHandler +{ + /** + * Retain the device. + * \param device A valid device created using createSubDevices + * \return + * CL_SUCCESS if the function executed successfully. + * CL_INVALID_DEVICE if device was not a valid subdevice + * CL_OUT_OF_RESOURCES + * CL_OUT_OF_HOST_MEMORY + */ + static cl_int retain(cl_device_id device) + { return ::clRetainDevice(device); } + /** + * Retain the device. + * \param device A valid device created using createSubDevices + * \return + * CL_SUCCESS if the function executed successfully. + * CL_INVALID_DEVICE if device was not a valid subdevice + * CL_OUT_OF_RESOURCES + * CL_OUT_OF_HOST_MEMORY + */ + static cl_int release(cl_device_id device) + { return ::clReleaseDevice(device); } +}; +#else // #if defined(CL_VERSION_1_2) +/** + * OpenCL 1.1 devices do not have retain/release. + */ +template <> +struct ReferenceHandler +{ + // cl_device_id does not have retain(). + static cl_int retain(cl_device_id) + { return CL_SUCCESS; } + // cl_device_id does not have release(). + static cl_int release(cl_device_id) + { return CL_SUCCESS; } +}; +#endif // #if defined(CL_VERSION_1_2) + +template <> +struct ReferenceHandler +{ + // cl_platform_id does not have retain(). + static cl_int retain(cl_platform_id) + { return CL_SUCCESS; } + // cl_platform_id does not have release(). + static cl_int release(cl_platform_id) + { return CL_SUCCESS; } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_context context) + { return ::clRetainContext(context); } + static cl_int release(cl_context context) + { return ::clReleaseContext(context); } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_command_queue queue) + { return ::clRetainCommandQueue(queue); } + static cl_int release(cl_command_queue queue) + { return ::clReleaseCommandQueue(queue); } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_mem memory) + { return ::clRetainMemObject(memory); } + static cl_int release(cl_mem memory) + { return ::clReleaseMemObject(memory); } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_sampler sampler) + { return ::clRetainSampler(sampler); } + static cl_int release(cl_sampler sampler) + { return ::clReleaseSampler(sampler); } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_program program) + { return ::clRetainProgram(program); } + static cl_int release(cl_program program) + { return ::clReleaseProgram(program); } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_kernel kernel) + { return ::clRetainKernel(kernel); } + static cl_int release(cl_kernel kernel) + { return ::clReleaseKernel(kernel); } +}; + +template <> +struct ReferenceHandler +{ + static cl_int retain(cl_event event) + { return ::clRetainEvent(event); } + static cl_int release(cl_event event) + { return ::clReleaseEvent(event); } +}; + + +// Extracts version number with major in the upper 16 bits, minor in the lower 16 +static cl_uint getVersion(const char *versionInfo) +{ + int highVersion = 0; + int lowVersion = 0; + int index = 7; + while(versionInfo[index] != '.' ) { + highVersion *= 10; + highVersion += versionInfo[index]-'0'; + ++index; + } + ++index; + while(versionInfo[index] != ' ' && versionInfo[index] != '\0') { + lowVersion *= 10; + lowVersion += versionInfo[index]-'0'; + ++index; + } + return (highVersion << 16) | lowVersion; +} + +static cl_uint getPlatformVersion(cl_platform_id platform) +{ + ::size_t size = 0; + clGetPlatformInfo(platform, CL_PLATFORM_VERSION, 0, NULL, &size); + char *versionInfo = (char *) alloca(size); + clGetPlatformInfo(platform, CL_PLATFORM_VERSION, size, &versionInfo[0], &size); + return getVersion(versionInfo); +} + +static cl_uint getDevicePlatformVersion(cl_device_id device) +{ + cl_platform_id platform; + clGetDeviceInfo(device, CL_DEVICE_PLATFORM, sizeof(platform), &platform, NULL); + return getPlatformVersion(platform); +} + +#if defined(CL_VERSION_1_2) && defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) +static cl_uint getContextPlatformVersion(cl_context context) +{ + // The platform cannot be queried directly, so we first have to grab a + // device and obtain its context + ::size_t size = 0; + clGetContextInfo(context, CL_CONTEXT_DEVICES, 0, NULL, &size); + if (size == 0) + return 0; + cl_device_id *devices = (cl_device_id *) alloca(size); + clGetContextInfo(context, CL_CONTEXT_DEVICES, size, devices, NULL); + return getDevicePlatformVersion(devices[0]); +} +#endif // #if defined(CL_VERSION_1_2) && defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + +template +class Wrapper +{ +public: + typedef T cl_type; + +protected: + cl_type object_; + +public: + Wrapper() : object_(NULL) { } + + Wrapper(const cl_type &obj) : object_(obj) { } + + ~Wrapper() + { + if (object_ != NULL) { release(); } + } + + Wrapper(const Wrapper& rhs) + { + object_ = rhs.object_; + if (object_ != NULL) { detail::errHandler(retain(), __RETAIN_ERR); } + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + Wrapper(Wrapper&& rhs) CL_HPP_NOEXCEPT + { + object_ = rhs.object_; + rhs.object_ = NULL; + } +#endif + + Wrapper& operator = (const Wrapper& rhs) + { + if (this != &rhs) { + if (object_ != NULL) { detail::errHandler(release(), __RELEASE_ERR); } + object_ = rhs.object_; + if (object_ != NULL) { detail::errHandler(retain(), __RETAIN_ERR); } + } + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + Wrapper& operator = (Wrapper&& rhs) + { + if (this != &rhs) { + if (object_ != NULL) { detail::errHandler(release(), __RELEASE_ERR); } + object_ = rhs.object_; + rhs.object_ = NULL; + } + return *this; + } +#endif + + Wrapper& operator = (const cl_type &rhs) + { + if (object_ != NULL) { detail::errHandler(release(), __RELEASE_ERR); } + object_ = rhs; + return *this; + } + + cl_type operator ()() const { return object_; } + + cl_type& operator ()() { return object_; } + +protected: + template + friend inline cl_int getInfoHelper(Func, cl_uint, U*, int, typename U::cl_type); + + cl_int retain() const + { + return ReferenceHandler::retain(object_); + } + + cl_int release() const + { + return ReferenceHandler::release(object_); + } +}; + +template <> +class Wrapper +{ +public: + typedef cl_device_id cl_type; + +protected: + cl_type object_; + bool referenceCountable_; + + static bool isReferenceCountable(cl_device_id device) + { + bool retVal = false; + if (device != NULL) { + int version = getDevicePlatformVersion(device); + if(version > ((1 << 16) + 1)) { + retVal = true; + } + } + return retVal; + } + +public: + Wrapper() : object_(NULL), referenceCountable_(false) + { + } + + Wrapper(const cl_type &obj) : object_(obj), referenceCountable_(false) + { + referenceCountable_ = isReferenceCountable(obj); + } + + ~Wrapper() + { + if (object_ != NULL) { release(); } + } + + Wrapper(const Wrapper& rhs) + { + object_ = rhs.object_; + referenceCountable_ = isReferenceCountable(object_); + if (object_ != NULL) { detail::errHandler(retain(), __RETAIN_ERR); } + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + Wrapper(Wrapper&& rhs) CL_HPP_NOEXCEPT + { + object_ = rhs.object_; + referenceCountable_ = rhs.referenceCountable_; + rhs.object_ = NULL; + rhs.referenceCountable_ = false; + } +#endif + + Wrapper& operator = (const Wrapper& rhs) + { + if (this != &rhs) { + if (object_ != NULL) { detail::errHandler(release(), __RELEASE_ERR); } + object_ = rhs.object_; + referenceCountable_ = rhs.referenceCountable_; + if (object_ != NULL) { detail::errHandler(retain(), __RETAIN_ERR); } + } + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + Wrapper& operator = (Wrapper&& rhs) + { + if (this != &rhs) { + if (object_ != NULL) { detail::errHandler(release(), __RELEASE_ERR); } + object_ = rhs.object_; + referenceCountable_ = rhs.referenceCountable_; + rhs.object_ = NULL; + rhs.referenceCountable_ = false; + } + return *this; + } +#endif + + Wrapper& operator = (const cl_type &rhs) + { + if (object_ != NULL) { detail::errHandler(release(), __RELEASE_ERR); } + object_ = rhs; + referenceCountable_ = isReferenceCountable(object_); + return *this; + } + + cl_type operator ()() const { return object_; } + + cl_type& operator ()() { return object_; } + +protected: + template + friend inline cl_int getInfoHelper(Func, cl_uint, U*, int, typename U::cl_type); + + template + friend inline cl_int getInfoHelper(Func, cl_uint, VECTOR_CLASS*, int, typename U::cl_type); + + cl_int retain() const + { + if( referenceCountable_ ) { + return ReferenceHandler::retain(object_); + } + else { + return CL_SUCCESS; + } + } + + cl_int release() const + { + if( referenceCountable_ ) { + return ReferenceHandler::release(object_); + } + else { + return CL_SUCCESS; + } + } +}; + +} // namespace detail +//! \endcond + +/*! \stuct ImageFormat + * \brief Adds constructors and member functions for cl_image_format. + * + * \see cl_image_format + */ +struct ImageFormat : public cl_image_format +{ + //! \brief Default constructor - performs no initialization. + ImageFormat(){} + + //! \brief Initializing constructor. + ImageFormat(cl_channel_order order, cl_channel_type type) + { + image_channel_order = order; + image_channel_data_type = type; + } + + //! \brief Assignment operator. + ImageFormat& operator = (const ImageFormat& rhs) + { + if (this != &rhs) { + this->image_channel_data_type = rhs.image_channel_data_type; + this->image_channel_order = rhs.image_channel_order; + } + return *this; + } +}; + +/*! \brief Class interface for cl_device_id. + * + * \note Copies of these objects are inexpensive, since they don't 'own' + * any underlying resources or data structures. + * + * \see cl_device_id + */ +class Device : public detail::Wrapper +{ +public: + //! \brief Default constructor - initializes to NULL. + Device() : detail::Wrapper() { } + + /*! \brief Constructor from cl_device_id. + * + * This simply copies the device ID value, which is an inexpensive operation. + */ + __CL_EXPLICIT_CONSTRUCTORS Device(const cl_device_id &device) : detail::Wrapper(device) { } + + /*! \brief Returns the first device on the default context. + * + * \see Context::getDefault() + */ + static Device getDefault(cl_int * err = NULL); + + /*! \brief Assignment operator from cl_device_id. + * + * This simply copies the device ID value, which is an inexpensive operation. + */ + Device& operator = (const cl_device_id& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Device(const Device& dev) : detail::Wrapper(dev) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Device& operator = (const Device &dev) + { + detail::Wrapper::operator=(dev); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Device(Device&& dev) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(dev)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Device& operator = (Device &&dev) + { + detail::Wrapper::operator=(std::move(dev)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + //! \brief Wrapper for clGetDeviceInfo(). + template + cl_int getInfo(cl_device_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetDeviceInfo, object_, name, param), + __GET_DEVICE_INFO_ERR); + } + + //! \brief Wrapper for clGetDeviceInfo() that returns by value. + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_device_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + /** + * CL 1.2 version + */ +#if defined(CL_VERSION_1_2) + //! \brief Wrapper for clCreateSubDevicesEXT(). + cl_int createSubDevices( + const cl_device_partition_property * properties, + VECTOR_CLASS* devices) + { + cl_uint n = 0; + cl_int err = clCreateSubDevices(object_, properties, 0, NULL, &n); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __CREATE_SUB_DEVICES); + } + + cl_device_id* ids = (cl_device_id*) alloca(n * sizeof(cl_device_id)); + err = clCreateSubDevices(object_, properties, n, ids, NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __CREATE_SUB_DEVICES); + } + + devices->assign(&ids[0], &ids[n]); + return CL_SUCCESS; + } +#endif // #if defined(CL_VERSION_1_2) + +/** + * CL 1.1 version that uses device fission. + */ +#if defined(CL_VERSION_1_1) +#if defined(USE_CL_DEVICE_FISSION) + cl_int createSubDevices( + const cl_device_partition_property_ext * properties, + VECTOR_CLASS* devices) + { + typedef CL_API_ENTRY cl_int + ( CL_API_CALL * PFN_clCreateSubDevicesEXT)( + cl_device_id /*in_device*/, + const cl_device_partition_property_ext * /* properties */, + cl_uint /*num_entries*/, + cl_device_id * /*out_devices*/, + cl_uint * /*num_devices*/ ) CL_EXT_SUFFIX__VERSION_1_1; + + static PFN_clCreateSubDevicesEXT pfn_clCreateSubDevicesEXT = NULL; + __INIT_CL_EXT_FCN_PTR(clCreateSubDevicesEXT); + + cl_uint n = 0; + cl_int err = pfn_clCreateSubDevicesEXT(object_, properties, 0, NULL, &n); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __CREATE_SUB_DEVICES); + } + + cl_device_id* ids = (cl_device_id*) alloca(n * sizeof(cl_device_id)); + err = pfn_clCreateSubDevicesEXT(object_, properties, n, ids, NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __CREATE_SUB_DEVICES); + } + + devices->assign(&ids[0], &ids[n]); + return CL_SUCCESS; + } +#endif // #if defined(USE_CL_DEVICE_FISSION) +#endif // #if defined(CL_VERSION_1_1) +}; + +/*! \brief Class interface for cl_platform_id. + * + * \note Copies of these objects are inexpensive, since they don't 'own' + * any underlying resources or data structures. + * + * \see cl_platform_id + */ +class Platform : public detail::Wrapper +{ +public: + //! \brief Default constructor - initializes to NULL. + Platform() : detail::Wrapper() { } + + /*! \brief Constructor from cl_platform_id. + * + * This simply copies the platform ID value, which is an inexpensive operation. + */ + __CL_EXPLICIT_CONSTRUCTORS Platform(const cl_platform_id &platform) : detail::Wrapper(platform) { } + + /*! \brief Assignment operator from cl_platform_id. + * + * This simply copies the platform ID value, which is an inexpensive operation. + */ + Platform& operator = (const cl_platform_id& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + //! \brief Wrapper for clGetPlatformInfo(). + cl_int getInfo(cl_platform_info name, STRING_CLASS* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetPlatformInfo, object_, name, param), + __GET_PLATFORM_INFO_ERR); + } + + //! \brief Wrapper for clGetPlatformInfo() that returns by value. + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_platform_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + /*! \brief Gets a list of devices for this platform. + * + * Wraps clGetDeviceIDs(). + */ + cl_int getDevices( + cl_device_type type, + VECTOR_CLASS* devices) const + { + cl_uint n = 0; + if( devices == NULL ) { + return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_DEVICE_IDS_ERR); + } + cl_int err = ::clGetDeviceIDs(object_, type, 0, NULL, &n); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_DEVICE_IDS_ERR); + } + + cl_device_id* ids = (cl_device_id*) alloca(n * sizeof(cl_device_id)); + err = ::clGetDeviceIDs(object_, type, n, ids, NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_DEVICE_IDS_ERR); + } + + devices->assign(&ids[0], &ids[n]); + return CL_SUCCESS; + } + +#if defined(USE_DX_INTEROP) + /*! \brief Get the list of available D3D10 devices. + * + * \param d3d_device_source. + * + * \param d3d_object. + * + * \param d3d_device_set. + * + * \param devices returns a vector of OpenCL D3D10 devices found. The cl::Device + * values returned in devices can be used to identify a specific OpenCL + * device. If \a devices argument is NULL, this argument is ignored. + * + * \return One of the following values: + * - CL_SUCCESS if the function is executed successfully. + * + * The application can query specific capabilities of the OpenCL device(s) + * returned by cl::getDevices. This can be used by the application to + * determine which device(s) to use. + * + * \note In the case that exceptions are enabled and a return value + * other than CL_SUCCESS is generated, then cl::Error exception is + * generated. + */ + cl_int getDevices( + cl_d3d10_device_source_khr d3d_device_source, + void * d3d_object, + cl_d3d10_device_set_khr d3d_device_set, + VECTOR_CLASS* devices) const + { + typedef CL_API_ENTRY cl_int (CL_API_CALL *PFN_clGetDeviceIDsFromD3D10KHR)( + cl_platform_id platform, + cl_d3d10_device_source_khr d3d_device_source, + void * d3d_object, + cl_d3d10_device_set_khr d3d_device_set, + cl_uint num_entries, + cl_device_id * devices, + cl_uint* num_devices); + + if( devices == NULL ) { + return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_DEVICE_IDS_ERR); + } + + static PFN_clGetDeviceIDsFromD3D10KHR pfn_clGetDeviceIDsFromD3D10KHR = NULL; + __INIT_CL_EXT_FCN_PTR_PLATFORM(object_, clGetDeviceIDsFromD3D10KHR); + + cl_uint n = 0; + cl_int err = pfn_clGetDeviceIDsFromD3D10KHR( + object_, + d3d_device_source, + d3d_object, + d3d_device_set, + 0, + NULL, + &n); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_DEVICE_IDS_ERR); + } + + cl_device_id* ids = (cl_device_id*) alloca(n * sizeof(cl_device_id)); + err = pfn_clGetDeviceIDsFromD3D10KHR( + object_, + d3d_device_source, + d3d_object, + d3d_device_set, + n, + ids, + NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_DEVICE_IDS_ERR); + } + + devices->assign(&ids[0], &ids[n]); + return CL_SUCCESS; + } +#endif + + /*! \brief Gets a list of available platforms. + * + * Wraps clGetPlatformIDs(). + */ + static cl_int get( + VECTOR_CLASS* platforms) + { + cl_uint n = 0; + + if( platforms == NULL ) { + return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_PLATFORM_IDS_ERR); + } + + cl_int err = ::clGetPlatformIDs(0, NULL, &n); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_PLATFORM_IDS_ERR); + } + + cl_platform_id* ids = (cl_platform_id*) alloca( + n * sizeof(cl_platform_id)); + err = ::clGetPlatformIDs(n, ids, NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_PLATFORM_IDS_ERR); + } + + platforms->assign(&ids[0], &ids[n]); + return CL_SUCCESS; + } + + /*! \brief Gets the first available platform. + * + * Wraps clGetPlatformIDs(), returning the first result. + */ + static cl_int get( + Platform * platform) + { + cl_uint n = 0; + + if( platform == NULL ) { + return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_PLATFORM_IDS_ERR); + } + + cl_int err = ::clGetPlatformIDs(0, NULL, &n); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_PLATFORM_IDS_ERR); + } + + cl_platform_id* ids = (cl_platform_id*) alloca( + n * sizeof(cl_platform_id)); + err = ::clGetPlatformIDs(n, ids, NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_PLATFORM_IDS_ERR); + } + + *platform = ids[0]; + return CL_SUCCESS; + } + + /*! \brief Gets the first available platform, returning it by value. + * + * Wraps clGetPlatformIDs(), returning the first result. + */ + static Platform get( + cl_int * errResult = NULL) + { + Platform platform; + cl_uint n = 0; + cl_int err = ::clGetPlatformIDs(0, NULL, &n); + if (err != CL_SUCCESS) { + detail::errHandler(err, __GET_PLATFORM_IDS_ERR); + if (errResult != NULL) { + *errResult = err; + } + return Platform(); + } + + cl_platform_id* ids = (cl_platform_id*) alloca( + n * sizeof(cl_platform_id)); + err = ::clGetPlatformIDs(n, ids, NULL); + + if (err != CL_SUCCESS) { + detail::errHandler(err, __GET_PLATFORM_IDS_ERR); + if (errResult != NULL) { + *errResult = err; + } + return Platform(); + } + + + return Platform(ids[0]); + } + + static Platform getDefault( + cl_int *errResult = NULL ) + { + return get(errResult); + } + + +#if defined(CL_VERSION_1_2) + //! \brief Wrapper for clUnloadCompiler(). + cl_int + unloadCompiler() + { + return ::clUnloadPlatformCompiler(object_); + } +#endif // #if defined(CL_VERSION_1_2) +}; // class Platform + +/** + * Deprecated APIs for 1.2 + */ +#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) || (defined(CL_VERSION_1_1) && !defined(CL_VERSION_1_2)) +/** + * Unload the OpenCL compiler. + * \note Deprecated for OpenCL 1.2. Use Platform::unloadCompiler instead. + */ +inline CL_EXT_PREFIX__VERSION_1_1_DEPRECATED cl_int +UnloadCompiler() CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED; +inline cl_int +UnloadCompiler() +{ + return ::clUnloadCompiler(); +} +#endif // #if defined(CL_VERSION_1_1) + +/*! \brief Class interface for cl_context. + * + * \note Copies of these objects are shallow, meaning that the copy will refer + * to the same underlying cl_context as the original. For details, see + * clRetainContext() and clReleaseContext(). + * + * \see cl_context + */ +class Context + : public detail::Wrapper +{ +private: + +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED + static std::atomic default_initialized_; +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED + static volatile int default_initialized_; +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED + static Context default_; + static volatile cl_int default_error_; +public: + /*! \brief Constructs a context including a list of specified devices. + * + * Wraps clCreateContext(). + */ + Context( + const VECTOR_CLASS& devices, + cl_context_properties* properties = NULL, + void (CL_CALLBACK * notifyFptr)( + const char *, + const void *, + ::size_t, + void *) = NULL, + void* data = NULL, + cl_int* err = NULL) + { + cl_int error; + + ::size_t numDevices = devices.size(); + cl_device_id* deviceIDs = (cl_device_id*) alloca(numDevices * sizeof(cl_device_id)); + for( ::size_t deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex ) { + deviceIDs[deviceIndex] = (devices[deviceIndex])(); + } + + object_ = ::clCreateContext( + properties, (cl_uint) numDevices, + deviceIDs, + notifyFptr, data, &error); + + detail::errHandler(error, __CREATE_CONTEXT_ERR); + if (err != NULL) { + *err = error; + } + } + + Context( + const Device& device, + cl_context_properties* properties = NULL, + void (CL_CALLBACK * notifyFptr)( + const char *, + const void *, + ::size_t, + void *) = NULL, + void* data = NULL, + cl_int* err = NULL) + { + cl_int error; + + cl_device_id deviceID = device(); + + object_ = ::clCreateContext( + properties, 1, + &deviceID, + notifyFptr, data, &error); + + detail::errHandler(error, __CREATE_CONTEXT_ERR); + if (err != NULL) { + *err = error; + } + } + + /*! \brief Constructs a context including all or a subset of devices of a specified type. + * + * Wraps clCreateContextFromType(). + */ + Context( + cl_device_type type, + cl_context_properties* properties = NULL, + void (CL_CALLBACK * notifyFptr)( + const char *, + const void *, + ::size_t, + void *) = NULL, + void* data = NULL, + cl_int* err = NULL) + { + cl_int error; + +#if !defined(__APPLE__) && !defined(__MACOS) + cl_context_properties prop[4] = {CL_CONTEXT_PLATFORM, 0, 0, 0 }; + + if (properties == NULL) { + // Get a valid platform ID as we cannot send in a blank one + VECTOR_CLASS platforms; + error = Platform::get(&platforms); + if (error != CL_SUCCESS) { + detail::errHandler(error, __CREATE_CONTEXT_FROM_TYPE_ERR); + if (err != NULL) { + *err = error; + } + return; + } + + // Check the platforms we found for a device of our specified type + cl_context_properties platform_id = 0; + for (unsigned int i = 0; i < platforms.size(); i++) { + + VECTOR_CLASS devices; + +#if defined(__CL_ENABLE_EXCEPTIONS) + try { +#endif + + error = platforms[i].getDevices(type, &devices); + +#if defined(__CL_ENABLE_EXCEPTIONS) + } catch (Error) {} + // Catch if exceptions are enabled as we don't want to exit if first platform has no devices of type + // We do error checking next anyway, and can throw there if needed +#endif + + // Only squash CL_SUCCESS and CL_DEVICE_NOT_FOUND + if (error != CL_SUCCESS && error != CL_DEVICE_NOT_FOUND) { + detail::errHandler(error, __CREATE_CONTEXT_FROM_TYPE_ERR); + if (err != NULL) { + *err = error; + } + } + + if (devices.size() > 0) { + platform_id = (cl_context_properties)platforms[i](); + break; + } + } + + if (platform_id == 0) { + detail::errHandler(CL_DEVICE_NOT_FOUND, __CREATE_CONTEXT_FROM_TYPE_ERR); + if (err != NULL) { + *err = CL_DEVICE_NOT_FOUND; + } + return; + } + + prop[1] = platform_id; + properties = &prop[0]; + } +#endif + object_ = ::clCreateContextFromType( + properties, type, notifyFptr, data, &error); + + detail::errHandler(error, __CREATE_CONTEXT_FROM_TYPE_ERR); + if (err != NULL) { + *err = error; + } + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Context(const Context& ctx) : detail::Wrapper(ctx) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Context& operator = (const Context &ctx) + { + detail::Wrapper::operator=(ctx); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Context(Context&& ctx) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(ctx)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Context& operator = (Context &&ctx) + { + detail::Wrapper::operator=(std::move(ctx)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + /*! \brief Returns a singleton context including all devices of CL_DEVICE_TYPE_DEFAULT. + * + * \note All calls to this function return the same cl_context as the first. + */ + static Context getDefault(cl_int * err = NULL) + { + int state = detail::compare_exchange( + &default_initialized_, + __DEFAULT_BEING_INITIALIZED, __DEFAULT_NOT_INITIALIZED); + + if (state & __DEFAULT_INITIALIZED) { + if (err != NULL) { + *err = default_error_; + } + return default_; + } + + if (state & __DEFAULT_BEING_INITIALIZED) { + // Assume writes will propagate eventually... + while(default_initialized_ != __DEFAULT_INITIALIZED) { + detail::fence(); + } + + if (err != NULL) { + *err = default_error_; + } + return default_; + } + + cl_int error; + default_ = Context( + CL_DEVICE_TYPE_DEFAULT, + NULL, + NULL, + NULL, + &error); + + detail::fence(); + + default_error_ = error; + // Assume writes will propagate eventually... + default_initialized_ = __DEFAULT_INITIALIZED; + + detail::fence(); + + if (err != NULL) { + *err = default_error_; + } + return default_; + + } + + //! \brief Default constructor - initializes to NULL. + Context() : detail::Wrapper() { } + + /*! \brief Constructor from cl_context - takes ownership. + * + * This effectively transfers ownership of a refcount on the cl_context + * into the new Context object. + */ + __CL_EXPLICIT_CONSTRUCTORS Context(const cl_context& context) : detail::Wrapper(context) { } + + /*! \brief Assignment operator from cl_context - takes ownership. + * + * This effectively transfers ownership of a refcount on the rhs and calls + * clReleaseContext() on the value previously held by this instance. + */ + Context& operator = (const cl_context& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + //! \brief Wrapper for clGetContextInfo(). + template + cl_int getInfo(cl_context_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetContextInfo, object_, name, param), + __GET_CONTEXT_INFO_ERR); + } + + //! \brief Wrapper for clGetContextInfo() that returns by value. + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_context_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + /*! \brief Gets a list of supported image formats. + * + * Wraps clGetSupportedImageFormats(). + */ + cl_int getSupportedImageFormats( + cl_mem_flags flags, + cl_mem_object_type type, + VECTOR_CLASS* formats) const + { + cl_uint numEntries; + + if (!formats) { + return CL_SUCCESS; + } + + cl_int err = ::clGetSupportedImageFormats( + object_, + flags, + type, + 0, + NULL, + &numEntries); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_SUPPORTED_IMAGE_FORMATS_ERR); + } + + if (numEntries > 0) { + ImageFormat* value = (ImageFormat*) + alloca(numEntries * sizeof(ImageFormat)); + err = ::clGetSupportedImageFormats( + object_, + flags, + type, + numEntries, + (cl_image_format*)value, + NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __GET_SUPPORTED_IMAGE_FORMATS_ERR); + } + + formats->assign(&value[0], &value[numEntries]); + } + else { + formats->clear(); + } + return CL_SUCCESS; + } +}; + +inline Device Device::getDefault(cl_int * err) +{ + cl_int error; + Device device; + + Context context = Context::getDefault(&error); + detail::errHandler(error, __CREATE_CONTEXT_ERR); + + if (error != CL_SUCCESS) { + if (err != NULL) { + *err = error; + } + } + else { + device = context.getInfo()[0]; + if (err != NULL) { + *err = CL_SUCCESS; + } + } + + return device; +} + + +#ifdef _WIN32 +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED +__declspec(selectany) std::atomic Context::default_initialized_; +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__declspec(selectany) volatile int Context::default_initialized_ = __DEFAULT_NOT_INITIALIZED; +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__declspec(selectany) Context Context::default_; +__declspec(selectany) volatile cl_int Context::default_error_ = CL_SUCCESS; +#else // !_WIN32 +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED +__attribute__((weak)) std::atomic Context::default_initialized_; +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__attribute__((weak)) volatile int Context::default_initialized_ = __DEFAULT_NOT_INITIALIZED; +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__attribute__((weak)) Context Context::default_; +__attribute__((weak)) volatile cl_int Context::default_error_ = CL_SUCCESS; +#endif // !_WIN32 + +/*! \brief Class interface for cl_event. + * + * \note Copies of these objects are shallow, meaning that the copy will refer + * to the same underlying cl_event as the original. For details, see + * clRetainEvent() and clReleaseEvent(). + * + * \see cl_event + */ +class Event : public detail::Wrapper +{ +public: + //! \brief Default constructor - initializes to NULL. + Event() : detail::Wrapper() { } + + /*! \brief Constructor from cl_event - takes ownership. + * + * This effectively transfers ownership of a refcount on the cl_event + * into the new Event object. + */ + __CL_EXPLICIT_CONSTRUCTORS Event(const cl_event& event) : detail::Wrapper(event) { } + + /*! \brief Assignment operator from cl_event - takes ownership. + * + * This effectively transfers ownership of a refcount on the rhs and calls + * clReleaseEvent() on the value previously held by this instance. + */ + Event& operator = (const cl_event& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + //! \brief Wrapper for clGetEventInfo(). + template + cl_int getInfo(cl_event_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetEventInfo, object_, name, param), + __GET_EVENT_INFO_ERR); + } + + //! \brief Wrapper for clGetEventInfo() that returns by value. + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_event_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + //! \brief Wrapper for clGetEventProfilingInfo(). + template + cl_int getProfilingInfo(cl_profiling_info name, T* param) const + { + return detail::errHandler(detail::getInfo( + &::clGetEventProfilingInfo, object_, name, param), + __GET_EVENT_PROFILE_INFO_ERR); + } + + //! \brief Wrapper for clGetEventProfilingInfo() that returns by value. + template typename + detail::param_traits::param_type + getProfilingInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_profiling_info, name>::param_type param; + cl_int result = getProfilingInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + /*! \brief Blocks the calling thread until this event completes. + * + * Wraps clWaitForEvents(). + */ + cl_int wait() const + { + return detail::errHandler( + ::clWaitForEvents(1, &object_), + __WAIT_FOR_EVENTS_ERR); + } + +#if defined(CL_VERSION_1_1) + /*! \brief Registers a user callback function for a specific command execution status. + * + * Wraps clSetEventCallback(). + */ + cl_int setCallback( + cl_int type, + void (CL_CALLBACK * pfn_notify)(cl_event, cl_int, void *), + void * user_data = NULL) + { + return detail::errHandler( + ::clSetEventCallback( + object_, + type, + pfn_notify, + user_data), + __SET_EVENT_CALLBACK_ERR); + } +#endif + + /*! \brief Blocks the calling thread until every event specified is complete. + * + * Wraps clWaitForEvents(). + */ + static cl_int + waitForEvents(const VECTOR_CLASS& events) + { + return detail::errHandler( + ::clWaitForEvents( + (cl_uint) events.size(), (events.size() > 0) ? (cl_event*)&events.front() : NULL), + __WAIT_FOR_EVENTS_ERR); + } +}; + +#if defined(CL_VERSION_1_1) +/*! \brief Class interface for user events (a subset of cl_event's). + * + * See Event for details about copy semantics, etc. + */ +class UserEvent : public Event +{ +public: + /*! \brief Constructs a user event on a given context. + * + * Wraps clCreateUserEvent(). + */ + UserEvent( + const Context& context, + cl_int * err = NULL) + { + cl_int error; + object_ = ::clCreateUserEvent( + context(), + &error); + + detail::errHandler(error, __CREATE_USER_EVENT_ERR); + if (err != NULL) { + *err = error; + } + } + + //! \brief Default constructor - initializes to NULL. + UserEvent() : Event() { } + + /*! \brief Sets the execution status of a user event object. + * + * Wraps clSetUserEventStatus(). + */ + cl_int setStatus(cl_int status) + { + return detail::errHandler( + ::clSetUserEventStatus(object_,status), + __SET_USER_EVENT_STATUS_ERR); + } +}; +#endif + +/*! \brief Blocks the calling thread until every event specified is complete. + * + * Wraps clWaitForEvents(). + */ +inline static cl_int +WaitForEvents(const VECTOR_CLASS& events) +{ + return detail::errHandler( + ::clWaitForEvents( + (cl_uint) events.size(), (events.size() > 0) ? (cl_event*)&events.front() : NULL), + __WAIT_FOR_EVENTS_ERR); +} + +/*! \brief Class interface for cl_mem. + * + * \note Copies of these objects are shallow, meaning that the copy will refer + * to the same underlying cl_mem as the original. For details, see + * clRetainMemObject() and clReleaseMemObject(). + * + * \see cl_mem + */ +class Memory : public detail::Wrapper +{ +public: + //! \brief Default constructor - initializes to NULL. + Memory() : detail::Wrapper() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * This effectively transfers ownership of a refcount on the cl_mem + * into the new Memory object. + */ + __CL_EXPLICIT_CONSTRUCTORS Memory(const cl_mem& memory) : detail::Wrapper(memory) { } + + /*! \brief Assignment operator from cl_mem - takes ownership. + * + * This effectively transfers ownership of a refcount on the rhs and calls + * clReleaseMemObject() on the value previously held by this instance. + */ + Memory& operator = (const cl_mem& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Memory(const Memory& mem) : detail::Wrapper(mem) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Memory& operator = (const Memory &mem) + { + detail::Wrapper::operator=(mem); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Memory(Memory&& mem) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(mem)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Memory& operator = (Memory &&mem) + { + detail::Wrapper::operator=(std::move(mem)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + //! \brief Wrapper for clGetMemObjectInfo(). + template + cl_int getInfo(cl_mem_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetMemObjectInfo, object_, name, param), + __GET_MEM_OBJECT_INFO_ERR); + } + + //! \brief Wrapper for clGetMemObjectInfo() that returns by value. + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_mem_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + +#if defined(CL_VERSION_1_1) + /*! \brief Registers a callback function to be called when the memory object + * is no longer needed. + * + * Wraps clSetMemObjectDestructorCallback(). + * + * Repeated calls to this function, for a given cl_mem value, will append + * to the list of functions called (in reverse order) when memory object's + * resources are freed and the memory object is deleted. + * + * \note + * The registered callbacks are associated with the underlying cl_mem + * value - not the Memory class instance. + */ + cl_int setDestructorCallback( + void (CL_CALLBACK * pfn_notify)(cl_mem, void *), + void * user_data = NULL) + { + return detail::errHandler( + ::clSetMemObjectDestructorCallback( + object_, + pfn_notify, + user_data), + __SET_MEM_OBJECT_DESTRUCTOR_CALLBACK_ERR); + } +#endif + +}; + +// Pre-declare copy functions +class Buffer; +template< typename IteratorType > +cl_int copy( IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer ); +template< typename IteratorType > +cl_int copy( const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator ); +template< typename IteratorType > +cl_int copy( const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer ); +template< typename IteratorType > +cl_int copy( const CommandQueue &queue, const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator ); + + +/*! \brief Class interface for Buffer Memory Objects. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class Buffer : public Memory +{ +public: + + /*! \brief Constructs a Buffer in a specified context. + * + * Wraps clCreateBuffer(). + * + * \param host_ptr Storage to be used if the CL_MEM_USE_HOST_PTR flag was + * specified. Note alignment & exclusivity requirements. + */ + Buffer( + const Context& context, + cl_mem_flags flags, + ::size_t size, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + object_ = ::clCreateBuffer(context(), flags, size, host_ptr, &error); + + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } + + /*! \brief Constructs a Buffer in the default context. + * + * Wraps clCreateBuffer(). + * + * \param host_ptr Storage to be used if the CL_MEM_USE_HOST_PTR flag was + * specified. Note alignment & exclusivity requirements. + * + * \see Context::getDefault() + */ + Buffer( + cl_mem_flags flags, + ::size_t size, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + + Context context = Context::getDefault(err); + + object_ = ::clCreateBuffer(context(), flags, size, host_ptr, &error); + + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } + + /*! + * \brief Construct a Buffer from a host container via iterators. + * IteratorType must be random access. + * If useHostPtr is specified iterators must represent contiguous data. + */ + template< typename IteratorType > + Buffer( + IteratorType startIterator, + IteratorType endIterator, + bool readOnly, + bool useHostPtr = false, + cl_int* err = NULL) + { + typedef typename std::iterator_traits::value_type DataType; + cl_int error; + + cl_mem_flags flags = 0; + if( readOnly ) { + flags |= CL_MEM_READ_ONLY; + } + else { + flags |= CL_MEM_READ_WRITE; + } + if( useHostPtr ) { + flags |= CL_MEM_USE_HOST_PTR; + } + + ::size_t size = sizeof(DataType)*(endIterator - startIterator); + + Context context = Context::getDefault(err); + + if( useHostPtr ) { + object_ = ::clCreateBuffer(context(), flags, size, static_cast(&*startIterator), &error); + } else { + object_ = ::clCreateBuffer(context(), flags, size, 0, &error); + } + + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + + if( !useHostPtr ) { + error = cl::copy(startIterator, endIterator, *this); + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } + } + + /*! + * \brief Construct a Buffer from a host container via iterators using a specified context. + * IteratorType must be random access. + * If useHostPtr is specified iterators must represent contiguous data. + */ + template< typename IteratorType > + Buffer(const Context &context, IteratorType startIterator, IteratorType endIterator, + bool readOnly, bool useHostPtr = false, cl_int* err = NULL); + + /*! + * \brief Construct a Buffer from a host container via iterators using a specified queue. + * If useHostPtr is specified iterators must represent contiguous data. + */ + template< typename IteratorType > + Buffer(const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, + bool readOnly, bool useHostPtr = false, cl_int* err = NULL); + + //! \brief Default constructor - initializes to NULL. + Buffer() : Memory() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Buffer(const cl_mem& buffer) : Memory(buffer) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Buffer& operator = (const cl_mem& rhs) + { + Memory::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Buffer(const Buffer& buf) : Memory(buf) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Buffer& operator = (const Buffer &buf) + { + Memory::operator=(buf); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Buffer(Buffer&& buf) CL_HPP_NOEXCEPT : Memory(std::move(buf)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Buffer& operator = (Buffer &&buf) + { + Memory::operator=(std::move(buf)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + +#if defined(CL_VERSION_1_1) + /*! \brief Creates a new buffer object from this. + * + * Wraps clCreateSubBuffer(). + */ + Buffer createSubBuffer( + cl_mem_flags flags, + cl_buffer_create_type buffer_create_type, + const void * buffer_create_info, + cl_int * err = NULL) + { + Buffer result; + cl_int error; + result.object_ = ::clCreateSubBuffer( + object_, + flags, + buffer_create_type, + buffer_create_info, + &error); + + detail::errHandler(error, __CREATE_SUBBUFFER_ERR); + if (err != NULL) { + *err = error; + } + + return result; + } +#endif +}; + +#if defined (USE_DX_INTEROP) +/*! \brief Class interface for creating OpenCL buffers from ID3D10Buffer's. + * + * This is provided to facilitate interoperability with Direct3D. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class BufferD3D10 : public Buffer +{ +public: + typedef CL_API_ENTRY cl_mem (CL_API_CALL *PFN_clCreateFromD3D10BufferKHR)( + cl_context context, cl_mem_flags flags, ID3D10Buffer* buffer, + cl_int* errcode_ret); + + /*! \brief Constructs a BufferD3D10, in a specified context, from a + * given ID3D10Buffer. + * + * Wraps clCreateFromD3D10BufferKHR(). + */ + BufferD3D10( + const Context& context, + cl_mem_flags flags, + ID3D10Buffer* bufobj, + cl_int * err = NULL) + { + static PFN_clCreateFromD3D10BufferKHR pfn_clCreateFromD3D10BufferKHR = NULL; + +#if defined(CL_VERSION_1_2) + vector props = context.getInfo(); + cl_platform platform = -1; + for( int i = 0; i < props.size(); ++i ) { + if( props[i] == CL_CONTEXT_PLATFORM ) { + platform = props[i+1]; + } + } + __INIT_CL_EXT_FCN_PTR_PLATFORM(platform, clCreateFromD3D10BufferKHR); +#endif +#if defined(CL_VERSION_1_1) + __INIT_CL_EXT_FCN_PTR(clCreateFromD3D10BufferKHR); +#endif + + cl_int error; + object_ = pfn_clCreateFromD3D10BufferKHR( + context(), + flags, + bufobj, + &error); + + detail::errHandler(error, __CREATE_GL_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } + + //! \brief Default constructor - initializes to NULL. + BufferD3D10() : Buffer() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS BufferD3D10(const cl_mem& buffer) : Buffer(buffer) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + BufferD3D10& operator = (const cl_mem& rhs) + { + Buffer::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + BufferD3D10(const BufferD3D10& buf) : Buffer(buf) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + BufferD3D10& operator = (const BufferD3D10 &buf) + { + Buffer::operator=(buf); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + BufferD3D10(BufferD3D10&& buf) CL_HPP_NOEXCEPT : Buffer(std::move(buf)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + BufferD3D10& operator = (BufferD3D10 &&buf) + { + Buffer::operator=(std::move(buf)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; +#endif + +/*! \brief Class interface for GL Buffer Memory Objects. + * + * This is provided to facilitate interoperability with OpenGL. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class BufferGL : public Buffer +{ +public: + /*! \brief Constructs a BufferGL in a specified context, from a given + * GL buffer. + * + * Wraps clCreateFromGLBuffer(). + */ + BufferGL( + const Context& context, + cl_mem_flags flags, + cl_GLuint bufobj, + cl_int * err = NULL) + { + cl_int error; + object_ = ::clCreateFromGLBuffer( + context(), + flags, + bufobj, + &error); + + detail::errHandler(error, __CREATE_GL_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } + + //! \brief Default constructor - initializes to NULL. + BufferGL() : Buffer() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS BufferGL(const cl_mem& buffer) : Buffer(buffer) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + BufferGL& operator = (const cl_mem& rhs) + { + Buffer::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + BufferGL(const BufferGL& buf) : Buffer(buf) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + BufferGL& operator = (const BufferGL &buf) + { + Buffer::operator=(buf); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + BufferGL(BufferGL&& buf) CL_HPP_NOEXCEPT : Buffer(std::move(buf)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + BufferGL& operator = (BufferGL &&buf) + { + Buffer::operator=(std::move(buf)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + //! \brief Wrapper for clGetGLObjectInfo(). + cl_int getObjectInfo( + cl_gl_object_type *type, + cl_GLuint * gl_object_name) + { + return detail::errHandler( + ::clGetGLObjectInfo(object_,type,gl_object_name), + __GET_GL_OBJECT_INFO_ERR); + } +}; + +/*! \brief C++ base class for Image Memory objects. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class Image : public Memory +{ +protected: + //! \brief Default constructor - initializes to NULL. + Image() : Memory() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Image(const cl_mem& image) : Memory(image) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Image& operator = (const cl_mem& rhs) + { + Memory::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image(const Image& img) : Memory(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image& operator = (const Image &img) + { + Memory::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image(Image&& img) CL_HPP_NOEXCEPT : Memory(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image& operator = (Image &&img) + { + Memory::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + +public: + //! \brief Wrapper for clGetImageInfo(). + template + cl_int getImageInfo(cl_image_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetImageInfo, object_, name, param), + __GET_IMAGE_INFO_ERR); + } + + //! \brief Wrapper for clGetImageInfo() that returns by value. + template typename + detail::param_traits::param_type + getImageInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_image_info, name>::param_type param; + cl_int result = getImageInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } +}; + +#if defined(CL_VERSION_1_2) +/*! \brief Class interface for 1D Image Memory objects. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class Image1D : public Image +{ +public: + /*! \brief Constructs a 1D Image in a specified context. + * + * Wraps clCreateImage(). + */ + Image1D( + const Context& context, + cl_mem_flags flags, + ImageFormat format, + ::size_t width, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + cl_image_desc desc = + { + CL_MEM_OBJECT_IMAGE1D, + width, + 0, 0, 0, 0, 0, 0, 0, 0 + }; + object_ = ::clCreateImage( + context(), + flags, + &format, + &desc, + host_ptr, + &error); + + detail::errHandler(error, __CREATE_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + } + + //! \brief Default constructor - initializes to NULL. + Image1D() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Image1D(const cl_mem& image1D) : Image(image1D) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Image1D& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image1D(const Image1D& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image1D& operator = (const Image1D &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image1D(Image1D&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image1D& operator = (Image1D &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; + +/*! \class Image1DBuffer + * \brief Image interface for 1D buffer images. + */ +class Image1DBuffer : public Image +{ +public: + Image1DBuffer( + const Context& context, + cl_mem_flags flags, + ImageFormat format, + ::size_t width, + const Buffer &buffer, + cl_int* err = NULL) + { + cl_int error; + cl_image_desc desc = + { + CL_MEM_OBJECT_IMAGE1D_BUFFER, + width, + 0, 0, 0, 0, 0, 0, 0, + buffer() + }; + object_ = ::clCreateImage( + context(), + flags, + &format, + &desc, + NULL, + &error); + + detail::errHandler(error, __CREATE_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + } + + Image1DBuffer() { } + + __CL_EXPLICIT_CONSTRUCTORS Image1DBuffer(const cl_mem& image1D) : Image(image1D) { } + + Image1DBuffer& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image1DBuffer(const Image1DBuffer& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image1DBuffer& operator = (const Image1DBuffer &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image1DBuffer(Image1DBuffer&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image1DBuffer& operator = (Image1DBuffer &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; + +/*! \class Image1DArray + * \brief Image interface for arrays of 1D images. + */ +class Image1DArray : public Image +{ +public: + Image1DArray( + const Context& context, + cl_mem_flags flags, + ImageFormat format, + ::size_t arraySize, + ::size_t width, + ::size_t rowPitch, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + cl_image_desc desc = + { + CL_MEM_OBJECT_IMAGE1D_ARRAY, + width, + 0, 0, // height, depth (unused) + arraySize, + rowPitch, + 0, 0, 0, 0 + }; + object_ = ::clCreateImage( + context(), + flags, + &format, + &desc, + host_ptr, + &error); + + detail::errHandler(error, __CREATE_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + } + + Image1DArray() { } + + __CL_EXPLICIT_CONSTRUCTORS Image1DArray(const cl_mem& imageArray) : Image(imageArray) { } + + Image1DArray& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image1DArray(const Image1DArray& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image1DArray& operator = (const Image1DArray &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image1DArray(Image1DArray&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image1DArray& operator = (Image1DArray &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; +#endif // #if defined(CL_VERSION_1_2) + + +/*! \brief Class interface for 2D Image Memory objects. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class Image2D : public Image +{ +public: + /*! \brief Constructs a 1D Image in a specified context. + * + * Wraps clCreateImage(). + */ + Image2D( + const Context& context, + cl_mem_flags flags, + ImageFormat format, + ::size_t width, + ::size_t height, + ::size_t row_pitch = 0, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + bool useCreateImage; + +#if defined(CL_VERSION_1_2) && defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + // Run-time decision based on the actual platform + { + cl_uint version = detail::getContextPlatformVersion(context()); + useCreateImage = (version >= 0x10002); // OpenCL 1.2 or above + } +#elif defined(CL_VERSION_1_2) + useCreateImage = true; +#else + useCreateImage = false; +#endif + +#if defined(CL_VERSION_1_2) + if (useCreateImage) + { + cl_image_desc desc = + { + CL_MEM_OBJECT_IMAGE2D, + width, + height, + 0, 0, // depth, array size (unused) + row_pitch, + 0, 0, 0, 0 + }; + object_ = ::clCreateImage( + context(), + flags, + &format, + &desc, + host_ptr, + &error); + + detail::errHandler(error, __CREATE_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + } +#endif // #if defined(CL_VERSION_1_2) +#if !defined(CL_VERSION_1_2) || defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + if (!useCreateImage) + { + object_ = ::clCreateImage2D( + context(), flags,&format, width, height, row_pitch, host_ptr, &error); + + detail::errHandler(error, __CREATE_IMAGE2D_ERR); + if (err != NULL) { + *err = error; + } + } +#endif // #if !defined(CL_VERSION_1_2) || defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + } + + //! \brief Default constructor - initializes to NULL. + Image2D() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Image2D(const cl_mem& image2D) : Image(image2D) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Image2D& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image2D(const Image2D& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image2D& operator = (const Image2D &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image2D(Image2D&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image2D& operator = (Image2D &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; + + +#if !defined(CL_VERSION_1_2) +/*! \brief Class interface for GL 2D Image Memory objects. + * + * This is provided to facilitate interoperability with OpenGL. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + * \note Deprecated for OpenCL 1.2. Please use ImageGL instead. + */ +class CL_EXT_PREFIX__VERSION_1_1_DEPRECATED Image2DGL CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED : public Image2D +{ +public: + /*! \brief Constructs an Image2DGL in a specified context, from a given + * GL Texture. + * + * Wraps clCreateFromGLTexture2D(). + */ + Image2DGL( + const Context& context, + cl_mem_flags flags, + cl_GLenum target, + cl_GLint miplevel, + cl_GLuint texobj, + cl_int * err = NULL) + { + cl_int error; + object_ = ::clCreateFromGLTexture2D( + context(), + flags, + target, + miplevel, + texobj, + &error); + + detail::errHandler(error, __CREATE_GL_TEXTURE_2D_ERR); + if (err != NULL) { + *err = error; + } + + } + + //! \brief Default constructor - initializes to NULL. + Image2DGL() : Image2D() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Image2DGL(const cl_mem& image) : Image2D(image) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Image2DGL& operator = (const cl_mem& rhs) + { + Image2D::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image2DGL(const Image2DGL& img) : Image2D(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image2DGL& operator = (const Image2DGL &img) + { + Image2D::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image2DGL(Image2DGL&& img) CL_HPP_NOEXCEPT : Image2D(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image2DGL& operator = (Image2DGL &&img) + { + Image2D::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; +#endif // #if !defined(CL_VERSION_1_2) + +#if defined(CL_VERSION_1_2) +/*! \class Image2DArray + * \brief Image interface for arrays of 2D images. + */ +class Image2DArray : public Image +{ +public: + Image2DArray( + const Context& context, + cl_mem_flags flags, + ImageFormat format, + ::size_t arraySize, + ::size_t width, + ::size_t height, + ::size_t rowPitch, + ::size_t slicePitch, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + cl_image_desc desc = + { + CL_MEM_OBJECT_IMAGE2D_ARRAY, + width, + height, + 0, // depth (unused) + arraySize, + rowPitch, + slicePitch, + 0, 0, 0 + }; + object_ = ::clCreateImage( + context(), + flags, + &format, + &desc, + host_ptr, + &error); + + detail::errHandler(error, __CREATE_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + } + + Image2DArray() { } + + __CL_EXPLICIT_CONSTRUCTORS Image2DArray(const cl_mem& imageArray) : Image(imageArray) { } + + Image2DArray& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image2DArray(const Image2DArray& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image2DArray& operator = (const Image2DArray &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image2DArray(Image2DArray&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image2DArray& operator = (Image2DArray &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; +#endif // #if defined(CL_VERSION_1_2) + +/*! \brief Class interface for 3D Image Memory objects. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class Image3D : public Image +{ +public: + /*! \brief Constructs a 3D Image in a specified context. + * + * Wraps clCreateImage(). + */ + Image3D( + const Context& context, + cl_mem_flags flags, + ImageFormat format, + ::size_t width, + ::size_t height, + ::size_t depth, + ::size_t row_pitch = 0, + ::size_t slice_pitch = 0, + void* host_ptr = NULL, + cl_int* err = NULL) + { + cl_int error; + bool useCreateImage; + +#if defined(CL_VERSION_1_2) && defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + // Run-time decision based on the actual platform + { + cl_uint version = detail::getContextPlatformVersion(context()); + useCreateImage = (version >= 0x10002); // OpenCL 1.2 or above + } +#elif defined(CL_VERSION_1_2) + useCreateImage = true; +#else + useCreateImage = false; +#endif + +#if defined(CL_VERSION_1_2) + if (useCreateImage) + { + cl_image_desc desc = + { + CL_MEM_OBJECT_IMAGE3D, + width, + height, + depth, + 0, // array size (unused) + row_pitch, + slice_pitch, + 0, 0, 0 + }; + object_ = ::clCreateImage( + context(), + flags, + &format, + &desc, + host_ptr, + &error); + + detail::errHandler(error, __CREATE_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + } +#endif // #if defined(CL_VERSION_1_2) +#if !defined(CL_VERSION_1_2) || defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + if (!useCreateImage) + { + object_ = ::clCreateImage3D( + context(), flags, &format, width, height, depth, row_pitch, + slice_pitch, host_ptr, &error); + + detail::errHandler(error, __CREATE_IMAGE3D_ERR); + if (err != NULL) { + *err = error; + } + } +#endif // #if !defined(CL_VERSION_1_2) || defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) + } + + //! \brief Default constructor - initializes to NULL. + Image3D() : Image() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Image3D(const cl_mem& image3D) : Image(image3D) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Image3D& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image3D(const Image3D& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image3D& operator = (const Image3D &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image3D(Image3D&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image3D& operator = (Image3D &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; + +#if !defined(CL_VERSION_1_2) +/*! \brief Class interface for GL 3D Image Memory objects. + * + * This is provided to facilitate interoperability with OpenGL. + * + * See Memory for details about copy semantics, etc. + * + * \see Memory + */ +class Image3DGL : public Image3D +{ +public: + /*! \brief Constructs an Image3DGL in a specified context, from a given + * GL Texture. + * + * Wraps clCreateFromGLTexture3D(). + */ + Image3DGL( + const Context& context, + cl_mem_flags flags, + cl_GLenum target, + cl_GLint miplevel, + cl_GLuint texobj, + cl_int * err = NULL) + { + cl_int error; + object_ = ::clCreateFromGLTexture3D( + context(), + flags, + target, + miplevel, + texobj, + &error); + + detail::errHandler(error, __CREATE_GL_TEXTURE_3D_ERR); + if (err != NULL) { + *err = error; + } + } + + //! \brief Default constructor - initializes to NULL. + Image3DGL() : Image3D() { } + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ + __CL_EXPLICIT_CONSTRUCTORS Image3DGL(const cl_mem& image) : Image3D(image) { } + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + Image3DGL& operator = (const cl_mem& rhs) + { + Image3D::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image3DGL(const Image3DGL& img) : Image3D(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Image3DGL& operator = (const Image3DGL &img) + { + Image3D::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Image3DGL(Image3DGL&& img) CL_HPP_NOEXCEPT : Image3D(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Image3DGL& operator = (Image3DGL &&img) + { + Image3D::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; +#endif // #if !defined(CL_VERSION_1_2) + +#if defined(CL_VERSION_1_2) +/*! \class ImageGL + * \brief general image interface for GL interop. + * We abstract the 2D and 3D GL images into a single instance here + * that wraps all GL sourced images on the grounds that setup information + * was performed by OpenCL anyway. + */ +class ImageGL : public Image +{ +public: + ImageGL( + const Context& context, + cl_mem_flags flags, + cl_GLenum target, + cl_GLint miplevel, + cl_GLuint texobj, + cl_int * err = NULL) + { + cl_int error; + object_ = ::clCreateFromGLTexture( + context(), + flags, + target, + miplevel, + texobj, + &error); + + detail::errHandler(error, __CREATE_GL_TEXTURE_ERR); + if (err != NULL) { + *err = error; + } + } + + ImageGL() : Image() { } + + __CL_EXPLICIT_CONSTRUCTORS ImageGL(const cl_mem& image) : Image(image) { } + + ImageGL& operator = (const cl_mem& rhs) + { + Image::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + ImageGL(const ImageGL& img) : Image(img) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + ImageGL& operator = (const ImageGL &img) + { + Image::operator=(img); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + ImageGL(ImageGL&& img) CL_HPP_NOEXCEPT : Image(std::move(img)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + ImageGL& operator = (ImageGL &&img) + { + Image::operator=(std::move(img)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) +}; +#endif // #if defined(CL_VERSION_1_2) + +/*! \brief Class interface for GL Render Buffer Memory Objects. +* +* This is provided to facilitate interoperability with OpenGL. +* +* See Memory for details about copy semantics, etc. +* +* \see Memory +*/ +class BufferRenderGL : +#if defined(CL_VERSION_1_2) + public ImageGL +#else // #if defined(CL_VERSION_1_2) + public Image2DGL +#endif //#if defined(CL_VERSION_1_2) +{ +public: + /*! \brief Constructs a BufferRenderGL in a specified context, from a given + * GL Renderbuffer. + * + * Wraps clCreateFromGLRenderbuffer(). + */ + BufferRenderGL( + const Context& context, + cl_mem_flags flags, + cl_GLuint bufobj, + cl_int * err = NULL) + { + cl_int error; + object_ = ::clCreateFromGLRenderbuffer( + context(), + flags, + bufobj, + &error); + + detail::errHandler(error, __CREATE_GL_RENDER_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } + + //! \brief Default constructor - initializes to NULL. +#if defined(CL_VERSION_1_2) + BufferRenderGL() : ImageGL() {}; +#else // #if defined(CL_VERSION_1_2) + BufferRenderGL() : Image2DGL() {}; +#endif //#if defined(CL_VERSION_1_2) + + /*! \brief Constructor from cl_mem - takes ownership. + * + * See Memory for further details. + */ +#if defined(CL_VERSION_1_2) + __CL_EXPLICIT_CONSTRUCTORS BufferRenderGL(const cl_mem& buffer) : ImageGL(buffer) { } +#else // #if defined(CL_VERSION_1_2) + __CL_EXPLICIT_CONSTRUCTORS BufferRenderGL(const cl_mem& buffer) : Image2DGL(buffer) { } +#endif //#if defined(CL_VERSION_1_2) + + + /*! \brief Assignment from cl_mem - performs shallow copy. + * + * See Memory for further details. + */ + BufferRenderGL& operator = (const cl_mem& rhs) + { +#if defined(CL_VERSION_1_2) + ImageGL::operator=(rhs); +#else // #if defined(CL_VERSION_1_2) + Image2DGL::operator=(rhs); +#endif //#if defined(CL_VERSION_1_2) + + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ +#if defined(CL_VERSION_1_2) + BufferRenderGL(const BufferRenderGL& buf) : ImageGL(buf) {} +#else // #if defined(CL_VERSION_1_2) + BufferRenderGL(const BufferRenderGL& buf) : Image2DGL(buf) {} +#endif //#if defined(CL_VERSION_1_2) + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + BufferRenderGL& operator = (const BufferRenderGL &rhs) + { +#if defined(CL_VERSION_1_2) + ImageGL::operator=(rhs); +#else // #if defined(CL_VERSION_1_2) + Image2DGL::operator=(rhs); +#endif //#if defined(CL_VERSION_1_2) + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ +#if defined(CL_VERSION_1_2) + BufferRenderGL(BufferRenderGL&& buf) CL_HPP_NOEXCEPT : ImageGL(std::move(buf)) {} +#else // #if defined(CL_VERSION_1_2) + BufferRenderGL(BufferRenderGL&& buf) CL_HPP_NOEXCEPT : Image2DGL(std::move(buf)) {} +#endif //#if defined(CL_VERSION_1_2) + + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + BufferRenderGL& operator = (BufferRenderGL &&buf) + { +#if defined(CL_VERSION_1_2) + ImageGL::operator=(std::move(buf)); +#else // #if defined(CL_VERSION_1_2) + Image2DGL::operator=(std::move(buf)); +#endif //#if defined(CL_VERSION_1_2) + + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + //! \brief Wrapper for clGetGLObjectInfo(). + cl_int getObjectInfo( + cl_gl_object_type *type, + cl_GLuint * gl_object_name) + { + return detail::errHandler( + ::clGetGLObjectInfo(object_, type, gl_object_name), + __GET_GL_OBJECT_INFO_ERR); + } +}; + +/*! \brief Class interface for cl_sampler. + * + * \note Copies of these objects are shallow, meaning that the copy will refer + * to the same underlying cl_sampler as the original. For details, see + * clRetainSampler() and clReleaseSampler(). + * + * \see cl_sampler + */ +class Sampler : public detail::Wrapper +{ +public: + //! \brief Default constructor - initializes to NULL. + Sampler() { } + + /*! \brief Constructs a Sampler in a specified context. + * + * Wraps clCreateSampler(). + */ + Sampler( + const Context& context, + cl_bool normalized_coords, + cl_addressing_mode addressing_mode, + cl_filter_mode filter_mode, + cl_int* err = NULL) + { + cl_int error; + object_ = ::clCreateSampler( + context(), + normalized_coords, + addressing_mode, + filter_mode, + &error); + + detail::errHandler(error, __CREATE_SAMPLER_ERR); + if (err != NULL) { + *err = error; + } + } + + /*! \brief Constructor from cl_sampler - takes ownership. + * + * This effectively transfers ownership of a refcount on the cl_sampler + * into the new Sampler object. + */ + __CL_EXPLICIT_CONSTRUCTORS Sampler(const cl_sampler& sampler) : detail::Wrapper(sampler) { } + + /*! \brief Assignment operator from cl_sampler - takes ownership. + * + * This effectively transfers ownership of a refcount on the rhs and calls + * clReleaseSampler() on the value previously held by this instance. + */ + Sampler& operator = (const cl_sampler& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Sampler(const Sampler& sam) : detail::Wrapper(sam) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Sampler& operator = (const Sampler &sam) + { + detail::Wrapper::operator=(sam); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Sampler(Sampler&& sam) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(sam)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Sampler& operator = (Sampler &&sam) + { + detail::Wrapper::operator=(std::move(sam)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + //! \brief Wrapper for clGetSamplerInfo(). + template + cl_int getInfo(cl_sampler_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetSamplerInfo, object_, name, param), + __GET_SAMPLER_INFO_ERR); + } + + //! \brief Wrapper for clGetSamplerInfo() that returns by value. + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_sampler_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } +}; + +class Program; +class CommandQueue; +class Kernel; + +//! \brief Class interface for specifying NDRange values. +class NDRange +{ +private: + size_t<3> sizes_; + cl_uint dimensions_; + +public: + //! \brief Default constructor - resulting range has zero dimensions. + NDRange() + : dimensions_(0) + { } + + //! \brief Constructs one-dimensional range. + NDRange(::size_t size0) + : dimensions_(1) + { + sizes_[0] = size0; + } + + //! \brief Constructs two-dimensional range. + NDRange(::size_t size0, ::size_t size1) + : dimensions_(2) + { + sizes_[0] = size0; + sizes_[1] = size1; + } + + //! \brief Constructs three-dimensional range. + NDRange(::size_t size0, ::size_t size1, ::size_t size2) + : dimensions_(3) + { + sizes_[0] = size0; + sizes_[1] = size1; + sizes_[2] = size2; + } + + /*! \brief Conversion operator to const ::size_t *. + * + * \returns a pointer to the size of the first dimension. + */ + operator const ::size_t*() const { + return (const ::size_t*) sizes_; + } + + //! \brief Queries the number of dimensions in the range. + ::size_t dimensions() const { return dimensions_; } +}; + +//! \brief A zero-dimensional range. +static const NDRange NullRange; + +//! \brief Local address wrapper for use with Kernel::setArg +struct LocalSpaceArg +{ + ::size_t size_; +}; + +namespace detail { + +template +struct KernelArgumentHandler +{ + static ::size_t size(const T&) { return sizeof(T); } + static const T* ptr(const T& value) { return &value; } +}; + +template <> +struct KernelArgumentHandler +{ + static ::size_t size(const LocalSpaceArg& value) { return value.size_; } + static const void* ptr(const LocalSpaceArg&) { return NULL; } +}; + +} +//! \endcond + +/*! __local + * \brief Helper function for generating LocalSpaceArg objects. + * Deprecated. Replaced with Local. + */ +inline CL_EXT_PREFIX__VERSION_1_1_DEPRECATED LocalSpaceArg +__local(::size_t size) CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED; +inline LocalSpaceArg +__local(::size_t size) +{ + LocalSpaceArg ret = { size }; + return ret; +} + +/*! Local + * \brief Helper function for generating LocalSpaceArg objects. + */ +inline LocalSpaceArg +Local(::size_t size) +{ + LocalSpaceArg ret = { size }; + return ret; +} + +//class KernelFunctor; + +/*! \brief Class interface for cl_kernel. + * + * \note Copies of these objects are shallow, meaning that the copy will refer + * to the same underlying cl_kernel as the original. For details, see + * clRetainKernel() and clReleaseKernel(). + * + * \see cl_kernel + */ +class Kernel : public detail::Wrapper +{ +public: + inline Kernel(const Program& program, const char* name, cl_int* err = NULL); + + //! \brief Default constructor - initializes to NULL. + Kernel() { } + + /*! \brief Constructor from cl_kernel - takes ownership. + * + * This effectively transfers ownership of a refcount on the cl_kernel + * into the new Kernel object. + */ + __CL_EXPLICIT_CONSTRUCTORS Kernel(const cl_kernel& kernel) : detail::Wrapper(kernel) { } + + /*! \brief Assignment operator from cl_kernel - takes ownership. + * + * This effectively transfers ownership of a refcount on the rhs and calls + * clReleaseKernel() on the value previously held by this instance. + */ + Kernel& operator = (const cl_kernel& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Kernel(const Kernel& kernel) : detail::Wrapper(kernel) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Kernel& operator = (const Kernel &kernel) + { + detail::Wrapper::operator=(kernel); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Kernel(Kernel&& kernel) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(kernel)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Kernel& operator = (Kernel &&kernel) + { + detail::Wrapper::operator=(std::move(kernel)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + template + cl_int getInfo(cl_kernel_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetKernelInfo, object_, name, param), + __GET_KERNEL_INFO_ERR); + } + + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_kernel_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + +#if defined(CL_VERSION_1_2) + template + cl_int getArgInfo(cl_uint argIndex, cl_kernel_arg_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetKernelArgInfo, object_, argIndex, name, param), + __GET_KERNEL_ARG_INFO_ERR); + } + + template typename + detail::param_traits::param_type + getArgInfo(cl_uint argIndex, cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_kernel_arg_info, name>::param_type param; + cl_int result = getArgInfo(argIndex, name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } +#endif // #if defined(CL_VERSION_1_2) + + template + cl_int getWorkGroupInfo( + const Device& device, cl_kernel_work_group_info name, T* param) const + { + return detail::errHandler( + detail::getInfo( + &::clGetKernelWorkGroupInfo, object_, device(), name, param), + __GET_KERNEL_WORK_GROUP_INFO_ERR); + } + + template typename + detail::param_traits::param_type + getWorkGroupInfo(const Device& device, cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_kernel_work_group_info, name>::param_type param; + cl_int result = getWorkGroupInfo(device, name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + template + cl_int setArg(cl_uint index, const T &value) + { + return detail::errHandler( + ::clSetKernelArg( + object_, + index, + detail::KernelArgumentHandler::size(value), + detail::KernelArgumentHandler::ptr(value)), + __SET_KERNEL_ARGS_ERR); + } + + cl_int setArg(cl_uint index, ::size_t size, const void* argPtr) + { + return detail::errHandler( + ::clSetKernelArg(object_, index, size, argPtr), + __SET_KERNEL_ARGS_ERR); + } +}; + +/*! \class Program + * \brief Program interface that implements cl_program. + */ +class Program : public detail::Wrapper +{ +public: + typedef VECTOR_CLASS > Binaries; + typedef VECTOR_CLASS > Sources; + + Program( + const STRING_CLASS& source, + bool build = false, + cl_int* err = NULL) + { + cl_int error; + + const char * strings = source.c_str(); + const ::size_t length = source.size(); + + Context context = Context::getDefault(err); + + object_ = ::clCreateProgramWithSource( + context(), (cl_uint)1, &strings, &length, &error); + + detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR); + + if (error == CL_SUCCESS && build) { + + error = ::clBuildProgram( + object_, + 0, + NULL, + "", + NULL, + NULL); + + detail::errHandler(error, __BUILD_PROGRAM_ERR); + } + + if (err != NULL) { + *err = error; + } + } + + Program( + const Context& context, + const STRING_CLASS& source, + bool build = false, + cl_int* err = NULL) + { + cl_int error; + + const char * strings = source.c_str(); + const ::size_t length = source.size(); + + object_ = ::clCreateProgramWithSource( + context(), (cl_uint)1, &strings, &length, &error); + + detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR); + + if (error == CL_SUCCESS && build) { + + error = ::clBuildProgram( + object_, + 0, + NULL, + "", + NULL, + NULL); + + detail::errHandler(error, __BUILD_PROGRAM_ERR); + } + + if (err != NULL) { + *err = error; + } + } + + Program( + const Context& context, + const Sources& sources, + cl_int* err = NULL) + { + cl_int error; + + const ::size_t n = (::size_t)sources.size(); + ::size_t* lengths = (::size_t*) alloca(n * sizeof(::size_t)); + const char** strings = (const char**) alloca(n * sizeof(const char*)); + + for (::size_t i = 0; i < n; ++i) { + strings[i] = sources[(int)i].first; + lengths[i] = sources[(int)i].second; + } + + object_ = ::clCreateProgramWithSource( + context(), (cl_uint)n, strings, lengths, &error); + + detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR); + if (err != NULL) { + *err = error; + } + } + + /** + * Construct a program object from a list of devices and a per-device list of binaries. + * \param context A valid OpenCL context in which to construct the program. + * \param devices A vector of OpenCL device objects for which the program will be created. + * \param binaries A vector of pairs of a pointer to a binary object and its length. + * \param binaryStatus An optional vector that on completion will be resized to + * match the size of binaries and filled with values to specify if each binary + * was successfully loaded. + * Set to CL_SUCCESS if the binary was successfully loaded. + * Set to CL_INVALID_VALUE if the length is 0 or the binary pointer is NULL. + * Set to CL_INVALID_BINARY if the binary provided is not valid for the matching device. + * \param err if non-NULL will be set to CL_SUCCESS on successful operation or one of the following errors: + * CL_INVALID_CONTEXT if context is not a valid context. + * CL_INVALID_VALUE if the length of devices is zero; or if the length of binaries does not match the length of devices; + * or if any entry in binaries is NULL or has length 0. + * CL_INVALID_DEVICE if OpenCL devices listed in devices are not in the list of devices associated with context. + * CL_INVALID_BINARY if an invalid program binary was encountered for any device. binaryStatus will return specific status for each device. + * CL_OUT_OF_HOST_MEMORY if there is a failure to allocate resources required by the OpenCL implementation on the host. + */ + Program( + const Context& context, + const VECTOR_CLASS& devices, + const Binaries& binaries, + VECTOR_CLASS* binaryStatus = NULL, + cl_int* err = NULL) + { + cl_int error; + + const ::size_t numDevices = devices.size(); + + // Catch size mismatch early and return + if(binaries.size() != numDevices) { + error = CL_INVALID_VALUE; + detail::errHandler(error, __CREATE_PROGRAM_WITH_BINARY_ERR); + if (err != NULL) { + *err = error; + } + return; + } + + ::size_t* lengths = (::size_t*) alloca(numDevices * sizeof(::size_t)); + const unsigned char** images = (const unsigned char**) alloca(numDevices * sizeof(const unsigned char**)); + + for (::size_t i = 0; i < numDevices; ++i) { + images[i] = (const unsigned char*)binaries[i].first; + lengths[i] = binaries[(int)i].second; + } + + cl_device_id* deviceIDs = (cl_device_id*) alloca(numDevices * sizeof(cl_device_id)); + for( ::size_t deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex ) { + deviceIDs[deviceIndex] = (devices[deviceIndex])(); + } + + if(binaryStatus) { + binaryStatus->resize(numDevices); + } + + object_ = ::clCreateProgramWithBinary( + context(), (cl_uint) devices.size(), + deviceIDs, + lengths, images, (binaryStatus != NULL && numDevices > 0) + ? &binaryStatus->front() + : NULL, &error); + + detail::errHandler(error, __CREATE_PROGRAM_WITH_BINARY_ERR); + if (err != NULL) { + *err = error; + } + } + + +#if defined(CL_VERSION_1_2) + /** + * Create program using builtin kernels. + * \param kernelNames Semi-colon separated list of builtin kernel names + */ + Program( + const Context& context, + const VECTOR_CLASS& devices, + const STRING_CLASS& kernelNames, + cl_int* err = NULL) + { + cl_int error; + + + ::size_t numDevices = devices.size(); + cl_device_id* deviceIDs = (cl_device_id*) alloca(numDevices * sizeof(cl_device_id)); + for( ::size_t deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex ) { + deviceIDs[deviceIndex] = (devices[deviceIndex])(); + } + + object_ = ::clCreateProgramWithBuiltInKernels( + context(), + (cl_uint) devices.size(), + deviceIDs, + kernelNames.c_str(), + &error); + + detail::errHandler(error, __CREATE_PROGRAM_WITH_BUILT_IN_KERNELS_ERR); + if (err != NULL) { + *err = error; + } + } +#endif // #if defined(CL_VERSION_1_2) + + Program() { } + + __CL_EXPLICIT_CONSTRUCTORS Program(const cl_program& program) : detail::Wrapper(program) { } + + Program& operator = (const cl_program& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + Program(const Program& program) : detail::Wrapper(program) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + Program& operator = (const Program &program) + { + detail::Wrapper::operator=(program); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + Program(Program&& program) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(program)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + Program& operator = (Program &&program) + { + detail::Wrapper::operator=(std::move(program)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + cl_int build( + const VECTOR_CLASS& devices, + const char* options = NULL, + void (CL_CALLBACK * notifyFptr)(cl_program, void *) = NULL, + void* data = NULL) const + { + ::size_t numDevices = devices.size(); + cl_device_id* deviceIDs = (cl_device_id*) alloca(numDevices * sizeof(cl_device_id)); + for( ::size_t deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex ) { + deviceIDs[deviceIndex] = (devices[deviceIndex])(); + } + + return detail::errHandler( + ::clBuildProgram( + object_, + (cl_uint) + devices.size(), + deviceIDs, + options, + notifyFptr, + data), + __BUILD_PROGRAM_ERR); + } + + cl_int build( + const char* options = NULL, + void (CL_CALLBACK * notifyFptr)(cl_program, void *) = NULL, + void* data = NULL) const + { + return detail::errHandler( + ::clBuildProgram( + object_, + 0, + NULL, + options, + notifyFptr, + data), + __BUILD_PROGRAM_ERR); + } + +#if defined(CL_VERSION_1_2) + cl_int compile( + const char* options = NULL, + void (CL_CALLBACK * notifyFptr)(cl_program, void *) = NULL, + void* data = NULL) const + { + return detail::errHandler( + ::clCompileProgram( + object_, + 0, + NULL, + options, + 0, + NULL, + NULL, + notifyFptr, + data), + __COMPILE_PROGRAM_ERR); + } +#endif + + template + cl_int getInfo(cl_program_info name, T* param) const + { + return detail::errHandler( + detail::getInfo(&::clGetProgramInfo, object_, name, param), + __GET_PROGRAM_INFO_ERR); + } + + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_program_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + template + cl_int getBuildInfo( + const Device& device, cl_program_build_info name, T* param) const + { + return detail::errHandler( + detail::getInfo( + &::clGetProgramBuildInfo, object_, device(), name, param), + __GET_PROGRAM_BUILD_INFO_ERR); + } + + template typename + detail::param_traits::param_type + getBuildInfo(const Device& device, cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_program_build_info, name>::param_type param; + cl_int result = getBuildInfo(device, name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + cl_int createKernels(VECTOR_CLASS* kernels) + { + cl_uint numKernels; + cl_int err = ::clCreateKernelsInProgram(object_, 0, NULL, &numKernels); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __CREATE_KERNELS_IN_PROGRAM_ERR); + } + + Kernel* value = (Kernel*) alloca(numKernels * sizeof(Kernel)); + err = ::clCreateKernelsInProgram( + object_, numKernels, (cl_kernel*) value, NULL); + if (err != CL_SUCCESS) { + return detail::errHandler(err, __CREATE_KERNELS_IN_PROGRAM_ERR); + } + + kernels->assign(&value[0], &value[numKernels]); + return CL_SUCCESS; + } +}; + +#if defined(CL_VERSION_1_2) +inline Program linkProgram( + Program input1, + Program input2, + const char* options = NULL, + void (CL_CALLBACK * notifyFptr)(cl_program, void *) = NULL, + void* data = NULL, + cl_int* err = NULL) +{ + cl_int error_local = CL_SUCCESS; + + cl_program programs[2] = { input1(), input2() }; + + Context ctx = input1.getInfo(&error_local); + if(error_local!=CL_SUCCESS) { + detail::errHandler(error_local, __LINK_PROGRAM_ERR); + } + + cl_program prog = ::clLinkProgram( + ctx(), + 0, + NULL, + options, + 2, + programs, + notifyFptr, + data, + &error_local); + + detail::errHandler(error_local,__COMPILE_PROGRAM_ERR); + if (err != NULL) { + *err = error_local; + } + + return Program(prog); +} + +inline Program linkProgram( + VECTOR_CLASS inputPrograms, + const char* options = NULL, + void (CL_CALLBACK * notifyFptr)(cl_program, void *) = NULL, + void* data = NULL, + cl_int* err = NULL) +{ + cl_int error_local = CL_SUCCESS; + + cl_program * programs = (cl_program*) alloca(inputPrograms.size() * sizeof(cl_program)); + + if (programs != NULL) { + for (unsigned int i = 0; i < inputPrograms.size(); i++) { + programs[i] = inputPrograms[i](); + } + } + + Context ctx; + if(inputPrograms.size() > 0) { + ctx = inputPrograms[0].getInfo(&error_local); + if(error_local!=CL_SUCCESS) { + detail::errHandler(error_local, __LINK_PROGRAM_ERR); + } + } + cl_program prog = ::clLinkProgram( + ctx(), + 0, + NULL, + options, + (cl_uint)inputPrograms.size(), + programs, + notifyFptr, + data, + &error_local); + + detail::errHandler(error_local,__COMPILE_PROGRAM_ERR); + if (err != NULL) { + *err = error_local; + } + + return Program(prog); +} +#endif + +template<> +inline VECTOR_CLASS cl::Program::getInfo(cl_int* err) const +{ + VECTOR_CLASS< ::size_t> sizes = getInfo(); + VECTOR_CLASS binaries; + for (VECTOR_CLASS< ::size_t>::iterator s = sizes.begin(); s != sizes.end(); ++s) + { + char *ptr = NULL; + if (*s != 0) + ptr = new char[*s]; + binaries.push_back(ptr); + } + + cl_int result = getInfo(CL_PROGRAM_BINARIES, &binaries); + if (err != NULL) { + *err = result; + } + return binaries; +} + +inline Kernel::Kernel(const Program& program, const char* name, cl_int* err) +{ + cl_int error; + + object_ = ::clCreateKernel(program(), name, &error); + detail::errHandler(error, __CREATE_KERNEL_ERR); + + if (err != NULL) { + *err = error; + } + +} + +/*! \class CommandQueue + * \brief CommandQueue interface for cl_command_queue. + */ +class CommandQueue : public detail::Wrapper +{ +private: +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED + static std::atomic default_initialized_; +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED + static volatile int default_initialized_; +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED + static CommandQueue default_; + static volatile cl_int default_error_; +public: + CommandQueue( + cl_command_queue_properties properties, + cl_int* err = NULL) + { + cl_int error; + + Context context = Context::getDefault(&error); + detail::errHandler(error, __CREATE_CONTEXT_ERR); + + if (error != CL_SUCCESS) { + if (err != NULL) { + *err = error; + } + } + else { + Device device = context.getInfo()[0]; + + object_ = ::clCreateCommandQueue( + context(), device(), properties, &error); + + detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR); + if (err != NULL) { + *err = error; + } + } + } + /*! + * \brief Constructs a CommandQueue for an implementation defined device in the given context + */ + explicit CommandQueue( + const Context& context, + cl_command_queue_properties properties = 0, + cl_int* err = NULL) + { + cl_int error; + VECTOR_CLASS devices; + error = context.getInfo(CL_CONTEXT_DEVICES, &devices); + + detail::errHandler(error, __CREATE_CONTEXT_ERR); + + if (error != CL_SUCCESS) + { + if (err != NULL) { + *err = error; + } + return; + } + + object_ = ::clCreateCommandQueue(context(), devices[0](), properties, &error); + + detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR); + + if (err != NULL) { + *err = error; + } + + } + + CommandQueue( + const Context& context, + const Device& device, + cl_command_queue_properties properties = 0, + cl_int* err = NULL) + { + cl_int error; + object_ = ::clCreateCommandQueue( + context(), device(), properties, &error); + + detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR); + if (err != NULL) { + *err = error; + } + } + + /*! \brief Copy constructor to forward copy to the superclass correctly. + * Required for MSVC. + */ + CommandQueue(const CommandQueue& queue) : detail::Wrapper(queue) {} + + /*! \brief Copy assignment to forward copy to the superclass correctly. + * Required for MSVC. + */ + CommandQueue& operator = (const CommandQueue &queue) + { + detail::Wrapper::operator=(queue); + return *this; + } + +#if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + /*! \brief Move constructor to forward move to the superclass correctly. + * Required for MSVC. + */ + CommandQueue(CommandQueue&& queue) CL_HPP_NOEXCEPT : detail::Wrapper(std::move(queue)) {} + + /*! \brief Move assignment to forward move to the superclass correctly. + * Required for MSVC. + */ + CommandQueue& operator = (CommandQueue &&queue) + { + detail::Wrapper::operator=(std::move(queue)); + return *this; + } +#endif // #if defined(CL_HPP_RVALUE_REFERENCES_SUPPORTED) + + static CommandQueue getDefault(cl_int * err = NULL) + { + int state = detail::compare_exchange( + &default_initialized_, + __DEFAULT_BEING_INITIALIZED, __DEFAULT_NOT_INITIALIZED); + + if (state & __DEFAULT_INITIALIZED) { + if (err != NULL) { + *err = default_error_; + } + return default_; + } + + if (state & __DEFAULT_BEING_INITIALIZED) { + // Assume writes will propagate eventually... + while(default_initialized_ != __DEFAULT_INITIALIZED) { + detail::fence(); + } + + if (err != NULL) { + *err = default_error_; + } + return default_; + } + + cl_int error; + + Context context = Context::getDefault(&error); + detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR); + + if (error != CL_SUCCESS) { + if (err != NULL) { + *err = error; + } + } + else { + Device device = context.getInfo()[0]; + + default_ = CommandQueue(context, device, 0, &error); + + detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR); + if (err != NULL) { + *err = error; + } + } + + detail::fence(); + + default_error_ = error; + // Assume writes will propagate eventually... + default_initialized_ = __DEFAULT_INITIALIZED; + + detail::fence(); + + if (err != NULL) { + *err = default_error_; + } + return default_; + + } + + CommandQueue() { } + + __CL_EXPLICIT_CONSTRUCTORS CommandQueue(const cl_command_queue& commandQueue) : detail::Wrapper(commandQueue) { } + + CommandQueue& operator = (const cl_command_queue& rhs) + { + detail::Wrapper::operator=(rhs); + return *this; + } + + template + cl_int getInfo(cl_command_queue_info name, T* param) const + { + return detail::errHandler( + detail::getInfo( + &::clGetCommandQueueInfo, object_, name, param), + __GET_COMMAND_QUEUE_INFO_ERR); + } + + template typename + detail::param_traits::param_type + getInfo(cl_int* err = NULL) const + { + typename detail::param_traits< + detail::cl_command_queue_info, name>::param_type param; + cl_int result = getInfo(name, ¶m); + if (err != NULL) { + *err = result; + } + return param; + } + + cl_int enqueueReadBuffer( + const Buffer& buffer, + cl_bool blocking, + ::size_t offset, + ::size_t size, + void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueReadBuffer( + object_, buffer(), blocking, offset, size, + ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_READ_BUFFER_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueWriteBuffer( + const Buffer& buffer, + cl_bool blocking, + ::size_t offset, + ::size_t size, + const void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueWriteBuffer( + object_, buffer(), blocking, offset, size, + ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_WRITE_BUFFER_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueCopyBuffer( + const Buffer& src, + const Buffer& dst, + ::size_t src_offset, + ::size_t dst_offset, + ::size_t size, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueCopyBuffer( + object_, src(), dst(), src_offset, dst_offset, size, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQEUE_COPY_BUFFER_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueReadBufferRect( + const Buffer& buffer, + cl_bool blocking, + const size_t<3>& buffer_offset, + const size_t<3>& host_offset, + const size_t<3>& region, + ::size_t buffer_row_pitch, + ::size_t buffer_slice_pitch, + ::size_t host_row_pitch, + ::size_t host_slice_pitch, + void *ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueReadBufferRect( + object_, + buffer(), + blocking, + (const ::size_t *)buffer_offset, + (const ::size_t *)host_offset, + (const ::size_t *)region, + buffer_row_pitch, + buffer_slice_pitch, + host_row_pitch, + host_slice_pitch, + ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_READ_BUFFER_RECT_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueWriteBufferRect( + const Buffer& buffer, + cl_bool blocking, + const size_t<3>& buffer_offset, + const size_t<3>& host_offset, + const size_t<3>& region, + ::size_t buffer_row_pitch, + ::size_t buffer_slice_pitch, + ::size_t host_row_pitch, + ::size_t host_slice_pitch, + const void *ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueWriteBufferRect( + object_, + buffer(), + blocking, + (const ::size_t *)buffer_offset, + (const ::size_t *)host_offset, + (const ::size_t *)region, + buffer_row_pitch, + buffer_slice_pitch, + host_row_pitch, + host_slice_pitch, + ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_WRITE_BUFFER_RECT_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueCopyBufferRect( + const Buffer& src, + const Buffer& dst, + const size_t<3>& src_origin, + const size_t<3>& dst_origin, + const size_t<3>& region, + ::size_t src_row_pitch, + ::size_t src_slice_pitch, + ::size_t dst_row_pitch, + ::size_t dst_slice_pitch, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueCopyBufferRect( + object_, + src(), + dst(), + (const ::size_t *)src_origin, + (const ::size_t *)dst_origin, + (const ::size_t *)region, + src_row_pitch, + src_slice_pitch, + dst_row_pitch, + dst_slice_pitch, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQEUE_COPY_BUFFER_RECT_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + +#if defined(CL_VERSION_1_2) + /** + * Enqueue a command to fill a buffer object with a pattern + * of a given size. The pattern is specified a as vector. + * \tparam PatternType The datatype of the pattern field. + * The pattern type must be an accepted OpenCL data type. + */ + template + cl_int enqueueFillBuffer( + const Buffer& buffer, + PatternType pattern, + ::size_t offset, + ::size_t size, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueFillBuffer( + object_, + buffer(), + static_cast(&pattern), + sizeof(PatternType), + offset, + size, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_FILL_BUFFER_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } +#endif // #if defined(CL_VERSION_1_2) + + cl_int enqueueReadImage( + const Image& image, + cl_bool blocking, + const size_t<3>& origin, + const size_t<3>& region, + ::size_t row_pitch, + ::size_t slice_pitch, + void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueReadImage( + object_, image(), blocking, (const ::size_t *) origin, + (const ::size_t *) region, row_pitch, slice_pitch, ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_READ_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueWriteImage( + const Image& image, + cl_bool blocking, + const size_t<3>& origin, + const size_t<3>& region, + ::size_t row_pitch, + ::size_t slice_pitch, + const void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueWriteImage( + object_, image(), blocking, (const ::size_t *) origin, + (const ::size_t *) region, row_pitch, slice_pitch, ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_WRITE_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueCopyImage( + const Image& src, + const Image& dst, + const size_t<3>& src_origin, + const size_t<3>& dst_origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueCopyImage( + object_, src(), dst(), (const ::size_t *) src_origin, + (const ::size_t *)dst_origin, (const ::size_t *) region, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_COPY_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + +#if defined(CL_VERSION_1_2) + /** + * Enqueue a command to fill an image object with a specified color. + * \param fillColor is the color to use to fill the image. + * This is a four component RGBA floating-point color value if + * the image channel data type is not an unnormalized signed or + * unsigned data type. + */ + cl_int enqueueFillImage( + const Image& image, + cl_float4 fillColor, + const size_t<3>& origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueFillImage( + object_, + image(), + static_cast(&fillColor), + (const ::size_t *) origin, + (const ::size_t *) region, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_FILL_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + /** + * Enqueue a command to fill an image object with a specified color. + * \param fillColor is the color to use to fill the image. + * This is a four component RGBA signed integer color value if + * the image channel data type is an unnormalized signed integer + * type. + */ + cl_int enqueueFillImage( + const Image& image, + cl_int4 fillColor, + const size_t<3>& origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueFillImage( + object_, + image(), + static_cast(&fillColor), + (const ::size_t *) origin, + (const ::size_t *) region, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_FILL_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + /** + * Enqueue a command to fill an image object with a specified color. + * \param fillColor is the color to use to fill the image. + * This is a four component RGBA unsigned integer color value if + * the image channel data type is an unnormalized unsigned integer + * type. + */ + cl_int enqueueFillImage( + const Image& image, + cl_uint4 fillColor, + const size_t<3>& origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueFillImage( + object_, + image(), + static_cast(&fillColor), + (const ::size_t *) origin, + (const ::size_t *) region, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_FILL_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } +#endif // #if defined(CL_VERSION_1_2) + + cl_int enqueueCopyImageToBuffer( + const Image& src, + const Buffer& dst, + const size_t<3>& src_origin, + const size_t<3>& region, + ::size_t dst_offset, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueCopyImageToBuffer( + object_, src(), dst(), (const ::size_t *) src_origin, + (const ::size_t *) region, dst_offset, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_COPY_IMAGE_TO_BUFFER_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueCopyBufferToImage( + const Buffer& src, + const Image& dst, + ::size_t src_offset, + const size_t<3>& dst_origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueCopyBufferToImage( + object_, src(), dst(), src_offset, + (const ::size_t *) dst_origin, (const ::size_t *) region, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_COPY_BUFFER_TO_IMAGE_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + void* enqueueMapBuffer( + const Buffer& buffer, + cl_bool blocking, + cl_map_flags flags, + ::size_t offset, + ::size_t size, + const VECTOR_CLASS* events = NULL, + Event* event = NULL, + cl_int* err = NULL) const + { + cl_event tmp; + cl_int error; + void * result = ::clEnqueueMapBuffer( + object_, buffer(), blocking, flags, offset, size, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL, + &error); + + detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + if (event != NULL && error == CL_SUCCESS) + *event = tmp; + + return result; + } + + void* enqueueMapImage( + const Image& buffer, + cl_bool blocking, + cl_map_flags flags, + const size_t<3>& origin, + const size_t<3>& region, + ::size_t * row_pitch, + ::size_t * slice_pitch, + const VECTOR_CLASS* events = NULL, + Event* event = NULL, + cl_int* err = NULL) const + { + cl_event tmp; + cl_int error; + void * result = ::clEnqueueMapImage( + object_, buffer(), blocking, flags, + (const ::size_t *) origin, (const ::size_t *) region, + row_pitch, slice_pitch, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL, + &error); + + detail::errHandler(error, __ENQUEUE_MAP_IMAGE_ERR); + if (err != NULL) { + *err = error; + } + if (event != NULL && error == CL_SUCCESS) + *event = tmp; + return result; + } + + cl_int enqueueUnmapMemObject( + const Memory& memory, + void* mapped_ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueUnmapMemObject( + object_, memory(), mapped_ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_UNMAP_MEM_OBJECT_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + +#if defined(CL_VERSION_1_2) + /** + * Enqueues a marker command which waits for either a list of events to complete, + * or all previously enqueued commands to complete. + * + * Enqueues a marker command which waits for either a list of events to complete, + * or if the list is empty it waits for all commands previously enqueued in command_queue + * to complete before it completes. This command returns an event which can be waited on, + * i.e. this event can be waited on to insure that all events either in the event_wait_list + * or all previously enqueued commands, queued before this command to command_queue, + * have completed. + */ + cl_int enqueueMarkerWithWaitList( + const VECTOR_CLASS *events = 0, + Event *event = 0) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueMarkerWithWaitList( + object_, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_MARKER_WAIT_LIST_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + /** + * A synchronization point that enqueues a barrier operation. + * + * Enqueues a barrier command which waits for either a list of events to complete, + * or if the list is empty it waits for all commands previously enqueued in command_queue + * to complete before it completes. This command blocks command execution, that is, any + * following commands enqueued after it do not execute until it completes. This command + * returns an event which can be waited on, i.e. this event can be waited on to insure that + * all events either in the event_wait_list or all previously enqueued commands, queued + * before this command to command_queue, have completed. + */ + cl_int enqueueBarrierWithWaitList( + const VECTOR_CLASS *events = 0, + Event *event = 0) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueBarrierWithWaitList( + object_, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_BARRIER_WAIT_LIST_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + /** + * Enqueues a command to indicate with which device a set of memory objects + * should be associated. + */ + cl_int enqueueMigrateMemObjects( + const VECTOR_CLASS &memObjects, + cl_mem_migration_flags flags, + const VECTOR_CLASS* events = NULL, + Event* event = NULL + ) const + { + cl_event tmp; + + cl_mem* localMemObjects = static_cast(alloca(memObjects.size() * sizeof(cl_mem))); + for( int i = 0; i < (int)memObjects.size(); ++i ) { + localMemObjects[i] = memObjects[i](); + } + + + cl_int err = detail::errHandler( + ::clEnqueueMigrateMemObjects( + object_, + (cl_uint)memObjects.size(), + static_cast(localMemObjects), + flags, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_UNMAP_MEM_OBJECT_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } +#endif // #if defined(CL_VERSION_1_2) + + cl_int enqueueNDRangeKernel( + const Kernel& kernel, + const NDRange& offset, + const NDRange& global, + const NDRange& local = NullRange, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueNDRangeKernel( + object_, kernel(), (cl_uint) global.dimensions(), + offset.dimensions() != 0 ? (const ::size_t*) offset : NULL, + (const ::size_t*) global, + local.dimensions() != 0 ? (const ::size_t*) local : NULL, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_NDRANGE_KERNEL_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueTask( + const Kernel& kernel, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueTask( + object_, kernel(), + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_TASK_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueNativeKernel( + void (CL_CALLBACK *userFptr)(void *), + std::pair args, + const VECTOR_CLASS* mem_objects = NULL, + const VECTOR_CLASS* mem_locs = NULL, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_mem * mems = (mem_objects != NULL && mem_objects->size() > 0) + ? (cl_mem*) alloca(mem_objects->size() * sizeof(cl_mem)) + : NULL; + + if (mems != NULL) { + for (unsigned int i = 0; i < mem_objects->size(); i++) { + mems[i] = ((*mem_objects)[i])(); + } + } + + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueNativeKernel( + object_, userFptr, args.first, args.second, + (mem_objects != NULL) ? (cl_uint) mem_objects->size() : 0, + mems, + (mem_locs != NULL && mem_locs->size() > 0) ? (const void **) &mem_locs->front() : NULL, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_NATIVE_KERNEL); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + +/** + * Deprecated APIs for 1.2 + */ +#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) || (defined(CL_VERSION_1_1) && !defined(CL_VERSION_1_2)) + CL_EXT_PREFIX__VERSION_1_1_DEPRECATED + cl_int enqueueMarker(Event* event = NULL) const CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueMarker( + object_, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_MARKER_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + CL_EXT_PREFIX__VERSION_1_1_DEPRECATED + cl_int enqueueWaitForEvents(const VECTOR_CLASS& events) const CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED + { + return detail::errHandler( + ::clEnqueueWaitForEvents( + object_, + (cl_uint) events.size(), + events.size() > 0 ? (const cl_event*) &events.front() : NULL), + __ENQUEUE_WAIT_FOR_EVENTS_ERR); + } +#endif // #if defined(CL_VERSION_1_1) + + cl_int enqueueAcquireGLObjects( + const VECTOR_CLASS* mem_objects = NULL, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueAcquireGLObjects( + object_, + (mem_objects != NULL) ? (cl_uint) mem_objects->size() : 0, + (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *) &mem_objects->front(): NULL, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_ACQUIRE_GL_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueReleaseGLObjects( + const VECTOR_CLASS* mem_objects = NULL, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueReleaseGLObjects( + object_, + (mem_objects != NULL) ? (cl_uint) mem_objects->size() : 0, + (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *) &mem_objects->front(): NULL, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_RELEASE_GL_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + +#if defined (USE_DX_INTEROP) +typedef CL_API_ENTRY cl_int (CL_API_CALL *PFN_clEnqueueAcquireD3D10ObjectsKHR)( + cl_command_queue command_queue, cl_uint num_objects, + const cl_mem* mem_objects, cl_uint num_events_in_wait_list, + const cl_event* event_wait_list, cl_event* event); +typedef CL_API_ENTRY cl_int (CL_API_CALL *PFN_clEnqueueReleaseD3D10ObjectsKHR)( + cl_command_queue command_queue, cl_uint num_objects, + const cl_mem* mem_objects, cl_uint num_events_in_wait_list, + const cl_event* event_wait_list, cl_event* event); + + cl_int enqueueAcquireD3D10Objects( + const VECTOR_CLASS* mem_objects = NULL, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + static PFN_clEnqueueAcquireD3D10ObjectsKHR pfn_clEnqueueAcquireD3D10ObjectsKHR = NULL; +#if defined(CL_VERSION_1_2) + cl_context context = getInfo(); + cl::Device device(getInfo()); + cl_platform_id platform = device.getInfo(); + __INIT_CL_EXT_FCN_PTR_PLATFORM(platform, clEnqueueAcquireD3D10ObjectsKHR); +#endif +#if defined(CL_VERSION_1_1) + __INIT_CL_EXT_FCN_PTR(clEnqueueAcquireD3D10ObjectsKHR); +#endif + + cl_event tmp; + cl_int err = detail::errHandler( + pfn_clEnqueueAcquireD3D10ObjectsKHR( + object_, + (mem_objects != NULL) ? (cl_uint) mem_objects->size() : 0, + (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *) &mem_objects->front(): NULL, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_ACQUIRE_GL_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } + + cl_int enqueueReleaseD3D10Objects( + const VECTOR_CLASS* mem_objects = NULL, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) const + { + static PFN_clEnqueueReleaseD3D10ObjectsKHR pfn_clEnqueueReleaseD3D10ObjectsKHR = NULL; +#if defined(CL_VERSION_1_2) + cl_context context = getInfo(); + cl::Device device(getInfo()); + cl_platform_id platform = device.getInfo(); + __INIT_CL_EXT_FCN_PTR_PLATFORM(platform, clEnqueueReleaseD3D10ObjectsKHR); +#endif // #if defined(CL_VERSION_1_2) +#if defined(CL_VERSION_1_1) + __INIT_CL_EXT_FCN_PTR(clEnqueueReleaseD3D10ObjectsKHR); +#endif // #if defined(CL_VERSION_1_1) + + cl_event tmp; + cl_int err = detail::errHandler( + pfn_clEnqueueReleaseD3D10ObjectsKHR( + object_, + (mem_objects != NULL) ? (cl_uint) mem_objects->size() : 0, + (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *) &mem_objects->front(): NULL, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_RELEASE_GL_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; + } +#endif + +/** + * Deprecated APIs for 1.2 + */ +#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS) || (defined(CL_VERSION_1_1) && !defined(CL_VERSION_1_2)) + CL_EXT_PREFIX__VERSION_1_1_DEPRECATED + cl_int enqueueBarrier() const CL_EXT_SUFFIX__VERSION_1_1_DEPRECATED + { + return detail::errHandler( + ::clEnqueueBarrier(object_), + __ENQUEUE_BARRIER_ERR); + } +#endif // #if defined(CL_VERSION_1_1) + + cl_int flush() const + { + return detail::errHandler(::clFlush(object_), __FLUSH_ERR); + } + + cl_int finish() const + { + return detail::errHandler(::clFinish(object_), __FINISH_ERR); + } +}; + +#ifdef _WIN32 +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED +__declspec(selectany) std::atomic CommandQueue::default_initialized_; +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__declspec(selectany) volatile int CommandQueue::default_initialized_ = __DEFAULT_NOT_INITIALIZED; +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__declspec(selectany) CommandQueue CommandQueue::default_; +__declspec(selectany) volatile cl_int CommandQueue::default_error_ = CL_SUCCESS; +#else // !_WIN32 +#ifdef CL_HPP_CPP11_ATOMICS_SUPPORTED +__attribute__((weak)) std::atomic CommandQueue::default_initialized_; +#else // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__attribute__((weak)) volatile int CommandQueue::default_initialized_ = __DEFAULT_NOT_INITIALIZED; +#endif // !CL_HPP_CPP11_ATOMICS_SUPPORTED +__attribute__((weak)) CommandQueue CommandQueue::default_; +__attribute__((weak)) volatile cl_int CommandQueue::default_error_ = CL_SUCCESS; +#endif // !_WIN32 + +template< typename IteratorType > +Buffer::Buffer( + const Context &context, + IteratorType startIterator, + IteratorType endIterator, + bool readOnly, + bool useHostPtr, + cl_int* err) +{ + typedef typename std::iterator_traits::value_type DataType; + cl_int error; + + cl_mem_flags flags = 0; + if( readOnly ) { + flags |= CL_MEM_READ_ONLY; + } + else { + flags |= CL_MEM_READ_WRITE; + } + if( useHostPtr ) { + flags |= CL_MEM_USE_HOST_PTR; + } + + ::size_t size = sizeof(DataType)*(endIterator - startIterator); + + if( useHostPtr ) { + object_ = ::clCreateBuffer(context(), flags, size, static_cast(&*startIterator), &error); + } else { + object_ = ::clCreateBuffer(context(), flags, size, 0, &error); + } + + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + + if( !useHostPtr ) { + CommandQueue queue(context, 0, &error); + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + + error = cl::copy(queue, startIterator, endIterator, *this); + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } +} + +template< typename IteratorType > +Buffer::Buffer( + const CommandQueue &queue, + IteratorType startIterator, + IteratorType endIterator, + bool readOnly, + bool useHostPtr, + cl_int* err) +{ + typedef typename std::iterator_traits::value_type DataType; + cl_int error; + + cl_mem_flags flags = 0; + if (readOnly) { + flags |= CL_MEM_READ_ONLY; + } + else { + flags |= CL_MEM_READ_WRITE; + } + if (useHostPtr) { + flags |= CL_MEM_USE_HOST_PTR; + } + + ::size_t size = sizeof(DataType)*(endIterator - startIterator); + + Context context = queue.getInfo(); + + if (useHostPtr) { + object_ = ::clCreateBuffer(context(), flags, size, static_cast(&*startIterator), &error); + } + else { + object_ = ::clCreateBuffer(context(), flags, size, 0, &error); + } + + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + + if (!useHostPtr) { + error = cl::copy(queue, startIterator, endIterator, *this); + detail::errHandler(error, __CREATE_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + } +} + +inline cl_int enqueueReadBuffer( + const Buffer& buffer, + cl_bool blocking, + ::size_t offset, + ::size_t size, + void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueReadBuffer(buffer, blocking, offset, size, ptr, events, event); +} + +inline cl_int enqueueWriteBuffer( + const Buffer& buffer, + cl_bool blocking, + ::size_t offset, + ::size_t size, + const void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueWriteBuffer(buffer, blocking, offset, size, ptr, events, event); +} + +inline void* enqueueMapBuffer( + const Buffer& buffer, + cl_bool blocking, + cl_map_flags flags, + ::size_t offset, + ::size_t size, + const VECTOR_CLASS* events = NULL, + Event* event = NULL, + cl_int* err = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + + void * result = ::clEnqueueMapBuffer( + queue(), buffer(), blocking, flags, offset, size, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (cl_event*) event, + &error); + + detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR); + if (err != NULL) { + *err = error; + } + return result; +} + +inline cl_int enqueueUnmapMemObject( + const Memory& memory, + void* mapped_ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR); + if (error != CL_SUCCESS) { + return error; + } + + cl_event tmp; + cl_int err = detail::errHandler( + ::clEnqueueUnmapMemObject( + queue(), memory(), mapped_ptr, + (events != NULL) ? (cl_uint) events->size() : 0, + (events != NULL && events->size() > 0) ? (cl_event*) &events->front() : NULL, + (event != NULL) ? &tmp : NULL), + __ENQUEUE_UNMAP_MEM_OBJECT_ERR); + + if (event != NULL && err == CL_SUCCESS) + *event = tmp; + + return err; +} + +inline cl_int enqueueCopyBuffer( + const Buffer& src, + const Buffer& dst, + ::size_t src_offset, + ::size_t dst_offset, + ::size_t size, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueCopyBuffer(src, dst, src_offset, dst_offset, size, events, event); +} + +/** + * Blocking copy operation between iterators and a buffer. + * Host to Device. + * Uses default command queue. + */ +template< typename IteratorType > +inline cl_int copy( IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer ) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + if (error != CL_SUCCESS) + return error; + + return cl::copy(queue, startIterator, endIterator, buffer); +} + +/** + * Blocking copy operation between iterators and a buffer. + * Device to Host. + * Uses default command queue. + */ +template< typename IteratorType > +inline cl_int copy( const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator ) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + if (error != CL_SUCCESS) + return error; + + return cl::copy(queue, buffer, startIterator, endIterator); +} + +/** + * Blocking copy operation between iterators and a buffer. + * Host to Device. + * Uses specified queue. + */ +template< typename IteratorType > +inline cl_int copy( const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer ) +{ + typedef typename std::iterator_traits::value_type DataType; + cl_int error; + + ::size_t length = endIterator-startIterator; + ::size_t byteLength = length*sizeof(DataType); + + DataType *pointer = + static_cast(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_WRITE, 0, byteLength, 0, 0, &error)); + // if exceptions enabled, enqueueMapBuffer will throw + if( error != CL_SUCCESS ) { + return error; + } +#if defined(_MSC_VER) + std::copy( + startIterator, + endIterator, + stdext::checked_array_iterator( + pointer, length)); +#else + std::copy(startIterator, endIterator, pointer); +#endif + Event endEvent; + error = queue.enqueueUnmapMemObject(buffer, pointer, 0, &endEvent); + // if exceptions enabled, enqueueUnmapMemObject will throw + if( error != CL_SUCCESS ) { + return error; + } + endEvent.wait(); + return CL_SUCCESS; +} + +/** + * Blocking copy operation between iterators and a buffer. + * Device to Host. + * Uses specified queue. + */ +template< typename IteratorType > +inline cl_int copy( const CommandQueue &queue, const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator ) +{ + typedef typename std::iterator_traits::value_type DataType; + cl_int error; + + ::size_t length = endIterator-startIterator; + ::size_t byteLength = length*sizeof(DataType); + + DataType *pointer = + static_cast(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_READ, 0, byteLength, 0, 0, &error)); + // if exceptions enabled, enqueueMapBuffer will throw + if( error != CL_SUCCESS ) { + return error; + } + std::copy(pointer, pointer + length, startIterator); + Event endEvent; + error = queue.enqueueUnmapMemObject(buffer, pointer, 0, &endEvent); + // if exceptions enabled, enqueueUnmapMemObject will throw + if( error != CL_SUCCESS ) { + return error; + } + endEvent.wait(); + return CL_SUCCESS; +} + +#if defined(CL_VERSION_1_1) +inline cl_int enqueueReadBufferRect( + const Buffer& buffer, + cl_bool blocking, + const size_t<3>& buffer_offset, + const size_t<3>& host_offset, + const size_t<3>& region, + ::size_t buffer_row_pitch, + ::size_t buffer_slice_pitch, + ::size_t host_row_pitch, + ::size_t host_slice_pitch, + void *ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueReadBufferRect( + buffer, + blocking, + buffer_offset, + host_offset, + region, + buffer_row_pitch, + buffer_slice_pitch, + host_row_pitch, + host_slice_pitch, + ptr, + events, + event); +} + +inline cl_int enqueueWriteBufferRect( + const Buffer& buffer, + cl_bool blocking, + const size_t<3>& buffer_offset, + const size_t<3>& host_offset, + const size_t<3>& region, + ::size_t buffer_row_pitch, + ::size_t buffer_slice_pitch, + ::size_t host_row_pitch, + ::size_t host_slice_pitch, + const void *ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueWriteBufferRect( + buffer, + blocking, + buffer_offset, + host_offset, + region, + buffer_row_pitch, + buffer_slice_pitch, + host_row_pitch, + host_slice_pitch, + ptr, + events, + event); +} + +inline cl_int enqueueCopyBufferRect( + const Buffer& src, + const Buffer& dst, + const size_t<3>& src_origin, + const size_t<3>& dst_origin, + const size_t<3>& region, + ::size_t src_row_pitch, + ::size_t src_slice_pitch, + ::size_t dst_row_pitch, + ::size_t dst_slice_pitch, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueCopyBufferRect( + src, + dst, + src_origin, + dst_origin, + region, + src_row_pitch, + src_slice_pitch, + dst_row_pitch, + dst_slice_pitch, + events, + event); +} +#endif + +inline cl_int enqueueReadImage( + const Image& image, + cl_bool blocking, + const size_t<3>& origin, + const size_t<3>& region, + ::size_t row_pitch, + ::size_t slice_pitch, + void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueReadImage( + image, + blocking, + origin, + region, + row_pitch, + slice_pitch, + ptr, + events, + event); +} + +inline cl_int enqueueWriteImage( + const Image& image, + cl_bool blocking, + const size_t<3>& origin, + const size_t<3>& region, + ::size_t row_pitch, + ::size_t slice_pitch, + const void* ptr, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueWriteImage( + image, + blocking, + origin, + region, + row_pitch, + slice_pitch, + ptr, + events, + event); +} + +inline cl_int enqueueCopyImage( + const Image& src, + const Image& dst, + const size_t<3>& src_origin, + const size_t<3>& dst_origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueCopyImage( + src, + dst, + src_origin, + dst_origin, + region, + events, + event); +} + +inline cl_int enqueueCopyImageToBuffer( + const Image& src, + const Buffer& dst, + const size_t<3>& src_origin, + const size_t<3>& region, + ::size_t dst_offset, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueCopyImageToBuffer( + src, + dst, + src_origin, + region, + dst_offset, + events, + event); +} + +inline cl_int enqueueCopyBufferToImage( + const Buffer& src, + const Image& dst, + ::size_t src_offset, + const size_t<3>& dst_origin, + const size_t<3>& region, + const VECTOR_CLASS* events = NULL, + Event* event = NULL) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.enqueueCopyBufferToImage( + src, + dst, + src_offset, + dst_origin, + region, + events, + event); +} + + +inline cl_int flush(void) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + return queue.flush(); +} + +inline cl_int finish(void) +{ + cl_int error; + CommandQueue queue = CommandQueue::getDefault(&error); + + if (error != CL_SUCCESS) { + return error; + } + + + return queue.finish(); +} + +// Kernel Functor support +// New interface as of September 2011 +// Requires the C++11 std::tr1::function (note do not support TR1) +// Visual Studio 2010 and GCC 4.2 + +struct EnqueueArgs +{ + CommandQueue queue_; + const NDRange offset_; + const NDRange global_; + const NDRange local_; + VECTOR_CLASS events_; + + EnqueueArgs(NDRange global) : + queue_(CommandQueue::getDefault()), + offset_(NullRange), + global_(global), + local_(NullRange) + { + + } + + EnqueueArgs(NDRange global, NDRange local) : + queue_(CommandQueue::getDefault()), + offset_(NullRange), + global_(global), + local_(local) + { + + } + + EnqueueArgs(NDRange offset, NDRange global, NDRange local) : + queue_(CommandQueue::getDefault()), + offset_(offset), + global_(global), + local_(local) + { + + } + + EnqueueArgs(Event e, NDRange global) : + queue_(CommandQueue::getDefault()), + offset_(NullRange), + global_(global), + local_(NullRange) + { + events_.push_back(e); + } + + EnqueueArgs(Event e, NDRange global, NDRange local) : + queue_(CommandQueue::getDefault()), + offset_(NullRange), + global_(global), + local_(local) + { + events_.push_back(e); + } + + EnqueueArgs(Event e, NDRange offset, NDRange global, NDRange local) : + queue_(CommandQueue::getDefault()), + offset_(offset), + global_(global), + local_(local) + { + events_.push_back(e); + } + + EnqueueArgs(const VECTOR_CLASS &events, NDRange global) : + queue_(CommandQueue::getDefault()), + offset_(NullRange), + global_(global), + local_(NullRange), + events_(events) + { + + } + + EnqueueArgs(const VECTOR_CLASS &events, NDRange global, NDRange local) : + queue_(CommandQueue::getDefault()), + offset_(NullRange), + global_(global), + local_(local), + events_(events) + { + + } + + EnqueueArgs(const VECTOR_CLASS &events, NDRange offset, NDRange global, NDRange local) : + queue_(CommandQueue::getDefault()), + offset_(offset), + global_(global), + local_(local), + events_(events) + { + + } + + EnqueueArgs(CommandQueue &queue, NDRange global) : + queue_(queue), + offset_(NullRange), + global_(global), + local_(NullRange) + { + + } + + EnqueueArgs(CommandQueue &queue, NDRange global, NDRange local) : + queue_(queue), + offset_(NullRange), + global_(global), + local_(local) + { + + } + + EnqueueArgs(CommandQueue &queue, NDRange offset, NDRange global, NDRange local) : + queue_(queue), + offset_(offset), + global_(global), + local_(local) + { + + } + + EnqueueArgs(CommandQueue &queue, Event e, NDRange global) : + queue_(queue), + offset_(NullRange), + global_(global), + local_(NullRange) + { + events_.push_back(e); + } + + EnqueueArgs(CommandQueue &queue, Event e, NDRange global, NDRange local) : + queue_(queue), + offset_(NullRange), + global_(global), + local_(local) + { + events_.push_back(e); + } + + EnqueueArgs(CommandQueue &queue, Event e, NDRange offset, NDRange global, NDRange local) : + queue_(queue), + offset_(offset), + global_(global), + local_(local) + { + events_.push_back(e); + } + + EnqueueArgs(CommandQueue &queue, const VECTOR_CLASS &events, NDRange global) : + queue_(queue), + offset_(NullRange), + global_(global), + local_(NullRange), + events_(events) + { + + } + + EnqueueArgs(CommandQueue &queue, const VECTOR_CLASS &events, NDRange global, NDRange local) : + queue_(queue), + offset_(NullRange), + global_(global), + local_(local), + events_(events) + { + + } + + EnqueueArgs(CommandQueue &queue, const VECTOR_CLASS &events, NDRange offset, NDRange global, NDRange local) : + queue_(queue), + offset_(offset), + global_(global), + local_(local), + events_(events) + { + + } +}; + +namespace detail { + +class NullType {}; + +template +struct SetArg +{ + static void set (Kernel kernel, T0 arg) + { + kernel.setArg(index, arg); + } +}; + +template +struct SetArg +{ + static void set (Kernel, NullType) + { + } +}; + +template < + typename T0, typename T1, typename T2, typename T3, + typename T4, typename T5, typename T6, typename T7, + typename T8, typename T9, typename T10, typename T11, + typename T12, typename T13, typename T14, typename T15, + typename T16, typename T17, typename T18, typename T19, + typename T20, typename T21, typename T22, typename T23, + typename T24, typename T25, typename T26, typename T27, + typename T28, typename T29, typename T30, typename T31 + +> +class KernelFunctorGlobal +{ +private: + Kernel kernel_; + +public: + KernelFunctorGlobal( + Kernel kernel) : + kernel_(kernel) + {} + + KernelFunctorGlobal( + const Program& program, + const STRING_CLASS name, + cl_int * err = NULL) : + kernel_(program, name.c_str(), err) + {} + + Event operator() ( + const EnqueueArgs& args, + T0 t0, + T1 t1 = NullType(), + T2 t2 = NullType(), + T3 t3 = NullType(), + T4 t4 = NullType(), + T5 t5 = NullType(), + T6 t6 = NullType(), + T7 t7 = NullType(), + T8 t8 = NullType(), + T9 t9 = NullType(), + T10 t10 = NullType(), + T11 t11 = NullType(), + T12 t12 = NullType(), + T13 t13 = NullType(), + T14 t14 = NullType(), + T15 t15 = NullType(), + T16 t16 = NullType(), + T17 t17 = NullType(), + T18 t18 = NullType(), + T19 t19 = NullType(), + T20 t20 = NullType(), + T21 t21 = NullType(), + T22 t22 = NullType(), + T23 t23 = NullType(), + T24 t24 = NullType(), + T25 t25 = NullType(), + T26 t26 = NullType(), + T27 t27 = NullType(), + T28 t28 = NullType(), + T29 t29 = NullType(), + T30 t30 = NullType(), + T31 t31 = NullType() + + ) + { + Event event; + SetArg<0, T0>::set(kernel_, t0); + SetArg<1, T1>::set(kernel_, t1); + SetArg<2, T2>::set(kernel_, t2); + SetArg<3, T3>::set(kernel_, t3); + SetArg<4, T4>::set(kernel_, t4); + SetArg<5, T5>::set(kernel_, t5); + SetArg<6, T6>::set(kernel_, t6); + SetArg<7, T7>::set(kernel_, t7); + SetArg<8, T8>::set(kernel_, t8); + SetArg<9, T9>::set(kernel_, t9); + SetArg<10, T10>::set(kernel_, t10); + SetArg<11, T11>::set(kernel_, t11); + SetArg<12, T12>::set(kernel_, t12); + SetArg<13, T13>::set(kernel_, t13); + SetArg<14, T14>::set(kernel_, t14); + SetArg<15, T15>::set(kernel_, t15); + SetArg<16, T16>::set(kernel_, t16); + SetArg<17, T17>::set(kernel_, t17); + SetArg<18, T18>::set(kernel_, t18); + SetArg<19, T19>::set(kernel_, t19); + SetArg<20, T20>::set(kernel_, t20); + SetArg<21, T21>::set(kernel_, t21); + SetArg<22, T22>::set(kernel_, t22); + SetArg<23, T23>::set(kernel_, t23); + SetArg<24, T24>::set(kernel_, t24); + SetArg<25, T25>::set(kernel_, t25); + SetArg<26, T26>::set(kernel_, t26); + SetArg<27, T27>::set(kernel_, t27); + SetArg<28, T28>::set(kernel_, t28); + SetArg<29, T29>::set(kernel_, t29); + SetArg<30, T30>::set(kernel_, t30); + SetArg<31, T31>::set(kernel_, t31); + + + args.queue_.enqueueNDRangeKernel( + kernel_, + args.offset_, + args.global_, + args.local_, + &args.events_, + &event); + + return event; + } + +}; + +//------------------------------------------------------------------------------------------------------ + + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25, + typename T26, + typename T27, + typename T28, + typename T29, + typename T30, + typename T31> +struct functionImplementation_ +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + T30, + T31> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 32)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + T30, + T31); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25, + T26 arg26, + T27 arg27, + T28 arg28, + T29 arg29, + T30 arg30, + T31 arg31) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25, + arg26, + arg27, + arg28, + arg29, + arg30, + arg31); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25, + typename T26, + typename T27, + typename T28, + typename T29, + typename T30> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + T30, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + T30, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 31)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + T30); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25, + T26 arg26, + T27 arg27, + T28 arg28, + T29 arg29, + T30 arg30) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25, + arg26, + arg27, + arg28, + arg29, + arg30); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25, + typename T26, + typename T27, + typename T28, + typename T29> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 30)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + T29); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25, + T26 arg26, + T27 arg27, + T28 arg28, + T29 arg29) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25, + arg26, + arg27, + arg28, + arg29); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25, + typename T26, + typename T27, + typename T28> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 29)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + T28); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25, + T26 arg26, + T27 arg27, + T28 arg28) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25, + arg26, + arg27, + arg28); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25, + typename T26, + typename T27> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 28)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + T27); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25, + T26 arg26, + T27 arg27) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25, + arg26, + arg27); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25, + typename T26> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 27)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + T26); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25, + T26 arg26) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25, + arg26); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24, + typename T25> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 26)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + T25); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24, + T25 arg25) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24, + arg25); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23, + typename T24> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 25)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + T24); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23, + T24 arg24) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23, + arg24); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22, + typename T23> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 24)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + T23); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22, + T23 arg23) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22, + arg23); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21, + typename T22> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 23)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + T22); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21, + T22 arg22) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21, + arg22); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20, + typename T21> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 22)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + T21); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20, + T21 arg21) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20, + arg21); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19, + typename T20> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 21)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + T20); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19, + T20 arg20) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19, + arg20); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18, + typename T19> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 20)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + T19); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18, + T19 arg19) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18, + arg19); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17, + typename T18> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 19)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + T18); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17, + T18 arg18) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17, + arg18); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16, + typename T17> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 18)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + T17); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16, + T17 arg17) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16, + arg17); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15, + typename T16> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 17)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + T16); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15, + T16 arg16) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15, + arg16); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14, + typename T15> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 16)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + T15); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14, + T15 arg15) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14, + arg15); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13, + typename T14> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 15)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + T14); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13, + T14 arg14) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13, + arg14); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12, + typename T13> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 14)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + T13); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12, + T13 arg13) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12, + arg13); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11, + typename T12> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 13)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + T12); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11, + T12 arg12) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11, + arg12); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10, + typename T11> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 12)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + T11); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10, + T11 arg11) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10, + arg11); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9, + typename T10> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 11)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + T10); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9, + T10 arg10) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9, + arg10); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8, + typename T9> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 10)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + T9); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8, + T9 arg9) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8, + arg9); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7, + typename T8> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 9)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + T8); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7, + T8 arg8) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7, + arg8); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6, + typename T7> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 8)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6, + T7); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6, + T7 arg7) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6, + arg7); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5, + typename T6> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + T6, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + T6, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 7)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5, + T6); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5, + T6 arg6) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5, + arg6); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4, + typename T5> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + T5, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + T5, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 6)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4, + T5); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4, + T5 arg5) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4, + arg5); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3, + typename T4> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + T4, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + T4, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 5)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3, + T4); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3, + T4 arg4) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3, + arg4); + } + + +}; + +template< + typename T0, + typename T1, + typename T2, + typename T3> +struct functionImplementation_ +< T0, + T1, + T2, + T3, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + T3, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 4)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2, + T3); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2, + T3 arg3) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2, + arg3); + } + + +}; + +template< + typename T0, + typename T1, + typename T2> +struct functionImplementation_ +< T0, + T1, + T2, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + T2, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 3)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1, + T2); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1, + T2 arg2) + { + return functor_( + enqueueArgs, + arg0, + arg1, + arg2); + } + + +}; + +template< + typename T0, + typename T1> +struct functionImplementation_ +< T0, + T1, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + T1, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 2)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0, + T1); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0, + T1 arg1) + { + return functor_( + enqueueArgs, + arg0, + arg1); + } + + +}; + +template< + typename T0> +struct functionImplementation_ +< T0, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> +{ + typedef detail::KernelFunctorGlobal< + T0, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType, + NullType> FunctorType; + + FunctorType functor_; + + functionImplementation_(const FunctorType &functor) : + functor_(functor) + { + + #if (defined(_WIN32) && defined(_VARIADIC_MAX) && (_VARIADIC_MAX < 1)) + // Fail variadic expansion for dev11 + static_assert(0, "Visual Studio has a hard limit of argument count for a std::function expansion. Please define _VARIADIC_MAX to be 10. If you need more arguments than that VC12 and below cannot support it."); + #endif + + } + + //! \brief Return type of the functor + typedef Event result_type; + + //! \brief Function signature of kernel functor with no event dependency. + typedef Event type_( + const EnqueueArgs&, + T0); + + Event operator()( + const EnqueueArgs& enqueueArgs, + T0 arg0) + { + return functor_( + enqueueArgs, + arg0); + } + + +}; + + + + + +} // namespace detail + +//---------------------------------------------------------------------------------------------- + +template < + typename T0, typename T1 = detail::NullType, typename T2 = detail::NullType, + typename T3 = detail::NullType, typename T4 = detail::NullType, + typename T5 = detail::NullType, typename T6 = detail::NullType, + typename T7 = detail::NullType, typename T8 = detail::NullType, + typename T9 = detail::NullType, typename T10 = detail::NullType, + typename T11 = detail::NullType, typename T12 = detail::NullType, + typename T13 = detail::NullType, typename T14 = detail::NullType, + typename T15 = detail::NullType, typename T16 = detail::NullType, + typename T17 = detail::NullType, typename T18 = detail::NullType, + typename T19 = detail::NullType, typename T20 = detail::NullType, + typename T21 = detail::NullType, typename T22 = detail::NullType, + typename T23 = detail::NullType, typename T24 = detail::NullType, + typename T25 = detail::NullType, typename T26 = detail::NullType, + typename T27 = detail::NullType, typename T28 = detail::NullType, + typename T29 = detail::NullType, typename T30 = detail::NullType, + typename T31 = detail::NullType + +> +struct make_kernel : + public detail::functionImplementation_< + T0, T1, T2, T3, + T4, T5, T6, T7, + T8, T9, T10, T11, + T12, T13, T14, T15, + T16, T17, T18, T19, + T20, T21, T22, T23, + T24, T25, T26, T27, + T28, T29, T30, T31 + + > +{ +public: + typedef detail::KernelFunctorGlobal< + T0, T1, T2, T3, + T4, T5, T6, T7, + T8, T9, T10, T11, + T12, T13, T14, T15, + T16, T17, T18, T19, + T20, T21, T22, T23, + T24, T25, T26, T27, + T28, T29, T30, T31 + + > FunctorType; + + make_kernel( + const Program& program, + const STRING_CLASS name, + cl_int * err = NULL) : + detail::functionImplementation_< + T0, T1, T2, T3, + T4, T5, T6, T7, + T8, T9, T10, T11, + T12, T13, T14, T15, + T16, T17, T18, T19, + T20, T21, T22, T23, + T24, T25, T26, T27, + T28, T29, T30, T31 + + >( + FunctorType(program, name, err)) + {} + + make_kernel( + const Kernel kernel) : + detail::functionImplementation_< + T0, T1, T2, T3, + T4, T5, T6, T7, + T8, T9, T10, T11, + T12, T13, T14, T15, + T16, T17, T18, T19, + T20, T21, T22, T23, + T24, T25, T26, T27, + T28, T29, T30, T31 + + >( + FunctorType(kernel)) + {} +}; + + +//---------------------------------------------------------------------------------------------------------------------- + +#undef __ERR_STR +#if !defined(__CL_USER_OVERRIDE_ERROR_STRINGS) +#undef __GET_DEVICE_INFO_ERR +#undef __GET_PLATFORM_INFO_ERR +#undef __GET_DEVICE_IDS_ERR +#undef __GET_CONTEXT_INFO_ERR +#undef __GET_EVENT_INFO_ERR +#undef __GET_EVENT_PROFILE_INFO_ERR +#undef __GET_MEM_OBJECT_INFO_ERR +#undef __GET_IMAGE_INFO_ERR +#undef __GET_SAMPLER_INFO_ERR +#undef __GET_KERNEL_INFO_ERR +#undef __GET_KERNEL_ARG_INFO_ERR +#undef __GET_KERNEL_WORK_GROUP_INFO_ERR +#undef __GET_PROGRAM_INFO_ERR +#undef __GET_PROGRAM_BUILD_INFO_ERR +#undef __GET_COMMAND_QUEUE_INFO_ERR + +#undef __CREATE_CONTEXT_ERR +#undef __CREATE_CONTEXT_FROM_TYPE_ERR +#undef __GET_SUPPORTED_IMAGE_FORMATS_ERR + +#undef __CREATE_BUFFER_ERR +#undef __CREATE_SUBBUFFER_ERR +#undef __CREATE_IMAGE2D_ERR +#undef __CREATE_IMAGE3D_ERR +#undef __CREATE_SAMPLER_ERR +#undef __SET_MEM_OBJECT_DESTRUCTOR_CALLBACK_ERR + +#undef __CREATE_USER_EVENT_ERR +#undef __SET_USER_EVENT_STATUS_ERR +#undef __SET_EVENT_CALLBACK_ERR +#undef __SET_PRINTF_CALLBACK_ERR + +#undef __WAIT_FOR_EVENTS_ERR + +#undef __CREATE_KERNEL_ERR +#undef __SET_KERNEL_ARGS_ERR +#undef __CREATE_PROGRAM_WITH_SOURCE_ERR +#undef __CREATE_PROGRAM_WITH_BINARY_ERR +#undef __CREATE_PROGRAM_WITH_BUILT_IN_KERNELS_ERR +#undef __BUILD_PROGRAM_ERR +#undef __CREATE_KERNELS_IN_PROGRAM_ERR + +#undef __CREATE_COMMAND_QUEUE_ERR +#undef __SET_COMMAND_QUEUE_PROPERTY_ERR +#undef __ENQUEUE_READ_BUFFER_ERR +#undef __ENQUEUE_WRITE_BUFFER_ERR +#undef __ENQUEUE_READ_BUFFER_RECT_ERR +#undef __ENQUEUE_WRITE_BUFFER_RECT_ERR +#undef __ENQEUE_COPY_BUFFER_ERR +#undef __ENQEUE_COPY_BUFFER_RECT_ERR +#undef __ENQUEUE_READ_IMAGE_ERR +#undef __ENQUEUE_WRITE_IMAGE_ERR +#undef __ENQUEUE_COPY_IMAGE_ERR +#undef __ENQUEUE_COPY_IMAGE_TO_BUFFER_ERR +#undef __ENQUEUE_COPY_BUFFER_TO_IMAGE_ERR +#undef __ENQUEUE_MAP_BUFFER_ERR +#undef __ENQUEUE_MAP_IMAGE_ERR +#undef __ENQUEUE_UNMAP_MEM_OBJECT_ERR +#undef __ENQUEUE_NDRANGE_KERNEL_ERR +#undef __ENQUEUE_TASK_ERR +#undef __ENQUEUE_NATIVE_KERNEL + +#undef __CL_EXPLICIT_CONSTRUCTORS + +#undef __UNLOAD_COMPILER_ERR +#endif //__CL_USER_OVERRIDE_ERROR_STRINGS + +#undef __CL_FUNCTION_TYPE + +// Extensions +/** + * Deprecated APIs for 1.2 + */ +#if defined(CL_VERSION_1_1) +#undef __INIT_CL_EXT_FCN_PTR +#endif // #if defined(CL_VERSION_1_1) +#undef __CREATE_SUB_DEVICES + +#if defined(USE_CL_DEVICE_FISSION) +#undef __PARAM_NAME_DEVICE_FISSION +#endif // USE_CL_DEVICE_FISSION + +#undef __DEFAULT_NOT_INITIALIZED +#undef __DEFAULT_BEING_INITIALIZED +#undef __DEFAULT_INITIALIZED + +#undef CL_HPP_RVALUE_REFERENCES_SUPPORTED +#undef CL_HPP_NOEXCEPT + +} // namespace cl + +#endif // CL_HPP_ diff --git a/GPU_version/vio/include/gpu_svo/cl_class.h b/GPU_version/vio/include/gpu_svo/cl_class.h new file mode 100644 index 0000000..915305e --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/cl_class.h @@ -0,0 +1,191 @@ +// +// Created by root on 4/27/21. +// + +#ifndef VIO_OPENCL_CL_CLASS_H +#define VIO_OPENCL_CL_CLASS_H +#include +#include +#include +#include +#include +#include +#include +#include + +class kernel{ +public: + kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());}; + ~kernel(){ + _buffers.clear(); + _images.clear(); + }; + template + int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){ + _buffers.push_back(std::pair,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(T) * buf_size), + buf_size),id)); + cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf); + if(err!=0){ + std::cerr<<"Error C:Kernel, F:write, L:enqueueWriteBuffer\n"<setArg(id,_buffers.back().first.first); + if(err!=0){ + std::cerr<<"Error C:Kernel, F:write, L3:setArg\n"<(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), + buf.size().width, + buf.size().height, + 0, + reinterpret_cast(buf.data)),id)); + + cl_int err=_kernel->setArg(id,_images.back().first); + if(err!=0){ + std::cerr<<"Error C:Kernel, F:write image, L:setArg\n"< + int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){ + try{ + if(id>_buffers.size())return CL_MAP_FAILURE; + for(auto i:_buffers)if(i.second==id) + queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Reload buffer on GPU failed\n"<(buf.data)); + } + + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Reload buffer on GPU failed\n"< read(size_t id){ + for(auto i:_buffers)if(i.second==id)return i.first; + } + cl::Image2D read_img(size_t id){ + for(auto i:_images)if(i.second==id)return i.first; + } +private: + std::vector,size_t>> _buffers; + std::vector> _images; + +}; +class read_cl{ +public: + read_cl(std::string path){ + src_str=(char *)malloc(0x100000); + FILE *fp; + fp = fopen(path.c_str(), "r"); + size = fread(src_str, 1, 0x100000, fp); + fclose(fp); + } + void print(){ + std::cerr< + int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) { + try{ + _kernels.at(id1).write(id2,buf,queue,context,size); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Write buffer on GPU failed\n"< + int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){ + _kernels.at(id1)._kernel->setArg(id2,value); + }; + template + int32_t reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) { + try{ + _kernels.at(id1).reload(id2,buf,queue); + return CL_SUCCESS; + }catch (std::exception& err){ + std::cerr<<"Reload buffer on GPU failed\n"<1 && y>1){ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/); + }else if(z<2 && y>1){ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/); + }else{ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/); + }; + queue->finish(); + if(err!=0){ + std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo(); + std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo()<<" ,Run out: "<getInfo();++i) + std::cerr<<"argument type "<getArgInfo(i)<<'\n'; + } + return err; + + } + template + void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, T* out){ + queue->enqueueReadBuffer(_kernels.at(id1).read(id2).first, CL_TRUE, 0, sizeof(T) * _kernels.at(id1).read(id2).second, out); + } + void clear_buf(); +private: + std::vector _kernels; + cl::Context* context = NULL; + cl::Device device; + cl::Program* program = NULL; + cl::CommandQueue* queue=NULL; +}; + +#endif //VIO_OPENCL_CL_CLASS_H diff --git a/GPU_version/vio/include/gpu_svo/config.h b/GPU_version/vio/include/gpu_svo/config.h new file mode 100644 index 0000000..f6d710d --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/config.h @@ -0,0 +1,182 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_CONFIG_H_ +#define SVO_CONFIG_H_ + +#include +#include +#include + +namespace svo { + +using std::string; + +/// Global configuration file of SVO. +/// Implements the Singleton design pattern to allow global access and to ensure +/// that only one instance exists. +class Config +{ +public: + static Config& getInstance(); + + /// Base-name of the tracefiles. + static string& traceName() { return getInstance().trace_name; } + + /// Directory where the tracefiles are saved. + static string& traceDir() { return getInstance().trace_dir; } + + /// Number of pyramid levels used for features. + static size_t& nPyrLevels() { return getInstance().n_pyr_levels; } + + /// Use the IMU to get relative rotations. + static bool& useImu() { return getInstance().use_imu; } + + /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment. + static size_t& coreNKfs() { return getInstance().core_n_kfs; } + + /// Initial scale of the map. Depends on the distance the camera is moved for the initialization. + static double& mapScale() { return getInstance().map_scale; } + + /// Feature grid size of a cell in [px]. + static size_t& gridSize() { return getInstance().grid_size; } + + /// Initialization: Minimum required disparity between the first two frames. + static double& initMinDisparity() { return getInstance().init_min_disparity; } + + /// Initialization: Minimum number of tracked features. + static size_t& initMinTracked() { return getInstance().init_min_tracked; } + + /// Initialization: Minimum number of inliers after RANSAC. + static size_t& initMinInliers() { return getInstance().init_min_inliers; } + + /// Maximum level of the Lucas Kanade tracker. + static size_t& kltMaxLevel() { return getInstance().klt_max_level; } + + /// Minimum level of the Lucas Kanade tracker. + static size_t& kltMinLevel() { return getInstance().klt_min_level; } + + /// Reprojection threshold [px]. + static double& reprojThresh() { return getInstance().reproj_thresh; } + + /// Reprojection threshold after pose optimization. + static double& poseOptimThresh() { return getInstance().poseoptim_thresh; } + + /// Number of iterations in local bundle adjustment. + static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; } + + /// Maximum number of points to optimize at every iteration. + static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; } + + /// Number of iterations in structure optimization. + static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; } + + /// Reprojection threshold after bundle adjustment. + static double& lobaThresh() { return getInstance().loba_thresh; } + + /// Threshold for the robust Huber kernel of the local bundle adjustment. + static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; } + + /// Number of iterations in the local bundle adjustment. + static size_t& lobaNumIter() { return getInstance().loba_num_iter; } + + /// Minimum distance between two keyframes. Relative to the average height in the map. + static double& kfSelectMinDist() { return getInstance().kfselect_mindist; } + + /// Select only features with a minimum Harris corner score for triangulation. + static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; } + + /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required! + static size_t& subpixNIter() { return getInstance().subpix_n_iter; } + + /// Limit the number of keyframes in the map. This makes nslam essentially. + /// a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. + /// Minimum number of keyframes is 3. + static size_t& maxNKfs() { return getInstance().max_n_kfs; } + + /// How much (in milliseconds) is the camera delayed with respect to the imu. + static double& imgImuDelay() { return getInstance().img_imu_delay; } + + /// Maximum number of features that should be tracked. + static size_t& maxFts() { return getInstance().max_fts; } + + /// If the number of tracked features drops below this threshold. Tracking quality is bad. + static size_t& qualityMinFts() { return getInstance().quality_min_fts; } + + /// If within one frame, this amount of features are dropped. Tracking quality is bad. + static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; } + + /// acc white noise in continuous. + static double& ACC_Noise() { return getInstance().ACC_noise; } + + /// Gro white noise in continuous. + static double& GYO_Noise() { return getInstance().GYO_noise; } + /// integration uncertainty continuous + static double& IUC() { return getInstance().iuc; } + /// acc bias in continuous + static double& ABC() { return getInstance().abc; } + /// gyro bias in continuous + static double& GBC() { return getInstance().gbc; } + /// error in the bias used for preintegration + static double& EBP() { return getInstance().ebp; } + /// the noise used for projection + static double& NRP() { return getInstance().nrp; } + static double& Svo_Ekf() { return getInstance().svo_ekf; } + static double& X_off() { return getInstance().acc_x_off; } + static double& Y_off() { return getInstance().acc_y_off; } + static double& Teta_off() { return getInstance().gyr_z_off; } + static double& Cmd_Cov() { return getInstance().cmd_cov; } + + +private: + Config(); + Config(Config const&); + void operator=(Config const&); + string trace_name; + string trace_dir; + size_t n_pyr_levels; + bool use_imu; + size_t core_n_kfs; + double map_scale; + size_t grid_size; + double init_min_disparity; + size_t init_min_tracked; + size_t init_min_inliers; + size_t klt_max_level; + size_t klt_min_level; + double reproj_thresh; + double poseoptim_thresh; + size_t poseoptim_num_iter; + size_t structureoptim_max_pts; + size_t structureoptim_num_iter; + double loba_thresh; + double loba_robust_huber_width; + size_t loba_num_iter; + double kfselect_mindist; + double triang_min_corner_score; + size_t triang_half_patch_size; + size_t subpix_n_iter; + size_t max_n_kfs; + double img_imu_delay; + size_t max_fts; + size_t quality_min_fts; + int quality_max_drop_fts; + double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise,svo_ekf,acc_x_off,acc_y_off,gyr_z_off,cmd_cov; +}; + +} // namespace svo + +#endif // SVO_CONFIG_H_ diff --git a/GPU_version/vio/include/gpu_svo/depth_filter.h b/GPU_version/vio/include/gpu_svo/depth_filter.h new file mode 100644 index 0000000..9e82df4 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/depth_filter.h @@ -0,0 +1,170 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_DEPTH_FILTER_H_ +#define SVO_DEPTH_FILTER_H_ + +#include +#include +#include +//#include +#include +#include +#include + +namespace svo { + +class Frame; +class Feature; +class Point; + +/// A seed is a probabilistic depth estimate for a single pixel. +struct Seed +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int batch_counter; + static int seed_counter; + int batch_id; //!< Batch id is the id of the keyframe for which the seed was created. + int id; //!< Seed ID, only used for visualization. + Feature* ftr; //!< Feature in the keyframe for which the depth should be computed. + float a; //!< a of Beta distribution: When high, probability of inlier is large. + float b; //!< b of Beta distribution: When high, probability of outlier is large. + float mu; //!< Mean of normal distribution. + float z_range; //!< Max range of the possible depth. + float sigma2; //!< Variance of normal distribution. + Matrix2d patch_cov; //!< Patch covariance in reference image. + Seed(Feature* ftr, float depth_mean, float depth_min); +}; + +/// Depth filter implements the Bayesian Update proposed in: +/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández. +/// In Image and Vision Computing, 29(7):434-441, 2011. +/// +/// The class uses a callback mechanism such that it can be used also by other +/// algorithms than nslam and for simplified testing. +class DepthFilter +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef boost::unique_lock lock_t; + typedef boost::function callback_t; + + /// Depth-filter config parameters + struct Options + { + bool check_ftr_angle; //!< gradient features are only updated if the epipolar line is orthogonal to the gradient. + bool epi_search_1d; //!< restrict Gauss Newton in the epipolar search to the epipolar line. + bool verbose; //!< display output. + bool use_photometric_disparity_error; //!< use photometric disparity error instead of 1px error in tau computation. + int max_n_kfs; //!< maximum number of keyframes for which we maintain seeds. + double sigma_i_sq; //!< image noise. + double seed_convergence_sigma2_thresh; //!< threshold on depth uncertainty for convergence. + Options() + : check_ftr_angle(false), + epi_search_1d(false), + verbose(false), + use_photometric_disparity_error(false), + max_n_kfs(10), + sigma_i_sq(5e-4), + seed_convergence_sigma2_thresh(200.0) + {} + } options_; + + DepthFilter( + feature_detection::DetectorPtr feature_detector, + callback_t seed_converged_cb); + + virtual ~DepthFilter(); + + /// Start this thread when seed updating should be in a parallel thread. + void startThread(); + + /// Stop the parallel thread that is running. + void stopThread(); + + /// Add frame to the queue to be processed. + void addFrame(FramePtr frame); + + /// Add new keyframe to the queue + void addKeyframe(FramePtr frame, double depth_mean, double depth_min); + + /// Remove all seeds which are initialized from the specified keyframe. This + /// function is used to make sure that no seeds points to a non-existent frame + /// when a frame is removed from the map. + void removeKeyframe(FramePtr frame); + + /// If the map is reset, call this function such that we don't have pointers + /// to old frames. + void reset(); + + /// Returns a copy of the seeds belonging to frame. Thread-safe. + /// Can be used to compute the Next-Best-View in parallel. + /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame + /// so it is not being deleted while you use it. + void getSeedsCopy(const FramePtr& frame, std::list& seeds); + + /// Return a reference to the seeds. This is NOT THREAD SAFE! + //std::list& getSeeds() { return seeds_; } Bug 238 + std::list>& getSeeds() { return seeds_; } + + /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty + static void updateSeed( + const float x, + const float tau2, + Seed* seed); + + /// Compute the uncertainty of the measurement. + static double computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle); + +protected: + feature_detection::DetectorPtr feature_detector_; + callback_t seed_converged_cb_; + std::list> seeds_; + boost::mutex seeds_mut_; + bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted. + boost::thread* thread_; + std::queue frame_queue_; + boost::mutex frame_queue_mut_; + boost::condition_variable frame_queue_cond_; + FramePtr new_keyframe_; //!< Next keyframe to extract new seeds. + bool new_keyframe_set_; //!< Do we have a new keyframe to process?. + double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. + double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. + Matcher matcher_; + uint seeds_cont_=0; + + /// Initialize new seeds from a frame. + void initializeSeeds(FramePtr frame); + + /// Update all seeds with a new measurement frame. + virtual void updateSeeds(FramePtr frame); + + /// When a new keyframe arrives, the frame queue should be cleared. + void clearFrameQueue(); + + /// A thread that is continuously updating the seeds. + void updateSeedsLoop(); +}; + +} // namespace svo + +#endif // SVO_DEPTH_FILTER_H_ diff --git a/GPU_version/vio/include/gpu_svo/feature.h b/GPU_version/vio/include/gpu_svo/feature.h new file mode 100644 index 0000000..2184c4b --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/feature.h @@ -0,0 +1,75 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FEATURE_H_ +#define SVO_FEATURE_H_ + +#include + +namespace svo { + +/// A salient image region that is tracked across frames. +struct Feature +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum FeatureType { + CORNER, + EDGELET + }; + + FeatureType type; //!< Type can be corner or edgelet. + Frame* frame; //!< Pointer to frame in which the feature was detected. + Vector2d px; //!< Coordinates in pixels on pyramid level 0. + Vector3d f; //!< Unit-bearing vector of the feature. + int level; //!< Image pyramid level where feature was extracted. + Point* point; //!< Pointer to 3D point which corresponds to the feature. + Vector2d grad; //!< Dominant gradient direction for edglets, normalized. + + Feature(Frame* _frame, const Vector2d& _px, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(frame->cam_->cam2world(px)), + level(_level), + point(NULL), + grad(1.0,0.0) + {} + + Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(NULL), + grad(1.0,0.0) + {} + + Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(_point), + grad(1.0,0.0) + {} +}; + +} // namespace svo + +#endif // SVO_FEATURE_H_ diff --git a/GPU_version/vio/include/gpu_svo/feature_alignment.h b/GPU_version/vio/include/gpu_svo/feature_alignment.h new file mode 100644 index 0000000..e3b40a8 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/feature_alignment.h @@ -0,0 +1,63 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FEATURE_ALIGNMENT_H_ +#define SVO_FEATURE_ALIGNMENT_H_ + +#include + +namespace svo { + +/// Subpixel refinement of a reference feature patch with the current image. +/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" +/// paper by Baker. +namespace feature_alignment { + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv); + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd = false); + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +bool align2D_NEON( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +} // namespace feature_alignment +} // namespace svo + +#endif // SVO_FEATURE_ALIGNMENT_H_ diff --git a/GPU_version/vio/include/gpu_svo/feature_detection.h b/GPU_version/vio/include/gpu_svo/feature_detection.h new file mode 100644 index 0000000..4dc5ec8 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/feature_detection.h @@ -0,0 +1,111 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FEATURE_DETECTION_H_ +#define SVO_FEATURE_DETECTION_H_ + +#include +#include +#include + +namespace svo { + +/// Implementation of various feature detectors. +namespace feature_detection { + +/// Temporary container used for corner detection. Features are initialized from these. +struct Corner +{ + int x; //!< x-coordinate of corner in the image. + int y; //!< y-coordinate of corner in the image. + int level; //!< pyramid level of the corner. + float score; //!< shi-tomasi score of the corner. + float angle; //!< for gradient-features: dominant gradient angle. + Corner(int x, int y, float score, int level, float angle) : + x(x), y(y), level(level), score(score), angle(angle) + {} +}; +typedef vector Corners; + +/// All detectors should derive from this abstract class. +class AbstractDetector +{ +public: + AbstractDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels); + + virtual ~AbstractDetector() { + }; + + virtual void detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts) = 0; + + /// Flag the grid cell as occupied + void setGridOccpuancy(const Vector2d& px); + + /// Set grid cells of existing features as occupied + void setExistingFeatures(const Features& fts); + +protected: + + static const int border_ = 8; //!< no feature should be within 8px of border. + const int cell_size_; + const int n_pyr_levels_; + const int grid_n_cols_; + const int grid_n_rows_; + vector grid_occupancy_; + + void resetGrid(); + + inline int getCellIndex(int x, int y, int level) + { + const int scale = (1< DetectorPtr; + +/// FAST detector by Edward Rosten. +class FastDetector : public AbstractDetector +{ +public: + FastDetector( + const int img_width, + const int img_height, + const int cell_size, + opencl* gpu_fast_, + const int n_pyr_levels); + + virtual ~FastDetector() {} + + virtual void detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts); + opencl* gpu_fast_; +}; + +} // namespace feature_detection +} // namespace svo + +#endif // SVO_FEATURE_DETECTION_H_ diff --git a/GPU_version/vio/include/gpu_svo/for_it.hpp b/GPU_version/vio/include/gpu_svo/for_it.hpp new file mode 100755 index 0000000..808f3cf --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/for_it.hpp @@ -0,0 +1,87 @@ +// +// Created by root on 5/12/21. +// + +#ifndef GPU_SVO_FOR_IT_H +#define GPU_SVO_FOR_IT_H +#include +#include +#include + +//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop +// Wrapper class +template +class for_impl +{ +public: + // The return value of the operator* of the iterator, this + // is what you will get inside of the for loop + struct item + { + size_t index; + typename T::value_type & item; + }; + typedef item value_type; + + // Custom iterator with minimal interface + struct iterator + { + iterator(typename T::iterator _it, size_t counter=0) : + it(_it), counter(counter) + {} + + iterator operator++() + { + return iterator(++it, ++counter); + } + + bool operator!=(iterator other) + { + return it != other.it; + } + + typename T::iterator::value_type item() + { + return *it; + } + + value_type operator*() + { + return value_type{counter, *it}; + } + + size_t index() + { + return counter; + } + + private: + typename T::iterator it; + size_t counter; + }; + + for_impl(T & t) : container(t) {} + + iterator begin() + { + return iterator(container.begin()); + } + + iterator end() + { + return iterator(container.end()); + } + +private: + T & container; +}; + + +// A templated free function allows you to create the wrapper class +// conveniently +template +for_impl _for(T & t) +{ + return for_impl(t); +} +#endif //GPU_SVO_FOR_IT_H diff --git a/GPU_version/vio/include/gpu_svo/frame.h b/GPU_version/vio/include/gpu_svo/frame.h new file mode 100644 index 0000000..3d1fd72 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/frame.h @@ -0,0 +1,185 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FRAME_H_ +#define SVO_FRAME_H_ + +#include +#include +#include +#include +#include + +namespace g2o { +class VertexSE3Expmap; +} +typedef g2o::VertexSE3Expmap g2oFrameSE3; + +namespace svo { + +class Point; +struct Feature; + +typedef list Features; +typedef vector ImgPyr; + + +/// A frame saves the image, the associated features and the estimated pose. +class Frame : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. + int id_; //!< Unique id of the frame. + double timestamp_; //!< Timestamp of when the image was recorded. + vk::AbstractCamera* cam_; //!< Camera model. + SE2_5 T_f_w_; //!< Transform (f)rame from (w)orld. + Matrix Cov_; //!< Covariance. + ImgPyr img_pyr_; //!< Image Pyramid. + Features fts_; //!< List of features in the image. + vector key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. + bool is_keyframe_; //!< Was this frames selected as keyframe? + g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. + int last_published_ts_; //!< Timestamp of last publishing. + + Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); + ~Frame(); + + /// Initialize new frame and create image pyramid. + void initFrame(const cv::Mat& img); + + /// Select this frame as keyframe. + void setKeyframe(); + + /// Add a feature to the image + void addFeature(Feature* ftr); + + /// The KeyPoints are those five features which are closest to the 4 image corners + /// and to the center and which have a 3D point assigned. These points are used + /// to quickly check whether two frames have overlapping field of view. + void setKeyPoints(); + + /// Check if we can select five better key-points. + void checkKeyPoints(Feature* ftr); + + /// If a point is deleted, we must remove the corresponding key-point. + void removeKeyPoint(Feature* ftr); + + /// Return number of point observations. + inline size_t nObs() const { return fts_.size(); } + + /// Check if a point in (w)orld coordinate frame is visible in the image. + bool isVisible(const Vector3d& xyz_w) const; + + /// Full resolution image stored in the frame. + inline const cv::Mat& img() const { return img_pyr_[0]; } + + /// Was this frame selected as keyframe? + inline bool isKeyframe() const { return is_keyframe_; } + + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). + inline Vector2d w2c(const Vector3d& xyz_w) const { + return cam_->world2cam( T_f_w_.se3()*xyz_w); + } + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). + inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); } + + /// Transforms point coordinates in world-frame (w) to camera-frams (f). + inline Vector3d w2f(const Vector3d& xyz_w) const { + return Vector3d(T_f_w_.se3()*xyz_w); + } + + /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). + inline Vector3d f2w(const Vector3d& f) const { + return getSE3Inv() * f; + } + + /// Projects Point from unit sphere (f) in camera pixels (c). + inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); } + + /// Return the pose of the frame in the (w)orld coordinate frame. + inline Vector2d pos() const { return T_f_w_.se2().translation(); } + + inline SE3 se3() const{ + return T_f_w_.se3(); + } + + inline SE3 getSE3Inv() const{ + Eigen::Matrix R; + R<& J) + { + const double x = xyz_in_f[0]; + const double y = xyz_in_f[1]; + const double z_inv = 1./xyz_in_f[2]; + const double z_inv_2 = z_inv*z_inv; + + J(0,0) = -z_inv; // -1/z + J(0,1) = x*z_inv_2; // x/z^2 + J(0,2) = -(1.0 + pow(x,2)/z_inv_2); // -(1.0 + x^2/z^2) + + J(1,0) = 1e-19; // 0 + J(1,1) = y*z_inv_2; // y/z^2 + J(1,2) = -x*y/z_inv_2; // -x*y/z^2 + } + inline static void jacobian_l2uv( + const Matrix& p, + Matrix& J){ + double A = -1.0*sqrt(p.z()); + double z_inv = -1/p.z(); + J(0,0) = z_inv; + J(0,1) = 1e-19; + J(0,2) = p.x()*A; + + J(1,0) = 1e-19; + J(1,1) = z_inv; + J(1,2) =A*p.y(); + } +}; + + +/// Some helper functions for the frame object. +namespace frame_utils { + +/// Creates an image pyramid of half-sampled images. +void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr); + +/// Get the average depth of the features in the image. +bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min); + +} // namespace frame_utils +} // namespace svo + +#endif // SVO_FRAME_H_ diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_base.h b/GPU_version/vio/include/gpu_svo/frame_handler_base.h new file mode 100644 index 0000000..99b4195 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/frame_handler_base.h @@ -0,0 +1,118 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FRAME_HANDLER_BASE_H_ +#define SVO_FRAME_HANDLER_BASE_H_ + +#include +#include +#include +#include +#include +#include + +namespace vk +{ +class AbstractCamera; +//class PerformanceMonitor; +} + +namespace svo +{ +class Point; +class Matcher; +class DepthFilter; + +/// Base class for various VO pipelines. Manages the map and the state machine. +class FrameHandlerBase : boost::noncopyable +{ +public: + enum Stage { + STAGE_PAUSED, + STAGE_FIRST_FRAME, + STAGE_SECOND_FRAME, + STAGE_DEFAULT_FRAME, + STAGE_RELOCALIZING + }; + enum TrackingQuality { + TRACKING_INSUFFICIENT, + TRACKING_BAD, + TRACKING_GOOD + }; + enum UpdateResult { + RESULT_NO_KEYFRAME, + RESULT_IS_KEYFRAME, + RESULT_FAILURE + }; + + FrameHandlerBase(); + + virtual ~FrameHandlerBase(); + + /// Get the current map. + const Map& map() const { return map_; } + + /// Will reset the map as soon as the current frame is finished processing. + void reset() { set_reset_ = true; } + + /// Start processing. + void start() { set_start_ = true; } + + /// Get the current stage of the algorithm. + Stage stage() const { return stage_; } + + /// Get tracking quality. + TrackingQuality trackingQuality() const { return tracking_quality_; } + + /// Get the processing time of the previous iteration. + //double lastProcessingTime() const { return timer_.getTime(); } + + /// Get the number of feature observations of the last frame. + // size_t lastNumObservations() const { return num_obs_last_; } + +protected: + Stage stage_; //!< Current stage of the algorithm. + bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration. + bool set_start_; //!< Flag the user can set to start the system when the next image is received. + Map map_; //!< Map of keyframes created by the slam system. + size_t num_obs_last_; //!< Number of observations in the previous frame. + TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features. + + /// Before a frame is processed, this function is called. + bool startFrameProcessingCommon(const double timestamp); + + /// When a frame is finished processing, this function is called. + int finishFrameProcessingCommon( + const size_t update_id, + const UpdateResult dropout, + const size_t num_observations); + + /// Reset the map and frame handler to start from scratch. + void resetCommon(); + + /// Reset the frame handler. Implement in derived class. + virtual void resetAll() { resetCommon(); } + + /// Set the tracking quality based on the number of tracked features. + virtual void setTrackingQuality(const size_t num_observations); + + /// Optimize some of the observed 3D points. + virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter); +}; + +} // namespace nslam + +#endif // SVO_FRAME_HANDLER_BASE_H_ diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h b/GPU_version/vio/include/gpu_svo/frame_handler_mono.h new file mode 100644 index 0000000..2e3b6ed --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/frame_handler_mono.h @@ -0,0 +1,109 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_FRAME_HANDLER_H_ +#define SVO_FRAME_HANDLER_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +/// Monocular Visual Odometry Pipeline as described in the SVO paper. +class FrameHandlerMono : public FrameHandlerBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix& init); + virtual ~FrameHandlerMono(); + + /// Provide an image. + void addImage(const cv::Mat& img, double timestamp,const ros::Time& time); + + /// Set the first frame (used for synthetic datasets in benchmark node) + void setFirstFrame(const FramePtr& first_frame); + + /// Get the last frame that has been processed. + FramePtr lastFrame() { return last_frame_; } + + /// Get the set of spatially closest keyframes of the last frame. + const set& coreKeyframes() { return core_kfs_; } + + /// Return the feature track to visualize the KLT tracking during initialization. + const vector& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; } + const vector& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; } + + /// Access the depth filter. + DepthFilter* depthFilter() const{ return depth_filter_; } + + /// An external place recognition module may know where to relocalize. + bool relocalizeFrameAtPose( + const int keyframe_id, + const SE3& T_kf_f, + const cv::Mat& img, + const double timestamp); + //std::shared_ptr imu_integPtr_; //Integration of IMU + void UpdateIMU(double* value,const ros::Time& time); + void UpdateCmd(double* value,const ros::Time& time); + UKF ukfPtr_; + +protected: + vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). + Reprojector reprojector_; //!< Projects points from other keyframes into the current frame + FramePtr new_frame_; //!< Current frame. + FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. + set core_kfs_; //!< Keyframes in the closer neighbourhood. + vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? + initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. + DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. + opencl* gpu_fast_; + ros::Time time_; + + /// Initialize the visual odometry algorithm. + virtual void initialize(); + + /// Processes the first frame and sets it as a keyframe. + virtual UpdateResult processFirstFrame(); + + /// Processes all frames after the first frame until a keyframe is selected. + virtual UpdateResult processSecondFrame(); + + /// Processes all frames after the first two keyframes. + virtual UpdateResult processFrame(); + + /// Try relocalizing the frame at relative position to provided keyframe. + virtual UpdateResult relocalizeFrame( + const SE3& T_cur_ref, + FramePtr ref_keyframe); + + /// Reset the frame handler. Implement in derived class. + virtual void resetAll(); + + /// Keyframe selection criterion. + virtual bool needNewKf(double scene_depth_mean); + + void setCoreKfs(size_t n_closest); +}; + +} // namespace svo + +#endif // SVO_FRAME_HANDLER_H_ diff --git a/GPU_version/vio/include/gpu_svo/global.h b/GPU_version/vio/include/gpu_svo/global.h new file mode 100644 index 0000000..60d0f55 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/global.h @@ -0,0 +1,120 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_GLOBAL_H_ +#define SVO_GLOBAL_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit +#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) +#endif +#include +#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x) +#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x) +#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x) +#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x) +#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x) + + +namespace svo +{ + using namespace Eigen; + using namespace Sophus; + + const double EPS = 0.0000000001; + const double PI = 3.14159265; + class SE2_5: public SE2{ + public: + SE2_5(SE2&& se2):T2_(se2){ + } + SE2_5(SE2& se2):T2_(se2){ + } + SE2_5(SE3&& se3){ + Quaterniond q=se3.unit_quaternion().normalized(); + double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x())); + if (std::abs(pitch) >= 1) + pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range + else + pitch = std::asin(pitch); + Eigen::Matrix R; + R<= 1) + pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range + else + pitch = std::asin(pitch); + Eigen::Matrix R; + R< R; + R< R; + R< FramePtr; +} // namespace svo + +#endif // SVO_GLOBAL_H_ diff --git a/GPU_version/vio/include/gpu_svo/homography.h b/GPU_version/vio/include/gpu_svo/homography.h new file mode 100644 index 0000000..7f5a7b2 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/homography.h @@ -0,0 +1,85 @@ +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + * + * This class implements the homography decomposition of Faugeras and Lustman's + * 1988 tech report. Code converted to Eigen from PTAM. + * + */ + +#ifndef HOMOGRAPHY_H_ +#define HOMOGRAPHY_H_ + +#include +#include +#include +#include + +namespace vk { + +using namespace Eigen; +using namespace std; + +struct HomographyDecomposition +{ + Vector3d t; + Matrix3d R; + double d; + Vector3d n; + + // Resolved Composition + Sophus::SE3 T; //!< second from first + int score; +}; + +class Homography +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Homography (const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px); + + void + calcFromPlaneParams (const Vector3d & normal, + const Vector3d & point_on_plane); + + void + calcFromMatches (); + + size_t + computeMatchesInliers (); + + bool + computeSE3fromMatches (); + + bool + decompose (); + + void + findBestDecomposition (); + + double thresh; + double error_multiplier2; + const vector >& fts_c1; //!< Features on first image on unit plane + const vector >& fts_c2; //!< Features on second image on unit plane + vector inliers; + SE3 T_c2_from_c1; //!< Relative translation and rotation of two images + Matrix3d H_c2_from_c1; //!< Homography + vector decompositions; +}; + + + + +} /* end namespace vk */ + +#endif /* HOMOGRAPHY_H_ */ diff --git a/GPU_version/vio/include/gpu_svo/initialization.h b/GPU_version/vio/include/gpu_svo/initialization.h new file mode 100644 index 0000000..28fddbf --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/initialization.h @@ -0,0 +1,87 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_INITIALIZATION_H +#define SVO_INITIALIZATION_H + +#include +#include +//#include "../../gtsam/imu_integration.h" + +namespace svo { + +class FrameHandlerMono; + +/// Bootstrapping the map from the first two views. +namespace initialization { + +enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; + +/// Tracks features using Lucas-Kanade tracker and then estimates a homography. +class KltHomographyInit { + friend class svo::FrameHandlerMono; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FramePtr frame_ref_; + KltHomographyInit(opencl* gpu_fast_):gpu_fast_(gpu_fast_),T_cur_from_ref_(0.0,0.0,0.0) {}; + ~KltHomographyInit() {}; + InitResult addFirstFrame(FramePtr frame_ref); + InitResult addSecondFrame(FramePtr frame_ref); + void reset(); + +protected: + vector px_ref_; //!< keypoints to be tracked in reference frame. + vector px_cur_; //!< tracked keypoints in current frame. + vector f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. + vector f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. + vector disparities_; //!< disparity between first and second frame. + vector inliers_; //!< inliers after the geometric check (e.g., Homography). + vector xyz_in_cur_; //!< 3D points computed during the geometric check. + SE2_5 T_cur_from_ref_; //!< computed transformation between the first two frames. + opencl* gpu_fast_; +}; + +/// Detect Fast corners in the image. +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec, + opencl* gpu_fast); + +/// Compute optical flow (Lucas Kanade) for selected keypoints. +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities); + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE2_5& T_cur_from_ref); + +} // namespace initialization +} // namespace svo + +#endif // SVO_INITIALIZATION_H diff --git a/GPU_version/vio/include/gpu_svo/map.h b/GPU_version/vio/include/gpu_svo/map.h new file mode 100644 index 0000000..d4a02e0 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/map.h @@ -0,0 +1,137 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_MAP_H_ +#define SVO_MAP_H_ + +#include +#include +#include +#include + +namespace svo { + +class Point; +class Feature; +class Seed; + +/// Container for converged 3D points that are not already assigned to two keyframes. +class MapPointCandidates +{ +public: + typedef pair PointCandidate; + typedef list PointCandidateList; + + /// The depth-filter is running in a parallel thread and fills the canidate list. + /// This mutex controls concurrent access to point_candidates. + boost::mutex mut_; + + /// Candidate points are created from converged seeds. + /// Until the next keyframe, these points can be used for reprojection and pose optimization. + PointCandidateList candidates_; + list< Point* > trash_points_; + + MapPointCandidates(); + ~MapPointCandidates(); + + /// Add a candidate point. + void newCandidatePoint(Point* point, double depth_sigma2); + + /// Adds the feature to the frame and deletes candidate from list. + void addCandidatePointToFrame(FramePtr frame); + + /// Remove a candidate point from the list of candidates. + bool deleteCandidatePoint(Point* point); + + /// Remove all candidates that belong to a frame. + void removeFrameCandidates(FramePtr frame); + + /// Reset the candidate list, remove and delete all points. + void reset(); + + void deleteCandidate(PointCandidate& c); + + void emptyTrash(); +}; +static bool syn_=false; +/// Map object which saves all keyframes which are in a map. +class Map : boost::noncopyable +{ +public: + list< FramePtr > keyframes_; //!< List of keyframes in the map. + list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also. + MapPointCandidates point_candidates_; + + + Map(); + ~Map(); + + /// Reset the map. Delete all keyframes and reset the frame and point counters. + void reset(); + + /// Delete a point in the map and remove all references in keyframes to it. + void safeDeletePoint(Point* pt); + + /// Moves the point to the trash queue which is cleaned now and then. + void deletePoint(Point* pt); + + /// Moves the frame to the trash queue which is cleaned now and then. + bool safeDeleteFrame(FramePtr frame); + + /// Remove the references between a point and a frame. + void removePtFrameRef(Frame* frame, Feature* ftr); + + /// Add a new keyframe to the map. + void addKeyframe(FramePtr new_keyframe); + + /// Given a frame, return all keyframes which have an overlapping field of view. + void getCloseKeyframes(const FramePtr& frame, list< pair >& close_kfs) const; + + /// Return the keyframe which is spatially closest and has overlapping field of view. + FramePtr getClosestKeyframe(const FramePtr& frame) const; + + /// Return the keyframe which is furthest apart from pos. + FramePtr getFurthestKeyframe(const Vector2d& pos) const; + + bool getKeyframeById(const int id, FramePtr& frame) const; + + /// Transform the whole map with rotation R, translation t and scale s. + void transform(const Matrix2d& R, const Vector2d& t, const double& s); + + /// Empty trash bin of deleted keyframes and map points. We don't delete the + /// points immediately to ensure proper cleanup and to provide the visualizer + /// a list of objects which must be removed. + void emptyTrash(); + + /// Return the keyframe which was last inserted in the map. + inline FramePtr lastKeyframe() { return keyframes_.back(); } + + /// Return the number of keyframes in the map + inline size_t size() const { return keyframes_.size(); } +}; + +/// A collection of debug functions to check the data consistency. +namespace map_debug { + +void mapStatistics(Map* map); +void mapValidation(Map* map, int id); +void frameValidation(Frame* frame, int id); +void pointValidation(Point* point, int id); + +} // namespace map_debug +} // namespace svo + +#endif // SVO_MAP_H_ diff --git a/GPU_version/vio/include/gpu_svo/matcher.h b/GPU_version/vio/include/gpu_svo/matcher.h new file mode 100644 index 0000000..1d37b57 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/matcher.h @@ -0,0 +1,130 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_MATCHER_H_ +#define SVO_MATCHER_H_ + +#include + +namespace vk { + class AbstractCamera; + namespace patch_score { + template class ZMSSD; + } +} + +namespace svo { + +class Point; +class Frame; +class Feature; + +/// Warp a patch from the reference view to the current view. +namespace warp { + +void getWarpMatrixAffine( + const vk::AbstractCamera& cam_ref, + const vk::AbstractCamera& cam_cur, + const Vector2d& px_ref, + const Vector3d& f_ref, + const double depth_ref, + const SE3& T_cur_ref, + const int level_ref, + Matrix2d& A_cur_ref); + +int getBestSearchLevel( + const Matrix2d& A_cur_ref, + const int max_level); + +bool warpAffine( + const Matrix2d& A_cur_ref, + const cv::Mat& img_ref, + const Vector2d& px_ref, + const int level_ref, + const int level_cur, + const int halfpatch_size, + uint8_t* patch); + +} // namespace warp + +/// Patch-matcher for reprojection-matching and epipolar search in triangulation. +class Matcher +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static const int halfpatch_size_ = 4; + static const int patch_size_ = 8; + + typedef vk::patch_score::ZMSSD PatchScore; + + struct Options + { + bool align_1d; //!< in epipolar search: align patch 1D along epipolar line + int align_max_iter; //!< number of iterations for aligning the feature patches in gauss newton + double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align + size_t max_epi_search_steps;//!< max number of evaluations along epipolar line + bool subpix_refinement; //!< do gauss newton feature patch alignment after epipolar search + bool epi_search_edgelet_filtering; + double epi_search_edgelet_max_angle; + Options() : + align_1d(false), + align_max_iter(10), + max_epi_length_optim(2.0), + max_epi_search_steps(1000), + subpix_refinement(true), + epi_search_edgelet_filtering(true), + epi_search_edgelet_max_angle(0.7) + {} + } options_; + + uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16))); + uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16))); + Matrix2d A_cur_ref_; //!< affine warp matrix + Vector2d epi_dir_; + double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search) + double h_inv_; //!< hessian of 1d image alignment along epipolar line + int search_level_; + bool reject_; + Feature* ref_ftr_; + Vector2d px_cur_; + + Matcher() = default; + ~Matcher() = default; + + /// Find a match by directly applying subpix refinement. + /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result! + bool findMatchDirect( + const Point& pt, + const Frame& frame, + Vector2d& px_cur); + + /// Find a match by searching along the epipolar line without using any features. + bool findEpipolarMatchDirect( + const Frame& ref_frame, + const Frame& cur_frame, + const Feature& ref_ftr, + const double d_estimate, + const double d_min, + const double d_max, + double& depth); + + void createPatchFromPatchWithBorder(); +}; + +} // namespace svo + +#endif // SVO_MATCHER_H_ diff --git a/GPU_version/vio/include/gpu_svo/math_utils.h b/GPU_version/vio/include/gpu_svo/math_utils.h new file mode 100644 index 0000000..6fffc38 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/math_utils.h @@ -0,0 +1,171 @@ +/* + * math_utils.h + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#ifndef MATH_UTILS_H_ +#define MATH_UTILS_H_ + + +#include +#include +#include + +namespace vk +{ + +using namespace Eigen; +using namespace std; +using namespace Sophus; + +Vector3d triangulateFeatureNonLin( + const Matrix3d& R, + const Vector3d& t, + const Vector3d& feature1, + const Vector3d& feature2); + +/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e. +/// perfect triangulation without noise! +bool depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c); + +double reprojError( + const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2); + +double computeInliers( + const vector& features1, + const vector& features2, + const Matrix3d& R, + const Vector3d& t, + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, + vector& inliers, + vector& outliers); + +void computeInliersOneView( + const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers); + +//! Direct Cosine Matrix to Roll Pitch Yaw +Vector3d dcm2rpy(const Matrix3d &R); + +//! Roll Pitch Yaw to Direct Cosine Matrix +Matrix3d rpy2dcm(const Vector3d &rpy); + +//! Angle Axis parametrization to Quaternion +Quaterniond angax2quat(const Vector3d& n, const double& angle); + +//! Angle Axis parametrization to Matrix representation +Matrix3d angax2dcm(const Vector3d& n, const double& angle); + +double sampsonusError( + const Vector2d &v2Dash, + const Matrix3d& m3Essential, + const Vector2d& v2); + +inline Matrix3d sqew(const Vector3d& v) +{ + Matrix3d v_sqew; + v_sqew << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return v_sqew; +} + +inline double norm_max(const Eigen::VectorXd & v) +{ + double max = -1; + for (int i=0; imax){ + max = abs; + } + } + return max; +} + +inline Vector2d project2d(const Vector3d& v) +{ + return v.head<2>()/v[2]; +} + +inline Vector3d unproject2d(const Vector2d& v) +{ + return Vector3d(v[0], v[1], 1.0); +} + +inline Vector3d project3d(const Vector4d& v) +{ + return v.head<3>()/v[3]; +} + +inline Vector4d unproject3d(const Vector3d& v) +{ + return Vector4d(v[0], v[1], v[2], 1.0); +} + +template +T getMedian(vector& data_vec) +{ + assert(!data_vec.empty()); + typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); + nth_element(data_vec.begin(), it, data_vec.end()); + return *it; +} + +inline double pyrFromZero_d(double x_0, int level) +{ + return x_0/(1< & frame_jac) +{ + const double x = xyz[0]; + const double y = xyz[1]; + const double z = xyz[2]; + const double z_2 = z*z; + + frame_jac(0,0) = -1./z *focal_length; + frame_jac(0,1) = 0; + frame_jac(0,2) = x/z_2 *focal_length; + frame_jac(0,3) = x*y/z_2 * focal_length; + frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; + frame_jac(0,5) = y/z *focal_length; + + frame_jac(1,0) = 0; + frame_jac(1,1) = -1./z *focal_length; + frame_jac(1,2) = y/z_2 *focal_length; + frame_jac(1,3) = (1+y*y/z_2) *focal_length; + frame_jac(1,4) = -x*y/z_2 *focal_length; + frame_jac(1,5) = -x/z *focal_length; +} + +} // end namespace vk + +#endif /* MATH_UTILS_H_ */ diff --git a/GPU_version/vio/include/gpu_svo/nlls_solver.h b/GPU_version/vio/include/gpu_svo/nlls_solver.h new file mode 100644 index 0000000..cb6320b --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/nlls_solver.h @@ -0,0 +1,169 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_H_ +#define LM_SOLVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +/** + * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems. + * + * The function implements two algorithms: Levenberg Marquardt and Gauss Newton + * + * Example implementations of this function can be found in the rpl_examples + * package: img_align_2d.cpp, img_align_3d.cpp + * + * Template Parameters: + * D : dimension of the residual + * T : type of the model, e.g. SE2, SE3 + */ + +template +class NLLSSolver { + +public: + typedef T ModelType; + enum Method{GaussNewton, LevenbergMarquardt}; + enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale}; + enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight}; + +protected: + Matrix H_; //!< Hessian approximation + Matrix Jres_; //!< Jacobian x Residual + Matrix x_; //!< update step + bool have_prior_; + ModelType prior_; + Matrix I_prior_; //!< Prior information matrix (inverse covariance) + double chi2_; + double rho_; + Method method_; + + /// If the flag linearize_system is set, the function must also compute the + /// Jacobian and set the member variables H_, Jres_ + virtual double + computeResiduals (const ModelType& model, + bool linearize_system, + bool compute_weight_scale) = 0; + + /// Solve the linear system H*x = Jres. This function must set the update + /// step in the member variable x_. Must return true if the system could be + /// solved and false if it was singular. + virtual int + solve () = 0; + + virtual void + update (const ModelType& old_model, ModelType& new_model) = 0; + + virtual void + applyPrior (const ModelType& current_model) { } + + virtual void + startIteration () { } + + virtual void + finishIteration () { } + + virtual void + finishTrial () { } + +public: + + /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this + /// ensures that x is a descent direction. If mu is large, x is a short step in + /// the steepest direction. This is good if the current iterate is far from the + /// solution. If mu is small, LM approximates gauss newton iteration and we + /// have (almost) quadratic convergence in the final stages. + double mu_init_, mu_; + double nu_init_, nu_; //!< Increase factor of mu after fail + size_t n_iter_init_, n_iter_; //!< Number of Iterations + size_t n_trials_; //!< Number of trials + size_t n_trials_max_; //!< Max number of trials + size_t n_meas_; //!< Number of measurements + bool stop_; //!< Stop flag + bool verbose_; //!< Output Statistics + double eps_; //!< Stop if update norm is smaller than eps + size_t iter_; //!< Current Iteration + + // robust least squares + bool use_weights_; + float scale_; + robust_cost::ScaleEstimatorPtr scale_estimator_; + robust_cost::WeightFunctionPtr weight_function_; + + NLLSSolver() : + have_prior_(false), + method_(LevenbergMarquardt), + mu_init_(0.01f), + mu_(mu_init_), + nu_init_(2.0), + nu_(nu_init_), + n_iter_init_(15), + n_iter_(n_iter_init_), + n_trials_(0), + n_trials_max_(5), + n_meas_(0), + stop_(false), + verbose_(true), + eps_(0.0000000001), + iter_(0), + use_weights_(false), + scale_(0.0), + scale_estimator_(NULL), + weight_function_(NULL) + { } + + virtual ~NLLSSolver() {} + + /// Calls the GaussNewton or LevenbergMarquardt optimization strategy + void optimize(ModelType& model); + + /// Gauss Newton optimization strategy + void optimizeGaussNewton(ModelType& model); + + /// Levenberg Marquardt optimization strategy + void optimizeLevenbergMarquardt(ModelType& model); + + /// Specify the robust cost that should be used and the appropriate scale estimator + void setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function); + + /// Add prior to optimization. + void setPrior( + const ModelType& prior, + const Matrix& Information); + + /// Reset all parameters to restart the optimization + void reset(); + + /// Get the squared error + const double& getChi2() const; + + /// The Information matrix is equal to the inverse covariance matrix. + const Matrix& getInformationMatrix() const; +}; + +} // end namespace vk + +#include "gpu_svo/nlls_solver_impl.hpp" + +#endif /* LM_SOLVER_H_ */ diff --git a/GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp b/GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp new file mode 100644 index 0000000..2ce9302 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp @@ -0,0 +1,348 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_IMPL_HPP_ +#define LM_SOLVER_IMPL_HPP_ + +#include + +template +void vk::NLLSSolver::optimize(ModelType& model) +{ + if(method_ == GaussNewton) + optimizeGaussNewton(model); + else if(method_ == LevenbergMarquardt) + optimizeLevenbergMarquardt(model); +} + +template +void vk::NLLSSolver::optimizeGaussNewton(ModelType& model) +{ + // Compute weight scale + if(use_weights_) + computeResiduals(model, false, true); + + // Save the old model to rollback in case of unsuccessful update + ModelType old_model(model); + + // perform iterative estimation + for (iter_ = 0; iter_::Identity(D,D); + Jres_.setZero(); + + // compute initial error + n_meas_ = 0; + computeResiduals(model, true, false); + + // add damping term: + H_ += (H_.diagonal()*mu_).asDiagonal(); + + // add prior + if(have_prior_) + applyPrior(model); + + // solve the linear system + if(solve()) + { + // update the model + update(model, new_model); + + // compute error with new model and compare to old error + n_meas_ = 0; + new_chi2 = computeResiduals(new_model, true, false); + rho_ = chi2_-new_chi2; + } + else + { + // matrix was singular and could not be computed + cout << "Matrix is close to singular!" << endl; + cout << "H = " << H_ << endl; + cout << "Jres = " << Jres_ << endl; + rho_ = -1; + } + + if(rho_>0) + { + // update decrased the error -> success + model = new_model; + chi2_ = new_chi2; + stop_ = vk::norm_max(x_)<=eps_ ? true : false; + mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.)); + nu_ = 2.; + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Success" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << "\t x_norm = " << vk::norm_max(x_) + << endl; + } + } + else + { + if(sign){ + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << "\t x_norm = " << vk::norm_max(x_) + << endl; + } + mu_ /= nu_; + nu_ /= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + sign=false; + }else{ + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << "\t x_norm = " << vk::norm_max(x_) + << endl; + } + mu_ *= nu_; + nu_ *= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + sign=true; + } + + } + + finishTrial(); + + } while(!(rho_>0 || stop_)); + if (stop_) + break; + + finishIteration(); + } +} + + +template +void vk::NLLSSolver::setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function) +{ + switch(scale_estimator) + { + case TDistScale: + if(verbose_) + printf("Using TDistribution Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator()); + use_weights_=true; + break; + case MADScale: + if(verbose_) + printf("Using MAD Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::MADScaleEstimator()); + use_weights_=true; + break; + case NormalScale: + if(verbose_) + printf("Using Normal Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator()); + use_weights_=true; + break; + default: + if(verbose_) + printf("Using Unit Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::UnitScaleEstimator()); + use_weights_=false; + } + + switch(weight_function) + { + case TDistWeight: + if(verbose_) + printf("Using TDistribution Weight Function\n"); + weight_function_.reset(new robust_cost::TDistributionWeightFunction()); + break; + case TukeyWeight: + if(verbose_) + printf("Using Tukey Weight Function\n"); + weight_function_.reset(new robust_cost::TukeyWeightFunction()); + break; + case HuberWeight: + if(verbose_) + printf("Using Huber Weight Function\n"); + weight_function_.reset(new robust_cost::HuberWeightFunction()); + break; + default: + if(verbose_) + printf("Using Unit Weight Function\n"); + weight_function_.reset(new robust_cost::UnitWeightFunction()); + } +} + +template +void vk::NLLSSolver::setPrior( + const T& prior, + const Matrix& Information) +{ + have_prior_ = true; + prior_ = prior; + I_prior_ = Information; +} + +template +void vk::NLLSSolver::reset() +{ + have_prior_ = false; + chi2_ = 1e10; + mu_ = mu_init_; + nu_ = nu_init_; + n_meas_ = 0; + n_iter_ = n_iter_init_; + iter_ = 0; + stop_ = false; +} + +template +inline const double& vk::NLLSSolver::getChi2() const +{ + return chi2_; +} + +template +inline const vk::Matrix& vk::NLLSSolver::getInformationMatrix() const +{ + return H_; +} + +#endif /* LM_SOLVER_IMPL_HPP_ */ diff --git a/GPU_version/vio/include/gpu_svo/output_helper.h b/GPU_version/vio/include/gpu_svo/output_helper.h new file mode 100644 index 0000000..b1572a8 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/output_helper.h @@ -0,0 +1,90 @@ +/* + * output_helper.h + * + * Created on: Jan 20, 2013 + * Author: cforster + */ + +#ifndef VIKIT_OUTPUT_HELPER_H_ +#define VIKIT_OUTPUT_HELPER_H_ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +void +publishTfTransform (const Sophus::SE3& T, const ros::Time& stamp, + const string& frame_id, const string& child_frame_id, + tf::TransformBroadcaster& br); + +void +publishPointMarker (ros::Publisher pub, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime = ros::Duration(0.0)); + +void +publishLineMarker (ros::Publisher pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime = ros::Duration(0.0)); + +void +publishArrowMarker (ros::Publisher pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishHexacopterMarker (ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishCameraMarker(ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color); +void +publishFrameMarker (ros::Publisher pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + ros::Duration lifetime = ros::Duration(0.0)); + + +#endif /* VIKIT_OUTPUT_HELPER_H_ */ diff --git a/GPU_version/vio/include/gpu_svo/params_helper.h b/GPU_version/vio/include/gpu_svo/params_helper.h new file mode 100644 index 0000000..f8a03dc --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/params_helper.h @@ -0,0 +1,57 @@ +// +// Created by root on 5/24/21. +// + +#ifndef GPU_SVO_PARAMS_HELPER_H +#define GPU_SVO_PARAMS_HELPER_H +/* + * ros_params_helper.h + * + * Created on: Feb 22, 2013 + * Author: cforster + * + * from libpointmatcher_ros + */ + +#include +#include + +namespace vk { + + inline + bool hasParam(const std::string& name) + { + return ros::param::has(name); + } + + template + T getParam(const std::string& name, const T& defaultValue) + { + T v; + if(ros::param::get(name, v)) + { + ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); + return v; + } + else + ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); + return defaultValue; + } + + template + T getParam(const std::string& name) + { + T v; + if(ros::param::get(name, v)) + { + ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); + return v; + } + else + ROS_ERROR_STREAM("Cannot find value for parameter: " << name); + return T(); + } + +} // namespace vk + +#endif //GPU_SVO_PARAMS_HELPER_H diff --git a/GPU_version/vio/include/gpu_svo/patch_score.h b/GPU_version/vio/include/gpu_svo/patch_score.h new file mode 100644 index 0000000..8f0516c --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/patch_score.h @@ -0,0 +1,225 @@ +/* + * patch_score.h + * + * Created on: Dec 5, 2013 + * Author: cforster + */ + +#ifndef VIKIT_PATCH_SCORE_H_ +#define VIKIT_PATCH_SCORE_H_ + +#include + +#if __SSE2__ +#include +#endif + +namespace vk { +namespace patch_score { + + +#if __SSE2__ +// Horizontal sum of uint16s stored in an XMM register +inline int SumXMM_16(__m128i &target) +{ + unsigned short int sums_store[8]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3] + + sums_store[4] + sums_store[5] + sums_store[6] + sums_store[7]; +} +// Horizontal sum of uint32s stored in an XMM register +inline int SumXMM_32(__m128i &target) +{ + unsigned int sums_store[4]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3]; +} +#endif + +/// Zero Mean Sum of Squared Differences Cost +template +class ZMSSD { +public: + + static const int patch_size_ = 2*HALF_PATCH_SIZE; + static const int patch_area_ = patch_size_*patch_size_; + static const int threshold_ = 2000*patch_area_; + uint8_t* ref_patch_; + int sumA_, sumAA_; + + ZMSSD(uint8_t* ref_patch) : + ref_patch_(ref_patch) + { + uint32_t sumA_uint=0, sumAA_uint=0; + for(int r = 0; r < patch_area_; r++) + { + uint8_t n = ref_patch_[r]; + sumA_uint += n; + sumAA_uint += n*n; + } + sumA_ = sumA_uint; + sumAA_ = sumAA_uint; + } + + static int threshold() { return threshold_; } + + int computeScore(uint8_t* cur_patch) const + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int r = 0; r < patch_area_; r++) + { + const uint8_t cur_pixel = cur_patch[r]; + sumB_uint += cur_pixel; + sumBB_uint += cur_pixel*cur_pixel; + sumAB_uint += cur_pixel * ref_patch_[r]; + } + const int sumB = sumB_uint; + const int sumBB = sumBB_uint; + const int sumAB = sumAB_uint; + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } + + int computeScore(uint8_t* cur_patch, int stride) const + { + int sumB, sumBB, sumAB; +#if __SSE2__ + if(patch_size_ == 8) + { + // From PTAM-GPL, Copyright 2008 Isis Innovation Limited + __m128i xImageAsEightBytes; + __m128i xImageAsWords; + __m128i xTemplateAsEightBytes; + __m128i xTemplateAsWords; + __m128i xZero; + __m128i xImageSums; // These sums are 8xuint16 + __m128i xImageSqSums; // These sums are 4xint32 + __m128i xCrossSums; // These sums are 4xint32 + __m128i xProduct; + + xImageSums = _mm_setzero_si128(); + xImageSqSums = _mm_setzero_si128(); + xCrossSums = _mm_setzero_si128(); + xZero = _mm_setzero_si128(); + + uint8_t* imagepointer = cur_patch; + uint8_t* templatepointer = ref_patch_; + long unsigned int cur_stride = stride; + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + sumB = SumXMM_16(xImageSums); + sumAB = SumXMM_32(xCrossSums); + sumBB = SumXMM_32(xImageSqSums); + } + else +#endif + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int y=0, r=0; y < patch_size_; ++y) + { + uint8_t* cur_patch_ptr = cur_patch + y*stride; + for(int x=0; x < patch_size_; ++x, ++r) + { + const uint8_t cur_px = cur_patch_ptr[x]; + sumB_uint += cur_px; + sumBB_uint += cur_px * cur_px; + sumAB_uint += cur_px * ref_patch_[r]; + } + } + sumB = sumB_uint; + sumBB = sumBB_uint; + sumAB = sumAB_uint; + } + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } +}; + +} // namespace patch_score +} // namespace vk + +#endif // VIKIT_PATCH_SCORE_H_ diff --git a/GPU_version/vio/include/gpu_svo/point.h b/GPU_version/vio/include/gpu_svo/point.h new file mode 100644 index 0000000..6b0f900 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/point.h @@ -0,0 +1,108 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_POINT_H_ +#define SVO_POINT_H_ + +#include +#include + +namespace g2o { + class VertexSBAPointXYZ; +} +typedef g2o::VertexSBAPointXYZ g2oPoint; + +namespace svo { + +class Feature; + +typedef Matrix Matrix23d; + +/// A 3D point on the surface of the scene. +class Point : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum PointType { + TYPE_DELETED, + TYPE_CANDIDATE, + TYPE_UNKNOWN, + TYPE_GOOD + }; + + static int point_counter_; //!< Counts the number of created points. Used to set the unique id. + int id_; //!< Unique ID of the point. + Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. + Vector3d normal_; //!< Surface normal at point. + Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. + bool normal_set_; //!< Flag whether the surface normal was estimated or not. + list obs_; //!< References to keyframes which observe the point. + size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. + g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. + int last_published_ts_; //!< Timestamp of last publishing. + int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice. + PointType type_; //!< Quality of the point. + int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. + int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. + int last_structure_optim_; //!< Timestamp of last point optimization + + Point(const Vector3d& pos); + Point(const Vector3d& pos, Feature* ftr); + ~Point(); + + /// Add a reference to a frame. + void addFrameRef(Feature* ftr); + + /// Remove reference to a frame. + bool deleteFrameRef(Frame* frame); + + /// Initialize point normal. The inital estimate will point towards the frame. + void initNormal(); + + /// Check whether mappoint has reference to a frame. + Feature* findFrameRef(Frame* frame); + + /// Get Frame with similar viewpoint. + bool getCloseViewObs(const Vector2d& pos, Feature*& obs) const; + + /// Get number of observations. + inline size_t nRefs() const { return obs_.size(); } + + /// Optimize point position through minimizing the reprojection error. + void optimize(const size_t n_iter); + + /// Jacobian of point projection on unit plane (focal length = 1) in frame (f). + inline static void jacobian_xyz2uv( + const Vector3d& p_in_f, + const Matrix3d& R_f_w, + Matrix23d& point_jac) + { + const double z_inv = 1.0/p_in_f[2]; + const double z_inv_sq = z_inv*z_inv; + point_jac(0, 0) = z_inv; + point_jac(0, 1) = 0.0; + point_jac(0, 2) = -p_in_f[0] * z_inv_sq; + point_jac(1, 0) = 0.0; + point_jac(1, 1) = z_inv; + point_jac(1, 2) = -p_in_f[1] * z_inv_sq; + point_jac = - point_jac * R_f_w; + } +}; + +} // namespace svo + +#endif // SVO_POINT_H_ diff --git a/GPU_version/vio/include/gpu_svo/pose_optimizer.h b/GPU_version/vio/include/gpu_svo/pose_optimizer.h new file mode 100644 index 0000000..d0ad926 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/pose_optimizer.h @@ -0,0 +1,50 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_POSE_OPTIMIZER_H_ +#define SVO_POSE_OPTIMIZER_H_ + +#include + +namespace svo { + +using namespace Eigen; +using namespace Sophus; +using namespace std; + +typedef Matrix Matrix6d; +typedef Matrix Matrix26d; +typedef Matrix Vector6d; + +class Point; + +/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs); + +} // namespace pose_optimizer +} // namespace svo + +#endif // SVO_POSE_OPTIMIZER_H_ diff --git a/GPU_version/vio/include/gpu_svo/reprojector.h b/GPU_version/vio/include/gpu_svo/reprojector.h new file mode 100644 index 0000000..8d50c35 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/reprojector.h @@ -0,0 +1,101 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_REPROJECTION_H_ +#define SVO_REPROJECTION_H_ + +#include +#include + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Map; +class Point; + +/// Project points from the map into the image and find the corresponding +/// feature (corner). We don't search a match for every point but only for one +/// point per cell. Thereby, we achieve a homogeneously distributed set of +/// matched features and at the same time we can save processing time by not +/// projecting all points. +class Reprojector +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Reprojector config parameters + struct Options { + size_t max_n_kfs; //!< max number of keyframes to reproject from + bool find_match_direct; + Options() + : max_n_kfs(10), + find_match_direct(true) + {} + } options_; + + size_t n_matches_; + size_t n_trials_; + + Reprojector(vk::AbstractCamera* cam, Map& map); + + ~Reprojector(); + + /// Project points from the map into the image. First finds keyframes with + /// overlapping field of view and projects only those map-points. + void reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs); + +private: + + /// A candidate is a point that projects into the image plane and for which we + /// will search a maching feature in the image. + struct Candidate { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Point* pt; //!< 3D point. + Vector2d px; //!< projected 2D pixel location. + Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} + }; + typedef std::list Cell; + typedef std::vector CandidateGrid; + + /// The grid stores a set of candidate matches. For every grid cell we try to find one match. + struct Grid + { + CandidateGrid cells; + vector cell_order; + int cell_size; + int grid_n_cols; + int grid_n_rows; + }; + + Grid grid_; + Matcher matcher_; + Map& map_; + + static bool pointQualityComparator(Candidate& lhs, Candidate& rhs); + void initializeGrid(vk::AbstractCamera* cam); + void resetGrid(); + bool reprojectCell(Cell& cell, FramePtr frame); + bool reprojectPoint(FramePtr frame, Point* point); +}; + +} // namespace svo + +#endif // SVO_REPROJECTION_H_ diff --git a/GPU_version/vio/include/gpu_svo/robust_cost.h b/GPU_version/vio/include/gpu_svo/robust_cost.h new file mode 100644 index 0000000..d03881a --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/robust_cost.h @@ -0,0 +1,156 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo 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. +* +* dvo 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 dvo. If not, see . +*/ + +#ifndef VIKIT_ROBUST_COST_H_ +#define VIKIT_ROBUST_COST_H_ + +#include +#include +#include +namespace vk { +namespace robust_cost { + +// interface for scale estimators +class ScaleEstimator +{ +public: + virtual ~ScaleEstimator() {}; + virtual float compute(std::vector& errors) const = 0; +}; +typedef std::shared_ptr ScaleEstimatorPtr; + +class UnitScaleEstimator : public ScaleEstimator +{ +public: + UnitScaleEstimator() {} + virtual ~UnitScaleEstimator() {} + virtual float compute(std::vector& errors) const { return 1.0f; }; +}; + +// estimates scale by fitting a t-distribution to the data with the given degrees of freedom +class TDistributionScaleEstimator : public ScaleEstimator +{ +public: + TDistributionScaleEstimator(const float dof = DEFAULT_DOF); + virtual ~TDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + + static const float DEFAULT_DOF; + static const float INITIAL_SIGMA; +protected: + float dof_; + float initial_sigma_; +}; + +// estimates scale by computing the median absolute deviation +class MADScaleEstimator : public ScaleEstimator +{ +public: + MADScaleEstimator() {}; + virtual ~MADScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + +private: + static const float NORMALIZER;; +}; + +// estimates scale by computing the standard deviation +class NormalDistributionScaleEstimator : public ScaleEstimator +{ +public: + NormalDistributionScaleEstimator() {}; + virtual ~NormalDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; +private: +}; + +/** + * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)). + * The errors are assumed to be normalized to unit variance. + * + * See: + * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54) + */ +class WeightFunction +{ +public: + virtual ~WeightFunction() {}; + virtual float value(const float& x) const = 0; + virtual void configure(const float& param) {}; +}; +typedef std::shared_ptr WeightFunctionPtr; + +class UnitWeightFunction : public WeightFunction +{ +public: + UnitWeightFunction() {}; + virtual ~UnitWeightFunction() {}; + virtual float value(const float& x) const { return 1.0f; }; +}; + +/** + * Tukey's hard re-descending function. + * + * See: + * http://en.wikipedia.org/wiki/Redescending_M-estimator + */ +class TukeyWeightFunction : public WeightFunction +{ +public: + TukeyWeightFunction(const float b = DEFAULT_B); + virtual ~TukeyWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_B; +private: + float b_square; +}; + +class TDistributionWeightFunction : public WeightFunction +{ +public: + TDistributionWeightFunction(const float dof = DEFAULT_DOF); + virtual ~TDistributionWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_DOF; +private: + float dof_; + float normalizer_; +}; + +class HuberWeightFunction : public WeightFunction +{ +public: + HuberWeightFunction(const float k = DEFAULT_K); + virtual ~HuberWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_K; +private: + float k; +}; + +} // namespace robust_cost +} // namespace vk +#endif // VIKIT_ROBUST_COST_H_ diff --git a/GPU_version/vio/include/gpu_svo/sparse_img_align.h b/GPU_version/vio/include/gpu_svo/sparse_img_align.h new file mode 100644 index 0000000..1fbaace --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/sparse_img_align.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_SPARSE_IMG_ALIGN_H_ +#define SVO_SPARSE_IMG_ALIGN_H_ + +#include +#include + + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Feature; + +/// Optimize the pose of the frame by minimizing the photometric error of feature patches. +class SparseImgAlign : public vk::NLLSSolver<3, SE2> +{ + static const int patch_halfsize_ = 12; + static const int patch_size_ = 12*patch_halfsize_; + static const int patch_area_ = patch_size_*patch_size_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + cv::Mat resimg_; + + SparseImgAlign( + int n_levels, + int min_level, + int n_iter, + Method method, + bool display, + bool verbose); + + size_t run( + FramePtr ref_frame, + FramePtr cur_frame); + + /// Return fisher information matrix, i.e. the Hessian of the log-likelihood + /// at the converged state. + Matrix getFisherInformation(); + +protected: + FramePtr ref_frame_; //!< reference frame, has depth for gradient pixels. + FramePtr cur_frame_; //!< only the image is known! + int level_; //!< current pyramid level on which the optimization runs. + bool display_; //!< display residual image. + int max_level_; //!< coarsest pyramid level for the alignment. + int min_level_; //!< finest pyramid level for the alignment. + + // cache: + Matrix jacobian_cache_; + bool have_ref_patch_cache_; + cv::Mat ref_patch_cache_; + std::vector visible_fts_; + + void precomputeReferencePatches(); + virtual double computeResiduals(const SE2& model, bool linearize_system, bool compute_weight_scale = false); + virtual int solve(); + virtual void update (const ModelType& old_model, ModelType& new_model); + virtual void startIteration(); + virtual void finishIteration(); +}; + +} // namespace svo + +#endif // SVO_SPARSE_IMG_ALIGN_H_ diff --git a/GPU_version/vio/include/gpu_svo/timer.h b/GPU_version/vio/include/gpu_svo/timer.h new file mode 100644 index 0000000..45bbc7f --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/timer.h @@ -0,0 +1,81 @@ +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include + +namespace vk +{ + +class Timer +{ +private: + timeval start_time_; + double time_; + double accumulated_; +public: + + /// The constructor directly starts the timer. + Timer() : + time_(0.0), + accumulated_(0.0) + { + start(); + } + + ~Timer() + {} + + inline void start() + { + accumulated_ = 0.0; + gettimeofday(&start_time_, NULL); + } + + inline void resume() + { + gettimeofday(&start_time_, NULL); + } + + inline double stop() + { + timeval end_time; + gettimeofday(&end_time, NULL); + long seconds = end_time.tv_sec - start_time_.tv_sec; + long useconds = end_time.tv_usec - start_time_.tv_usec; + time_ = ((seconds) + useconds*0.000001) + accumulated_; + accumulated_ = time_; + return time_; + } + + inline double getTime() const + { + return time_; + } + + inline void reset() + { + time_ = 0.0; + accumulated_ = 0.0; + } + + static double getCurrentTime() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec + time_now.tv_usec*0.000001; + } + + static double getCurrentSecond() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec; + } + +}; + +} // end namespace vk + +#endif diff --git a/GPU_version/vio/include/gpu_svo/ukf.h b/GPU_version/vio/include/gpu_svo/ukf.h new file mode 100644 index 0000000..6875b50 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/ukf.h @@ -0,0 +1,155 @@ +// +// Created by root on 6/8/21. +// +#ifndef GPU_SVO_UKF_H +#define GPU_SVO_UKF_H + +#include +#include +#include "gpu_svo/config.h" +#include +class Base{ +public: + Base(const Eigen::Matrix &init) { + state_=init; + cov_.setIdentity(); + cov_*=0.0001; + }; + virtual ~Base(){ + }; + void predict(double dx,double dz,double dpitch,double time){ + if(t_==0.0){ + t_=time; + return; + } + double dt=time-t_; + t_=time; + state_h_(2)=state_(2)+dt*dpitch; + state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2)); + state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2)); + Eigen::Matrix R; + R< G; + G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)), + 0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)), + 0,0,1.0; + cov_h_=G*cov_*G.transpose()+R; + predict_up=true; + } + void correct(double ddx, double ddz, double dpitch, double time){ + Eigen::Matrix H; + Eigen::Matrix Q; + Eigen::Matrix E; + Eigen::Matrix k; + double dt=time-t_; + t_=time; + H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), + 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), + 0,0,1.0; + Q< R; + R< G; + G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), + 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), + 0,0,1.0; + cov_h_=G*cov_*G.transpose()+R; + yaw_t_1_=state_(2); + + } + void correct(double x, double z,double pitch,size_t match,double time){ + Eigen::Matrix H; + Eigen::Matrix Q; + Eigen::Matrix E; + Eigen::Matrix k; + t_=time; + ++match; + H<<1.0,0,0, + 0,1.0,0, + 0,0,1.0; + Q< cov_; + Eigen::Matrix state_; + bool predict_up=false; +private: + double t_=0.0; + Eigen::Matrix cov_h_; + Eigen::Matrix state_h_;//state{x,z,pitch} + double yaw_t_1_=0.0; + +}; + +class UKF { +public: + UKF(const Eigen::Matrix& init) : filter_(new Base(init)) { + }; + virtual ~UKF() { + delete filter_; + }; + void UpdateIMU(double x,double z,double pitch,const ros::Time& time) { + while(lock)usleep(5); + lock=true; + if(abs(x)<1.0)x=pow(x,3);//picked up from your code + if(abs(z)<1.0)z=pow(z,3);//picked up from your code + if(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec()); + lock=false; + }; + void UpdateCmd(double x,double z,double pitch,const ros::Time& time) { + while(lock)usleep(5); + lock=true; + filter_->predict(x,z,pitch,1e-9*time.toNSec()); + lock=false; + }; + std::pair,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { + while(lock)usleep(5); + lock=true; + if(pitch<0.0) + pitch=M_PI-pitch; + else + pitch=pitch-M_PI; + if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec()); + lock=false; + double pitch_f; + if(filter_->state_(0)<0.0) + pitch_f=M_PI-filter_->state_(2); + else + pitch_f=filter_->state_(2)-M_PI; + return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + }; + std::pair,svo::SE2_5> get_location(){ + double pitch_f; + if(filter_->state_(0)<0.0) + pitch_f=M_PI-filter_->state_(2); + else + pitch_f=filter_->state_(2)-M_PI; + return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + } +private: + Base* filter_= nullptr; + bool lock=false; +}; +#endif //GPU_SVO_UKF_H diff --git a/GPU_version/vio/include/gpu_svo/vision.h b/GPU_version/vio/include/gpu_svo/vision.h new file mode 100644 index 0000000..3b95183 --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/vision.h @@ -0,0 +1,80 @@ +/* + * vision.h + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#ifndef VIKIT_VISION_H_ +#define VIKIT_VISION_H_ + +#include +#include +#include + +namespace vk +{ + +//! Return value between 0 and 1 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_32f(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_32F); + float x = floor(u); + float y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + float wx0 = 1.0-subpix_x; + float wx1 = subpix_x; + float wy0 = 1.0-subpix_y; + float wy1 = subpix_y; + + float val00 = mat.at(y,x); + float val10 = mat.at(y,x+1); + float val01 = mat.at(y+1,x); + float val11 = mat.at(y+1,x+1); + return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; +} + +//! Return value between 0 and 255 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_8u(const cv::Mat& mat, float u, float v) +{ + if(mat.empty())return 0.0; + if(mat.type()!=CV_8U)return 0.0; + int x = floor(u); + int y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + + float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); + float w01 = (1.0f-subpix_x)*subpix_y; + float w10 = subpix_x*(1.0f-subpix_y); + float w11 = 1.0f - w00 - w01 - w10; + + const int stride = mat.step.p[0]; + unsigned char* ptr = mat.data + y*stride + x; + if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0; + if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0; + return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; +} + +void halfSample(const cv::Mat& in, cv::Mat& out); + +float shiTomasiScore(const cv::Mat& img, int u, int v); + +void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); + +#ifdef __SSE2__ + +/// Used to convert a Kinect depthmap +/// Code by Christian Kerl DVO, GPL Licence +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); + +#endif + +} // namespace vk + +#endif // VIKIT_VISION_H_ diff --git a/GPU_version/vio/include/gpu_svo/visualizer.h b/GPU_version/vio/include/gpu_svo/visualizer.h new file mode 100644 index 0000000..307c27e --- /dev/null +++ b/GPU_version/vio/include/gpu_svo/visualizer.h @@ -0,0 +1,62 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef SVO_VISUALIZER_H_ +#define SVO_VISUALIZER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Point; +class Map; +class FrameHandlerMono; + +typedef boost::shared_ptr FramePtr; + +/// This class bundles all functions to publish visualisation messages. +class Visualizer +{ +public: + ros::NodeHandle pnh_; + size_t trace_id_; + ros::Publisher pub_frames_; + ros::Publisher pub_points_; + ros::Publisher pub_pose_with_cov_; + + Visualizer(); + + ~Visualizer() {}; + + void publishMinimal( + UKF& ukf, + const double timestamp); + +}; + +} // end namespace svo + +#endif /* SVO_VISUALIZER_H_ */ diff --git a/GPU_version/vio/kernel/fast-gray.cl b/GPU_version/vio/kernel/fast-gray.cl new file mode 100644 index 0000000..ce6a927 --- /dev/null +++ b/GPU_version/vio/kernel/fast-gray.cl @@ -0,0 +1,83 @@ +// Copyright (C) 2021 Majid Geravand +// Copyright (C) 2021 Gfuse + +// Enable OpenCL 32-bit integer atomic functions. +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable + +__kernel void fast_gray( + __read_only image2d_t image, + __global int2 * corners, + __global int * icorner, + int fast_count +) { + + // Prepare a suitable OpenCL image sampler. + sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; + + // Use global work item as 2D image coordinates. + int const x = get_global_id(0); + int const y = get_global_id(1); + if(x>16 && x16){ + int2 const xy = (int2)(x, y); + // Read the candidate pixel. + int const p00 = read_imageui(image, sampler, xy).x; + + // Read other pixels in a circle around the candidate pixel. + int const p01 = read_imageui(image, sampler, xy + (int2)( 0, 3)).x; + int const p05 = read_imageui(image, sampler, xy + (int2)( 3, 0)).x; + int const p09 = read_imageui(image, sampler, xy + (int2)( 0, -3)).x; + int const p13 = read_imageui(image, sampler, xy + (int2)(-3, 0)).x; + + // Check the absolute difference of each circle pixel. + int const d01 = (abs(p01 - p00) > FAST_THRESH); + int const d05 = (abs(p05 - p00) > FAST_THRESH); + int const d09 = (abs(p09 - p00) > FAST_THRESH); + int const d13 = (abs(p13 - p00) > FAST_THRESH); + + // Check if any two adjacent circle pixels have a high absolute difference. + + if ((d01 && d05) ||(d05 && d09) ||(d09 && d13) ||(d13 && d01)) { + // Read other pixels in a circle around the candidate pixel. + int const p02 = read_imageui(image, sampler, xy + (int2)( 1, 3)).x; + int const p03 = read_imageui(image, sampler, xy + (int2)( 2, 2)).x; + int const p04 = read_imageui(image, sampler, xy + (int2)( 3, 1)).x; + int const p06 = read_imageui(image, sampler, xy + (int2)( 3, -1)).x; + int const p07 = read_imageui(image, sampler, xy + (int2)( 2, -2)).x; + int const p08 = read_imageui(image, sampler, xy + (int2)( 1, -3)).x; + int const p10 = read_imageui(image, sampler, xy + (int2)(-1, -3)).x; + int const p11 = read_imageui(image, sampler, xy + (int2)(-2, -2)).x; + int const p12 = read_imageui(image, sampler, xy + (int2)(-3, -1)).x; + int const p14 = read_imageui(image, sampler, xy + (int2)(-3, 1)).x; + int const p15 = read_imageui(image, sampler, xy + (int2)(-2, 2)).x; + int const p16 = read_imageui(image, sampler, xy + (int2)(-1, 3)).x; + // Select the maximum score. + int sco = p00; + sco = max(sco, p01); + sco = max(sco, p02); + sco = max(sco, p03); + sco = max(sco, p04); + sco = max(sco, p05); + sco = max(sco, p06); + sco = max(sco, p07); + sco = max(sco, p08); + sco = max(sco, p09); + sco = max(sco, p10); + sco = max(sco, p11); + sco = max(sco, p12); + sco = max(sco, p13); + sco = max(sco, p14); + sco = max(sco, p15); + + // Keep this score if it is as good as the maximum. + if (p00 >= sco) { + // Atomically append to corner buffer. + int const icorn = atom_inc(icorner); + if (icorn < fast_count) + corners[icorn] = xy; + } + } + + } + +} + diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch new file mode 100644 index 0000000..36114e8 --- /dev/null +++ b/GPU_version/vio/launch/Px30.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/live.launch b/GPU_version/vio/launch/live.launch new file mode 100644 index 0000000..7cd0ad8 --- /dev/null +++ b/GPU_version/vio/launch/live.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/test.launch b/GPU_version/vio/launch/test.launch new file mode 100644 index 0000000..4bcd965 --- /dev/null +++ b/GPU_version/vio/launch/test.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/unity.launch b/GPU_version/vio/launch/unity.launch new file mode 100644 index 0000000..ea34997 --- /dev/null +++ b/GPU_version/vio/launch/unity.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/GPU_version/vio/msg/Info.msg b/GPU_version/vio/msg/Info.msg new file mode 100644 index 0000000..b962e1b --- /dev/null +++ b/GPU_version/vio/msg/Info.msg @@ -0,0 +1,6 @@ +Header header +float32 processing_time +int32[] keyframes +int32 num_matches +int32 tracking_quality +int32 stage \ No newline at end of file diff --git a/GPU_version/vio/out.txt b/GPU_version/vio/out.txt new file mode 100644 index 0000000..b027790 --- /dev/null +++ b/GPU_version/vio/out.txt @@ -0,0 +1,26087 @@ +header: + seq: 0 + stamp: + secs: 1624875191 + nsecs: 558718920 + frame_id: "/world" +pose: + pose: + position: + x: 0.0626414404632 + y: -0.0114145588813 + z: 0.0 + orientation: + x: 0.0 + y: 0.0405619092502 + z: 0.0 + w: 0.999177027117 + covariance: [0.07868209584252406, -9.461082738964364e-19, 0.0, 0.0, 3.330677936045202e-14, 0.0, -9.461082738975269e-19, 0.07868209584252475, 0.0, 0.0, -1.9069085356217147e-12, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 3.3306779360448494e-14, -1.906908535621978e-12, 0.0, 0.0, 0.07868205381370827, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 1 + stamp: + secs: 1624875193 + nsecs: 318856001 + frame_id: "/world" +pose: + pose: + position: + x: 0.0277667589782 + y: -0.02601682998 + z: 0.0 + orientation: + x: 0.0 + y: 0.116883729185 + z: 0.0 + w: 0.993145605564 + covariance: [3.9857697567496766, -1.9086436829079788e-06, 0.0, 0.0, -0.001240613762362176, 0.0, -1.9086436829079516e-06, 3.985767469007555, 0.0, 0.0, 0.0007026569307809167, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0012406137623621864, 0.0007026569307809231, 0.0, 0.0, 2.745309716392042, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 2 + stamp: + secs: 1624875196 + nsecs: 599162102 + frame_id: "/world" +pose: + pose: + position: + x: 0.0166163353051 + y: -0.0430530963954 + z: 0.0 + orientation: + x: -0.0 + y: -0.0822472404752 + z: -0.0 + w: 0.996611956297 + covariance: [2.648854370068479, -5.4919736476426895e-06, 0.0, 0.0, -0.00048189566741691197, 0.0, -5.491973647643477e-06, 2.648847894412179, 0.0, 0.0, 0.0002940932594437838, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00048189566741691566, 0.0002940932594437862, 0.0, 0.0, 2.1038345230631976, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 3 + stamp: + secs: 1624875196 + nsecs: 679157019 + frame_id: "/world" +pose: + pose: + position: + x: 0.0185721548702 + y: -0.021561037511 + z: 0.0 + orientation: + x: 0.0 + y: 0.0301557821628 + z: 0.0 + w: 0.999545210985 + covariance: [2.2650572600435086, -6.206231117237778e-07, 0.0, 0.0, -5.1855607892080387e-05, 0.0, -6.206231117238479e-07, 2.265056535524967, 0.0, 0.0, 3.204631952791545e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.185560789208069e-05, 3.204631952791566e-05, 0.0, 0.0, 2.205627128639379, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 4 + stamp: + secs: 1624875196 + nsecs: 759172916 + frame_id: "/world" +pose: + pose: + position: + x: 0.0177732846889 + y: -0.021611066444 + z: 0.0 + orientation: + x: -0.0 + y: -0.00465191474583 + z: -0.0 + w: 0.999989179786 + covariance: [2.970371492501345, -2.361407898433349e-07, 0.0, 0.0, -1.2170615535642942e-05, 0.0, -2.3614078984334695e-07, 2.970371223555147, 0.0, 0.0, 1.0536763116137051e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2170615535642988e-05, 1.0536763116137083e-05, 0.0, 0.0, 2.9516132984610235, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 5 + stamp: + secs: 1624875196 + nsecs: 839164019 + frame_id: "/world" +pose: + pose: + position: + x: -0.00310131463006 + y: 0.0211506595427 + z: 0.0 + orientation: + x: 0.0 + y: 0.0035474838215 + z: 0.0 + w: 0.999993707659 + covariance: [2.5812404954914494, -7.110686924094391e-08, 0.0, 0.0, -4.216770829268579e-06, 0.0, -7.110686924094516e-08, 2.5812404162307985, 0.0, 0.0, 3.0142217862990555e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.216770829268589e-06, 3.0142217862990584e-06, 0.0, 0.0, 2.5755160780979995, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 6 + stamp: + secs: 1624875196 + nsecs: 919202089 + frame_id: "/world" +pose: + pose: + position: + x: 0.000477166316428 + y: 0.0384757898906 + z: 0.0 + orientation: + x: -0.0 + y: -0.00126263206941 + z: -0.0 + w: 0.99999920288 + covariance: [2.537205330000075, -2.527215897142597e-08, 0.0, 0.0, -1.7133958960150043e-06, 0.0, -2.5272158971426105e-08, 2.537205302204348, 0.0, 0.0, 7.931589233478379e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7133958960150049e-06, 7.931589233478382e-07, 0.0, 0.0, 2.5348276350127437, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 7 + stamp: + secs: 1624875196 + nsecs: 999176979 + frame_id: "/world" +pose: + pose: + position: + x: -0.00841003601336 + y: 0.0445356374802 + z: 0.0 + orientation: + x: 0.0 + y: 0.000264512627741 + z: 0.0 + w: 0.999999965017 + covariance: [1.7166537101529034, -4.74156415987445e-09, 0.0, 0.0, -4.6331904613388105e-07, 0.0, -4.741564159874444e-09, 1.7166537049789405, 0.0, 0.0, 2.2821098956012866e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.633190461338812e-07, 2.28210989560129e-07, 0.0, 0.0, 1.716099172882871, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 8 + stamp: + secs: 1624875197 + nsecs: 79178095 + frame_id: "/world" +pose: + pose: + position: + x: -0.0106598057741 + y: -0.000272719281852 + z: 0.0 + orientation: + x: -0.0 + y: -0.00109045418478 + z: -0.0 + w: 0.999999405455 + covariance: [2.97412162957741, -9.030197073518596e-09, 0.0, 0.0, -6.52621086885288e-07, 0.0, -9.030197073518579e-09, 2.974121619781132, 0.0, 0.0, 5.009301948222319e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.52621086885288e-07, 5.009301948222322e-07, 0.0, 0.0, 2.97269104584511, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 9 + stamp: + secs: 1624875197 + nsecs: 159200907 + frame_id: "/world" +pose: + pose: + position: + x: -0.0110024714484 + y: -0.000454368531155 + z: 0.0 + orientation: + x: 0.0 + y: 0.000391571405235 + z: 0.0 + w: 0.999999923336 + covariance: [3.2019225668767475, -3.150000231885707e-08, 0.0, 0.0, -5.4152226047997e-07, 0.0, -3.150000231885696e-08, 3.201922537539638, 0.0, 0.0, 5.376111404939688e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.415222604799701e-07, 5.3761114049396896e-05, 0.0, 0.0, 3.1956213759867746, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 10 + stamp: + secs: 1624875197 + nsecs: 239206076 + frame_id: "/world" +pose: + pose: + position: + x: -0.0154062289642 + y: 0.00489890487622 + z: 0.0 + orientation: + x: 0.0 + y: 9.93034279586e-05 + z: 0.0 + w: 0.999999995069 + covariance: [2.785900300607663, -2.0825685832678915e-08, 0.0, 0.0, -1.1811200678641733e-05, 0.0, -2.0825685832678872e-08, 2.785900289763596, 0.0, 0.0, 3.514303452296117e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1811200678641733e-05, 3.514303452296115e-05, 0.0, 0.0, 2.7815324820862957, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 11 + stamp: + secs: 1624875197 + nsecs: 319202900 + frame_id: "/world" +pose: + pose: + position: + x: -0.0154715009505 + y: 0.0045443071716 + z: 0.0 + orientation: + x: 0.0 + y: 0.000283981783532 + z: 0.0 + w: 0.999999959677 + covariance: [2.4808350941025124, -1.2165864201800308e-08, 0.0, 0.0, -7.225990872024654e-06, 0.0, -1.216586420180026e-08, 2.4808350909881196, 0.0, 0.0, 1.2033121906349419e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.225990872024646e-06, 1.203312190634942e-05, 0.0, 0.0, 2.478277299746955, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 12 + stamp: + secs: 1624875197 + nsecs: 399205923 + frame_id: "/world" +pose: + pose: + position: + x: -0.0156086820159 + y: 0.00447412807491 + z: 0.0 + orientation: + x: 0.0 + y: 0.000243722180649 + z: 0.0 + w: 0.9999999703 + covariance: [2.998512383676597, -9.051111680390655e-09, 0.0, 0.0, -3.5665094589854947e-06, 0.0, -9.051111680390632e-09, 2.998512382287478, 0.0, 0.0, 4.628005164744784e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.566509458985493e-06, 4.628005164744786e-06, 0.0, 0.0, 2.996544542977281, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 13 + stamp: + secs: 1624875197 + nsecs: 479234934 + frame_id: "/world" +pose: + pose: + position: + x: -0.00753809395827 + y: -0.013918683117 + z: 0.0 + orientation: + x: 0.0 + y: 7.98221087963e-05 + z: 0.0 + w: 0.999999996814 + covariance: [3.0802457190027672, -1.5789072296466504e-08, 0.0, 0.0, -9.523338022074358e-06, 0.0, -1.578907229646647e-08, 3.0802457174258118, 0.0, 0.0, 3.35105621493877e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.523338022074358e-06, 3.3510562149387676e-06, 0.0, 0.0, 3.0764571790267463, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 14 + stamp: + secs: 1624875197 + nsecs: 559230089 + frame_id: "/world" +pose: + pose: + position: + x: -0.00865124083866 + y: -0.0117786464916 + z: 0.0 + orientation: + x: 0.0 + y: 0.000113053577061 + z: 0.0 + w: 0.999999993609 + covariance: [2.9750188485756457, -6.46668939815125e-09, 0.0, 0.0, -3.5521739086318263e-06, 0.0, -6.466689398151236e-09, 2.975018847913869, 0.0, 0.0, 9.573491194190574e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5521739086318254e-06, 9.573491194190576e-07, 0.0, 0.0, 2.9733936909736176, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 15 + stamp: + secs: 1624875197 + nsecs: 639235020 + frame_id: "/world" +pose: + pose: + position: + x: 0.0122449486726 + y: 0.0206285034035 + z: 0.0 + orientation: + x: -0.0 + y: -0.00165594184217 + z: -0.0 + w: 0.999998628927 + covariance: [2.497795840650686, -1.5216582773403297e-09, 0.0, 0.0, -6.322098562405417e-07, 0.0, -1.521658277340327e-09, 2.497795840489801, 0.0, 0.0, 1.5909211054305536e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.322098562405412e-07, 1.5909211054305533e-07, 0.0, 0.0, 2.497395812876691, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 16 + stamp: + secs: 1624875197 + nsecs: 719242096 + frame_id: "/world" +pose: + pose: + position: + x: 0.0121887815088 + y: 0.0205940943095 + z: 0.0 + orientation: + x: 0.0 + y: 0.000218444049936 + z: 0.0 + w: 0.999999976141 + covariance: [3.2431991179042714, -1.5322275059058665e-09, 0.0, 0.0, -3.968414989115551e-07, 0.0, -1.5322275059058626e-09, 3.2431991177342256, 0.0, 0.0, 7.016327791980096e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.96841498911555e-07, 7.016327791980096e-08, 0.0, 0.0, 3.2427560585507944, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 17 + stamp: + secs: 1624875197 + nsecs: 799248934 + frame_id: "/world" +pose: + pose: + position: + x: 0.016367676523 + y: 0.0220660175989 + z: 0.0 + orientation: + x: -0.0 + y: -6.22874647356e-05 + z: -0.0 + w: 0.99999999806 + covariance: [2.889313343455879, -5.444253147615977e-09, 0.0, 0.0, -1.5612749740107182e-06, 0.0, -5.444253147615965e-09, 2.8893133428248454, 0.0, 0.0, 1.7149417702318355e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.561274974010718e-06, 1.7149417702318358e-07, 0.0, 0.0, 2.8875144824557153, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 18 + stamp: + secs: 1624875197 + nsecs: 879255056 + frame_id: "/world" +pose: + pose: + position: + x: 0.0161051003639 + y: 0.0218814048318 + z: 0.0 + orientation: + x: 0.0 + y: 0.000196843413986 + z: 0.0 + w: 0.999999980626 + covariance: [3.4595888930318353, -7.92738684852753e-09, 0.0, 0.0, -1.6410666325730565e-06, 0.0, -7.927386848527507e-09, 3.4595888920979974, 0.0, 0.0, 1.3307795397788244e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6410666325730558e-06, 1.3307795397788252e-06, 0.0, 0.0, 3.4568423557996124, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 19 + stamp: + secs: 1624875197 + nsecs: 959276915 + frame_id: "/world" +pose: + pose: + position: + x: 0.0130993629182 + y: 0.0250284135089 + z: 0.0 + orientation: + x: 0.0 + y: 0.00066997042759 + z: 0.0 + w: 0.99999977557 + covariance: [3.912394148645007, -6.332127865566567e-09, 0.0, 0.0, -8.786893683355767e-07, 0.0, -6.332127865566549e-09, 3.9123941478991178, 0.0, 0.0, 1.1393342997112697e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.786893683355764e-07, 1.1393342997112695e-06, 0.0, 0.0, 3.91012073471249, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 20 + stamp: + secs: 1624875198 + nsecs: 39307117 + frame_id: "/world" +pose: + pose: + position: + x: 0.00833075308554 + y: 0.0317229185638 + z: 0.0 + orientation: + x: 0.0 + y: 0.00027808093495 + z: 0.0 + w: 0.999999961335 + covariance: [5.01015990711351, -9.080446634843297e-09, 0.0, 0.0, -6.972112514446612e-07, 0.0, -9.080446634843274e-09, 5.010159906045985, 0.0, 0.0, 1.1130939871191168e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.97211251444661e-07, 1.1130939871191164e-06, 0.0, 0.0, 5.00671884952045, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 21 + stamp: + secs: 1624875198 + nsecs: 119277000 + frame_id: "/world" +pose: + pose: + position: + x: 0.00823993144951 + y: 0.0316700863355 + z: 0.0 + orientation: + x: -0.0 + y: -5.58432567746e-05 + z: -0.0 + w: 0.999999998441 + covariance: [5.004386410861576, -6.851749157573967e-09, 0.0, 0.0, -3.0935299231502707e-07, 0.0, -6.85174915757395e-09, 5.004386410057005, 0.0, 0.0, 5.81077222483251e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0935299231502707e-07, 5.81077222483251e-07, 0.0, 0.0, 5.001676088404593, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 22 + stamp: + secs: 1624875198 + nsecs: 199289083 + frame_id: "/world" +pose: + pose: + position: + x: 0.00827987548645 + y: 0.031693332903 + z: 0.0 + orientation: + x: 0.0 + y: 8.51195637217e-05 + z: 0.0 + w: 0.999999996377 + covariance: [6.66802314523911, -1.569710539431659e-09, 0.0, 0.0, -5.539167690567263e-07, 0.0, -1.569710539431655e-09, 6.668023145054946, 0.0, 0.0, 1.0173250208534339e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.539167690567263e-07, 1.017325020853434e-06, 0.0, 0.0, 6.667412684419782, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 23 + stamp: + secs: 1624875198 + nsecs: 279292107 + frame_id: "/world" +pose: + pose: + position: + x: -0.00601672476321 + y: 0.0597962557111 + z: 0.0 + orientation: + x: -0.0 + y: -0.000182427478844 + z: -0.0 + w: 0.99999998336 + covariance: [5.773828017096222, -1.1100559383853702e-09, 0.0, 0.0, 2.10490979463681e-07, 0.0, -1.1100559383853677e-09, 5.7738280169660925, 0.0, 0.0, -3.4896775196976567e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 2.1049097946368103e-07, -3.4896775196976546e-07, 0.0, 0.0, 5.773379186049659, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 24 + stamp: + secs: 1624875198 + nsecs: 359307051 + frame_id: "/world" +pose: + pose: + position: + x: -0.00596939635664 + y: 0.0546731450517 + z: 0.0 + orientation: + x: 0.0 + y: 0.000179420412757 + z: 0.0 + w: 0.999999983904 + covariance: [4.870448395193584, -2.715895373187792e-11, 0.0, 0.0, -9.458401783861916e-07, 0.0, -2.715895373187782e-11, 4.870448395190856, 0.0, 0.0, 7.909643535115203e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.458401783861919e-07, 7.909643535115208e-07, 0.0, 0.0, 4.870414096763582, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 25 + stamp: + secs: 1624875198 + nsecs: 439341068 + frame_id: "/world" +pose: + pose: + position: + x: -0.00402204831515 + y: 0.0564710529867 + z: 0.0 + orientation: + x: 0.0 + y: 7.87603672793e-05 + z: 0.0 + w: 0.999999996898 + covariance: [4.419267489978145, -1.1171516780614223e-09, 0.0, 0.0, -2.0239861397417157e-06, 0.0, -1.1171516780614192e-09, 4.419267489849727, 0.0, 0.0, 2.060603675568863e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.023986139741715e-06, 2.0606036755688636e-06, 0.0, 0.0, 4.418661799397737, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 26 + stamp: + secs: 1624875198 + nsecs: 519325018 + frame_id: "/world" +pose: + pose: + position: + x: -0.00539573052245 + y: 0.0492981006915 + z: 0.0 + orientation: + x: 0.0 + y: 1.23684445111e-05 + z: 0.0 + w: 0.999999999924 + covariance: [5.51978421669653, -3.180136437192512e-10, 0.0, 0.0, -6.154035046796035e-07, 0.0, -3.180136437192502e-10, 5.5197842166599465, 0.0, 0.0, 6.139961769247182e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.15403504679603e-07, 6.139961769247183e-07, 0.0, 0.0, 5.5196107542858615, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 27 + stamp: + secs: 1624875198 + nsecs: 599320889 + frame_id: "/world" +pose: + pose: + position: + x: -0.00353864683181 + y: 0.0508350238058 + z: 0.0 + orientation: + x: -0.0 + y: -1.93948657842e-05 + z: -0.0 + w: 0.999999999812 + covariance: [5.478221361043458, -1.0789933998159499e-09, 0.0, 0.0, -1.5158996146087268e-06, 0.0, -1.0789933998159466e-09, 5.478221360919302, 0.0, 0.0, 8.868260064139848e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5158996146087268e-06, 8.868260064139852e-07, 0.0, 0.0, 5.477634604668419, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 28 + stamp: + secs: 1624875198 + nsecs: 679327011 + frame_id: "/world" +pose: + pose: + position: + x: -0.00218483209211 + y: 0.0511190010454 + z: 0.0 + orientation: + x: 0.0 + y: 3.32196708283e-05 + z: 0.0 + w: 0.999999999448 + covariance: [5.467158230533972, -1.661966266679308e-09, 0.0, 0.0, -1.8198609214126284e-06, 0.0, -1.661966266679303e-09, 5.467158230340817, 0.0, 0.0, 8.615414251639713e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8198609214126289e-06, 8.615414251639717e-07, 0.0, 0.0, 5.4662322915130686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 29 + stamp: + secs: 1624875198 + nsecs: 759337902 + frame_id: "/world" +pose: + pose: + position: + x: -0.00152390603179 + y: 0.0512636655905 + z: 0.0 + orientation: + x: 0.0 + y: 7.71127429373e-06 + z: 0.0 + w: 0.99999999997 + covariance: [5.8648786261082435, -2.331664130147377e-09, 0.0, 0.0, -2.3820189290707838e-06, 0.0, -2.3316641301473703e-09, 5.86487862583418, 0.0, 0.0, 8.776682654593583e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3820189290707833e-06, 8.776682654593584e-07, 0.0, 0.0, 5.863543396083182, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 30 + stamp: + secs: 1624875198 + nsecs: 839340925 + frame_id: "/world" +pose: + pose: + position: + x: 0.0011554025041 + y: 0.0530192721025 + z: 0.0 + orientation: + x: 0.0 + y: 0.000139748276753 + z: 0.0 + w: 0.999999990235 + covariance: [7.044799202883239, -8.478551110188377e-09, 0.0, 0.0, -5.012356389703555e-06, 0.0, -8.478551110188356e-09, 7.04479920186819, 0.0, 0.0, 1.4697270129322411e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.012356389703555e-06, 1.469727012932241e-06, 0.0, 0.0, 7.039731729255961, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 31 + stamp: + secs: 1624875198 + nsecs: 919356108 + frame_id: "/world" +pose: + pose: + position: + x: 0.00269970605271 + y: 0.0535692293035 + z: 0.0 + orientation: + x: 0.0 + y: 8.12446361621e-05 + z: 0.0 + w: 0.9999999967 + covariance: [5.865526536324696, -4.009301911530781e-09, 0.0, 0.0, -1.788656659783739e-06, 0.0, -4.009301911530773e-09, 5.865526535843565, 0.0, 0.0, 6.326158745551525e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7886566597837392e-06, 6.326158745551528e-07, 0.0, 0.0, 5.863097758433728, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 32 + stamp: + secs: 1624875198 + nsecs: 999368906 + frame_id: "/world" +pose: + pose: + position: + x: 0.000421112315937 + y: 0.050847345662 + z: 0.0 + orientation: + x: 0.0 + y: 0.000191866996085 + z: 0.0 + w: 0.999999981594 + covariance: [5.573063470694434, -5.9085955978354e-09, 0.0, 0.0, -1.2104428003757742e-06, 0.0, -5.908595597835388e-09, 5.5730634699821735, 0.0, 0.0, 2.8792081789548466e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2104428003757736e-06, 2.879208178954846e-07, 0.0, 0.0, 5.569342875920074, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 33 + stamp: + secs: 1624875199 + nsecs: 79366922 + frame_id: "/world" +pose: + pose: + position: + x: -0.00156417319524 + y: 0.0410298336757 + z: 0.0 + orientation: + x: 0.0 + y: 0.000130407421243 + z: 0.0 + w: 0.999999991497 + covariance: [7.571954289010189, -4.237598046875895e-09, 0.0, 0.0, -7.460709969432444e-07, 0.0, -4.237598046875886e-09, 7.571954288499087, 0.0, 0.0, 1.2687838593861394e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.460709969432439e-07, 1.2687838593861388e-07, 0.0, 0.0, 7.569264328726531, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 34 + stamp: + secs: 1624875199 + nsecs: 159377098 + frame_id: "/world" +pose: + pose: + position: + x: -0.00122771819987 + y: 0.0396489609107 + z: 0.0 + orientation: + x: 0.0 + y: 0.000123348167764 + z: 0.0 + w: 0.999999992393 + covariance: [6.473580227328412, -5.363980208289023e-09, 0.0, 0.0, -2.0225825376281164e-06, 0.0, -5.363980208289009e-09, 6.473580226679866, 0.0, 0.0, 1.437545984320914e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0225825376281156e-06, 1.4375459843209138e-06, 0.0, 0.0, 6.470020128622338, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 35 + stamp: + secs: 1624875199 + nsecs: 239401102 + frame_id: "/world" +pose: + pose: + position: + x: 0.0051781728797 + y: 0.0351760222334 + z: 0.0 + orientation: + x: 0.0 + y: 0.000100643935232 + z: 0.0 + w: 0.999999994935 + covariance: [7.2935232724597325, -5.909388020211995e-09, 0.0, 0.0, -4.024387795638281e-06, 0.0, -5.90938802021198e-09, 7.29352327174132, 0.0, 0.0, 2.0485951522887395e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.02438779563828e-06, 2.0485951522887382e-06, 0.0, 0.0, 7.289493575011101, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 36 + stamp: + secs: 1624875199 + nsecs: 319385052 + frame_id: "/world" +pose: + pose: + position: + x: 0.0103827542099 + y: 0.00999221418783 + z: 0.0 + orientation: + x: 0.0 + y: 0.0003627373181 + z: 0.0 + w: 0.999999934211 + covariance: [8.421403995211696, -1.2500644469310046e-08, 0.0, 0.0, -8.238106675534144e-06, 0.0, -1.2500644469310014e-08, 8.421403993664072, 0.0, 0.0, 3.0276717194640595e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.238106675534142e-06, 3.0276717194640574e-06, 0.0, 0.0, 8.412570672956345, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 37 + stamp: + secs: 1624875199 + nsecs: 399395943 + frame_id: "/world" +pose: + pose: + position: + x: 0.0105383052281 + y: 0.0100450272921 + z: 0.0 + orientation: + x: 0.0 + y: 0.000656240017072 + z: 0.0 + w: 0.999999784674 + covariance: [10.059449134368599, -2.055039118497138e-08, 0.0, 0.0, -1.8867792152113142e-05, 0.0, -2.0550391184971324e-08, 10.059449131755892, 0.0, 0.0, 4.502900959104725e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.886779215211314e-05, 4.502900959104724e-06, 0.0, 0.0, 10.044621476619097, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 38 + stamp: + secs: 1624875199 + nsecs: 479382038 + frame_id: "/world" +pose: + pose: + position: + x: 0.0106460562493 + y: 0.0099990778026 + z: 0.0 + orientation: + x: 0.0 + y: 0.000497412762367 + z: 0.0 + w: 0.99999987629 + covariance: [10.947172175722772, -1.9727848714141727e-08, 0.0, 0.0, -2.1405640273161004e-05, 0.0, -1.9727848714141674e-08, 10.947172173138343, 0.0, 0.0, 4.8710773357471715e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1405640273161007e-05, 4.8710773357471715e-06, 0.0, 0.0, 10.932711711383535, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 39 + stamp: + secs: 1624875199 + nsecs: 559417963 + frame_id: "/world" +pose: + pose: + position: + x: 0.0183208528313 + y: 0.00821377788867 + z: 0.0 + orientation: + x: 0.0 + y: 3.29001507659e-05 + z: 0.0 + w: 0.999999999459 + covariance: [11.420217134486125, -3.418769972795095e-08, 0.0, 0.0, -3.163393196271123e-05, 0.0, -3.418769972795087e-08, 11.420217129744085, 0.0, 0.0, 7.82972515259146e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.163393196271123e-05, 7.82972515259146e-06, 0.0, 0.0, 11.394160540528814, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 40 + stamp: + secs: 1624875199 + nsecs: 639408112 + frame_id: "/world" +pose: + pose: + position: + x: -0.000715318423737 + y: 0.00496986031106 + z: 0.0 + orientation: + x: -0.0 + y: -2.2172795399e-05 + z: -0.0 + w: 0.999999999754 + covariance: [8.114031492796547, -2.1333618529755705e-08, 0.0, 0.0, -1.4699975136932473e-05, 0.0, -2.1333618529755646e-08, 8.114031489771032, 0.0, 0.0, 7.256197278731696e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4699975136932478e-05, 7.256197278731695e-06, 0.0, 0.0, 8.09735712011555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 41 + stamp: + secs: 1624875199 + nsecs: 719436884 + frame_id: "/world" +pose: + pose: + position: + x: -0.00140749099762 + y: 0.00484300107616 + z: 0.0 + orientation: + x: -0.0 + y: -0.000116152639797 + z: -0.0 + w: 0.999999993254 + covariance: [8.805861136832347, -2.794979980724563e-08, 0.0, 0.0, -1.3585168096472849e-05, 0.0, -2.7949799807245557e-08, 8.80586113282478, 0.0, 0.0, 1.2441321253646953e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3585168096472852e-05, 1.2441321253646958e-05, 0.0, 0.0, 8.783403422518505, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 42 + stamp: + secs: 1624875199 + nsecs: 799429893 + frame_id: "/world" +pose: + pose: + position: + x: -0.00208705541652 + y: 0.0051879453949 + z: 0.0 + orientation: + x: 0.0 + y: 0.000305811394084 + z: 0.0 + w: 0.99999995324 + covariance: [9.11153103833801, -3.017494058291259e-08, 0.0, 0.0, -1.3293412437218361e-05, 0.0, -3.0174940582912515e-08, 9.111531033987271, 0.0, 0.0, 1.1866143304580043e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3293412437218363e-05, 1.1866143304580048e-05, 0.0, 0.0, 9.086726718793535, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 43 + stamp: + secs: 1624875199 + nsecs: 879475117 + frame_id: "/world" +pose: + pose: + position: + x: 0.000833026600846 + y: 0.0164104680495 + z: 0.0 + orientation: + x: -0.0 + y: -0.000341267352612 + z: -0.0 + w: 0.999999941768 + covariance: [10.429549924358632, -1.4239469457389318e-08, 0.0, 0.0, -6.3328065618231e-06, 0.0, -1.4239469457389285e-08, 10.429549922307963, 0.0, 0.0, 5.834703781534976e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.3328065618231036e-06, 5.834703781534978e-06, 0.0, 0.0, 10.417878557067265, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 44 + stamp: + secs: 1624875199 + nsecs: 959450006 + frame_id: "/world" +pose: + pose: + position: + x: 0.00117863922695 + y: 0.0163072606007 + z: 0.0 + orientation: + x: 0.0 + y: 0.000148209862774 + z: 0.0 + w: 0.999999989017 + covariance: [8.716945349140108, -1.2633551845756085e-08, 0.0, 0.0, -9.01127956118428e-06, 0.0, -1.2633551845756057e-08, 8.71694534730184, 0.0, 0.0, 5.786114230873473e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.01127956118428e-06, 5.786114230873476e-06, 0.0, 0.0, 8.706295560327522, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 45 + stamp: + secs: 1624875200 + nsecs: 39455891 + frame_id: "/world" +pose: + pose: + position: + x: 0.00191347018293 + y: 0.0303008315088 + z: 0.0 + orientation: + x: -0.0 + y: -1.89652015222e-05 + z: -0.0 + w: 0.99999999982 + covariance: [9.071013981238169, -2.0391317329312797e-08, 0.0, 0.0, -1.9993804152798187e-05, 0.0, -2.0391317329312747e-08, 9.071013978193548, 0.0, 0.0, 1.2356219059953533e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9993804152798187e-05, 1.235621905995354e-05, 0.0, 0.0, 9.053185736361623, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 46 + stamp: + secs: 1624875200 + nsecs: 119455099 + frame_id: "/world" +pose: + pose: + position: + x: 0.00522871971408 + y: 0.0127281908142 + z: 0.0 + orientation: + x: 0.0 + y: 5.58547897419e-05 + z: 0.0 + w: 0.99999999844 + covariance: [6.850193881378459, -9.301507255491722e-09, 0.0, 0.0, -9.716836926241863e-06, 0.0, -9.301507255491697e-09, 6.850193879970645, 0.0, 0.0, 5.857141325473772e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.716836926241866e-06, 5.8571413254737795e-06, 0.0, 0.0, 6.841899640798484, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 47 + stamp: + secs: 1624875200 + nsecs: 199474096 + frame_id: "/world" +pose: + pose: + position: + x: 0.00692046202895 + y: 0.00831617702429 + z: 0.0 + orientation: + x: -0.0 + y: -0.00017924013968 + z: -0.0 + w: 0.999999983936 + covariance: [6.813636944206522, -9.165813940183094e-09, 0.0, 0.0, -8.504696513851456e-06, 0.0, -9.16581394018307e-09, 6.8136369427833, 0.0, 0.0, 5.018490966792904e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.504696513851464e-06, 5.01849096679291e-06, 0.0, 0.0, 6.8052296475089475, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 48 + stamp: + secs: 1624875200 + nsecs: 279485941 + frame_id: "/world" +pose: + pose: + position: + x: 0.00350393720251 + y: 0.0279228469351 + z: 0.0 + orientation: + x: 0.0 + y: 0.000139258630048 + z: 0.0 + w: 0.999999990304 + covariance: [6.80397922008437, -1.1487301687072829e-08, 0.0, 0.0, -6.8872417429891e-06, 0.0, -1.1487301687072796e-08, 6.803979218264553, 0.0, 0.0, 4.246626583476236e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.887241742989105e-06, 4.24662658347624e-06, 0.0, 0.0, 6.793111744317717, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 49 + stamp: + secs: 1624875200 + nsecs: 359488964 + frame_id: "/world" +pose: + pose: + position: + x: 0.00411981504066 + y: 0.0278321183957 + z: 0.0 + orientation: + x: 0.0 + y: 6.70055884419e-05 + z: 0.0 + w: 0.999999997755 + covariance: [6.263785616830026, -6.0212980423773e-09, 0.0, 0.0, -3.97246087593067e-06, 0.0, -6.021298042377284e-09, 6.263785615868659, 0.0, 0.0, 2.3303442376061175e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.972460875930671e-06, 2.330344237606118e-06, 0.0, 0.0, 6.258009766210611, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 50 + stamp: + secs: 1624875200 + nsecs: 439486027 + frame_id: "/world" +pose: + pose: + position: + x: 0.00170615913537 + y: 0.0227744893462 + z: 0.0 + orientation: + x: 0.0 + y: 9.95140267022e-05 + z: 0.0 + w: 0.999999995048 + covariance: [6.612827744697019, -4.670546484113974e-09, 0.0, 0.0, -3.6402228387361465e-06, 0.0, -4.67054648411396e-09, 6.612827743941922, 0.0, 0.0, 1.909185784228944e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.640222838736148e-06, 1.9091857842289445e-06, 0.0, 0.0, 6.608272718282844, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 51 + stamp: + secs: 1624875200 + nsecs: 519515038 + frame_id: "/world" +pose: + pose: + position: + x: 0.00169110298905 + y: 0.0227685736552 + z: 0.0 + orientation: + x: -0.0 + y: -8.95433212223e-06 + z: -0.0 + w: 0.99999999996 + covariance: [8.08927721725767, -1.6891452036913866e-09, 0.0, 0.0, -1.2708184239368811e-06, 0.0, -1.6891452036913808e-09, 8.089277216985941, 0.0, 0.0, 7.187257386142542e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2708184239368815e-06, 7.187257386142546e-07, 0.0, 0.0, 8.087637278832627, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 52 + stamp: + secs: 1624875200 + nsecs: 599533081 + frame_id: "/world" +pose: + pose: + position: + x: 0.00311351073892 + y: 0.0430950522637 + z: 0.0 + orientation: + x: 0.0 + y: 0.000259467151856 + z: 0.0 + w: 0.999999966338 + covariance: [9.867491828635, -6.219476929989891e-09, 0.0, 0.0, -3.671787789530499e-06, 0.0, -6.219476929989876e-09, 9.867491827611309, 0.0, 0.0, 3.180515435328955e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.671787789530501e-06, 3.180515435328955e-06, 0.0, 0.0, 9.86117128550643, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 53 + stamp: + secs: 1624875200 + nsecs: 679512978 + frame_id: "/world" +pose: + pose: + position: + x: 0.00697223945168 + y: 0.0797212810187 + z: 0.0 + orientation: + x: -0.0 + y: -0.000397378624356 + z: -0.0 + w: 0.999999921045 + covariance: [8.53537921340122, -8.990785794317857e-09, 0.0, 0.0, -2.963454538326025e-06, 0.0, -8.990785794317835e-09, 8.535379211910438, 0.0, 0.0, 3.6671407348286737e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.963454538326025e-06, 3.667140734828673e-06, 0.0, 0.0, 8.525912122733539, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 54 + stamp: + secs: 1624875200 + nsecs: 759515047 + frame_id: "/world" +pose: + pose: + position: + x: 0.00710837077299 + y: 0.0798090072252 + z: 0.0 + orientation: + x: -0.0 + y: -0.000107432669544 + z: -0.0 + w: 0.999999994229 + covariance: [8.989838112870954, -1.4613674772677515e-08, 0.0, 0.0, -2.815725423658808e-06, 0.0, -1.4613674772677473e-08, 8.989838110448014, 0.0, 0.0, 7.145359338246082e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.815725423658809e-06, 7.1453593382460825e-06, 0.0, 0.0, 8.974076526306552, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 55 + stamp: + secs: 1624875200 + nsecs: 839543104 + frame_id: "/world" +pose: + pose: + position: + x: 0.00859331568175 + y: 0.103013890972 + z: 0.0 + orientation: + x: 0.0 + y: 0.0022884793268 + z: 0.0 + w: 0.999997381428 + covariance: [9.189678237325923, -1.8658923711305735e-08, 0.0, 0.0, -8.668323780503765e-06, 0.0, -1.865892371130568e-08, 9.1896782342381, 0.0, 0.0, 9.343818663895769e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.668323780503763e-06, 9.343818663895767e-06, 0.0, 0.0, 9.169177335594483, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 56 + stamp: + secs: 1624875200 + nsecs: 919538975 + frame_id: "/world" +pose: + pose: + position: + x: 0.00428757485044 + y: 0.11077822985 + z: 0.0 + orientation: + x: 0.0 + y: 0.000504456930103 + z: 0.0 + w: 0.999999872762 + covariance: [7.530283569338185, -1.4107957036767555e-08, 0.0, 0.0, -1.2473398889976944e-05, 0.0, -1.4107957036767514e-08, 7.530283566963214, 0.0, 0.0, 5.511335954322725e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2473398889976943e-05, 5.511335954322724e-06, 0.0, 0.0, 7.514519150095854, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 57 + stamp: + secs: 1624875200 + nsecs: 999536991 + frame_id: "/world" +pose: + pose: + position: + x: 0.00151663760433 + y: 0.0811101772075 + z: 0.0 + orientation: + x: -0.0 + y: -5.61993155065e-05 + z: -0.0 + w: 0.999999998421 + covariance: [8.519030551815368, -1.4853078972163737e-09, 0.0, 0.0, -7.695498016380804e-07, 0.0, -1.4853078972163686e-09, 8.519030551566688, 0.0, 0.0, 7.780477217188373e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.695498016380817e-07, 7.780477217188366e-07, 0.0, 0.0, 8.517372433936337, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 58 + stamp: + secs: 1624875201 + nsecs: 79540968 + frame_id: "/world" +pose: + pose: + position: + x: 0.00291423774383 + y: 0.0714232394328 + z: 0.0 + orientation: + x: 0.0 + y: 4.80716095452e-05 + z: 0.0 + w: 0.999999998845 + covariance: [7.327921697120641, -7.038941843246094e-10, 0.0, 0.0, -4.0854791091300086e-07, 0.0, -7.03894184324607e-10, 7.327921697004113, 0.0, 0.0, 3.679089009349502e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.085479109130009e-07, 3.6790890093495e-07, 0.0, 0.0, 7.327140188449183, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 59 + stamp: + secs: 1624875201 + nsecs: 159560919 + frame_id: "/world" +pose: + pose: + position: + x: -0.00323561132099 + y: 0.0777530353866 + z: 0.0 + orientation: + x: 0.0 + y: 0.000172843880588 + z: 0.0 + w: 0.999999985062 + covariance: [7.632862195453625, -5.396386791882401e-09, 0.0, 0.0, -1.98587669485776e-06, 0.0, -5.396386791882387e-09, 7.6328621945144075, 0.0, 0.0, 1.5511758753030348e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9858766948577596e-06, 1.5511758753030348e-06, 0.0, 0.0, 7.6264959731959285, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 60 + stamp: + secs: 1624875201 + nsecs: 239557981 + frame_id: "/world" +pose: + pose: + position: + x: -0.0102893451605 + y: 0.0737500433133 + z: 0.0 + orientation: + x: -0.0 + y: -0.000276517209936 + z: -0.0 + w: 0.999999961769 + covariance: [8.584408983084927, -1.2642446357245383e-08, 0.0, 0.0, -4.038700503584341e-06, 0.0, -1.2642446357245352e-08, 8.584408980842381, 0.0, 0.0, 2.7363723425999526e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.038700503584339e-06, 2.7363723425999526e-06, 0.0, 0.0, 8.569116971122988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 61 + stamp: + secs: 1624875201 + nsecs: 319547892 + frame_id: "/world" +pose: + pose: + position: + x: -0.010261107154 + y: 0.0739101693853 + z: 0.0 + orientation: + x: -0.0 + y: -5.95845939277e-05 + z: -0.0 + w: 0.999999998225 + covariance: [8.091584179325764, -6.003352241560462e-09, 0.0, 0.0, -1.9086220101898923e-06, 0.0, -6.003352241560447e-09, 8.091584178258362, 0.0, 0.0, 1.2097003053667123e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.908622010189891e-06, 1.209700305366712e-06, 0.0, 0.0, 8.084284323254858, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 62 + stamp: + secs: 1624875201 + nsecs: 399564028 + frame_id: "/world" +pose: + pose: + position: + x: -0.00385669686624 + y: 0.0672982451832 + z: 0.0 + orientation: + x: -0.0 + y: -0.000738125484401 + z: -0.0 + w: 0.999999727585 + covariance: [8.797555779431198, -1.600774559183516e-08, 0.0, 0.0, -8.431719548659337e-06, 0.0, -1.600774559183512e-08, 8.797555776546139, 0.0, 0.0, 3.184758622294765e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.431719548659337e-06, 3.184758622294765e-06, 0.0, 0.0, 8.777563454693103, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 63 + stamp: + secs: 1624875201 + nsecs: 479578972 + frame_id: "/world" +pose: + pose: + position: + x: -0.00350858971035 + y: 0.067701371288 + z: 0.0 + orientation: + x: -0.0 + y: -0.000345703414971 + z: -0.0 + w: 0.999999940245 + covariance: [8.18183584110367, -1.883703010278204e-08, 0.0, 0.0, -9.578913580447987e-06, 0.0, -1.8837030102781997e-08, 8.181835837672871, 0.0, 0.0, 3.182161552695932e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.57891358044799e-06, 3.1821615526959317e-06, 0.0, 0.0, 8.157835581057473, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 64 + stamp: + secs: 1624875201 + nsecs: 559628963 + frame_id: "/world" +pose: + pose: + position: + x: -0.0111300352788 + y: 0.067056245337 + z: 0.0 + orientation: + x: 0.0 + y: 0.000128814391306 + z: 0.0 + w: 0.999999991703 + covariance: [7.241779245825406, -1.663592656885984e-08, 0.0, 0.0, -6.593230475229682e-06, 0.0, -1.6635926568859794e-08, 7.241779242773938, 0.0, 0.0, 1.6264059237312903e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.5932304752296835e-06, 1.6264059237312895e-06, 0.0, 0.0, 7.220188521838424, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 65 + stamp: + secs: 1624875201 + nsecs: 639638901 + frame_id: "/world" +pose: + pose: + position: + x: -0.00929179928672 + y: 0.0578352446516 + z: 0.0 + orientation: + x: 0.0 + y: 8.59037006861e-05 + z: 0.0 + w: 0.99999999631 + covariance: [7.600496343640866, -1.1505592590481727e-08, 0.0, 0.0, -3.873183760455839e-06, 0.0, -1.15055925904817e-08, 7.600496341523942, 0.0, 0.0, 7.956870880153922e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.873183760455842e-06, 7.956870880153921e-07, 0.0, 0.0, 7.585424968516159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 66 + stamp: + secs: 1624875201 + nsecs: 719597101 + frame_id: "/world" +pose: + pose: + position: + x: -0.0194372786599 + y: 0.0736482208398 + z: 0.0 + orientation: + x: -0.0 + y: -0.000165875077853 + z: -0.0 + w: 0.999999986243 + covariance: [8.568504390771684, -1.3788963894632275e-08, 0.0, 0.0, -3.974822469017283e-06, 0.0, -1.3788963894632245e-08, 8.5685043882266, 0.0, 0.0, 3.407900477125552e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.974822469017285e-06, 3.4079004771255523e-06, 0.0, 0.0, 8.550165670015407, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 67 + stamp: + secs: 1624875201 + nsecs: 799607038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0189989177182 + y: 0.0739950149124 + z: 0.0 + orientation: + x: -0.0 + y: -0.000383313890377 + z: -0.0 + w: 0.999999926535 + covariance: [8.098124631112075, -1.5159058190265913e-08, 0.0, 0.0, -6.4103104581784824e-06, 0.0, -1.5159058190265883e-08, 8.098124628315821, 0.0, 0.0, 6.729358811134934e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.4103104581784824e-06, 6.729358811134931e-06, 0.0, 0.0, 8.077564574050317, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 68 + stamp: + secs: 1624875201 + nsecs: 879617929 + frame_id: "/world" +pose: + pose: + position: + x: -0.0239214889737 + y: 0.0713017685962 + z: 0.0 + orientation: + x: -0.0 + y: -0.000150143592512 + z: -0.0 + w: 0.999999988728 + covariance: [6.623175627488095, -1.5173527259693928e-08, 0.0, 0.0, -6.4085129237086795e-06, 0.0, -1.5173527259693895e-08, 6.62317562468812, 0.0, 0.0, 5.7301476360502045e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.408512923708683e-06, 5.730147636050202e-06, 0.0, 0.0, 6.602152181844446, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 69 + stamp: + secs: 1624875201 + nsecs: 959651947 + frame_id: "/world" +pose: + pose: + position: + x: -0.0224349219106 + y: 0.0719322817239 + z: 0.0 + orientation: + x: -0.0 + y: -0.000190531457338 + z: -0.0 + w: 0.999999981849 + covariance: [6.222725174686373, -1.3381555984559277e-08, 0.0, 0.0, -5.424212123960663e-06, 0.0, -1.3381555984559242e-08, 6.222725172211248, 0.0, 0.0, 3.416152921665299e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.4242121239606604e-06, 3.4161529216652967e-06, 0.0, 0.0, 6.203848129824357, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 70 + stamp: + secs: 1624875202 + nsecs: 39701939 + frame_id: "/world" +pose: + pose: + position: + x: -0.0249211441792 + y: 0.0686466062594 + z: 0.0 + orientation: + x: -0.0 + y: -2.38326114436e-05 + z: -0.0 + w: 0.999999999716 + covariance: [6.099456279905903, -7.960471946582176e-09, 0.0, 0.0, -3.04103704253204e-06, 0.0, -7.960471946582161e-09, 6.0994562784294635, 0.0, 0.0, 1.5209934075323622e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0410370425320393e-06, 1.520993407532361e-06, 0.0, 0.0, 6.088089134366699, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 71 + stamp: + secs: 1624875202 + nsecs: 119643927 + frame_id: "/world" +pose: + pose: + position: + x: -0.0243534614371 + y: 0.0705517257814 + z: 0.0 + orientation: + x: -0.0 + y: -1.13757055991e-05 + z: -0.0 + w: 0.999999999935 + covariance: [6.561078424409695, -8.07391982956032e-09, 0.0, 0.0, -3.5291716057884617e-06, 0.0, -8.073919829560303e-09, 6.561078422904348, 0.0, 0.0, 1.0175506089896602e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.529171605788462e-06, 1.01755060898966e-06, 0.0, 0.0, 6.549351964106195, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 72 + stamp: + secs: 1624875202 + nsecs: 199664116 + frame_id: "/world" +pose: + pose: + position: + x: -0.025419868889 + y: 0.0693357429654 + z: 0.0 + orientation: + x: -0.0 + y: -4.80184683067e-05 + z: -0.0 + w: 0.999999998847 + covariance: [6.7252134227399125, -1.1866666244636634e-08, 0.0, 0.0, -4.75954256596035e-06, 0.0, -1.1866666244636614e-08, 6.725213420512024, 0.0, 0.0, 3.4063648153580773e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.759542565960349e-06, 3.4063648153580756e-06, 0.0, 0.0, 6.7075861958796175, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 73 + stamp: + secs: 1624875202 + nsecs: 279659033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0253446741531 + y: 0.0694086183036 + z: 0.0 + orientation: + x: -0.0 + y: -0.000100761670914 + z: -0.0 + w: 0.999999994924 + covariance: [6.77842123012036, -1.1774321664977542e-08, 0.0, 0.0, -7.82425314367777e-07, 0.0, -1.1774321664977517e-08, 6.7784212279133165, 0.0, 0.0, 3.899854464605632e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.824253143677766e-07, 3.899854464605631e-06, 0.0, 0.0, 6.760663426466538, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 74 + stamp: + secs: 1624875202 + nsecs: 359657049 + frame_id: "/world" +pose: + pose: + position: + x: -0.0252920790212 + y: 0.0694601097115 + z: 0.0 + orientation: + x: -0.0 + y: -0.000125277904577 + z: -0.0 + w: 0.999999992153 + covariance: [9.108895719258024, -1.3400596682813527e-08, 0.0, 0.0, 1.398736772090177e-06, 0.0, -1.3400596682813499e-08, 9.108895716746556, 0.0, 0.0, 4.010259942346317e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 1.3987367720901762e-06, 4.010259942346316e-06, 0.0, 0.0, 9.088497017582053, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 75 + stamp: + secs: 1624875202 + nsecs: 439666033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0245682481402 + y: 0.0683523318375 + z: 0.0 + orientation: + x: 0.0 + y: 1.69631372461e-05 + z: 0.0 + w: 0.999999999856 + covariance: [6.3192041936226095, -5.999413326056214e-09, 0.0, 0.0, -5.653885152104655e-07, 0.0, -5.999413326056206e-09, 6.319204192501843, 0.0, 0.0, 3.7769954710761027e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.653885152104661e-07, 3.776995471076103e-06, 0.0, 0.0, 6.309947517610192, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 76 + stamp: + secs: 1624875202 + nsecs: 519676924 + frame_id: "/world" +pose: + pose: + position: + x: -0.0248253198229 + y: 0.0678310434513 + z: 0.0 + orientation: + x: 0.0 + y: 0.000908551780223 + z: 0.0 + w: 0.999999587267 + covariance: [6.6461358273646, -1.2636131845770824e-08, 0.0, 0.0, -3.5843337853883538e-06, 0.0, -1.2636131845770797e-08, 6.646135825062068, 0.0, 0.0, 9.228018464882356e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5843337853883555e-06, 9.228018464882356e-06, 0.0, 0.0, 6.626136639460119, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 77 + stamp: + secs: 1624875202 + nsecs: 599704027 + frame_id: "/world" +pose: + pose: + position: + x: -0.0246557397701 + y: 0.0680612655764 + z: 0.0 + orientation: + x: 0.0 + y: 0.000148794474769 + z: 0.0 + w: 0.99999998893 + covariance: [6.757613583721207, -1.5035661073238473e-08, 0.0, 0.0, -3.364742899821791e-06, 0.0, -1.5035661073238433e-08, 6.757613581010451, 0.0, 0.0, 6.922522447066454e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.364742899821792e-06, 6.922522447066457e-06, 0.0, 0.0, 6.733409954662621, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 78 + stamp: + secs: 1624875202 + nsecs: 679709911 + frame_id: "/world" +pose: + pose: + position: + x: -0.0391279068951 + y: 0.0686073061185 + z: 0.0 + orientation: + x: 0.0 + y: 0.000241586721999 + z: 0.0 + w: 0.999999970818 + covariance: [9.121360969018584, -3.0099151060539856e-08, 0.0, 0.0, -5.616379266345125e-06, 0.0, -3.0099151060539776e-08, 9.121360963623529, 0.0, 0.0, 8.197711330131567e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.616379266345128e-06, 8.197711330131568e-06, 0.0, 0.0, 9.072211631705299, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 79 + stamp: + secs: 1624875202 + nsecs: 759694099 + frame_id: "/world" +pose: + pose: + position: + x: -0.0357354233791 + y: 0.0703866815002 + z: 0.0 + orientation: + x: -0.0 + y: -8.57534647727e-05 + z: -0.0 + w: 0.999999996323 + covariance: [7.517635917918099, -1.845248168703214e-08, 0.0, 0.0, -9.1783229220357e-06, 0.0, -1.8452481687032093e-08, 7.517635914602209, 0.0, 0.0, 5.182350111917112e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.1783229220357e-06, 5.182350111917112e-06, 0.0, 0.0, 7.487159214400504, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 80 + stamp: + secs: 1624875202 + nsecs: 839711905 + frame_id: "/world" +pose: + pose: + position: + x: -0.0358494481744 + y: 0.0733075810375 + z: 0.0 + orientation: + x: 0.0 + y: 0.000129061164933 + z: 0.0 + w: 0.999999991672 + covariance: [7.0448275793123525, -1.8677459267813282e-08, 0.0, 0.0, -1.1823782523226391e-05, 0.0, -1.8677459267813226e-08, 7.044827575879366, 0.0, 0.0, 5.142632493651937e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1823782523226395e-05, 5.142632493651941e-06, 0.0, 0.0, 7.013503044290827, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 81 + stamp: + secs: 1624875202 + nsecs: 919713974 + frame_id: "/world" +pose: + pose: + position: + x: -0.0319756613284 + y: 0.0836901609972 + z: 0.0 + orientation: + x: -0.0 + y: -8.64613818563e-06 + z: -0.0 + w: 0.999999999963 + covariance: [8.318771696901253, -2.1501370798348492e-08, 0.0, 0.0, -1.3802808737078841e-05, 0.0, -2.1501370798348432e-08, 8.318771692873073, 0.0, 0.0, 3.786312426854718e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3802808737078838e-05, 3.7863124268547194e-06, 0.0, 0.0, 8.282280046217688, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 82 + stamp: + secs: 1624875202 + nsecs: 999722004 + frame_id: "/world" +pose: + pose: + position: + x: -0.0334419058191 + y: 0.0839242132844 + z: 0.0 + orientation: + x: -0.0 + y: -0.000124270968843 + z: -0.0 + w: 0.999999992278 + covariance: [7.285612154395065, -1.6342063560385515e-08, 0.0, 0.0, -1.0141404376069271e-05, 0.0, -1.6342063560385462e-08, 7.285612151289366, 0.0, 0.0, 8.922283412928617e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0141404376069268e-05, 8.922283412928615e-06, 0.0, 0.0, 7.25751311151408, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 83 + stamp: + secs: 1624875203 + nsecs: 79710960 + frame_id: "/world" +pose: + pose: + position: + x: -0.0303143634306 + y: 0.0833411030182 + z: 0.0 + orientation: + x: -0.0 + y: -0.000128054513425 + z: -0.0 + w: 0.999999991801 + covariance: [8.431441274547014, -1.95115995774335e-08, 0.0, 0.0, -1.1274742519905508e-05, 0.0, -1.9511599577433442e-08, 8.4314412708876, 0.0, 0.0, 1.594044930932453e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1274742519905505e-05, 1.5940449309324528e-05, 0.0, 0.0, 8.397533699243569, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 84 + stamp: + secs: 1624875203 + nsecs: 159800053 + frame_id: "/world" +pose: + pose: + position: + x: -0.0333926767638 + y: 0.0837905940273 + z: 0.0 + orientation: + x: -0.0 + y: -4.15998685284e-06 + z: -0.0 + w: 0.999999999991 + covariance: [7.314944221099614, -1.318445115279974e-08, 0.0, 0.0, -9.331533933212873e-06, 0.0, -1.31844511527997e-08, 7.314944218648792, 0.0, 0.0, 9.750010452442614e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.331533933212872e-06, 9.750010452442615e-06, 0.0, 0.0, 7.291785464543493, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 85 + stamp: + secs: 1624875203 + nsecs: 239774942 + frame_id: "/world" +pose: + pose: + position: + x: -0.0360141124574 + y: 0.0790400687962 + z: 0.0 + orientation: + x: 0.0 + y: 0.000293510242743 + z: 0.0 + w: 0.999999956926 + covariance: [6.978099933885809, -1.4952354418531818e-08, 0.0, 0.0, -1.0655198054753374e-05, 0.0, -1.4952354418531775e-08, 6.9780999311261, 0.0, 0.0, 9.98428926432431e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0655198054753369e-05, 9.98428926432431e-06, 0.0, 0.0, 6.951495230131825, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 86 + stamp: + secs: 1624875203 + nsecs: 319755077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0355504228923 + y: 0.077667870848 + z: 0.0 + orientation: + x: 0.0 + y: 0.000376245096193 + z: 0.0 + w: 0.99999992922 + covariance: [7.504243794219253, -1.2935522286394913e-08, 0.0, 0.0, -1.1915271432441385e-05, 0.0, -1.2935522286394876e-08, 7.5042437918469505, 0.0, 0.0, 1.1468752133489334e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1915271432441378e-05, 1.1468752133489331e-05, 0.0, 0.0, 7.481082809492836, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 87 + stamp: + secs: 1624875203 + nsecs: 399734974 + frame_id: "/world" +pose: + pose: + position: + x: -0.0349862402678 + y: 0.0739927356118 + z: 0.0 + orientation: + x: 0.0 + y: 0.000607947744483 + z: 0.0 + w: 0.9999998152 + covariance: [7.713897496531171, -1.5901087807584802e-08, 0.0, 0.0, -1.8105505661713887e-05, 0.0, -1.590108780758476e-08, 7.713897493613873, 0.0, 0.0, 1.4801147127398404e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8105505661713887e-05, 1.4801147127398399e-05, 0.0, 0.0, 7.685198230770616, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 88 + stamp: + secs: 1624875203 + nsecs: 479762077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0347605880696 + y: 0.0701045180516 + z: 0.0 + orientation: + x: 0.0 + y: 0.000505360135904 + z: 0.0 + w: 0.999999872306 + covariance: [7.101440844022547, -1.419990563435143e-08, 0.0, 0.0, -1.3402881791117292e-05, 0.0, -1.4199905634351396e-08, 7.101440841400693, 0.0, 0.0, 1.0734608052494551e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.340288179111729e-05, 1.073460805249455e-05, 0.0, 0.0, 7.075585879977653, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 89 + stamp: + secs: 1624875203 + nsecs: 559773922 + frame_id: "/world" +pose: + pose: + position: + x: -0.0280624079141 + y: 0.0599279721018 + z: 0.0 + orientation: + x: 0.0 + y: 0.000294375979358 + z: 0.0 + w: 0.999999956671 + covariance: [8.33925833512028, -1.4982528737658184e-08, 0.0, 0.0, -1.0793047900885696e-05, 0.0, -1.4982528737658145e-08, 8.33925833234873, 0.0, 0.0, 9.173222276566887e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0793047900885698e-05, 9.173222276566883e-06, 0.0, 0.0, 8.311750911733496, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 90 + stamp: + secs: 1624875203 + nsecs: 639780045 + frame_id: "/world" +pose: + pose: + position: + x: -0.0335914191368 + y: 0.068271664857 + z: 0.0 + orientation: + x: 0.0 + y: 0.000299552264821 + z: 0.0 + w: 0.999999955134 + covariance: [10.039272299912353, -2.764161774070556e-08, 0.0, 0.0, -1.5610417568628978e-05, 0.0, -2.7641617740705484e-08, 10.039272294772228, 0.0, 0.0, 1.0877050641572195e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.561041756862898e-05, 1.087705064157219e-05, 0.0, 0.0, 9.987924799465647, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 91 + stamp: + secs: 1624875203 + nsecs: 719793081 + frame_id: "/world" +pose: + pose: + position: + x: -0.0374426645985 + y: 0.0911020852136 + z: 0.0 + orientation: + x: 0.0 + y: 0.000196891644747 + z: 0.0 + w: 0.999999980617 + covariance: [9.639342151606105, -2.35957948317196e-08, 0.0, 0.0, -1.1672919145424246e-05, 0.0, -2.3595794831719533e-08, 9.63934214720756, 0.0, 0.0, 9.183323248734129e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.167291914542425e-05, 9.183323248734127e-06, 0.0, 0.0, 9.59505936700601, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 92 + stamp: + secs: 1624875203 + nsecs: 799793005 + frame_id: "/world" +pose: + pose: + position: + x: -0.0375681665693 + y: 0.0913249069635 + z: 0.0 + orientation: + x: 0.0 + y: 0.000226810757925 + z: 0.0 + w: 0.999999974278 + covariance: [9.482085923006688, -2.6060272628416725e-08, 0.0, 0.0, -9.934435716555884e-06, 0.0, -2.606027262841666e-08, 9.48208591814944, 0.0, 0.0, 9.88806944555719e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.934435716555889e-06, 9.888069445557187e-06, 0.0, 0.0, 9.432571381436997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 93 + stamp: + secs: 1624875203 + nsecs: 879812002 + frame_id: "/world" +pose: + pose: + position: + x: -0.0334300742323 + y: 0.0806404033139 + z: 0.0 + orientation: + x: 0.0 + y: 0.000223402172281 + z: 0.0 + w: 0.999999975046 + covariance: [8.433778160851855, -2.629951011160394e-08, 0.0, 0.0, -4.844915937276256e-05, 0.0, -2.6299510111603877e-08, 8.433778155450582, 0.0, 0.0, 9.269780866550429e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.8449159372762576e-05, 9.269780866550427e-06, 0.0, 0.0, 8.383086340861613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 94 + stamp: + secs: 1624875203 + nsecs: 959813118 + frame_id: "/world" +pose: + pose: + position: + x: -0.0336253723319 + y: 0.0805512368916 + z: 0.0 + orientation: + x: 0.0 + y: 9.42333835948e-05 + z: 0.0 + w: 0.99999999556 + covariance: [8.045179990062792, -1.4100781995689595e-08, 0.0, 0.0, -3.517744781753176e-05, 0.0, -1.4100781995689559e-08, 8.045179986809439, 0.0, 0.0, 4.873501403325072e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5177447817531774e-05, 4.873501403325073e-06, 0.0, 0.0, 8.017902386431224, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 95 + stamp: + secs: 1624875204 + nsecs: 39814949 + frame_id: "/world" +pose: + pose: + position: + x: -0.0334344133328 + y: 0.0802346128939 + z: 0.0 + orientation: + x: 0.0 + y: 0.000279779220882 + z: 0.0 + w: 0.999999960862 + covariance: [7.921664405493227, -1.8614460351733513e-08, 0.0, 0.0, -5.850218631517861e-05, 0.0, -1.861446035173346e-08, 7.921664399043073, 0.0, 0.0, 7.040418938267963e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.8502186315178614e-05, 7.040418938267966e-06, 0.0, 0.0, 7.885373897412788, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 96 + stamp: + secs: 1624875204 + nsecs: 119822979 + frame_id: "/world" +pose: + pose: + position: + x: -0.0330891247788 + y: 0.0797503087314 + z: 0.0 + orientation: + x: 0.0 + y: 5.24634781386e-05 + z: 0.0 + w: 0.999999998624 + covariance: [8.746755316105848, -3.0247501594423685e-08, 0.0, 0.0, -7.565537696330997e-05, 0.0, -3.024750159442359e-08, 8.746755302479208, 0.0, 0.0, 1.5179242466294636e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.565537696330998e-05, 1.5179242466294638e-05, 0.0, 0.0, 8.687530581197112, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 97 + stamp: + secs: 1624875204 + nsecs: 199822903 + frame_id: "/world" +pose: + pose: + position: + x: -0.0348103293266 + y: 0.080537289134 + z: 0.0 + orientation: + x: 0.0 + y: 1.57452694423e-05 + z: 0.0 + w: 0.999999999876 + covariance: [7.416206619207253, -1.9980174270606337e-08, 0.0, 0.0, -3.75835439344141e-05, 0.0, -1.9980174270606288e-08, 7.4162066093967285, 0.0, 0.0, 9.768748634105246e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.758354393441413e-05, 9.768748634105249e-06, 0.0, 0.0, 7.3768559733558074, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 98 + stamp: + secs: 1624875204 + nsecs: 279830933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0426732865265 + y: 0.0632944030238 + z: 0.0 + orientation: + x: 0.0 + y: 0.000435899205198 + z: 0.0 + w: 0.999999904996 + covariance: [9.509712700464613, -2.907689202292338e-08, 0.0, 0.0, -3.798967154278551e-05, 0.0, -2.9076892022923294e-08, 9.509712685214032, 0.0, 0.0, 1.2972023288238188e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.798967154278552e-05, 1.2972023288238194e-05, 0.0, 0.0, 9.452222309105554, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 99 + stamp: + secs: 1624875204 + nsecs: 359844923 + frame_id: "/world" +pose: + pose: + position: + x: -0.0364095085237 + y: 0.0735433905239 + z: 0.0 + orientation: + x: 0.0 + y: 0.000302572321777 + z: 0.0 + w: 0.999999954225 + covariance: [7.62001667319008, -1.3378280079389074e-08, 0.0, 0.0, -1.4994116984846813e-05, 0.0, -1.3378280079389032e-08, 7.620016666000185, 0.0, 0.0, 5.724949435110537e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4994116984846822e-05, 5.7249494351105385e-06, 0.0, 0.0, 7.593463948315021, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 100 + stamp: + secs: 1624875204 + nsecs: 439868927 + frame_id: "/world" +pose: + pose: + position: + x: -0.0313737162804 + y: 0.0760022367155 + z: 0.0 + orientation: + x: 0.0 + y: 0.000506162606537 + z: 0.0 + w: 0.9999998719 + covariance: [7.769324931388732, -2.0662753242132215e-08, 0.0, 0.0, -1.4849010551710104e-05, 0.0, -2.0662753242132162e-08, 7.769324919759799, 0.0, 0.0, 5.980376331816624e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4849010551710111e-05, 5.980376331816627e-06, 0.0, 0.0, 7.727817200270315, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 101 + stamp: + secs: 1624875204 + nsecs: 519855022 + frame_id: "/world" +pose: + pose: + position: + x: -0.031551637875 + y: 0.0725291640665 + z: 0.0 + orientation: + x: 0.0 + y: 0.000290982160143 + z: 0.0 + w: 0.999999957665 + covariance: [7.115523193047412, -1.315038054970947e-08, 0.0, 0.0, -7.1096587583041725e-06, 0.0, -1.3150380549709435e-08, 7.115523185551469, 0.0, 0.0, 4.059976341087383e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.109658758304175e-06, 4.059976341087384e-06, 0.0, 0.0, 7.088808932343369, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 102 + stamp: + secs: 1624875204 + nsecs: 599853039 + frame_id: "/world" +pose: + pose: + position: + x: -0.0323597773349 + y: 0.0696835169043 + z: 0.0 + orientation: + x: -0.0 + y: -0.000106701835415 + z: -0.0 + w: 0.999999994307 + covariance: [7.573943875723878, -1.9028743410787258e-08, 0.0, 0.0, -1.4372466872498991e-05, 0.0, -1.90287434107872e-08, 7.5739438646998645, 0.0, 0.0, 6.8748125203851776e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4372466872498993e-05, 6.874812520385176e-06, 0.0, 0.0, 7.534610411005288, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 103 + stamp: + secs: 1624875204 + nsecs: 679869890 + frame_id: "/world" +pose: + pose: + position: + x: -0.03228643597 + y: 0.109915818393 + z: 0.0 + orientation: + x: -0.0 + y: -0.000447712695107 + z: -0.0 + w: 0.999999899777 + covariance: [6.480988369349932, -9.671406094890439e-09, 0.0, 0.0, -8.33087495299004e-06, 0.0, -9.67140609489041e-09, 6.480988363702931, 0.0, 0.0, 3.694727035042329e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.330874952990043e-06, 3.694727035042329e-06, 0.0, 0.0, 6.460823805900393, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 104 + stamp: + secs: 1624875204 + nsecs: 759877920 + frame_id: "/world" +pose: + pose: + position: + x: -0.0323303498459 + y: 0.110033152689 + z: 0.0 + orientation: + x: -0.0 + y: -2.00787093161e-05 + z: -0.0 + w: 0.999999999798 + covariance: [6.693914934352689, -7.735936145291188e-09, 0.0, 0.0, -5.931417913245355e-06, 0.0, -7.735936145291164e-09, 6.693914929798057, 0.0, 0.0, 2.8129797925213293e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.931417913245358e-06, 2.8129797925213284e-06, 0.0, 0.0, 6.677637570372443, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 105 + stamp: + secs: 1624875204 + nsecs: 839879036 + frame_id: "/world" +pose: + pose: + position: + x: -0.0369019408 + y: 0.0912384053109 + z: 0.0 + orientation: + x: -0.0 + y: -0.000178014710985 + z: -0.0 + w: 0.999999984155 + covariance: [7.402095031194055, -1.3693514381237084e-08, 0.0, 0.0, -1.33606260220477e-05, 0.0, -1.3693514381237045e-08, 7.402095023018279, 0.0, 0.0, 3.1850208178896675e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3360626022047705e-05, 3.1850208178896675e-06, 0.0, 0.0, 7.372820948617109, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 106 + stamp: + secs: 1624875204 + nsecs: 919888020 + frame_id: "/world" +pose: + pose: + position: + x: -0.0377105370856 + y: 0.0987327387627 + z: 0.0 + orientation: + x: -0.0 + y: -0.000387741925565 + z: -0.0 + w: 0.999999924828 + covariance: [6.987321788197188, -4.687965056701079e-09, 0.0, 0.0, -4.668391382497971e-06, 0.0, -4.687965056701063e-09, 6.987321785397244, 0.0, 0.0, 1.0611954546734462e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.668391382497971e-06, 1.0611954546734462e-06, 0.0, 0.0, 6.9772828530380036, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 107 + stamp: + secs: 1624875204 + nsecs: 999908924 + frame_id: "/world" +pose: + pose: + position: + x: -0.0371838858774 + y: 0.118426473796 + z: 0.0 + orientation: + x: -0.0 + y: -0.000100235213112 + z: -0.0 + w: 0.999999994976 + covariance: [8.28620479495662, -9.716063064200675e-09, 0.0, 0.0, -1.1998637802450901e-05, 0.0, -9.71606306420064e-09, 8.286204789043293, 0.0, 0.0, 2.3004634007116598e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1998637802450901e-05, 2.30046340071166e-06, 0.0, 0.0, 8.265107038820284, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 108 + stamp: + secs: 1624875205 + nsecs: 79900026 + frame_id: "/world" +pose: + pose: + position: + x: -0.0295876315872 + y: 0.118669524871 + z: 0.0 + orientation: + x: 0.0 + y: 0.000221306682333 + z: 0.0 + w: 0.999999975512 + covariance: [8.896149225125173, -1.7042592842124988e-08, 0.0, 0.0, -1.9703407152141874e-05, 0.0, -1.7042592842124925e-08, 8.896149214577948, 0.0, 0.0, 3.4618500107663594e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.970340715214187e-05, 3.4618500107663602e-06, 0.0, 0.0, 8.858579547283219, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 109 + stamp: + secs: 1624875205 + nsecs: 159907103 + frame_id: "/world" +pose: + pose: + position: + x: -0.0272439622515 + y: 0.120752394253 + z: 0.0 + orientation: + x: -0.0 + y: -0.000216381988245 + z: -0.0 + w: 0.999999976589 + covariance: [8.230910041996028, -2.0015263906443513e-08, 0.0, 0.0, -2.464919371328051e-05, 0.0, -2.0015263906443443e-08, 8.230910029432133, 0.0, 0.0, 7.478190350609672e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.464919371328051e-05, 7.478190350609667e-06, 0.0, 0.0, 8.186214640661008, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 110 + stamp: + secs: 1624875205 + nsecs: 239924908 + frame_id: "/world" +pose: + pose: + position: + x: -0.0300047123936 + y: 0.122337701584 + z: 0.0 + orientation: + x: -0.0 + y: -8.54557996142e-05 + z: -0.0 + w: 0.999999996349 + covariance: [7.267089404091235, -1.6983701117622025e-08, 0.0, 0.0, -1.929684868601596e-05, 0.0, -1.6983701117621962e-08, 7.267089393312902, 0.0, 0.0, 7.235859684014616e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9296848686015954e-05, 7.235859684014615e-06, 0.0, 0.0, 7.2287205260432135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 111 + stamp: + secs: 1624875205 + nsecs: 319930077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0254589001811 + y: 0.119233956743 + z: 0.0 + orientation: + x: 0.0 + y: 0.00121346093124 + z: 0.0 + w: 0.999999263756 + covariance: [8.527085347302895, -6.211006211196333e-08, 0.0, 0.0, -3.9416274697805474e-05, 0.0, -6.211006211196311e-08, 8.527085307316467, 0.0, 0.0, 1.646837849440847e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.941627469780548e-05, 1.646837849440847e-05, 0.0, 0.0, 8.384022995161962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 112 + stamp: + secs: 1624875205 + nsecs: 399924994 + frame_id: "/world" +pose: + pose: + position: + x: -0.0191717968258 + y: 0.112081331784 + z: 0.0 + orientation: + x: 0.0 + y: 0.00050703342535 + z: 0.0 + w: 0.999999871459 + covariance: [8.205524875819822, -7.948871951901189e-08, 0.0, 0.0, -3.113575069020661e-05, 0.0, -7.948871951901161e-08, 8.205524824426247, 0.0, 0.0, 1.3754513718484028e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.113575069020662e-05, 1.375451371848403e-05, 0.0, 0.0, 8.01831685956801, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 113 + stamp: + secs: 1624875205 + nsecs: 479929924 + frame_id: "/world" +pose: + pose: + position: + x: -0.019027565437 + y: 0.112177657429 + z: 0.0 + orientation: + x: 0.0 + y: 0.000822530400298 + z: 0.0 + w: 0.999999661722 + covariance: [11.441073211862467, -1.0316439306056284e-07, 0.0, 0.0, -3.12743268416472e-05, 0.0, -1.0316439306056246e-07, 11.44107314508102, 0.0, 0.0, 1.4195929844540016e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.127432684164722e-05, 1.419592984454002e-05, 0.0, 0.0, 11.196687897264052, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 114 + stamp: + secs: 1624875205 + nsecs: 559950113 + frame_id: "/world" +pose: + pose: + position: + x: -0.0210576933732 + y: 0.115665354138 + z: 0.0 + orientation: + x: 0.0 + y: 0.000270276464025 + z: 0.0 + w: 0.999999963475 + covariance: [8.13774550442011, -3.696702205566356e-08, 0.0, 0.0, -9.934176703571233e-06, 0.0, -3.696702205566345e-08, 8.137745480476646, 0.0, 0.0, 4.359854061065982e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.934176703571243e-06, 4.359854061065983e-06, 0.0, 0.0, 8.049011402189779, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 115 + stamp: + secs: 1624875205 + nsecs: 639954090 + frame_id: "/world" +pose: + pose: + position: + x: -0.0164297639932 + y: 0.107676307495 + z: 0.0 + orientation: + x: -0.0 + y: -5.65932394491e-06 + z: -0.0 + w: 0.999999999984 + covariance: [11.387100179643973, -6.911797980429505e-08, 0.0, 0.0, -1.639183956934392e-05, 0.0, -6.911797980429483e-08, 11.387100134818729, 0.0, 0.0, 1.0726704392408861e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6391839569343927e-05, 1.0726704392408861e-05, 0.0, 0.0, 11.219362372714695, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 116 + stamp: + secs: 1624875205 + nsecs: 719960928 + frame_id: "/world" +pose: + pose: + position: + x: -0.0213398656461 + y: 0.114773469669 + z: 0.0 + orientation: + x: 0.0 + y: 6.31732850517e-05 + z: 0.0 + w: 0.999999998005 + covariance: [9.01729915379195, -2.4985298102972893e-08, 0.0, 0.0, -6.866929554495262e-06, 0.0, -2.498529810297281e-08, 9.017299137584967, 0.0, 0.0, 4.271105721858221e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.866929554495267e-06, 4.27110572185822e-06, 0.0, 0.0, 8.956213470836927, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 117 + stamp: + secs: 1624875205 + nsecs: 799964905 + frame_id: "/world" +pose: + pose: + position: + x: -0.0150689309174 + y: 0.119041533162 + z: 0.0 + orientation: + x: -0.0 + y: -0.000747142303102 + z: -0.0 + w: 0.999999720889 + covariance: [10.446460046645145, -4.0231689299839674e-08, 0.0, 0.0, -1.7154389453283383e-05, 0.0, -4.0231689299839535e-08, 10.44646002048617, 0.0, 0.0, 7.874826912710021e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.715438945328339e-05, 7.87482691271002e-06, 0.0, 0.0, 10.346312721684747, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 118 + stamp: + secs: 1624875205 + nsecs: 879976034 + frame_id: "/world" +pose: + pose: + position: + x: -0.0152028905119 + y: 0.118913917244 + z: 0.0 + orientation: + x: 0.0 + y: 0.000154696188932 + z: 0.0 + w: 0.999999988035 + covariance: [7.173005164257486, -1.5223530251365087e-08, 0.0, 0.0, -5.9585109362328016e-06, 0.0, -1.522353025136503e-08, 7.173005154350345, 0.0, 0.0, 2.734125064774302e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.958510936232805e-06, 2.734125064774304e-06, 0.0, 0.0, 7.134628513457068, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 119 + stamp: + secs: 1624875205 + nsecs: 959978104 + frame_id: "/world" +pose: + pose: + position: + x: -0.00801960073402 + y: 0.100663937289 + z: 0.0 + orientation: + x: 0.0 + y: 0.000629390214359 + z: 0.0 + w: 0.999999801934 + covariance: [6.945884318211814, -1.7463299561609653e-08, 0.0, 0.0, -5.793947836373753e-06, 0.0, -1.7463299561609587e-08, 6.945884306831183, 0.0, 0.0, 4.304443611337498e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.793947836373758e-06, 4.304443611337499e-06, 0.0, 0.0, 6.901133612232493, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 120 + stamp: + secs: 1624875206 + nsecs: 40004015 + frame_id: "/world" +pose: + pose: + position: + x: -0.00414912958592 + y: 0.0878300958002 + z: 0.0 + orientation: + x: 0.0 + y: 0.000438251637767 + z: 0.0 + w: 0.999999903968 + covariance: [7.4926129271462845, -1.2096210836197932e-08, 0.0, 0.0, -4.043765942228804e-06, 0.0, -1.2096210836197886e-08, 7.492612919262481, 0.0, 0.0, 3.4372724017956937e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.043765942228807e-06, 3.437272401795693e-06, 0.0, 0.0, 7.461317550273498, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 121 + stamp: + secs: 1624875206 + nsecs: 119993925 + frame_id: "/world" +pose: + pose: + position: + x: -0.0073795802927 + y: 0.0967339621749 + z: 0.0 + orientation: + x: -0.0 + y: -0.000439794111542 + z: -0.0 + w: 0.999999903291 + covariance: [7.701036946058357, -8.93633875679909e-09, 0.0, 0.0, -3.774665092942195e-06, 0.0, -8.936338756799057e-09, 7.701036940235576, 0.0, 0.0, 3.673966288812995e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.774665092942195e-06, 3.6739662888129945e-06, 0.0, 0.0, 7.677656206381975, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 122 + stamp: + secs: 1624875206 + nsecs: 199990988 + frame_id: "/world" +pose: + pose: + position: + x: 0.00590296657193 + y: 0.0870611630806 + z: 0.0 + orientation: + x: -0.0 + y: -0.000245233237902 + z: -0.0 + w: 0.99999996993 + covariance: [9.673842735264099, -1.7750030812507113e-08, 0.0, 0.0, -8.611322265545378e-06, 0.0, -1.7750030812507047e-08, 9.673842723715985, 0.0, 0.0, 8.398941009656522e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.61132226554538e-06, 8.398941009656518e-06, 0.0, 0.0, 9.62657252735015, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 123 + stamp: + secs: 1624875206 + nsecs: 280004025 + frame_id: "/world" +pose: + pose: + position: + x: 0.00604095125703 + y: 0.0870582844085 + z: 0.0 + orientation: + x: -0.0 + y: -0.00050761321999 + z: -0.0 + w: 0.999999871164 + covariance: [12.43677419595611, -2.6447421051599345e-08, 0.0, 0.0, -1.1174048995145272e-05, 0.0, -2.644742105159925e-08, 12.436774178763578, 0.0, 0.0, 1.0984735170862596e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1174048995145272e-05, 1.098473517086259e-05, 0.0, 0.0, 12.36575019592895, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 124 + stamp: + secs: 1624875206 + nsecs: 360021114 + frame_id: "/world" +pose: + pose: + position: + x: 0.00622219065922 + y: 0.0870520009727 + z: 0.0 + orientation: + x: -0.0 + y: -0.000699498887091 + z: -0.0 + w: 0.999999755351 + covariance: [14.372975523073068, -3.856813577255185e-08, 0.0, 0.0, -3.4103785618229964e-05, 0.0, -3.856813577255169e-08, 14.372975497948964, 0.0, 0.0, 1.5607064444001132e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.4103785618229964e-05, 1.5607064444001125e-05, 0.0, 0.0, 14.268464567103893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 125 + stamp: + secs: 1624875206 + nsecs: 440040112 + frame_id: "/world" +pose: + pose: + position: + x: 0.00648388920499 + y: 0.087039142216 + z: 0.0 + orientation: + x: -0.0 + y: -0.000364052463476 + z: -0.0 + w: 0.999999933733 + covariance: [15.698147203980811, -6.205182949796135e-08, 0.0, 0.0, -8.625247049311097e-05, 0.0, -6.205182949796108e-08, 15.698147162920892, 0.0, 0.0, 2.2846529007194405e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.625247049311096e-05, 2.2846529007194394e-05, 0.0, 0.0, 15.52835409385362, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 126 + stamp: + secs: 1624875206 + nsecs: 520046949 + frame_id: "/world" +pose: + pose: + position: + x: 0.00673856730769 + y: 0.0870258019752 + z: 0.0 + orientation: + x: -0.0 + y: -0.000219481712455 + z: -0.0 + w: 0.999999975914 + covariance: [16.562788976924928, -8.196982293205892e-08, 0.0, 0.0, -0.00011212846635116913, 0.0, -8.196982293205858e-08, 16.562788921847304, 0.0, 0.0, 2.520918204595399e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001121284663511691, 2.5209182045953984e-05, 0.0, 0.0, 16.336381408256706, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 127 + stamp: + secs: 1624875206 + nsecs: 600040913 + frame_id: "/world" +pose: + pose: + position: + x: -0.0118226743684 + y: 0.125534539005 + z: 0.0 + orientation: + x: -0.0 + y: -0.000179863940087 + z: -0.0 + w: 0.999999983824 + covariance: [11.98557641008186, -4.735377711478341e-08, 0.0, 0.0, -6.182260793324025e-05, 0.0, -4.735377711478322e-08, 11.985576377936805, 0.0, 0.0, 1.2120700309714208e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.182260793324025e-05, 1.2120700309714203e-05, 0.0, 0.0, 11.852829574845542, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 128 + stamp: + secs: 1624875206 + nsecs: 680032969 + frame_id: "/world" +pose: + pose: + position: + x: -0.00358778358862 + y: 0.121348575434 + z: 0.0 + orientation: + x: -0.0 + y: -0.00021148952336 + z: -0.0 + w: 0.999999977636 + covariance: [12.002886937248846, -5.137843157222523e-08, 0.0, 0.0, -5.9351638173244137e-05, 0.0, -5.1378431572225e-08, 12.00288690192099, 0.0, 0.0, 1.8407112533970862e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.935163817324413e-05, 1.8407112533970856e-05, 0.0, 0.0, 11.857166640922612, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 129 + stamp: + secs: 1624875206 + nsecs: 760040998 + frame_id: "/world" +pose: + pose: + position: + x: -0.0034342642929 + y: 0.121457433822 + z: 0.0 + orientation: + x: -0.0 + y: -0.000203036045378 + z: -0.0 + w: 0.999999979388 + covariance: [14.110864609412767, -6.561860154883016e-08, 0.0, 0.0, -6.286107217317734e-05, 0.0, -6.561860154882986e-08, 14.110864564015765, 0.0, 0.0, 2.680153089562183e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.286107217317735e-05, 2.680153089562182e-05, 0.0, 0.0, 13.923339626447135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 130 + stamp: + secs: 1624875206 + nsecs: 840075016 + frame_id: "/world" +pose: + pose: + position: + x: -0.00324231239711 + y: 0.121585841705 + z: 0.0 + orientation: + x: -0.0 + y: -0.000292935295582 + z: -0.0 + w: 0.999999957094 + covariance: [15.54312003188267, -8.304185618653361e-08, 0.0, 0.0, -6.592128498099272e-05, 0.0, -8.304185618653324e-08, 15.54311997410282, 0.0, 0.0, 2.921173627178575e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.592128498099274e-05, 2.921173627178575e-05, 0.0, 0.0, 15.30349213562298, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 131 + stamp: + secs: 1624875206 + nsecs: 920078039 + frame_id: "/world" +pose: + pose: + position: + x: -0.00302361249356 + y: 0.121731778987 + z: 0.0 + orientation: + x: -0.0 + y: -0.000450632305206 + z: -0.0 + w: 0.999999898465 + covariance: [16.497130769741215, -1.0409295814226703e-07, 0.0, 0.0, -8.077191472153303e-05, 0.0, -1.0409295814226658e-07, 16.497130697044557, 0.0, 0.0, 4.856964973052148e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.077191472153306e-05, 4.856964973052148e-05, 0.0, 0.0, 16.193760451356805, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 132 + stamp: + secs: 1624875207 + nsecs: 104904 + frame_id: "/world" +pose: + pose: + position: + x: -0.00285189389597 + y: 0.121843772701 + z: 0.0 + orientation: + x: -0.0 + y: -0.00039333393658 + z: -0.0 + w: 0.999999922644 + covariance: [17.07133732929121, -1.0780789463311362e-07, 0.0, 0.0, -0.00011627017603990898, 0.0, -1.0780789463311314e-07, 17.07133725356757, 0.0, 0.0, 6.163202288238802e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011627017603990895, 6.163202288238802e-05, 0.0, 0.0, 16.755001242382612, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 133 + stamp: + secs: 1624875207 + nsecs: 80077887 + frame_id: "/world" +pose: + pose: + position: + x: -0.000899907061973 + y: 0.101518237692 + z: 0.0 + orientation: + x: 0.0 + y: 0.000278561093999 + z: 0.0 + w: 0.999999961202 + covariance: [17.453667445252414, -1.1853761274752016e-07, 0.0, 0.0, -0.00016843024170309969, 0.0, -1.1853761274751966e-07, 17.45366736053163, 0.0, 0.0, 6.896404912057336e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00016843024170309966, 6.896404912057336e-05, 0.0, 0.0, 17.103765371456525, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 134 + stamp: + secs: 1624875207 + nsecs: 160103083 + frame_id: "/world" +pose: + pose: + position: + x: -0.00103902445065 + y: 0.101253517056 + z: 0.0 + orientation: + x: 0.0 + y: 5.84115800298e-05 + z: 0.0 + w: 0.999999998294 + covariance: [12.273625864956246, -6.271352525529773e-08, 0.0, 0.0, -0.0001137816747960645, 0.0, -6.271352525529745e-08, 12.273625818985993, 0.0, 0.0, 3.4367307116886763e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001137816747960645, 3.436730711688677e-05, 0.0, 0.0, 12.08590181160497, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 135 + stamp: + secs: 1624875207 + nsecs: 240103006 + frame_id: "/world" +pose: + pose: + position: + x: -0.000390043920062 + y: 0.101927319247 + z: 0.0 + orientation: + x: -0.0 + y: -0.000336182994033 + z: -0.0 + w: 0.99999994349 + covariance: [10.562844906216656, -4.9939961183818776e-08, 0.0, 0.0, -0.00010868681370867, 0.0, -4.993996118381856e-08, 10.562844868391801, 0.0, 0.0, 5.461641748247413e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010868681370866999, 5.4616417482474135e-05, 0.0, 0.0, 10.413738693653256, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 136 + stamp: + secs: 1624875207 + nsecs: 320096970 + frame_id: "/world" +pose: + pose: + position: + x: 0.00127437919955 + y: 0.0816681951608 + z: 0.0 + orientation: + x: 0.0 + y: 0.000112415035063 + z: 0.0 + w: 0.999999993681 + covariance: [7.208099421191652, -1.987133450625604e-08, 0.0, 0.0, -4.144300592438179e-05, 0.0, -1.987133450625596e-08, 7.208099406236338, 0.0, 0.0, 3.270464772036236e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.144300592438176e-05, 3.270464772036236e-05, 0.0, 0.0, 7.149154372130515, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 137 + stamp: + secs: 1624875207 + nsecs: 400090933 + frame_id: "/world" +pose: + pose: + position: + x: -0.00289825132248 + y: 0.0525498229613 + z: 0.0 + orientation: + x: 0.0 + y: 7.18132574679e-05 + z: 0.0 + w: 0.999999997421 + covariance: [8.412196626844693, -2.26810057297239e-08, 0.0, 0.0, -4.313746439650783e-05, 0.0, -2.2681005729723805e-08, 8.412196610223987, 0.0, 0.0, 4.2618769181704716e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.313746439650783e-05, 4.261876918170472e-05, 0.0, 0.0, 8.346581736285472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 138 + stamp: + secs: 1624875207 + nsecs: 480118990 + frame_id: "/world" +pose: + pose: + position: + x: 0.00720479302003 + y: 0.0443325749279 + z: 0.0 + orientation: + x: -0.0 + y: -1.55225316731e-05 + z: -0.0 + w: 0.99999999988 + covariance: [8.928652463750794, -1.1724424424484668e-08, 0.0, 0.0, -2.2108597970937403e-05, 0.0, -1.1724424424484618e-08, 8.928652455190523, 0.0, 0.0, 2.203688228467378e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.21085979709374e-05, 2.203688228467378e-05, 0.0, 0.0, 8.894787787891726, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 139 + stamp: + secs: 1624875207 + nsecs: 560122967 + frame_id: "/world" +pose: + pose: + position: + x: -0.0128096386582 + y: 0.0378178580138 + z: 0.0 + orientation: + x: -0.0 + y: -8.07911457483e-05 + z: -0.0 + w: 0.999999996736 + covariance: [6.8336848919233, -1.0477567063887202e-08, 0.0, 0.0, -1.4151740789859036e-05, 0.0, -1.0477567063887161e-08, 6.833684884585229, 0.0, 0.0, 1.5226295966830767e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.415174078985903e-05, 1.5226295966830768e-05, 0.0, 0.0, 6.80412691156414, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 140 + stamp: + secs: 1624875207 + nsecs: 640136957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0149143323978 + y: 0.0415490026648 + z: 0.0 + orientation: + x: -0.0 + y: -0.000222483247492 + z: -0.0 + w: 0.999999975251 + covariance: [7.466843865013546, -1.569206218808295e-08, 0.0, 0.0, -1.2777970616101708e-05, 0.0, -1.569206218808288e-08, 7.466843854346528, 0.0, 0.0, 1.4216596824294303e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2777970616101698e-05, 1.4216596824294303e-05, 0.0, 0.0, 7.423020443978329, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 141 + stamp: + secs: 1624875207 + nsecs: 720158100 + frame_id: "/world" +pose: + pose: + position: + x: -0.014823062599 + y: 0.0413551887415 + z: 0.0 + orientation: + x: -0.0 + y: -3.37122084418e-05 + z: -0.0 + w: 0.999999999432 + covariance: [9.553263868468989, -2.6688680375918763e-08, 0.0, 0.0, -3.835948170575854e-05, 0.0, -2.6688680375918644e-08, 9.553263850427031, 0.0, 0.0, 1.863547431942443e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.835948170575853e-05, 1.863547431942443e-05, 0.0, 0.0, 9.479063959603724, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 142 + stamp: + secs: 1624875207 + nsecs: 800143957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0147698730563 + y: 0.0412368251038 + z: 0.0 + orientation: + x: 0.0 + y: 6.13761816821e-05 + z: 0.0 + w: 0.999999998116 + covariance: [9.443064280565821, -2.1318077353931887e-08, 0.0, 0.0, -4.5051341660735395e-05, 0.0, -2.131807735393178e-08, 9.443064265887031, 0.0, 0.0, 1.546688931973036e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.5051341660735395e-05, 1.5466889319730363e-05, 0.0, 0.0, 9.383922568145994, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 143 + stamp: + secs: 1624875207 + nsecs: 880158901 + frame_id: "/world" +pose: + pose: + position: + x: -0.0146794294455 + y: 0.041025942505 + z: 0.0 + orientation: + x: 0.0 + y: 2.56824550383e-05 + z: 0.0 + w: 0.99999999967 + covariance: [9.418669112098017, -2.7242793037898744e-08, 0.0, 0.0, -8.908288263019422e-05, 0.0, -2.7242793037898608e-08, 9.418669091703258, 0.0, 0.0, 2.103907998509946e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.908288263019419e-05, 2.1039079985099462e-05, 0.0, 0.0, 9.343869076775565, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 144 + stamp: + secs: 1624875207 + nsecs: 960160017 + frame_id: "/world" +pose: + pose: + position: + x: -0.0135377177133 + y: 0.037605237358 + z: 0.0 + orientation: + x: 0.0 + y: 7.22420081113e-05 + z: 0.0 + w: 0.999999997391 + covariance: [6.946199450456184, -1.3227450350216998e-08, 0.0, 0.0, -4.7335576120779765e-05, 0.0, -1.3227450350216937e-08, 6.946199439813389, 0.0, 0.0, 1.0948916472548836e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.733557612077979e-05, 1.0948916472548836e-05, 0.0, 0.0, 6.910079001408521, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 145 + stamp: + secs: 1624875208 + nsecs: 40215015 + frame_id: "/world" +pose: + pose: + position: + x: -0.0128195939237 + y: 0.042806257001 + z: 0.0 + orientation: + x: 0.0 + y: 0.000119064320668 + z: 0.0 + w: 0.999999992912 + covariance: [6.8769808631577405, -1.833722385114722e-08, 0.0, 0.0, -4.8028451951369065e-05, 0.0, -1.8337223851147134e-08, 6.876980846675615, 0.0, 0.0, 1.4201792930430281e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.8028451951369085e-05, 1.4201792930430286e-05, 0.0, 0.0, 6.8277012541151505, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 146 + stamp: + secs: 1624875208 + nsecs: 120165110 + frame_id: "/world" +pose: + pose: + position: + x: -0.015793247659 + y: 0.0427431204849 + z: 0.0 + orientation: + x: 0.0 + y: 7.89602271637e-05 + z: 0.0 + w: 0.999999996883 + covariance: [6.824536148661112, -8.935442863584124e-09, 0.0, 0.0, -2.1303478040974663e-05, 0.0, -8.935442863584079e-09, 6.824536140481649, 0.0, 0.0, 6.469423811725233e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.130347804097467e-05, 6.469423811725235e-06, 0.0, 0.0, 6.800542259752465, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 147 + stamp: + secs: 1624875208 + nsecs: 200181961 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181206641222 + y: 0.0418335898525 + z: 0.0 + orientation: + x: 0.0 + y: 0.0001691767861 + z: 0.0 + w: 0.99999998569 + covariance: [6.822896802966476, -1.172489813112443e-08, 0.0, 0.0, -2.3567356382524365e-05, 0.0, -1.1724898131124386e-08, 6.822896791598783, 0.0, 0.0, 6.277272593692823e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.356735638252437e-05, 6.277272593692824e-06, 0.0, 0.0, 6.791585393541631, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 148 + stamp: + secs: 1624875208 + nsecs: 280190945 + frame_id: "/world" +pose: + pose: + position: + x: -0.0114746649269 + y: 0.0399861298173 + z: 0.0 + orientation: + x: 0.0 + y: 6.25935319439e-05 + z: 0.0 + w: 0.999999998041 + covariance: [7.436668502707751, -6.96273578148777e-09, 0.0, 0.0, -1.2690245022702874e-05, 0.0, -6.962735781487745e-09, 7.436668495878887, 0.0, 0.0, 3.583747509979331e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2690245022702877e-05, 3.583747509979332e-06, 0.0, 0.0, 7.418049160128272, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 149 + stamp: + secs: 1624875208 + nsecs: 360193968 + frame_id: "/world" +pose: + pose: + position: + x: -0.0118367842192 + y: 0.0402448295915 + z: 0.0 + orientation: + x: 0.0 + y: 5.87744669989e-05 + z: 0.0 + w: 0.999999998273 + covariance: [7.677567474879912, -6.109812028818983e-09, 0.0, 0.0, -9.058449846653822e-06, 0.0, -6.1098120288189595e-09, 7.677567468775152, 0.0, 0.0, 2.8870314009206156e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.058449846653824e-06, 2.8870314009206156e-06, 0.0, 0.0, 7.661213300609898, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 150 + stamp: + secs: 1624875208 + nsecs: 440201998 + frame_id: "/world" +pose: + pose: + position: + x: -0.0117660978114 + y: 0.0400731898197 + z: 0.0 + orientation: + x: 0.0 + y: 0.000156091439701 + z: 0.0 + w: 0.999999987818 + covariance: [6.519531624775144, -9.315369936048031e-09, 0.0, 0.0, -7.461677622000863e-06, 0.0, -9.315369936048002e-09, 6.519531615252853, 0.0, 0.0, 2.5551696638479396e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.461677622000863e-06, 2.5551696638479383e-06, 0.0, 0.0, 6.494293773235496, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 151 + stamp: + secs: 1624875208 + nsecs: 520207882 + frame_id: "/world" +pose: + pose: + position: + x: -0.00830707602976 + y: 0.0319101061725 + z: 0.0 + orientation: + x: 0.0 + y: 0.000789109207757 + z: 0.0 + w: 0.999999688653 + covariance: [10.104087688943478, -1.9780659259029803e-08, 0.0, 0.0, -3.8125742951822444e-05, 0.0, -1.9780659259029727e-08, 10.104087668389345, 0.0, 0.0, 7.1031826286766625e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.812574295182243e-05, 7.103182628676659e-06, 0.0, 0.0, 10.050337764322126, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 152 + stamp: + secs: 1624875208 + nsecs: 600233078 + frame_id: "/world" +pose: + pose: + position: + x: -0.00798108363877 + y: 0.0309254449279 + z: 0.0 + orientation: + x: 0.0 + y: 9.9627700789e-05 + z: 0.0 + w: 0.999999995037 + covariance: [7.77877960755447, -8.605921583893514e-09, 0.0, 0.0, -2.5420089739485696e-05, 0.0, -8.605921583893489e-09, 7.778779598426011, 0.0, 0.0, 3.7428013481683887e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.54200897394857e-05, 3.742801348168388e-06, 0.0, 0.0, 7.755329523436467, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 153 + stamp: + secs: 1624875208 + nsecs: 680222988 + frame_id: "/world" +pose: + pose: + position: + x: -0.00465251031109 + y: 0.0416688568682 + z: 0.0 + orientation: + x: -0.0 + y: -0.000199636142007 + z: -0.0 + w: 0.999999980073 + covariance: [9.75932562145142, -3.252206519308464e-08, 0.0, 0.0, -0.00011377786124882633, 0.0, -3.252206519308453e-08, 9.75932558264222, 0.0, 0.0, 2.0772904785948194e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011377786124882636, 2.077290478594819e-05, 0.0, 0.0, 9.671461838633698, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 154 + stamp: + secs: 1624875208 + nsecs: 760227919 + frame_id: "/world" +pose: + pose: + position: + x: -0.00424379613653 + y: 0.0399870587937 + z: 0.0 + orientation: + x: 0.0 + y: 0.000162811533582 + z: 0.0 + w: 0.999999986746 + covariance: [6.45689552331072, -1.2026399411211204e-08, 0.0, 0.0, -3.761811464999286e-05, 0.0, -1.2026399411211159e-08, 6.456895508614034, 0.0, 0.0, 7.881078605519454e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.761811464999285e-05, 7.881078605519456e-06, 0.0, 0.0, 6.424317185702538, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 155 + stamp: + secs: 1624875208 + nsecs: 840251923 + frame_id: "/world" +pose: + pose: + position: + x: -0.00431322281044 + y: 0.0398971164949 + z: 0.0 + orientation: + x: 0.0 + y: 0.000419285998979 + z: 0.0 + w: 0.9999999121 + covariance: [6.723897587035414, -2.167886026853318e-08, 0.0, 0.0, -3.968381954455601e-05, 0.0, -2.1678860268533102e-08, 6.723897559220825, 0.0, 0.0, 1.1995946268085832e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.968381954455599e-05, 1.1995946268085827e-05, 0.0, 0.0, 6.665457954348747, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 156 + stamp: + secs: 1624875208 + nsecs: 920262098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0194397291655 + y: 0.0457855225935 + z: 0.0 + orientation: + x: 0.0 + y: 0.00059028538488 + z: 0.0 + w: 0.999999825782 + covariance: [8.177748985634265, -2.3050020999189632e-08, 0.0, 0.0, -3.102764957761143e-05, 0.0, -2.305002099918955e-08, 8.177748955725175, 0.0, 0.0, 1.121717729421225e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.102764957761141e-05, 1.1217177294212246e-05, 0.0, 0.0, 8.11546869637249, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 157 + stamp: + secs: 1624875209 + nsecs: 256062 + frame_id: "/world" +pose: + pose: + position: + x: -0.022799208238 + y: 0.0640961202877 + z: 0.0 + orientation: + x: 0.0 + y: 0.000164304591471 + z: 0.0 + w: 0.999999986502 + covariance: [7.970174626408329, -1.806620810150319e-08, 0.0, 0.0, -1.785617020234456e-05, 0.0, -1.8066208101503122e-08, 7.970174602809171, 0.0, 0.0, 9.102517258967023e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7856170202344555e-05, 9.102517258967021e-06, 0.0, 0.0, 7.921067409416407, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 158 + stamp: + secs: 1624875209 + nsecs: 80250978 + frame_id: "/world" +pose: + pose: + position: + x: -0.0128529516545 + y: 0.0666497969411 + z: 0.0 + orientation: + x: 0.0 + y: 6.59608430512e-06 + z: 0.0 + w: 0.999999999978 + covariance: [7.192435082014609, -1.6092744115661282e-08, 0.0, 0.0, -1.2837964162613844e-05, 0.0, -1.609274411566122e-08, 7.192435060897825, 0.0, 0.0, 9.414629565172762e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2837964162613835e-05, 9.414629565172762e-06, 0.0, 0.0, 7.148339944325768, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 159 + stamp: + secs: 1624875209 + nsecs: 160267115 + frame_id: "/world" +pose: + pose: + position: + x: -0.00709496494856 + y: 0.0591973623407 + z: 0.0 + orientation: + x: 0.0 + y: 5.35825546367e-05 + z: 0.0 + w: 0.999999998564 + covariance: [6.935599725676636, -1.0980864186474564e-08, 0.0, 0.0, -8.85161870391562e-06, 0.0, -1.0980864186474526e-08, 6.935599711244111, 0.0, 0.0, 6.104560762904957e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.851618703915614e-06, 6.104560762904956e-06, 0.0, 0.0, 6.905335931120634, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 160 + stamp: + secs: 1624875209 + nsecs: 240287066 + frame_id: "/world" +pose: + pose: + position: + x: -0.00704366296217 + y: 0.0591708539645 + z: 0.0 + orientation: + x: 0.0 + y: 0.000270673281179 + z: 0.0 + w: 0.999999963368 + covariance: [8.286701073151663, -2.01395338418274e-08, 0.0, 0.0, -2.176702469744414e-05, 0.0, -2.013953384182733e-08, 8.28670104660332, 0.0, 0.0, 9.55985668628129e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.176702469744413e-05, 9.559856686281287e-06, 0.0, 0.0, 8.230845060153573, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 161 + stamp: + secs: 1624875209 + nsecs: 320278883 + frame_id: "/world" +pose: + pose: + position: + x: -0.00815993705308 + y: 0.0522132462525 + z: 0.0 + orientation: + x: 0.0 + y: 0.00111350575419 + z: 0.0 + w: 0.999999380052 + covariance: [8.059664631671845, -3.4515022172870804e-08, 0.0, 0.0, -5.309597041293203e-05, 0.0, -3.451502217287069e-08, 8.059664585794687, 0.0, 0.0, 2.0224659245168253e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.309597041293202e-05, 2.022465924516825e-05, 0.0, 0.0, 7.963233806229068, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 162 + stamp: + secs: 1624875209 + nsecs: 400302887 + frame_id: "/world" +pose: + pose: + position: + x: -0.00674425295535 + y: 0.0515928980872 + z: 0.0 + orientation: + x: 0.0 + y: 0.000322014702566 + z: 0.0 + w: 0.999999948153 + covariance: [7.215389769771783, -1.9288686350532923e-08, 0.0, 0.0, -2.9640276755799692e-05, 0.0, -1.928868635053286e-08, 7.215389744040491, 0.0, 0.0, 1.1888709420833739e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.964027675579968e-05, 1.188870942083374e-05, 0.0, 0.0, 7.161419847327116, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 163 + stamp: + secs: 1624875209 + nsecs: 480295897 + frame_id: "/world" +pose: + pose: + position: + x: -0.00535171029442 + y: 0.0444656954088 + z: 0.0 + orientation: + x: 0.0 + y: 0.000670480367451 + z: 0.0 + w: 0.999999775228 + covariance: [7.621855067512566, -2.1048786529035143e-08, 0.0, 0.0, -2.4997800392939763e-05, 0.0, -2.104878652903508e-08, 7.6218550394146956, 0.0, 0.0, 1.7625882324868487e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.499780039293976e-05, 1.762588232486848e-05, 0.0, 0.0, 7.562905277445575, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 164 + stamp: + secs: 1624875209 + nsecs: 560297012 + frame_id: "/world" +pose: + pose: + position: + x: -0.00490622434128 + y: 0.0484797995982 + z: 0.0 + orientation: + x: 0.0 + y: 0.000295624809824 + z: 0.0 + w: 0.999999956303 + covariance: [7.761939811123906, -1.633199719560009e-08, 0.0, 0.0, -2.1521513190144015e-05, 0.0, -1.6331997195600037e-08, 7.7619397893694995, 0.0, 0.0, 1.4715830111992615e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.152151319014401e-05, 1.4715830111992619e-05, 0.0, 0.0, 7.716103918308565, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 165 + stamp: + secs: 1624875209 + nsecs: 640314102 + frame_id: "/world" +pose: + pose: + position: + x: -0.00440278462669 + y: 0.04736773214 + z: 0.0 + orientation: + x: 0.0 + y: 0.000174758488617 + z: 0.0 + w: 0.99999998473 + covariance: [7.117648981047658, -1.1559468581547875e-08, 0.0, 0.0, -1.6568145915942416e-05, 0.0, -1.1559468581547839e-08, 7.117648965642969, 0.0, 0.0, 8.801286873490394e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.656814591594241e-05, 8.801286873490399e-06, 0.0, 0.0, 7.085164771971066, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 166 + stamp: + secs: 1624875209 + nsecs: 720318079 + frame_id: "/world" +pose: + pose: + position: + x: -0.000384015334056 + y: 0.0177155005967 + z: 0.0 + orientation: + x: 0.0 + y: 0.000105118395626 + z: 0.0 + w: 0.999999994475 + covariance: [8.360350492667594, -1.563637914132273e-08, 0.0, 0.0, -1.8340262477343687e-05, 0.0, -1.563637914132268e-08, 8.360350471807404, 0.0, 0.0, 8.916055667387592e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8340262477343683e-05, 8.916055667387596e-06, 0.0, 0.0, 8.316287821734402, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 167 + stamp: + secs: 1624875209 + nsecs: 800335884 + frame_id: "/world" +pose: + pose: + position: + x: 0.000954561760729 + y: 0.0180908254226 + z: 0.0 + orientation: + x: 0.0 + y: 0.000546314680005 + z: 0.0 + w: 0.99999985077 + covariance: [8.010118066408628, -5.759492792130526e-09, 0.0, 0.0, -6.7259655290054475e-06, 0.0, -5.7594927921305085e-09, 8.010118058725244, 0.0, 0.0, 3.2685854132455958e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.725965529005443e-06, 3.268585413245596e-06, 0.0, 0.0, 7.993861289832357, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 168 + stamp: + secs: 1624875209 + nsecs: 880331039 + frame_id: "/world" +pose: + pose: + position: + x: 0.00246137562442 + y: 0.0163296260352 + z: 0.0 + orientation: + x: 0.0 + y: 0.000461138848796 + z: 0.0 + w: 0.999999893675 + covariance: [7.915962973102289, -1.3814425604921042e-08, 0.0, 0.0, -1.2481285532515644e-05, 0.0, -1.3814425604920996e-08, 7.915962954634977, 0.0, 0.0, 4.755891430606253e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2481285532515647e-05, 4.755891430606255e-06, 0.0, 0.0, 7.8765833916579915, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 169 + stamp: + secs: 1624875209 + nsecs: 960352898 + frame_id: "/world" +pose: + pose: + position: + x: -0.0100773542362 + y: 0.0292824050552 + z: 0.0 + orientation: + x: 0.0 + y: 0.000681116345361 + z: 0.0 + w: 0.99999976804 + covariance: [7.88103457757156, -1.6574580942594496e-08, 0.0, 0.0, -1.3353977609426377e-05, 0.0, -1.657458094259444e-08, 7.881034555376621, 0.0, 0.0, 4.191392305688272e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3353977609426373e-05, 4.191392305688273e-06, 0.0, 0.0, 7.83357285830948, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 170 + stamp: + secs: 1624875210 + nsecs: 40358067 + frame_id: "/world" +pose: + pose: + position: + x: -0.0106618125587 + y: 0.0306947329199 + z: 0.0 + orientation: + x: 0.0 + y: 0.000113066445526 + z: 0.0 + w: 0.999999993608 + covariance: [7.860281241850908, -1.4813807787364622e-08, 0.0, 0.0, -9.203507645808067e-06, 0.0, -1.4813807787364577e-08, 7.860281221994587, 0.0, 0.0, 3.108933218997189e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.203507645808065e-06, 3.1089332189971894e-06, 0.0, 0.0, 7.81742965562886, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 171 + stamp: + secs: 1624875210 + nsecs: 120342016 + frame_id: "/world" +pose: + pose: + position: + x: -0.0105325979795 + y: 0.0269727787451 + z: 0.0 + orientation: + x: 0.0 + y: 4.78080583843e-05 + z: 0.0 + w: 0.999999998857 + covariance: [7.178235052669103, -2.094325874554809e-08, 0.0, 0.0, -8.272576998437199e-06, 0.0, -2.0943258745548025e-08, 7.178235024612341, 0.0, 0.0, 9.326132816017948e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.272576998437197e-06, 9.32613281601795e-06, 0.0, 0.0, 7.116532876488781, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 172 + stamp: + secs: 1624875210 + nsecs: 200356007 + frame_id: "/world" +pose: + pose: + position: + x: -0.0105313098903 + y: 0.0199832932663 + z: 0.0 + orientation: + x: 0.0 + y: 0.000120603740297 + z: 0.0 + w: 0.999999992727 + covariance: [8.386959631370619, -1.882487925237094e-08, 0.0, 0.0, -8.261540415933233e-06, 0.0, -1.8824879252370885e-08, 8.386959606186826, 0.0, 0.0, 9.442580125905483e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.261540415933235e-06, 9.442580125905483e-06, 0.0, 0.0, 8.33113875301727, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 173 + stamp: + secs: 1624875210 + nsecs: 280395031 + frame_id: "/world" +pose: + pose: + position: + x: -0.00318740072793 + y: 0.00800981619549 + z: 0.0 + orientation: + x: 0.0 + y: 0.000138945189056 + z: 0.0 + w: 0.999999990347 + covariance: [10.093679037491674, -3.172278331174134e-08, 0.0, 0.0, -2.2952756596027054e-05, 0.0, -3.172278331174123e-08, 10.093678995110102, 0.0, 0.0, 1.2813862961293596e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.295275659602706e-05, 1.2813862961293596e-05, 0.0, 0.0, 9.998830977887543, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 174 + stamp: + secs: 1624875210 + nsecs: 360379934 + frame_id: "/world" +pose: + pose: + position: + x: -0.000534101832514 + y: 0.0230027238806 + z: 0.0 + orientation: + x: 0.0 + y: 0.000279899939588 + z: 0.0 + w: 0.999999960828 + covariance: [12.797062347486403, -5.528143333264437e-08, 0.0, 0.0, -6.918497148058396e-05, 0.0, -5.52814333326442e-08, 12.797062273278796, 0.0, 0.0, 1.7922257878854964e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.918497148058396e-05, 1.7922257878854964e-05, 0.0, 0.0, 12.6309421780887, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 175 + stamp: + secs: 1624875210 + nsecs: 440391064 + frame_id: "/world" +pose: + pose: + position: + x: -0.0078481121437 + y: 0.0243956875063 + z: 0.0 + orientation: + x: 0.0 + y: 0.000394373064062 + z: 0.0 + w: 0.999999922235 + covariance: [12.358742729768982, -4.0223120905861544e-08, 0.0, 0.0, -6.137175667704506e-05, 0.0, -4.022312090586141e-08, 12.35874267548251, 0.0, 0.0, 1.3245993654007504e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.137175667704506e-05, 1.3245993654007506e-05, 0.0, 0.0, 12.237382167404583, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 176 + stamp: + secs: 1624875210 + nsecs: 520392895 + frame_id: "/world" +pose: + pose: + position: + x: -0.0120751952342 + y: 0.0486421515121 + z: 0.0 + orientation: + x: 0.0 + y: 0.000809691648214 + z: 0.0 + w: 0.9999996722 + covariance: [10.604469579166993, -4.4040793329149745e-08, 0.0, 0.0, -6.85467312260383e-05, 0.0, -4.404079332914959e-08, 10.604469518741816, 0.0, 0.0, 1.3460924401926606e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.85467312260383e-05, 1.3460924401926606e-05, 0.0, 0.0, 10.470433491022542, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 177 + stamp: + secs: 1624875210 + nsecs: 600393057 + frame_id: "/world" +pose: + pose: + position: + x: -0.0125715066798 + y: 0.0495557071975 + z: 0.0 + orientation: + x: 0.0 + y: 0.000616659473777 + z: 0.0 + w: 0.999999809866 + covariance: [8.830694496468327, -3.481588787436093e-08, 0.0, 0.0, -4.771327164654715e-05, 0.0, -3.481588787436081e-08, 8.830694448220122, 0.0, 0.0, 1.349508943510053e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.7713271646547164e-05, 1.3495089435100534e-05, 0.0, 0.0, 8.723359106133234, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 178 + stamp: + secs: 1624875210 + nsecs: 680402040 + frame_id: "/world" +pose: + pose: + position: + x: -0.0155446334977 + y: 0.049998122813 + z: 0.0 + orientation: + x: 0.0 + y: 0.000326282442666 + z: 0.0 + w: 0.99999994677 + covariance: [7.470306373454512, -2.652542158956627e-08, 0.0, 0.0, -2.9973022287421443e-05, 0.0, -2.6525421589566175e-08, 7.470306336436203, 0.0, 0.0, 1.1188769963264152e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.997302228742145e-05, 1.118876996326415e-05, 0.0, 0.0, 7.38731131466513, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 179 + stamp: + secs: 1624875210 + nsecs: 760426044 + frame_id: "/world" +pose: + pose: + position: + x: -0.0231732609881 + y: 0.0760459258295 + z: 0.0 + orientation: + x: 0.0 + y: 6.8375235061e-05 + z: 0.0 + w: 0.999999997662 + covariance: [7.015894259568247, -1.0808900369121554e-08, 0.0, 0.0, -1.1697762616268786e-05, 0.0, -1.0808900369121514e-08, 7.015894244462713, 0.0, 0.0, 4.367502749136635e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.169776261626879e-05, 4.367502749136633e-06, 0.0, 0.0, 6.981881317409915, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 180 + stamp: + secs: 1624875210 + nsecs: 840425968 + frame_id: "/world" +pose: + pose: + position: + x: -0.0231729148197 + y: 0.0760328953711 + z: 0.0 + orientation: + x: 0.0 + y: 5.01324258526e-06 + z: 0.0 + w: 0.999999999987 + covariance: [6.323785466256531, -4.1214081507936595e-09, 0.0, 0.0, -4.364186846968509e-06, 0.0, -4.121408150793644e-09, 6.323785460485989, 0.0, 0.0, 1.5417406418444905e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.3641868469685115e-06, 1.5417406418444897e-06, 0.0, 0.0, 6.310769432678757, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 181 + stamp: + secs: 1624875210 + nsecs: 920433044 + frame_id: "/world" +pose: + pose: + position: + x: -0.0224087644207 + y: 0.0800141599579 + z: 0.0 + orientation: + x: -0.0 + y: -3.86937353458e-05 + z: -0.0 + w: 0.999999999251 + covariance: [7.974362316814121, -1.0078204788437789e-08, 0.0, 0.0, -7.967838132593319e-06, 0.0, -1.0078204788437756e-08, 7.974362302603188, 0.0, 0.0, 2.0747548364531703e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.967838132593322e-06, 2.07475483645317e-06, 0.0, 0.0, 7.942087164310923, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 182 + stamp: + secs: 1624875211 + nsecs: 436068 + frame_id: "/world" +pose: + pose: + position: + x: -0.00983568301505 + y: 0.0687038501986 + z: 0.0 + orientation: + x: -0.0 + y: -0.00106431326412 + z: -0.0 + w: 0.999999433618 + covariance: [7.186002704088982, -9.655327196667142e-09, 0.0, 0.0, -5.2320802207445285e-06, 0.0, -9.655327196667107e-09, 7.186002690457327, 0.0, 0.0, 5.954146985832429e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.23208022074453e-06, 5.9541469858324275e-06, 0.0, 0.0, 7.154754573597556, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 183 + stamp: + secs: 1624875211 + nsecs: 80427885 + frame_id: "/world" +pose: + pose: + position: + x: -0.00902950411389 + y: 0.0664716435005 + z: 0.0 + orientation: + x: -0.0 + y: -0.000238657832247 + z: -0.0 + w: 0.999999971521 + covariance: [7.599574518277454, -1.2058403267867718e-08, 0.0, 0.0, -4.079508322704058e-06, 0.0, -1.2058403267867677e-08, 7.599574501301535, 0.0, 0.0, 1.0464486040981578e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.07950832270406e-06, 1.046448604098158e-05, 0.0, 0.0, 7.560158302250148, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 184 + stamp: + secs: 1624875211 + nsecs: 160442114 + frame_id: "/world" +pose: + pose: + position: + x: -0.00826873224254 + y: 0.0615008751875 + z: 0.0 + orientation: + x: 0.0 + y: 0.000293545088177 + z: 0.0 + w: 0.999999956916 + covariance: [7.764955093872776, -1.510382358741674e-08, 0.0, 0.0, -4.766024887494078e-06, 0.0, -1.5103823587416694e-08, 7.764955072681582, 0.0, 0.0, 1.0898716002070731e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.766024887494079e-06, 1.0898716002070731e-05, 0.0, 0.0, 7.715086290107682, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 185 + stamp: + secs: 1624875211 + nsecs: 240469933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0232321340134 + y: 0.0805863922666 + z: 0.0 + orientation: + x: 0.0 + y: 0.000138567496037 + z: 0.0 + w: 0.9999999904 + covariance: [7.132146166063532, -1.4179419684333335e-08, 0.0, 0.0, -8.057110570101356e-06, 0.0, -1.4179419684333286e-08, 7.132146146252177, 0.0, 0.0, 1.1450649000165187e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.057110570101356e-06, 1.1450649000165191e-05, 0.0, 0.0, 7.08490380640583, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 186 + stamp: + secs: 1624875211 + nsecs: 320468903 + frame_id: "/world" +pose: + pose: + position: + x: -0.023177810942 + y: 0.0810181460981 + z: 0.0 + orientation: + x: -0.0 + y: -0.000302191601472 + z: -0.0 + w: 0.99999995434 + covariance: [7.584216297599288, -1.522399334008695e-08, 0.0, 0.0, -1.0100333061746702e-05, 0.0, -1.52239933400869e-08, 7.5842162764387355, 0.0, 0.0, 1.1846198010126057e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.01003330617467e-05, 1.1846198010126057e-05, 0.0, 0.0, 7.533084367899911, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 187 + stamp: + secs: 1624875211 + nsecs: 400448084 + frame_id: "/world" +pose: + pose: + position: + x: -0.0246157225004 + y: 0.0803188923408 + z: 0.0 + orientation: + x: 0.0 + y: 1.63249101386e-05 + z: 0.0 + w: 0.999999999867 + covariance: [7.7705336038552755, -1.9363699405482024e-08, 0.0, 0.0, -9.796078137347503e-06, 0.0, -1.936369940548196e-08, 7.770533577068504, 0.0, 0.0, 1.1383578260260003e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.796078137347506e-06, 1.138357826026e-05, 0.0, 0.0, 7.704808591826622, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 188 + stamp: + secs: 1624875211 + nsecs: 480456114 + frame_id: "/world" +pose: + pose: + position: + x: -0.0191191003109 + y: 0.0824786301304 + z: 0.0 + orientation: + x: -0.0 + y: -0.000250565596069 + z: -0.0 + w: 0.999999968608 + covariance: [6.537203639118754, -9.202816322008706e-09, 0.0, 0.0, -3.7663284783790862e-06, 0.0, -9.202816322008675e-09, 6.537203626416594, 0.0, 0.0, 5.915890205915832e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7663284783790896e-06, 5.915890205915833e-06, 0.0, 0.0, 6.505742829300434, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 189 + stamp: + secs: 1624875211 + nsecs: 560573101 + frame_id: "/world" +pose: + pose: + position: + x: -0.019123401819 + y: 0.0824317893342 + z: 0.0 + orientation: + x: -0.0 + y: -0.000146822538995 + z: -0.0 + w: 0.999999989222 + covariance: [8.083099985978045, -1.3173391486577902e-08, 0.0, 0.0, -6.174259060755e-06, 0.0, -1.3173391486577859e-08, 8.083099967897114, 0.0, 0.0, 1.0973876197864512e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.1742590607550024e-06, 1.0973876197864512e-05, 0.0, 0.0, 8.03774063242927, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 190 + stamp: + secs: 1624875211 + nsecs: 640486956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0186038963151 + y: 0.0820760558576 + z: 0.0 + orientation: + x: -0.0 + y: -7.81217568574e-05 + z: -0.0 + w: 0.999999996948 + covariance: [7.918804186049428, -7.568744729070334e-09, 0.0, 0.0, -4.3117852134978584e-06, 0.0, -7.568744729070311e-09, 7.918804175684448, 0.0, 0.0, 6.3473499389384234e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.31178521349786e-06, 6.347349938938423e-06, 0.0, 0.0, 7.892638009446651, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 191 + stamp: + secs: 1624875211 + nsecs: 720500946 + frame_id: "/world" +pose: + pose: + position: + x: -0.0220251127769 + y: 0.0839647553845 + z: 0.0 + orientation: + x: -0.0 + y: -0.000266515418573 + z: -0.0 + w: 0.999999964485 + covariance: [8.747097927012694, -1.6088407155295003e-08, 0.0, 0.0, -8.921766863993656e-06, 0.0, -1.608840715529495e-08, 8.747097905117393, 0.0, 0.0, 9.208106051768979e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.921766863993656e-06, 9.20810605176898e-06, 0.0, 0.0, 8.690934778510362, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 192 + stamp: + secs: 1624875211 + nsecs: 800513029 + frame_id: "/world" +pose: + pose: + position: + x: -0.0231022602129 + y: 0.0879211128612 + z: 0.0 + orientation: + x: -0.0 + y: -0.000147930642053 + z: -0.0 + w: 0.999999989058 + covariance: [7.416607864763635, -1.0989477567228006e-08, 0.0, 0.0, -3.5401927362533585e-06, 0.0, -1.0989477567227964e-08, 7.416607849838129, 0.0, 0.0, 4.5129310973093975e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.540192736253358e-06, 4.512931097309397e-06, 0.0, 0.0, 7.377840802223861, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 193 + stamp: + secs: 1624875211 + nsecs: 880506992 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181176998161 + y: 0.0698117593498 + z: 0.0 + orientation: + x: 0.0 + y: 0.000127562472115 + z: 0.0 + w: 0.999999991864 + covariance: [6.469281351674116, -1.4026887184188946e-08, 0.0, 0.0, -8.396399080925909e-07, 0.0, -1.4026887184188905e-08, 6.469281332665043, 0.0, 0.0, 2.866599864404733e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.39639908092591e-07, 2.8665998644047324e-06, 0.0, 0.0, 6.418913450761649, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 194 + stamp: + secs: 1624875211 + nsecs: 960508108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181175366193 + y: 0.0697404555457 + z: 0.0 + orientation: + x: 0.0 + y: 0.00011070477273 + z: 0.0 + w: 0.999999993872 + covariance: [8.943509735937656, -1.6738549308010085e-08, 0.0, 0.0, -7.665533679942136e-07, 0.0, -1.6738549308010035e-08, 8.943509713271787, 0.0, 0.0, 5.3342649241086245e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.665533679942135e-07, 5.334264924108624e-06, 0.0, 0.0, 8.883042584457487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 195 + stamp: + secs: 1624875212 + nsecs: 40527105 + frame_id: "/world" +pose: + pose: + position: + x: -0.0180712202289 + y: 0.0698714420166 + z: 0.0 + orientation: + x: 0.0 + y: 2.20533420624e-05 + z: 0.0 + w: 0.999999999757 + covariance: [6.830264513086506, -7.943505349507593e-09, 0.0, 0.0, -1.3650081557621468e-06, 0.0, -7.943505349507571e-09, 6.8302645023438595, 0.0, 0.0, 3.949112682261664e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.365008155762146e-06, 3.949112682261662e-06, 0.0, 0.0, 6.801318237749353, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 196 + stamp: + secs: 1624875212 + nsecs: 120568991 + frame_id: "/world" +pose: + pose: + position: + x: -0.0160408975811 + y: 0.0863791249241 + z: 0.0 + orientation: + x: -0.0 + y: -0.000698906897176 + z: -0.0 + w: 0.999999755765 + covariance: [8.262555377865864, -2.206724991237872e-08, 0.0, 0.0, -5.07491280887148e-06, 0.0, -2.206724991237865e-08, 8.262555348126636, 0.0, 0.0, 9.165867744559197e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.074912808871479e-06, 9.165867744559192e-06, 0.0, 0.0, 8.180907733998044, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 197 + stamp: + secs: 1624875212 + nsecs: 200536013 + frame_id: "/world" +pose: + pose: + position: + x: -0.0158797585815 + y: 0.0866600514372 + z: 0.0 + orientation: + x: -0.0 + y: -0.000497181814508 + z: -0.0 + w: 0.999999876405 + covariance: [10.004591957841006, -2.4370478338748635e-08, 0.0, 0.0, -2.5904599937744524e-05, 0.0, -2.4370478338748556e-08, 10.004591925019659, 0.0, 0.0, 1.4754146576318153e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.590459993774452e-05, 1.475414657631815e-05, 0.0, 0.0, 9.914049336899938, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 198 + stamp: + secs: 1624875212 + nsecs: 280575037 + frame_id: "/world" +pose: + pose: + position: + x: -0.020683404716 + y: 0.109502357081 + z: 0.0 + orientation: + x: -0.0 + y: -0.000286065089184 + z: -0.0 + w: 0.999999959083 + covariance: [9.665004642755298, -2.6996371155788128e-08, 0.0, 0.0, -5.1635517617148555e-05, 0.0, -2.699637115578804e-08, 9.665004606124663, 0.0, 0.0, 2.107338243741557e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.1635517617148534e-05, 2.1073382437415567e-05, 0.0, 0.0, 9.56474020391021, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 199 + stamp: + secs: 1624875212 + nsecs: 360554934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0331409833412 + y: 0.104252469362 + z: 0.0 + orientation: + x: -0.0 + y: -0.000168912280692 + z: -0.0 + w: 0.999999985734 + covariance: [7.028999668278716, -1.8211507761816688e-08, 0.0, 0.0, -2.9159456314371464e-05, 0.0, -1.8211507761816628e-08, 7.028999643355398, 0.0, 0.0, 1.1763427240111754e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.915945631437147e-05, 1.1763427240111754e-05, 0.0, 0.0, 6.961001765197858, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 200 + stamp: + secs: 1624875212 + nsecs: 440562963 + frame_id: "/world" +pose: + pose: + position: + x: -0.0291194286024 + y: 0.104704058283 + z: 0.0 + orientation: + x: -0.0 + y: -0.000387441630365 + z: -0.0 + w: 0.999999924944 + covariance: [10.560805584308309, -4.1754072877182755e-08, 0.0, 0.0, -4.503214115392472e-05, 0.0, -4.175407287718263e-08, 10.560805527113844, 0.0, 0.0, 3.4709356772770977e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.5032141153924736e-05, 3.4709356772770977e-05, 0.0, 0.0, 10.405326384711078, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 201 + stamp: + secs: 1624875212 + nsecs: 520561934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0291437757307 + y: 0.104708406448 + z: 0.0 + orientation: + x: -0.0 + y: -0.000191813442701 + z: -0.0 + w: 0.999999981604 + covariance: [13.065046913214923, -3.950434849077144e-08, 0.0, 0.0, -3.849022642119572e-05, 0.0, -3.950434849077131e-08, 13.065046859200244, 0.0, 0.0, 3.541656264387066e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.8490226421195736e-05, 3.541656264387066e-05, 0.0, 0.0, 12.91773969736562, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 202 + stamp: + secs: 1624875212 + nsecs: 600589991 + frame_id: "/world" +pose: + pose: + position: + x: -0.0213544731109 + y: 0.124756651907 + z: 0.0 + orientation: + x: 0.0 + y: 0.000486766932246 + z: 0.0 + w: 0.999999881529 + covariance: [10.851156603513438, -3.569154189017329e-08, 0.0, 0.0, -1.4103411899669695e-05, 0.0, -3.5691541890173165e-08, 10.851156554988238, 0.0, 0.0, 3.208936534005192e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4103411899669693e-05, 3.208936534005193e-05, 0.0, 0.0, 10.71697564074222, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 203 + stamp: + secs: 1624875212 + nsecs: 680576086 + frame_id: "/world" +pose: + pose: + position: + x: -0.021796526766 + y: 0.126181732667 + z: 0.0 + orientation: + x: 0.0 + y: 0.000501677365425 + z: 0.0 + w: 0.99999987416 + covariance: [10.026919660392476, -3.626922363175355e-08, 0.0, 0.0, 1.3632207344308284e-06, 0.0, -3.6269223631753416e-08, 10.026919611209731, 0.0, 0.0, 2.4659614455371135e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 1.3632207344308278e-06, 2.465961445537114e-05, 0.0, 0.0, 9.888687388754795, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 204 + stamp: + secs: 1624875212 + nsecs: 760581970 + frame_id: "/world" +pose: + pose: + position: + x: -0.0127766377848 + y: 0.10640089455 + z: 0.0 + orientation: + x: 0.0 + y: 0.000400220092614 + z: 0.0 + w: 0.999999919912 + covariance: [9.681160290342383, -3.2869746090759e-08, 0.0, 0.0, 3.670244806653676e-06, 0.0, -3.286974609075887e-08, 9.681160245911615, 0.0, 0.0, 2.9211805918098226e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 3.670244806653674e-06, 2.9211805918098236e-05, 0.0, 0.0, 9.554493378684269, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 205 + stamp: + secs: 1624875212 + nsecs: 840596914 + frame_id: "/world" +pose: + pose: + position: + x: -0.0130636162653 + y: 0.107370504727 + z: 0.0 + orientation: + x: 0.0 + y: 0.000567141096636 + z: 0.0 + w: 0.999999839175 + covariance: [9.535723199858577, -3.0766073407375036e-08, 0.0, 0.0, -1.935334815062944e-07, 0.0, -3.076607340737493e-08, 9.535723158675706, 0.0, 0.0, 3.936674862646941e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9353348150629506e-07, 3.9366748626469405e-05, 0.0, 0.0, 9.415676704162546, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 206 + stamp: + secs: 1624875212 + nsecs: 920597076 + frame_id: "/world" +pose: + pose: + position: + x: -0.00856008411143 + y: 0.0815715804821 + z: 0.0 + orientation: + x: 0.0 + y: 0.000451971559954 + z: 0.0 + w: 0.999999897861 + covariance: [9.493107801503454, -3.43300801299575e-08, 0.0, 0.0, -6.8368000403997145e-06, 0.0, -3.433008012995739e-08, 9.493107756311513, 0.0, 0.0, 4.4907949426489564e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.836800040399719e-06, 4.4907949426489564e-05, 0.0, 0.0, 9.357324384951845, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 207 + stamp: + secs: 1624875213 + nsecs: 623941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0095098528835 + y: 0.0811611875532 + z: 0.0 + orientation: + x: 0.0 + y: 0.000292108769076 + z: 0.0 + w: 0.999999957336 + covariance: [9.424085494430916, -2.2937212227709485e-08, 0.0, 0.0, -5.645977868463417e-06, 0.0, -2.2937212227709413e-08, 9.424085464436235, 0.0, 0.0, 2.755101245260627e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.645977868463418e-06, 2.755101245260627e-05, 0.0, 0.0, 9.332700389819948, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 208 + stamp: + secs: 1624875213 + nsecs: 80640078 + frame_id: "/world" +pose: + pose: + position: + x: -0.0107032806713 + y: 0.0843095749305 + z: 0.0 + orientation: + x: 0.0 + y: 0.000381248297151 + z: 0.0 + w: 0.999999927325 + covariance: [7.614198492987615, -1.338441152398738e-08, 0.0, 0.0, -3.5673819162171517e-06, 0.0, -1.3384411523987333e-08, 7.614198475664542, 0.0, 0.0, 1.4584542206871601e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5673819162171517e-06, 1.4584542206871596e-05, 0.0, 0.0, 7.5606288718635515, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 209 + stamp: + secs: 1624875213 + nsecs: 160621881 + frame_id: "/world" +pose: + pose: + position: + x: -0.0114452163989 + y: 0.0828675430336 + z: 0.0 + orientation: + x: 0.0 + y: 0.00114520196392 + z: 0.0 + w: 0.999999344256 + covariance: [10.995937424932654, -3.446494769125035e-08, 0.0, 0.0, -1.8472173072267833e-05, 0.0, -3.446494769125023e-08, 10.995937381006696, 0.0, 0.0, 2.8672191032091034e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8472173072267833e-05, 2.8672191032091034e-05, 0.0, 0.0, 10.856320962188638, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 210 + stamp: + secs: 1624875213 + nsecs: 240650892 + frame_id: "/world" +pose: + pose: + position: + x: -0.0114378374222 + y: 0.0828176699206 + z: 0.0 + orientation: + x: 0.0 + y: 0.000866994304709 + z: 0.0 + w: 0.99999962416 + covariance: [13.369654023691988, -3.348623619622862e-08, 0.0, 0.0, -2.1080803192592627e-05, 0.0, -3.34862361962285e-08, 13.369653981126522, 0.0, 0.0, 2.4863090645364298e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.108080319259263e-05, 2.48630906453643e-05, 0.0, 0.0, 13.233522825016742, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 211 + stamp: + secs: 1624875213 + nsecs: 320632935 + frame_id: "/world" +pose: + pose: + position: + x: -0.0149587019424 + y: 0.06639876558 + z: 0.0 + orientation: + x: 0.0 + y: 0.000348596337149 + z: 0.0 + w: 0.99999993924 + covariance: [9.623103450494243, -2.0690116865032264e-08, 0.0, 0.0, -4.3512497805511696e-05, 0.0, -2.0690116865032198e-08, 9.62310342407196, 0.0, 0.0, 1.0656564438337907e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.35124978055117e-05, 1.0656564438337909e-05, 0.0, 0.0, 9.538166052409322, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 212 + stamp: + secs: 1624875213 + nsecs: 400671005 + frame_id: "/world" +pose: + pose: + position: + x: -0.0140596128258 + y: 0.0662089599225 + z: 0.0 + orientation: + x: 0.0 + y: 0.000979610305584 + z: 0.0 + w: 0.999999520182 + covariance: [7.695561784228927, -1.943456818954749e-08, 0.0, 0.0, -6.297347425260782e-05, 0.0, -1.943456818954743e-08, 7.695561759209189, 0.0, 0.0, 4.169279890360219e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.29734742526078e-05, 4.1692798903602195e-05, 0.0, 0.0, 7.617158977765893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 213 + stamp: + secs: 1624875213 + nsecs: 480642080 + frame_id: "/world" +pose: + pose: + position: + x: -0.00980882101926 + y: 0.0908215280766 + z: 0.0 + orientation: + x: 0.0 + y: 0.00016383257526 + z: 0.0 + w: 0.999999986579 + covariance: [7.79071536291052, -1.3791290563832538e-08, 0.0, 0.0, -4.1218383287498905e-05, 0.0, -1.37912905638325e-08, 7.790715345798184, 0.0, 0.0, 3.781940856050122e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.1218383287498905e-05, 3.7819408560501204e-05, 0.0, 0.0, 7.73655047101038, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 214 + stamp: + secs: 1624875213 + nsecs: 560708046 + frame_id: "/world" +pose: + pose: + position: + x: -0.00919102742584 + y: 0.0904681647563 + z: 0.0 + orientation: + x: 0.0 + y: 0.000168351273657 + z: 0.0 + w: 0.999999985829 + covariance: [8.684801218346973, -1.7269911272970548e-08, 0.0, 0.0, -3.9409482674005705e-05, 0.0, -1.7269911272970505e-08, 8.68480119816773, 0.0, 0.0, 4.125013032872227e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.9409482674005705e-05, 4.125013032872225e-05, 0.0, 0.0, 8.619751679963834, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 215 + stamp: + secs: 1624875213 + nsecs: 640685081 + frame_id: "/world" +pose: + pose: + position: + x: -0.00549337978693 + y: 0.0758198585554 + z: 0.0 + orientation: + x: 0.0 + y: 0.000330666118897 + z: 0.0 + w: 0.99999994533 + covariance: [8.146471604595344, -1.1932778198038118e-08, 0.0, 0.0, -2.2309730947958627e-05, 0.0, -1.1932778198038087e-08, 8.146471591087437, 0.0, 0.0, 2.3099622957807657e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.230973094795863e-05, 2.3099622957807654e-05, 0.0, 0.0, 8.102327828421888, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 216 + stamp: + secs: 1624875213 + nsecs: 720674992 + frame_id: "/world" +pose: + pose: + position: + x: -0.00159734613872 + y: 0.0490004319276 + z: 0.0 + orientation: + x: 0.0 + y: 0.000569733312645 + z: 0.0 + w: 0.999999837702 + covariance: [8.838515911004842, -1.4564324044184868e-08, 0.0, 0.0, -2.431473893225501e-05, 0.0, -1.4564324044184828e-08, 8.83851589509928, 0.0, 0.0, 1.9013181663671565e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4314738932255016e-05, 1.9013181663671565e-05, 0.0, 0.0, 8.785757962602657, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 217 + stamp: + secs: 1624875213 + nsecs: 800682068 + frame_id: "/world" +pose: + pose: + position: + x: -0.0113349313614 + y: 0.0498066920242 + z: 0.0 + orientation: + x: 0.0 + y: 0.000161658865196 + z: 0.0 + w: 0.999999986933 + covariance: [8.218064706592152, -1.59587688256641e-08, 0.0, 0.0, -2.1820647267454723e-05, 0.0, -1.595876882566406e-08, 8.21806468957466, 0.0, 0.0, 1.2594309781348223e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.182064726745473e-05, 1.2594309781348223e-05, 0.0, 0.0, 8.160951204947706, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 218 + stamp: + secs: 1624875213 + nsecs: 880716085 + frame_id: "/world" +pose: + pose: + position: + x: -0.0113277372242 + y: 0.0497100834728 + z: 0.0 + orientation: + x: 0.0 + y: 0.000819836949795 + z: 0.0 + w: 0.999999663934 + covariance: [10.000153394096245, -2.7302082810878932e-08, 0.0, 0.0, -3.8725182392431846e-05, 0.0, -2.7302082810878866e-08, 10.000153365396978, 0.0, 0.0, 2.355109328767587e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.872518239243185e-05, 2.3551093287675875e-05, 0.0, 0.0, 9.903709583206593, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 219 + stamp: + secs: 1624875213 + nsecs: 960705042 + frame_id: "/world" +pose: + pose: + position: + x: -0.0192124000355 + y: 0.0450494807384 + z: 0.0 + orientation: + x: 0.0 + y: 0.00101346587396 + z: 0.0 + w: 0.999999486443 + covariance: [10.984120929045536, -3.575181782387615e-08, 0.0, 0.0, -7.026926182898199e-05, 0.0, -3.575181782387608e-08, 10.984120891673602, 0.0, 0.0, 3.909286675599668e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.026926182898199e-05, 3.9092866755996665e-05, 0.0, 0.0, 10.858493164311907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 220 + stamp: + secs: 1624875214 + nsecs: 40746927 + frame_id: "/world" +pose: + pose: + position: + x: -0.0165467783032 + y: 0.0466394932217 + z: 0.0 + orientation: + x: -0.0 + y: -0.00065006089166 + z: -0.0 + w: 0.99999978871 + covariance: [13.428276980904421, -5.5804333688649894e-08, 0.0, 0.0, -0.00011541124358577878, 0.0, -5.5804333688649775e-08, 13.428276923161008, 0.0, 0.0, 8.351877369994101e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011541124358577878, 8.3518773699941e-05, 0.0, 0.0, 13.235014532351718, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 221 + stamp: + secs: 1624875214 + nsecs: 120723009 + frame_id: "/world" +pose: + pose: + position: + x: -0.0190028365585 + y: 0.0471179168186 + z: 0.0 + orientation: + x: 0.0 + y: 0.00033460438045 + z: 0.0 + w: 0.99999994402 + covariance: [15.161411125037562, -9.157607069873001e-08, 0.0, 0.0, -0.00015562112856633566, 0.0, -9.157607069872987e-08, 15.161411033008541, 0.0, 0.0, 0.0001471009557213077, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00015562112856633568, 0.00014710095572130767, 0.0, 0.0, 14.850608433289601, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 222 + stamp: + secs: 1624875214 + nsecs: 200736046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181089283459 + y: 0.0481035964141 + z: 0.0 + orientation: + x: -0.0 + y: -0.00016222116632 + z: -0.0 + w: 0.999999986842 + covariance: [16.286124003398502, -1.1312095212792795e-07, 0.0, 0.0, -0.00015770875301688168, 0.0, -1.1312095212792777e-07, 16.28612389163296, 0.0, 0.0, 0.00015914704463789315, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00015770875301688173, 0.0001591470446378931, 0.0, 0.0, 15.905335213399297, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 223 + stamp: + secs: 1624875214 + nsecs: 280742884 + frame_id: "/world" +pose: + pose: + position: + x: -0.018159104865 + y: 0.0477963223956 + z: 0.0 + orientation: + x: 0.0 + y: 0.000393143595934 + z: 0.0 + w: 0.999999922719 + covariance: [17.001206967230225, -1.270972969660823e-07, 0.0, 0.0, -0.00017441715812368725, 0.0, -1.2709729696608206e-07, 17.00120684276304, 0.0, 0.0, 0.00015121756151262295, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001744171581236873, 0.00015121756151262295, 0.0, 0.0, 16.574725994275816, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 224 + stamp: + secs: 1624875214 + nsecs: 360743999 + frame_id: "/world" +pose: + pose: + position: + x: -0.0182160652014 + y: 0.0474449960682 + z: 0.0 + orientation: + x: 0.0 + y: 0.00076112912509 + z: 0.0 + w: 0.999999710341 + covariance: [17.483140787206395, -1.4646384395349095e-07, 0.0, 0.0, -0.00020523705177009923, 0.0, -1.4646384395349066e-07, 17.483140644015865, 0.0, 0.0, 0.00013999709851596158, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00020523705177009926, 0.0001399970985159616, 0.0, 0.0, 16.993534431385132, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 225 + stamp: + secs: 1624875214 + nsecs: 440733910 + frame_id: "/world" +pose: + pose: + position: + x: -0.0186621167391 + y: 0.0487912904748 + z: 0.0 + orientation: + x: 0.0 + y: 0.00093784764219 + z: 0.0 + w: 0.999999560221 + covariance: [12.306867833443702, -7.524491212127605e-08, 0.0, 0.0, -0.00014436643544157884, 0.0, -7.52449121212759e-08, 12.306867759049144, 0.0, 0.0, 6.646470767904184e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001443664354415789, 6.646470767904186e-05, 0.0, 0.0, 12.05431817702347, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 226 + stamp: + secs: 1624875214 + nsecs: 520785093 + frame_id: "/world" +pose: + pose: + position: + x: -0.0235126651708 + y: 0.0482636666767 + z: 0.0 + orientation: + x: 0.0 + y: 0.00308402012799 + z: 0.0 + w: 0.999995244399 + covariance: [14.41018854653725, -1.0641739309206826e-07, 0.0, 0.0, -0.0002561515041211722, 0.0, -1.0641739309206805e-07, 14.410188436424312, 0.0, 0.0, 8.501234222329104e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00025615150412117225, 8.501234222329105e-05, 0.0, 0.0, 14.056829144237353, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 227 + stamp: + secs: 1624875214 + nsecs: 600769043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0247792706345 + y: 0.0476411217211 + z: 0.0 + orientation: + x: 0.0 + y: 0.000924290035299 + z: 0.0 + w: 0.999999572844 + covariance: [11.336553398099136, -6.678087185206585e-08, 0.0, 0.0, -0.0001578443214773427, 0.0, -6.678087185206572e-08, 11.336553326787596, 0.0, 0.0, 5.074488717051578e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00015784432147734271, 5.074488717051581e-05, 0.0, 0.0, 11.114057832786154, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 228 + stamp: + secs: 1624875214 + nsecs: 680773020 + frame_id: "/world" +pose: + pose: + position: + x: -0.0293357788352 + y: 0.0523418514678 + z: 0.0 + orientation: + x: -0.0 + y: -0.00017183197794 + z: -0.0 + w: 0.999999985237 + covariance: [10.227518496429008, -5.517297893350526e-08, 0.0, 0.0, -0.00011469416191763879, 0.0, -5.5172978933505145e-08, 10.227518435415199, 0.0, 0.0, 3.8249608310407525e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001146941619176388, 3.824960831040753e-05, 0.0, 0.0, 10.044433244315645, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 229 + stamp: + secs: 1624875214 + nsecs: 760782957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0294784160203 + y: 0.0520255801178 + z: 0.0 + orientation: + x: -0.0 + y: -0.000157881443796 + z: -0.0 + w: 0.999999987537 + covariance: [12.957890067993716, -8.960211435010073e-08, 0.0, 0.0, -0.00015545473005135034, 0.0, -8.960211435010056e-08, 12.957889965407121, 0.0, 0.0, 4.879861993829889e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00015545473005135034, 4.879861993829891e-05, 0.0, 0.0, 12.663062897119918, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 230 + stamp: + secs: 1624875214 + nsecs: 840786934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0295271723573 + y: 0.051917083446 + z: 0.0 + orientation: + x: -0.0 + y: -2.49930357664e-05 + z: -0.0 + w: 0.999999999688 + covariance: [12.444671490057745, -5.810325962447611e-08, 0.0, 0.0, -9.463280036044766e-05, 0.0, -5.8103259624475994e-08, 12.444671422900962, 0.0, 0.0, 2.8699086210446023e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.463280036044762e-05, 2.8699086210446033e-05, 0.0, 0.0, 12.252614001490857, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 231 + stamp: + secs: 1624875214 + nsecs: 920783043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0173604112871 + y: 0.0647574094903 + z: 0.0 + orientation: + x: 0.0 + y: 2.79474916125e-05 + z: 0.0 + w: 0.999999999609 + covariance: [14.474684288166884, -8.5613008527528e-08, 0.0, 0.0, -0.00011501018415212504, 0.0, -8.56130085275278e-08, 14.474684186858815, 0.0, 0.0, 3.2267958810366445e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011501018415212503, 3.226795881036646e-05, 0.0, 0.0, 14.192049147471753, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 232 + stamp: + secs: 1624875215 + nsecs: 812054 + frame_id: "/world" +pose: + pose: + position: + x: -0.0162211578552 + y: 0.0652148119595 + z: 0.0 + orientation: + x: -0.0 + y: -0.000409377389836 + z: -0.0 + w: 0.999999916205 + covariance: [9.932377933449597, -4.1451160095002954e-08, 0.0, 0.0, -4.670969070906065e-05, 0.0, -4.1451160095002854e-08, 9.932377883825563, 0.0, 0.0, 1.7024134839142332e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.670969070906064e-05, 1.7024134839142325e-05, 0.0, 0.0, 9.79393297325805, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 233 + stamp: + secs: 1624875215 + nsecs: 80796003 + frame_id: "/world" +pose: + pose: + position: + x: -0.0140489176671 + y: 0.0653343496659 + z: 0.0 + orientation: + x: -0.0 + y: -0.00114260654068 + z: -0.0 + w: 0.999999347225 + covariance: [9.692137105855485, -5.242785158759063e-08, 0.0, 0.0, -3.884050898243834e-05, 0.0, -5.2427851587590514e-08, 9.692137042278011, 0.0, 0.0, 2.239779602353155e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.8840508982438344e-05, 2.2397796023531548e-05, 0.0, 0.0, 9.515179846888241, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 234 + stamp: + secs: 1624875215 + nsecs: 160804033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0140146482188 + y: 0.0646526973958 + z: 0.0 + orientation: + x: -0.0 + y: -0.000409120996199 + z: -0.0 + w: 0.99999991631 + covariance: [8.5422467352846, -3.83885710319484e-08, 0.0, 0.0, -2.4084581860038875e-05, 0.0, -3.838857103194832e-08, 8.542246688446985, 0.0, 0.0, 1.3588046848154355e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4084581860038885e-05, 1.358804684815435e-05, 0.0, 0.0, 8.410903328986965, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 235 + stamp: + secs: 1624875215 + nsecs: 240817070 + frame_id: "/world" +pose: + pose: + position: + x: -0.0140491346804 + y: 0.0644322652712 + z: 0.0 + orientation: + x: -0.0 + y: -0.000373032452539 + z: -0.0 + w: 0.999999930423 + covariance: [11.72363589418439, -6.912360917656791e-08, 0.0, 0.0, -3.813410017622279e-05, 0.0, -6.912360917656778e-08, 11.723635809402596, 0.0, 0.0, 1.8337400887682836e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.8134100176222804e-05, 1.8337400887682833e-05, 0.0, 0.0, 11.485542216007612, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 236 + stamp: + secs: 1624875215 + nsecs: 320810080 + frame_id: "/world" +pose: + pose: + position: + x: -0.00851616063149 + y: 0.0642177750238 + z: 0.0 + orientation: + x: -0.0 + y: -0.000423519112991 + z: -0.0 + w: 0.999999910316 + covariance: [14.210323805794355, -1.5511871991266888e-07, 0.0, 0.0, -0.00011215151708580078, 0.0, -1.551187199126686e-07, 14.210323613875508, 0.0, 0.0, 2.627655946248338e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011215151708580079, 2.6276559462483374e-05, 0.0, 0.0, 13.668816405323348, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 237 + stamp: + secs: 1624875215 + nsecs: 400810003 + frame_id: "/world" +pose: + pose: + position: + x: -0.00877059495051 + y: 0.0659161507532 + z: 0.0 + orientation: + x: 0.0 + y: 0.000179595577218 + z: 0.0 + w: 0.999999983873 + covariance: [8.79974717572571, -5.130803723864651e-08, 0.0, 0.0, -4.784474728330341e-05, 0.0, -5.130803723864639e-08, 8.799747111986104, 0.0, 0.0, 1.3939714740681665e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.784474728330342e-05, 1.3939714740681672e-05, 0.0, 0.0, 8.615848765149087, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 238 + stamp: + secs: 1624875215 + nsecs: 480870008 + frame_id: "/world" +pose: + pose: + position: + x: -0.00845969109738 + y: 0.0659351495679 + z: 0.0 + orientation: + x: 0.0 + y: 0.000185490076511 + z: 0.0 + w: 0.999999982797 + covariance: [9.210114708680772, -5.443961286757261e-08, 0.0, 0.0, -7.705167037827037e-05, 0.0, -5.4439612867572476e-08, 9.210114640144267, 0.0, 0.0, 2.4469464161682183e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.70516703782704e-05, 2.4469464161682183e-05, 0.0, 0.0, 9.013585999339064, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 239 + stamp: + secs: 1624875215 + nsecs: 560842037 + frame_id: "/world" +pose: + pose: + position: + x: -0.0051985662262 + y: 0.0659274276258 + z: 0.0 + orientation: + x: -0.0 + y: -0.00195858210414 + z: -0.0 + w: 0.999998081976 + covariance: [12.215267207716787, -8.055490695554372e-08, 0.0, 0.0, -0.00012161242400084725, 0.0, -8.055490695554352e-08, 12.215267104588495, 0.0, 0.0, 3.559297496412426e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00012161242400084726, 3.559297496412425e-05, 0.0, 0.0, 11.924876873559459, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 240 + stamp: + secs: 1624875215 + nsecs: 640876055 + frame_id: "/world" +pose: + pose: + position: + x: -0.00327553492845 + y: 0.0663960617926 + z: 0.0 + orientation: + x: -0.0 + y: -0.00156285755483 + z: -0.0 + w: 0.999998778737 + covariance: [10.55054359885904, -5.4152738420484285e-08, 0.0, 0.0, -8.784640589624779e-05, 0.0, -5.4152738420484146e-08, 10.550543528577931, 0.0, 0.0, 2.4345226219172533e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.784640589624779e-05, 2.434522621917253e-05, 0.0, 0.0, 10.353573181533516, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 241 + stamp: + secs: 1624875215 + nsecs: 720865965 + frame_id: "/world" +pose: + pose: + position: + x: -0.00434722958263 + y: 0.06423670905 + z: 0.0 + orientation: + x: -0.0 + y: -6.38446039172e-05 + z: -0.0 + w: 0.999999997962 + covariance: [8.828169644815633, -3.902870911971071e-08, 0.0, 0.0, -7.134134492095369e-05, 0.0, -3.902870911971062e-08, 8.828169593198318, 0.0, 0.0, 2.594494988387849e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.134134492095365e-05, 2.594494988387848e-05, 0.0, 0.0, 8.685790638101329, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 242 + stamp: + secs: 1624875215 + nsecs: 800864935 + frame_id: "/world" +pose: + pose: + position: + x: -0.00297010914268 + y: 0.0647492858916 + z: 0.0 + orientation: + x: -0.0 + y: -0.00076509924747 + z: -0.0 + w: 0.999999707312 + covariance: [9.17954416560928, -3.7234061100837046e-08, 0.0, 0.0, -6.408638828874446e-05, 0.0, -3.723406110083696e-08, 9.179544115803964, 0.0, 0.0, 2.83640195133529e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.408638828874441e-05, 2.8364019513352903e-05, 0.0, 0.0, 9.04414332849416, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 243 + stamp: + secs: 1624875215 + nsecs: 880872965 + frame_id: "/world" +pose: + pose: + position: + x: -0.0036990268919 + y: 0.0651792634514 + z: 0.0 + orientation: + x: 0.0 + y: 0.000173045211764 + z: 0.0 + w: 0.999999985028 + covariance: [8.352152273720842, -2.791866130978887e-08, 0.0, 0.0, -4.3799434993384336e-05, 0.0, -2.7918661309788822e-08, 8.352152236021958, 0.0, 0.0, 1.8509253724775445e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.379943499338434e-05, 1.850925372477544e-05, 0.0, 0.0, 8.250548869313363, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 244 + stamp: + secs: 1624875215 + nsecs: 960884094 + frame_id: "/world" +pose: + pose: + position: + x: -0.0051950953878 + y: 0.0654242392408 + z: 0.0 + orientation: + x: 0.0 + y: 0.000745254998048 + z: 0.0 + w: 0.999999722297 + covariance: [8.961322293318196, -3.00283817178981e-08, 0.0, 0.0, -3.9949602005538735e-05, 0.0, -3.0028381717898045e-08, 8.961322252392279, 0.0, 0.0, 1.5287486886346224e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.994960200553875e-05, 1.5287486886346227e-05, 0.0, 0.0, 8.852036184758443, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 245 + stamp: + secs: 1624875216 + nsecs: 40893078 + frame_id: "/world" +pose: + pose: + position: + x: -0.00462501474052 + y: 0.0570189132863 + z: 0.0 + orientation: + x: 0.0 + y: 0.000339209737102 + z: 0.0 + w: 0.999999942468 + covariance: [9.227803356502848, -3.054075899487807e-08, 0.0, 0.0, -3.5462707163230826e-05, 0.0, -3.054075899487801e-08, 9.227803314752151, 0.0, 0.0, 2.2000972444372317e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5462707163230826e-05, 2.200097244437232e-05, 0.0, 0.0, 9.116464924661317, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 246 + stamp: + secs: 1624875216 + nsecs: 120889902 + frame_id: "/world" +pose: + pose: + position: + x: -0.00474314028046 + y: 0.0580832813365 + z: 0.0 + orientation: + x: -0.0 + y: -5.64664644595e-05 + z: -0.0 + w: 0.999999998406 + covariance: [7.561712211729876, -1.6953993885667214e-08, 0.0, 0.0, -1.7867531518259483e-05, 0.0, -1.6953993885667177e-08, 7.561712188600815, 0.0, 0.0, 1.4742806683286563e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7867531518259466e-05, 1.4742806683286557e-05, 0.0, 0.0, 7.4996740251886, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 247 + stamp: + secs: 1624875216 + nsecs: 200895071 + frame_id: "/world" +pose: + pose: + position: + x: -0.00952733263024 + y: 0.0624660762898 + z: 0.0 + orientation: + x: -0.0 + y: -3.49796891346e-05 + z: -0.0 + w: 0.999999999388 + covariance: [9.624967807753034, -2.772411411376943e-08, 0.0, 0.0, -3.556741596344105e-05, 0.0, -2.7724114113769373e-08, 9.624967770016786, 0.0, 0.0, 2.0345766301141725e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5567415963441026e-05, 2.0345766301141722e-05, 0.0, 0.0, 9.523615842787363, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 248 + stamp: + secs: 1624875216 + nsecs: 280922890 + frame_id: "/world" +pose: + pose: + position: + x: -0.00968246941909 + y: 0.0628698965267 + z: 0.0 + orientation: + x: -0.0 + y: -1.9143100464e-05 + z: -0.0 + w: 0.999999999817 + covariance: [7.657982433235563, -1.2256123116096942e-08, 0.0, 0.0, -2.0001014487176816e-05, 0.0, -1.2256123116096925e-08, 7.657982416551683, 0.0, 0.0, 1.0810327728259119e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0001014487176812e-05, 1.0810327728259119e-05, 0.0, 0.0, 7.6130521195637355, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 249 + stamp: + secs: 1624875216 + nsecs: 360913038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0175889191978 + y: 0.070601657139 + z: 0.0 + orientation: + x: -0.0 + y: -0.000114967754822 + z: -0.0 + w: 0.999999993391 + covariance: [7.799800514213597, -1.7765688985013408e-08, 0.0, 0.0, -2.4113895835383272e-05, 0.0, -1.776568898501339e-08, 7.799800490144185, 0.0, 0.0, 1.779217385452581e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4113895835383262e-05, 1.779217385452581e-05, 0.0, 0.0, 7.73497596581193, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 250 + stamp: + secs: 1624875216 + nsecs: 440912962 + frame_id: "/world" +pose: + pose: + position: + x: -0.0111347615334 + y: 0.0720797402101 + z: 0.0 + orientation: + x: -0.0 + y: -0.00313113339479 + z: -0.0 + w: 0.99999509799 + covariance: [11.086264808328348, -2.7451609546839754e-08, 0.0, 0.0, -2.5393042739386356e-05, 0.0, -2.7451609546839717e-08, 11.086264771241598, 0.0, 0.0, 2.231606862979602e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5393042739386336e-05, 2.2316068629796017e-05, 0.0, 0.0, 10.986202199779125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 251 + stamp: + secs: 1624875216 + nsecs: 520924091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0176145459004 + y: 0.0952932995625 + z: 0.0 + orientation: + x: -0.0 + y: -0.000539994284706 + z: -0.0 + w: 0.999999854203 + covariance: [8.957720488420737, -1.9913520809775903e-08, 0.0, 0.0, -1.3186543950569981e-05, 0.0, -1.991352080977587e-08, 8.957720461628089, 0.0, 0.0, 1.8129654095901322e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.318654395056997e-05, 1.8129654095901315e-05, 0.0, 0.0, 8.884689190699593, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 252 + stamp: + secs: 1624875216 + nsecs: 600929976 + frame_id: "/world" +pose: + pose: + position: + x: -0.00227311510975 + y: 0.0856314424076 + z: 0.0 + orientation: + x: -0.0 + y: -0.000571691948247 + z: -0.0 + w: 0.999999836584 + covariance: [9.249217489578697, -3.214591485542136e-08, 0.0, 0.0, -2.669066316909474e-05, 0.0, -3.21459148554213e-08, 9.24921744684516, 0.0, 0.0, 3.4796363510082736e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.669066316909473e-05, 3.479636351008273e-05, 0.0, 0.0, 9.130697227271575, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 253 + stamp: + secs: 1624875216 + nsecs: 680928946 + frame_id: "/world" +pose: + pose: + position: + x: -0.00407794366128 + y: 0.0821168499716 + z: 0.0 + orientation: + x: -0.0 + y: -0.000270479546038 + z: -0.0 + w: 0.99999996342 + covariance: [9.28130171182913, -1.2152334580806941e-08, 0.0, 0.0, -1.0872731885630971e-05, 0.0, -1.215233458080692e-08, 9.28130169563849, 0.0, 0.0, 1.3310922499452967e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0872731885630968e-05, 1.3310922499452959e-05, 0.0, 0.0, 9.23636468046703, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 254 + stamp: + secs: 1624875216 + nsecs: 760956049 + frame_id: "/world" +pose: + pose: + position: + x: -0.00293510007491 + y: 0.0816278190751 + z: 0.0 + orientation: + x: 0.0 + y: 0.000215235737207 + z: 0.0 + w: 0.999999976837 + covariance: [8.370450718232606, -1.4457045498095123e-08, 0.0, 0.0, -6.25518563050342e-06, 0.0, -1.4457045498095094e-08, 8.370450699149815, 0.0, 0.0, 1.2192957973476712e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.25518563050342e-06, 1.2192957973476712e-05, 0.0, 0.0, 8.316493186861369, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 255 + stamp: + secs: 1624875216 + nsecs: 840961933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0171471293749 + y: 0.0712330221549 + z: 0.0 + orientation: + x: 0.0 + y: 0.000280165973088 + z: 0.0 + w: 0.999999960754 + covariance: [6.713941878726659, -1.3621562469917298e-08, 0.0, 0.0, -5.93445102532608e-06, 0.0, -1.3621562469917268e-08, 6.713941860848399, 0.0, 0.0, 7.480209401786917e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.934451025326083e-06, 7.480209401786922e-06, 0.0, 0.0, 6.662499164396891, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 256 + stamp: + secs: 1624875216 + nsecs: 920965910 + frame_id: "/world" +pose: + pose: + position: + x: -0.0162324440825 + y: 0.0705944233176 + z: 0.0 + orientation: + x: 0.0 + y: 0.000470254718033 + z: 0.0 + w: 0.99999988943 + covariance: [8.17245387035625, -1.5381028822196277e-08, 0.0, 0.0, -5.166583246871375e-06, 0.0, -1.538102882219625e-08, 8.172453850212307, 0.0, 0.0, 6.080520172012385e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.166583246871377e-06, 6.080520172012387e-06, 0.0, 0.0, 8.11397794221845, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 257 + stamp: + secs: 1624875217 + nsecs: 967979 + frame_id: "/world" +pose: + pose: + position: + x: -0.0241125071193 + y: 0.0840498281046 + z: 0.0 + orientation: + x: 0.0 + y: 0.000283471962543 + z: 0.0 + w: 0.999999959822 + covariance: [7.25094580332763, -1.1441669771911934e-08, 0.0, 0.0, -2.5177522174211317e-06, 0.0, -1.144166977191191e-08, 7.250945788365458, 0.0, 0.0, 3.091392209867352e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5177522174211326e-06, 3.0913922098673513e-06, 0.0, 0.0, 7.207096967173331, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 258 + stamp: + secs: 1624875217 + nsecs: 80965996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0241688069637 + y: 0.0839436861047 + z: 0.0 + orientation: + x: 0.0 + y: 0.000460681419104 + z: 0.0 + w: 0.999999893886 + covariance: [9.436493746824015, -2.1560195130023e-08, 0.0, 0.0, -2.4052885787123106e-06, 0.0, -2.1560195130022954e-08, 9.43649371865951, 0.0, 0.0, -2.76934132889672e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4052885787123123e-06, -2.7693413288967194e-06, 0.0, 0.0, 9.352887104348817, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 259 + stamp: + secs: 1624875217 + nsecs: 160985947 + frame_id: "/world" +pose: + pose: + position: + x: -0.0241411081453 + y: 0.084122340971 + z: 0.0 + orientation: + x: 0.0 + y: 0.000531346590116 + z: 0.0 + w: 0.999999858835 + covariance: [8.436984782275893, -2.0531034005104698e-08, 0.0, 0.0, -5.212591808795111e-06, 0.0, -2.0531034005104665e-08, 8.436984755475983, 0.0, 0.0, -6.592266707913469e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.212591808795113e-06, -6.592266707913466e-06, 0.0, 0.0, 8.356424664524289, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 260 + stamp: + secs: 1624875217 + nsecs: 240998983 + frame_id: "/world" +pose: + pose: + position: + x: -0.0252538267731 + y: 0.0874980426231 + z: 0.0 + orientation: + x: 0.0 + y: 0.000432292809821 + z: 0.0 + w: 0.999999906561 + covariance: [7.356601899518692, -1.920943968622396e-08, 0.0, 0.0, -2.592706261830272e-05, 0.0, -1.920943968622393e-08, 7.356601874216965, 0.0, 0.0, -4.924689770982821e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.592706261830273e-05, -4.924689770982818e-06, 0.0, 0.0, 7.279908667478952, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 261 + stamp: + secs: 1624875217 + nsecs: 320986986 + frame_id: "/world" +pose: + pose: + position: + x: -0.0217531034728 + y: 0.0881097431874 + z: 0.0 + orientation: + x: 0.0 + y: 0.000334791440941 + z: 0.0 + w: 0.999999943957 + covariance: [10.75306856667875, -2.6608138860369167e-08, 0.0, 0.0, -4.6659721097720626e-05, 0.0, -2.660813886036911e-08, 10.75306853104519, 0.0, 0.0, -5.327562974131941e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.665972109772062e-05, -5.3275629741319374e-06, 0.0, 0.0, 10.646055575762476, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 262 + stamp: + secs: 1624875217 + nsecs: 400985956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0218563625432 + y: 0.0879927508321 + z: 0.0 + orientation: + x: 0.0 + y: 0.000338186469781 + z: 0.0 + w: 0.999999942815 + covariance: [11.347516422114746, -2.475000148122466e-08, 0.0, 0.0, -4.257785933597125e-05, 0.0, -2.4750001481224614e-08, 11.347516388460528, 0.0, 0.0, -3.890403036416949e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.257785933597124e-05, -3.8904030364169465e-06, 0.0, 0.0, 11.246997731326996, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 263 + stamp: + secs: 1624875217 + nsecs: 481002092 + frame_id: "/world" +pose: + pose: + position: + x: -0.014028992128 + y: 0.0846464230234 + z: 0.0 + orientation: + x: 0.0 + y: 0.000469277313099 + z: 0.0 + w: 0.999999889889 + covariance: [10.21196899125339, -2.8039177571170695e-08, 0.0, 0.0, -6.1188805334025e-05, 0.0, -2.8039177571170626e-08, 10.211968951882042, 0.0, 0.0, 2.305113504930296e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.118880533402497e-05, 2.3051135049302984e-06, 0.0, 0.0, 10.096346062494673, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 264 + stamp: + secs: 1624875217 + nsecs: 561059952 + frame_id: "/world" +pose: + pose: + position: + x: -0.0144321910938 + y: 0.0848011778457 + z: 0.0 + orientation: + x: 0.0 + y: 0.000294406595817 + z: 0.0 + w: 0.999999956662 + covariance: [8.692478145507991, -2.1424637800399577e-08, 0.0, 0.0, -4.9723440631133514e-05, 0.0, -2.1424637800399524e-08, 8.692478114596213, 0.0, 0.0, 4.502069410925643e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.972344063113353e-05, 4.502069410925643e-06, 0.0, 0.0, 8.60291855181104, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 265 + stamp: + secs: 1624875217 + nsecs: 641020060 + frame_id: "/world" +pose: + pose: + position: + x: -0.0158168247522 + y: 0.110145320944 + z: 0.0 + orientation: + x: -0.0 + y: -0.000246962277513 + z: -0.0 + w: 0.999999969505 + covariance: [9.105591692913698, -2.2984388471479632e-08, 0.0, 0.0, -5.037639655991242e-05, 0.0, -2.2984388471479582e-08, 9.105591658718529, 0.0, 0.0, -6.487841183486836e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.037639655991244e-05, -6.487841183486838e-06, 0.0, 0.0, 9.007905997274118, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 266 + stamp: + secs: 1624875217 + nsecs: 721040964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0163910006196 + y: 0.10997282385 + z: 0.0 + orientation: + x: 0.0 + y: 0.000504698130175 + z: 0.0 + w: 0.99999987264 + covariance: [10.476532926547655, -2.3728347597892907e-08, 0.0, 0.0, -5.480030347788705e-05, 0.0, -2.3728347597892854e-08, 10.476532890473688, 0.0, 0.0, 9.39089268945085e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.480030347788706e-05, 9.390892689450846e-07, 0.0, 0.0, 10.375182415363529, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 267 + stamp: + secs: 1624875217 + nsecs: 801040888 + frame_id: "/world" +pose: + pose: + position: + x: -0.0157819679493 + y: 0.10356212302 + z: 0.0 + orientation: + x: 0.0 + y: 0.000707661733369 + z: 0.0 + w: 0.999999749607 + covariance: [8.765977667677582, -1.704787079257099e-08, 0.0, 0.0, -3.759672604481405e-05, 0.0, -1.704787079257095e-08, 8.765977641454265, 0.0, 0.0, 1.4020459532891577e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.759672604481405e-05, 1.4020459532891572e-05, 0.0, 0.0, 8.692711181407114, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 268 + stamp: + secs: 1624875217 + nsecs: 881045103 + frame_id: "/world" +pose: + pose: + position: + x: -0.0157617936655 + y: 0.103537593058 + z: 0.0 + orientation: + x: 0.0 + y: 0.00023024572507 + z: 0.0 + w: 0.999999973493 + covariance: [9.127118255804017, -1.8589753929306925e-08, 0.0, 0.0, -3.834930374885794e-05, 0.0, -1.858975392930688e-08, 9.12711822714422, 0.0, 0.0, 2.3249747638941744e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.834930374885794e-05, 2.3249747638941744e-05, 0.0, 0.0, 9.047146788471325, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 269 + stamp: + secs: 1624875217 + nsecs: 961061954 + frame_id: "/world" +pose: + pose: + position: + x: -0.0160022069674 + y: 0.101264606954 + z: 0.0 + orientation: + x: -0.0 + y: -2.05955976219e-06 + z: -0.0 + w: 0.999999999998 + covariance: [9.280148608135404, -1.853655774619999e-08, 0.0, 0.0, -3.393871719767575e-05, 0.0, -1.853655774619995e-08, 9.280148579657743, 0.0, 0.0, 2.3649423924133597e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.393871719767576e-05, 2.364942392413359e-05, 0.0, 0.0, 9.200572872772808, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 270 + stamp: + secs: 1624875218 + nsecs: 41070938 + frame_id: "/world" +pose: + pose: + position: + x: -0.0166786285013 + y: 0.101258173606 + z: 0.0 + orientation: + x: 0.0 + y: 4.00811291363e-05 + z: 0.0 + w: 0.999999999197 + covariance: [7.573238550013201, -1.1599592163956767e-08, 0.0, 0.0, -1.9306425249721244e-05, 0.0, -1.1599592163956737e-08, 7.57323853224109, 0.0, 0.0, 1.2136398932431394e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9306425249721248e-05, 1.2136398932431394e-05, 0.0, 0.0, 7.523366607453662, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 271 + stamp: + secs: 1624875218 + nsecs: 121073961 + frame_id: "/world" +pose: + pose: + position: + x: -0.0310024038017 + y: 0.123518610006 + z: 0.0 + orientation: + x: 0.0 + y: 2.28278189278e-05 + z: 0.0 + w: 0.999999999739 + covariance: [8.611350154351175, -2.0922805244139845e-08, 0.0, 0.0, -3.360651221799119e-05, 0.0, -2.0922805244139802e-08, 8.61135012239874, 0.0, 0.0, 1.7682404887827358e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.36065122179912e-05, 1.7682404887827355e-05, 0.0, 0.0, 8.521660623371176, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 272 + stamp: + secs: 1624875218 + nsecs: 201076984 + frame_id: "/world" +pose: + pose: + position: + x: -0.0295473596786 + y: 0.122755551436 + z: 0.0 + orientation: + x: -0.0 + y: -0.000174224676154 + z: -0.0 + w: 0.999999984823 + covariance: [8.137870649464606, -1.6359401516603505e-08, 0.0, 0.0, -2.435632795036903e-05, 0.0, -1.6359401516603472e-08, 8.137870624513376, 0.0, 0.0, 1.229064719502848e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4356327950369032e-05, 1.229064719502848e-05, 0.0, 0.0, 8.067570042218467, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 273 + stamp: + secs: 1624875218 + nsecs: 281084061 + frame_id: "/world" +pose: + pose: + position: + x: -0.0413032629031 + y: 0.124896552262 + z: 0.0 + orientation: + x: 0.0 + y: 0.000336973830841 + z: 0.0 + w: 0.999999943224 + covariance: [9.966079966340287, -2.697620301510589e-08, 0.0, 0.0, -4.8079123576179595e-05, 0.0, -2.6976203015105838e-08, 9.966079925033428, 0.0, 0.0, 1.3908743027610083e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.807912357617959e-05, 1.3908743027610076e-05, 0.0, 0.0, 9.850077924132883, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 274 + stamp: + secs: 1624875218 + nsecs: 361097097 + frame_id: "/world" +pose: + pose: + position: + x: -0.0404537513774 + y: 0.121312596848 + z: 0.0 + orientation: + x: 0.0 + y: 0.000235917738546 + z: 0.0 + w: 0.999999972171 + covariance: [7.774236493718708, -1.6656342044344385e-08, 0.0, 0.0, -3.073700647049805e-05, 0.0, -1.665634204434434e-08, 7.774236467995816, 0.0, 0.0, 5.92645014369391e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.073700647049804e-05, 5.92645014369391e-06, 0.0, 0.0, 7.701916927177735, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 275 + stamp: + secs: 1624875218 + nsecs: 441102982 + frame_id: "/world" +pose: + pose: + position: + x: -0.0417942331941 + y: 0.123835068238 + z: 0.0 + orientation: + x: 0.0 + y: 3.14010999258e-05 + z: 0.0 + w: 0.999999999507 + covariance: [7.854946947060816, -1.970889987304746e-08, 0.0, 0.0, -2.7456760721409598e-05, 0.0, -1.9708899873047416e-08, 7.854946916286739, 0.0, 0.0, 3.626418537073347e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7456760721409587e-05, 3.626418537073347e-06, 0.0, 0.0, 7.768245371350959, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 276 + stamp: + secs: 1624875218 + nsecs: 521126986 + frame_id: "/world" +pose: + pose: + position: + x: -0.0405717424084 + y: 0.13558013802 + z: 0.0 + orientation: + x: -0.0 + y: -4.84430042242e-05 + z: -0.0 + w: 0.999999998827 + covariance: [8.729551391164732, -2.1489390754909866e-08, 0.0, 0.0, -2.5533321877220025e-05, 0.0, -2.1489390754909826e-08, 8.729551357365079, 0.0, 0.0, 2.262257605360362e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5533321877220008e-05, 2.262257605360362e-06, 0.0, 0.0, 8.634244638027033, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 277 + stamp: + secs: 1624875218 + nsecs: 601133108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0491459520337 + y: 0.140117946717 + z: 0.0 + orientation: + x: 0.0 + y: 0.000189809280159 + z: 0.0 + w: 0.999999981986 + covariance: [9.115796598182168, -2.102238871220785e-08, 0.0, 0.0, -3.6303744557460175e-05, 0.0, -2.1022388712207814e-08, 9.11579656475338, 0.0, 0.0, 1.7719671767232925e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.630374455746016e-05, 1.7719671767232919e-06, 0.0, 0.0, 9.021709465624518, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 278 + stamp: + secs: 1624875218 + nsecs: 681112051 + frame_id: "/world" +pose: + pose: + position: + x: -0.0352096968992 + y: 0.149100843891 + z: 0.0 + orientation: + x: 0.0 + y: 0.000313488462214 + z: 0.0 + w: 0.999999950862 + covariance: [10.521102410110904, -3.0689420227105474e-08, 0.0, 0.0, -6.187476188549111e-05, 0.0, -3.0689420227105434e-08, 10.521102360220619, 0.0, 0.0, 2.5528904946696843e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.187476188549111e-05, 2.5528904946696835e-06, 0.0, 0.0, 10.38264195141639, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 279 + stamp: + secs: 1624875218 + nsecs: 761149883 + frame_id: "/world" +pose: + pose: + position: + x: -0.03534098592 + y: 0.149286543155 + z: 0.0 + orientation: + x: 0.0 + y: 0.000844725026851 + z: 0.0 + w: 0.99999964322 + covariance: [11.294115144251242, -4.029153742967264e-08, 0.0, 0.0, -7.445542228972995e-05, 0.0, -4.029153742967259e-08, 11.294115077580472, 0.0, 0.0, 7.270024378021731e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.445542228972996e-05, 7.2700243780217315e-06, 0.0, 0.0, 11.111232566026207, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 280 + stamp: + secs: 1624875218 + nsecs: 841130018 + frame_id: "/world" +pose: + pose: + position: + x: -0.0354464900307 + y: 0.149434071783 + z: 0.0 + orientation: + x: 0.0 + y: 0.000960975370219 + z: 0.0 + w: 0.999999538263 + covariance: [11.672857023527532, -4.081922672465459e-08, 0.0, 0.0, -6.683877956246246e-05, 0.0, -4.081922672465455e-08, 11.672856955283638, 0.0, 0.0, 1.0013183260459795e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.683877956246249e-05, 1.0013183260459795e-05, 0.0, 0.0, 11.486050684066587, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 281 + stamp: + secs: 1624875218 + nsecs: 921133995 + frame_id: "/world" +pose: + pose: + position: + x: -0.0357860045865 + y: 0.151722708619 + z: 0.0 + orientation: + x: 0.0 + y: 6.71629794451e-06 + z: 0.0 + w: 0.999999999977 + covariance: [8.22779942780541, -2.316269969148566e-08, 0.0, 0.0, -3.674960538501994e-05, 0.0, -2.3162699691485644e-08, 8.227799388786872, 0.0, 0.0, 1.2992120337523732e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.674960538501995e-05, 1.299212033752373e-05, 0.0, 0.0, 8.119437360492938, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 282 + stamp: + secs: 1624875219 + nsecs: 1161098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0359471147765 + y: 0.15195411062 + z: 0.0 + orientation: + x: -0.0 + y: -0.000491861364237 + z: -0.0 + w: 0.999999879036 + covariance: [11.494299815047237, -4.762118831298934e-08, 0.0, 0.0, -4.9055425024230915e-05, 0.0, -4.76211883129893e-08, 11.494299734782413, 0.0, 0.0, 3.500129792175325e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.905542502423091e-05, 3.500129792175325e-05, 0.0, 0.0, 11.270982545901205, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 283 + stamp: + secs: 1624875219 + nsecs: 81146002 + frame_id: "/world" +pose: + pose: + position: + x: -0.0366984363367 + y: 0.151700355494 + z: 0.0 + orientation: + x: 0.0 + y: 0.000262541183048 + z: 0.0 + w: 0.999999965536 + covariance: [10.262376702020225, -3.312905331430495e-08, 0.0, 0.0, -2.0144639796169324e-05, 0.0, -3.312905331430493e-08, 10.26237664619625, 0.0, 0.0, 2.2122363025372166e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.014463979616933e-05, 2.212236302537217e-05, 0.0, 0.0, 10.105191890081077, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 284 + stamp: + secs: 1624875219 + nsecs: 161160946 + frame_id: "/world" +pose: + pose: + position: + x: -0.0401339948277 + y: 0.134152669667 + z: 0.0 + orientation: + x: 0.0 + y: 7.53329863328e-05 + z: 0.0 + w: 0.999999997162 + covariance: [7.870386796872362, -2.0839167897119414e-08, 0.0, 0.0, -6.048299715137678e-06, 0.0, -2.0839167897119394e-08, 7.870386761751339, 0.0, 0.0, 1.0571023990326054e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.048299715137681e-06, 1.057102399032605e-05, 0.0, 0.0, 7.769861519823542, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 285 + stamp: + secs: 1624875219 + nsecs: 241172075 + frame_id: "/world" +pose: + pose: + position: + x: -0.0419956496837 + y: 0.133349295471 + z: 0.0 + orientation: + x: 0.0 + y: 3.8919773273e-05 + z: 0.0 + w: 0.999999999243 + covariance: [9.757901201824362, -1.615013858566068e-08, 0.0, 0.0, -3.905498453618557e-06, 0.0, -1.615013858566066e-08, 9.757901174604209, 0.0, 0.0, 7.3169956535761965e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.905498453618559e-06, 7.3169956535761965e-06, 0.0, 0.0, 9.679662175491416, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 286 + stamp: + secs: 1624875219 + nsecs: 321162939 + frame_id: "/world" +pose: + pose: + position: + x: -0.0420276855955 + y: 0.120683444408 + z: 0.0 + orientation: + x: 0.0 + y: 0.000256109132191 + z: 0.0 + w: 0.999999967204 + covariance: [10.823373759355457, -1.7367828654690994e-08, 0.0, 0.0, -1.592441507760842e-05, 0.0, -1.736782865469097e-08, 10.823373730072234, 0.0, 0.0, 7.157799035097808e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5924415077608422e-05, 7.157799035097807e-06, 0.0, 0.0, 10.738663322249108, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 287 + stamp: + secs: 1624875219 + nsecs: 401169062 + frame_id: "/world" +pose: + pose: + position: + x: -0.0507467133355 + y: 0.0938097956612 + z: 0.0 + orientation: + x: 0.0 + y: 0.00018453424974 + z: 0.0 + w: 0.999999982974 + covariance: [11.437187084941213, -2.8901893232344862e-08, 0.0, 0.0, -4.3524269510643165e-05, 0.0, -2.8901893232344825e-08, 11.43718703590184, 0.0, 0.0, 9.79693011595575e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.3524269510643165e-05, 9.796930115955749e-06, 0.0, 0.0, 11.294640879337265, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 288 + stamp: + secs: 1624875219 + nsecs: 481182098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0506857715807 + y: 0.0942720548888 + z: 0.0 + orientation: + x: -0.0 + y: -8.74122999981e-05 + z: -0.0 + w: 0.99999999618 + covariance: [11.739959508623494, -3.257103593075112e-08, 0.0, 0.0, -9.294943096418808e-05, 0.0, -3.257103593075107e-08, 11.739959452359033, 0.0, 0.0, 1.204969520079699e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.294943096418808e-05, 1.2049695200796989e-05, 0.0, 0.0, 11.5781916668335, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 289 + stamp: + secs: 1624875219 + nsecs: 561197996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0509143516252 + y: 0.0943352868521 + z: 0.0 + orientation: + x: -0.0 + y: -5.92030544848e-05 + z: -0.0 + w: 0.999999998247 + covariance: [9.167023623234195, -2.0478284458541065e-08, 0.0, 0.0, -8.290888327183702e-05, 0.0, -2.0478284458541022e-08, 9.167023586338006, 0.0, 0.0, 7.986376467238491e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.290888327183702e-05, 7.986376467238493e-06, 0.0, 0.0, 9.06449808532327, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 290 + stamp: + secs: 1624875219 + nsecs: 641206980 + frame_id: "/world" +pose: + pose: + position: + x: -0.0511430949886 + y: 0.094181785855 + z: 0.0 + orientation: + x: 0.0 + y: 0.000241381877212 + z: 0.0 + w: 0.999999970867 + covariance: [9.353654567295555, -2.8959586566399096e-08, 0.0, 0.0, -0.00011722356317028861, 0.0, -2.895958656639905e-08, 9.353654510850925, 0.0, 0.0, 9.291887180752112e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001172235631702886, 9.291887180752112e-06, 0.0, 0.0, 9.208057042642073, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 291 + stamp: + secs: 1624875219 + nsecs: 721213102 + frame_id: "/world" +pose: + pose: + position: + x: -0.050830567826 + y: 0.0943375293178 + z: 0.0 + orientation: + x: 0.0 + y: 0.000331138239506 + z: 0.0 + w: 0.999999945174 + covariance: [8.403328624832772, -1.9044157169744098e-08, 0.0, 0.0, -6.843436980133473e-05, 0.0, -1.904415716974407e-08, 8.403328586434307, 0.0, 0.0, 5.201724966056587e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.843436980133473e-05, 5.201724966056586e-06, 0.0, 0.0, 8.306848301619763, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 292 + stamp: + secs: 1624875219 + nsecs: 801218987 + frame_id: "/world" +pose: + pose: + position: + x: -0.0508578682909 + y: 0.0941211765351 + z: 0.0 + orientation: + x: 0.0 + y: 5.21441833382e-05 + z: 0.0 + w: 0.99999999864 + covariance: [8.08015777626766, -1.7567855022527648e-08, 0.0, 0.0, -4.650740645991955e-05, 0.0, -1.756785502252762e-08, 8.08015773949523, 0.0, 0.0, 4.166463138912054e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.650740645991952e-05, 4.166463138912054e-06, 0.0, 0.0, 7.990455010738556, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 293 + stamp: + secs: 1624875219 + nsecs: 881227016 + frame_id: "/world" +pose: + pose: + position: + x: -0.0507480716805 + y: 0.0940406008887 + z: 0.0 + orientation: + x: 0.0 + y: 7.20921506875e-05 + z: 0.0 + w: 0.999999997401 + covariance: [9.936361020735706, -2.5390082679973924e-08, 0.0, 0.0, -4.776805727068017e-05, 0.0, -2.5390082679973887e-08, 9.936360966201422, 0.0, 0.0, 4.262076606348141e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.776805727068014e-05, 4.262076606348142e-06, 0.0, 0.0, 9.805963393961004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 294 + stamp: + secs: 1624875219 + nsecs: 961224079 + frame_id: "/world" +pose: + pose: + position: + x: -0.0509077950302 + y: 0.0939293788225 + z: 0.0 + orientation: + x: 0.0 + y: 0.000143551224914 + z: 0.0 + w: 0.999999989697 + covariance: [10.956125884437435, -2.8958589000604986e-08, 0.0, 0.0, -4.1490499612089104e-05, 0.0, -2.8958589000604943e-08, 10.956125821305882, 0.0, 0.0, 2.987127645119009e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.149049961208909e-05, 2.98712764511901e-06, 0.0, 0.0, 10.806078681936123, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 295 + stamp: + secs: 1624875220 + nsecs: 41249990 + frame_id: "/world" +pose: + pose: + position: + x: -0.0530612294007 + y: 0.0915742887627 + z: 0.0 + orientation: + x: 0.0 + y: 0.000187613908266 + z: 0.0 + w: 0.999999982401 + covariance: [11.484871785570213, -2.976972514142668e-08, 0.0, 0.0, -5.993975313747133e-05, 0.0, -2.976972514142664e-08, 11.484871719720353, 0.0, 0.0, 3.5720020391979653e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.993975313747131e-05, 3.5720020391979657e-06, 0.0, 0.0, 11.32934237189766, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 296 + stamp: + secs: 1624875220 + nsecs: 121259928 + frame_id: "/world" +pose: + pose: + position: + x: -0.0440826894036 + y: 0.0826286869839 + z: 0.0 + orientation: + x: 0.0 + y: 0.000172882266807 + z: 0.0 + w: 0.999999985056 + covariance: [9.108335692150984, -2.2387113586914255e-08, 0.0, 0.0, -6.179075216859556e-05, 0.0, -2.2387113586914225e-08, 9.10833564131358, 0.0, 0.0, 2.981889370687656e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.179075216859557e-05, 2.981889370687656e-06, 0.0, 0.0, 8.989635544123699, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 297 + stamp: + secs: 1624875220 + nsecs: 201256037 + frame_id: "/world" +pose: + pose: + position: + x: -0.04364657404 + y: 0.0878379821386 + z: 0.0 + orientation: + x: 0.0 + y: 8.16192947917e-05 + z: 0.0 + w: 0.999999996669 + covariance: [8.31225010077153, -1.503888141954269e-08, 0.0, 0.0, -4.005553518492027e-05, 0.0, -1.503888141954267e-08, 8.312250066045534, 0.0, 0.0, 2.3590728638680935e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.005553518492028e-05, 2.3590728638680935e-06, 0.0, 0.0, 8.231721709126264, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 298 + stamp: + secs: 1624875220 + nsecs: 281263113 + frame_id: "/world" +pose: + pose: + position: + x: -0.0402217839338 + y: 0.0988122246293 + z: 0.0 + orientation: + x: 0.0 + y: 0.000268627049241 + z: 0.0 + w: 0.99999996392 + covariance: [7.305062166783383, -1.1574652741335422e-08, 0.0, 0.0, -4.003355845117491e-05, 0.0, -1.1574652741335406e-08, 7.305062139284948, 0.0, 0.0, 2.103506726880203e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.003355845117491e-05, 2.1035067268802043e-06, 0.0, 0.0, 7.242470541656813, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 299 + stamp: + secs: 1624875220 + nsecs: 361269951 + frame_id: "/world" +pose: + pose: + position: + x: -0.0420152515519 + y: 0.102702965718 + z: 0.0 + orientation: + x: 0.0 + y: 0.000143539446017 + z: 0.0 + w: 0.999999989698 + covariance: [8.444681606362355, -9.967363868672109e-09, 0.0, 0.0, -3.807250272708009e-05, 0.0, -9.967363868672097e-09, 8.444681582097752, 0.0, 0.0, 1.6902185979647448e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.80725027270801e-05, 1.6902185979647455e-06, 0.0, 0.0, 8.390504493613426, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 300 + stamp: + secs: 1624875220 + nsecs: 441265106 + frame_id: "/world" +pose: + pose: + position: + x: -0.0386827177282 + y: 0.121875523491 + z: 0.0 + orientation: + x: -0.0 + y: -0.00030935597492 + z: -0.0 + w: 0.999999952149 + covariance: [8.076866386553156, -1.0250932619542888e-08, 0.0, 0.0, -3.146828803862271e-05, 0.0, -1.0250932619542877e-08, 8.076866360724768, 0.0, 0.0, 1.3176934142929505e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.146828803862272e-05, 1.317693414292951e-06, 0.0, 0.0, 8.020519312994516, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 301 + stamp: + secs: 1624875220 + nsecs: 521287918 + frame_id: "/world" +pose: + pose: + position: + x: -0.0392771954222 + y: 0.123831444139 + z: 0.0 + orientation: + x: 0.0 + y: 3.61263976611e-05 + z: 0.0 + w: 0.999999999347 + covariance: [6.623998264349917, -6.5677722763286285e-09, 0.0, 0.0, -1.642587479189633e-05, 0.0, -6.567772276328617e-09, 6.6239982474547965, 0.0, 0.0, 1.389356026604102e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6425874791896334e-05, 1.3893560266041028e-06, 0.0, 0.0, 6.587538557310554, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 302 + stamp: + secs: 1624875220 + nsecs: 601290941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0386279976936 + y: 0.131041876207 + z: 0.0 + orientation: + x: -0.0 + y: -2.56956272488e-05 + z: -0.0 + w: 0.99999999967 + covariance: [7.392537469691228, -9.958020728081738e-09, 0.0, 0.0, -3.8946570057107736e-05, 0.0, -9.958020728081729e-09, 7.3925374432495525, 0.0, 0.0, 2.7477995639328975e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.8946570057107736e-05, 2.7477995639328983e-06, 0.0, 0.0, 7.336783314601888, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 303 + stamp: + secs: 1624875220 + nsecs: 681297064 + frame_id: "/world" +pose: + pose: + position: + x: -0.0414422589829 + y: 0.138639221643 + z: 0.0 + orientation: + x: 0.0 + y: 0.000373988484053 + z: 0.0 + w: 0.999999930066 + covariance: [7.684249052048632, -9.92226702661323e-09, 0.0, 0.0, -4.5007794470416434e-05, 0.0, -9.922267026613224e-09, 7.6842490258649505, 0.0, 0.0, 2.8190060223778648e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.5007794470416434e-05, 2.819006022377864e-05, 0.0, 0.0, 7.630183713578768, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 304 + stamp: + secs: 1624875220 + nsecs: 761307001 + frame_id: "/world" +pose: + pose: + position: + x: -0.0373052896417 + y: 0.154477511254 + z: 0.0 + orientation: + x: 0.0 + y: 5.52525407851e-05 + z: 0.0 + w: 0.999999998474 + covariance: [8.650166895199602, -1.614123769359958e-08, 0.0, 0.0, -6.811219698180215e-05, 0.0, -1.614123769359957e-08, 8.650166857197721, 0.0, 0.0, 7.063440669719506e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.811219698180218e-05, 7.063440669719505e-05, 0.0, 0.0, 8.570785952443588, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 305 + stamp: + secs: 1624875220 + nsecs: 841311932 + frame_id: "/world" +pose: + pose: + position: + x: -0.0455832535158 + y: 0.169073571052 + z: 0.0 + orientation: + x: -0.0 + y: -7.20760201515e-05 + z: -0.0 + w: 0.999999997403 + covariance: [8.172929668619382, -1.9442834853130295e-08, 0.0, 0.0, -6.163821957454615e-05, 0.0, -1.9442834853130285e-08, 8.172929627341622, 0.0, 0.0, 6.527255561400155e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.163821957454616e-05, 6.527255561400157e-05, 0.0, 0.0, 8.084878567895604, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 306 + stamp: + secs: 1624875220 + nsecs: 921345949 + frame_id: "/world" +pose: + pose: + position: + x: -0.0405702054442 + y: 0.172153590049 + z: 0.0 + orientation: + x: -0.0 + y: -0.000136196987495 + z: -0.0 + w: 0.999999990725 + covariance: [8.876226691480298, -2.2655033872161156e-08, 0.0, 0.0, -5.0760949525738725e-05, 0.0, -2.265503387216114e-08, 8.876226646114478, 0.0, 0.0, 5.451091389335607e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.076094952573874e-05, 5.451091389335609e-05, 0.0, 0.0, 8.777801002290708, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 307 + stamp: + secs: 1624875221 + nsecs: 1331091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0500341407528 + y: 0.186118424945 + z: 0.0 + orientation: + x: -0.0 + y: -4.19403585529e-05 + z: -0.0 + w: 0.999999999121 + covariance: [6.837278091313184, -1.4689432244921367e-08, 0.0, 0.0, -6.040516710851096e-05, 0.0, -1.4689432244921354e-08, 6.837278062620064, 0.0, 0.0, 2.3808039153338415e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.040516710851095e-05, 2.3808039153338425e-05, 0.0, 0.0, 6.776001203295684, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 308 + stamp: + secs: 1624875221 + nsecs: 81346989 + frame_id: "/world" +pose: + pose: + position: + x: -0.0499181508243 + y: 0.191301984421 + z: 0.0 + orientation: + x: -0.0 + y: -0.000287204368998 + z: -0.0 + w: 0.999999958757 + covariance: [6.295347389948719, -1.0531651607812786e-08, 0.0, 0.0, -5.497852019346576e-05, 0.0, -1.0531651607812775e-08, 6.295347368435783, 0.0, 0.0, 1.2673757263606595e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.4978520193465764e-05, 1.2673757263606599e-05, 0.0, 0.0, 6.2531948160421456, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 309 + stamp: + secs: 1624875221 + nsecs: 161340952 + frame_id: "/world" +pose: + pose: + position: + x: -0.0529618996259 + y: 0.196186894873 + z: 0.0 + orientation: + x: -0.0 + y: -0.000186089707945 + z: -0.0 + w: 0.999999982685 + covariance: [6.652382209711909, -1.198051344399224e-08, 0.0, 0.0, -4.813290781989206e-05, 0.0, -1.1980513443992225e-08, 6.652382183989336, 0.0, 0.0, 1.680000708657885e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.8132907819892074e-05, 1.680000708657885e-05, 0.0, 0.0, 6.60666057624035, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 310 + stamp: + secs: 1624875221 + nsecs: 241344929 + frame_id: "/world" +pose: + pose: + position: + x: -0.0528704522124 + y: 0.1963531912 + z: 0.0 + orientation: + x: -0.0 + y: -0.000229931966781 + z: -0.0 + w: 0.999999973566 + covariance: [7.382100257126343, -1.0107633865919422e-08, 0.0, 0.0, -3.430761923445087e-05, 0.0, -1.010763386591941e-08, 7.382100235074146, 0.0, 0.0, 1.4442856987379058e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.4307619234450875e-05, 1.444285698737906e-05, 0.0, 0.0, 7.344463663332957, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 311 + stamp: + secs: 1624875221 + nsecs: 321346045 + frame_id: "/world" +pose: + pose: + position: + x: -0.0533593469362 + y: 0.196500725456 + z: 0.0 + orientation: + x: -0.0 + y: -2.63955654781e-05 + z: -0.0 + w: 0.999999999652 + covariance: [7.695303181027461, -1.726936916128343e-08, 0.0, 0.0, -4.298376217503588e-05, 0.0, -1.7269369161283424e-08, 7.695303142567898, 0.0, 0.0, 1.9442973310061736e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.2983762175035885e-05, 1.9442973310061743e-05, 0.0, 0.0, 7.633119312841269, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 312 + stamp: + secs: 1624875221 + nsecs: 401350975 + frame_id: "/world" +pose: + pose: + position: + x: -0.0560189669171 + y: 0.231673164043 + z: 0.0 + orientation: + x: -0.0 + y: -0.000105766038888 + z: -0.0 + w: 0.999999994407 + covariance: [7.825738821258574, -2.3382484454757755e-08, 0.0, 0.0, -3.770039274386143e-05, 0.0, -2.3382484454757738e-08, 7.825738768825556, 0.0, 0.0, 1.841927894040702e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7700392743861414e-05, 1.8419278940407032e-05, 0.0, 0.0, 7.742514152839822, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 313 + stamp: + secs: 1624875221 + nsecs: 481375933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0566678028165 + y: 0.232508391451 + z: 0.0 + orientation: + x: 0.0 + y: 7.64249724257e-05 + z: 0.0 + w: 0.99999999708 + covariance: [7.147004456028989, -1.5061677602219725e-08, 0.0, 0.0, -1.7693586672381528e-05, 0.0, -1.5061677602219712e-08, 7.147004422186928, 0.0, 0.0, 9.59770172978949e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7693586672381514e-05, 9.597701729789495e-06, 0.0, 0.0, 7.093333154081446, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 314 + stamp: + secs: 1624875221 + nsecs: 561382055 + frame_id: "/world" +pose: + pose: + position: + x: -0.0701676750194 + y: 0.249377442953 + z: 0.0 + orientation: + x: 0.0 + y: 0.000458451283096 + z: 0.0 + w: 0.999999894911 + covariance: [9.40326995384146, -3.292029368329633e-08, 0.0, 0.0, -2.1176431612586124e-05, 0.0, -3.29202936832963e-08, 9.403269879784174, 0.0, 0.0, 1.3740445097146178e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.117643161258612e-05, 1.3740445097146183e-05, 0.0, 0.0, 9.286026718342473, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 315 + stamp: + secs: 1624875221 + nsecs: 641385078 + frame_id: "/world" +pose: + pose: + position: + x: -0.0705068605907 + y: 0.249980716333 + z: 0.0 + orientation: + x: 0.0 + y: 0.000293310785133 + z: 0.0 + w: 0.999999956984 + covariance: [7.632448783993459, -2.3666454482574033e-08, 0.0, 0.0, -1.0942832580317297e-05, 0.0, -2.366645448257401e-08, 7.632448730854225, 0.0, 0.0, 1.7022433590610865e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0942832580317299e-05, 1.7022433590610882e-05, 0.0, 0.0, 7.547443882691174, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 316 + stamp: + secs: 1624875221 + nsecs: 721393108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0706053455488 + y: 0.25080279629 + z: 0.0 + orientation: + x: 0.0 + y: 0.000404046725976 + z: 0.0 + w: 0.999999918373 + covariance: [8.669472644573155, -3.779404832558173e-08, 0.0, 0.0, -2.213674850989822e-05, 0.0, -3.7794048325581693e-08, 8.669472560468591, 0.0, 0.0, 3.300031701119948e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.213674850989822e-05, 3.300031701119949e-05, 0.0, 0.0, 8.532626873262812, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 317 + stamp: + secs: 1624875221 + nsecs: 801403046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0712796480039 + y: 0.251550515907 + z: 0.0 + orientation: + x: 0.0 + y: 0.000218163124004 + z: 0.0 + w: 0.999999976202 + covariance: [9.124203499853934, -4.032077708106989e-08, 0.0, 0.0, -2.6019624311797317e-05, 0.0, -4.032077708106986e-08, 9.124203410627832, 0.0, 0.0, 2.8094152066056154e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6019624311797317e-05, 2.809415206605617e-05, 0.0, 0.0, 8.977156517061232, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 318 + stamp: + secs: 1624875221 + nsecs: 881417036 + frame_id: "/world" +pose: + pose: + position: + x: -0.0716627308374 + y: 0.252220441949 + z: 0.0 + orientation: + x: 0.0 + y: 0.000575995011715 + z: 0.0 + w: 0.999999834115 + covariance: [9.320979289029747, -4.085185402064913e-08, 0.0, 0.0, -2.6688982727458233e-05, 0.0, -4.08518540206491e-08, 9.320979198973179, 0.0, 0.0, 2.0781558009068097e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.668898272745822e-05, 2.0781558009068097e-05, 0.0, 0.0, 9.171325414587248, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 319 + stamp: + secs: 1624875221 + nsecs: 961416006 + frame_id: "/world" +pose: + pose: + position: + x: -0.0741272547439 + y: 0.260007364409 + z: 0.0 + orientation: + x: 0.0 + y: 0.000502093568935 + z: 0.0 + w: 0.999999873951 + covariance: [7.605938570201902, -2.4410787895315712e-08, 0.0, 0.0, -1.5458357850416515e-05, 0.0, -2.44107878953157e-08, 7.605938516544692, 0.0, 0.0, 1.2013740223274786e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.54583578504165e-05, 1.2013740223274786e-05, 0.0, 0.0, 7.515572972427861, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 320 + stamp: + secs: 1624875222 + nsecs: 41435957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0744895320578 + y: 0.260706318685 + z: 0.0 + orientation: + x: 0.0 + y: 0.00094011842317 + z: 0.0 + w: 0.999999558089 + covariance: [8.644542766133092, -4.218829134512626e-08, 0.0, 0.0, -0.00035438970629395045, 0.0, -4.218829134512624e-08, 8.644542665709478, 0.0, 0.0, 0.00010375213230725927, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00035438970629395034, 0.0001037521323072593, 0.0, 0.0, 8.519318948187347, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 321 + stamp: + secs: 1624875222 + nsecs: 121417046 + frame_id: "/world" +pose: + pose: + position: + x: -0.073728448516 + y: 0.262463043371 + z: 0.0 + orientation: + x: 0.0 + y: 0.000850540625029 + z: 0.0 + w: 0.99999963829 + covariance: [9.09027759180096, -6.255757708703855e-08, 0.0, 0.0, -0.0004861411611669955, 0.0, -6.25575770870385e-08, 9.090277420476971, 0.0, 0.0, 0.00013646891887184132, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004861411611669956, 0.00013646891887184132, 0.0, 0.0, 8.972100546676437, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 322 + stamp: + secs: 1624875222 + nsecs: 201446056 + frame_id: "/world" +pose: + pose: + position: + x: -0.0753196083749 + y: 0.267610544528 + z: 0.0 + orientation: + x: 0.0 + y: 0.000393453177922 + z: 0.0 + w: 0.999999922597 + covariance: [6.895115540837979, -5.360725015969211e-08, 0.0, 0.0, -0.00026284689757416713, 0.0, -5.360725015969213e-08, 6.8951153836708645, 0.0, 0.0, 7.340802493458177e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002628468975741673, 7.340802493458174e-05, 0.0, 0.0, 6.823016386639036, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 323 + stamp: + secs: 1624875222 + nsecs: 281445026 + frame_id: "/world" +pose: + pose: + position: + x: -0.0734415834068 + y: 0.29507902126 + z: 0.0 + orientation: + x: 0.0 + y: 0.000593674156915 + z: 0.0 + w: 0.999999823775 + covariance: [7.534053966213511, -9.971996965262691e-08, 0.0, 0.0, -0.00024255588304807237, 0.0, -9.971996965262695e-08, 7.534053662589303, 0.0, 0.0, 6.731662594109268e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002425558830480725, 6.731662594109269e-05, 0.0, 0.0, 7.430677072130171, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 324 + stamp: + secs: 1624875222 + nsecs: 361438990 + frame_id: "/world" +pose: + pose: + position: + x: -0.0734114036026 + y: 0.29502116852 + z: 0.0 + orientation: + x: 0.0 + y: 0.000126201824366 + z: 0.0 + w: 0.999999992037 + covariance: [7.7002047367825135, -3.2072169031116545e-08, 0.0, 0.0, -8.4765710421603e-05, 0.0, -3.207216903111656e-08, 7.700204639426623, 0.0, 0.0, 2.352651037624412e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.476571042160307e-05, 2.3526510376244127e-05, 0.0, 0.0, 7.6659920243540025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 325 + stamp: + secs: 1624875222 + nsecs: 441457987 + frame_id: "/world" +pose: + pose: + position: + x: -0.0678767945351 + y: 0.303147965681 + z: 0.0 + orientation: + x: 0.0 + y: 0.00031941876975 + z: 0.0 + w: 0.999999948986 + covariance: [8.636056714967788, -5.198217792309791e-08, 0.0, 0.0, -8.63692644685609e-05, 0.0, -5.1982177923097924e-08, 8.636056554528057, 0.0, 0.0, 2.1734303233260745e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.636926446856094e-05, 2.173430323326075e-05, 0.0, 0.0, 8.587403754695718, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 326 + stamp: + secs: 1624875222 + nsecs: 521461964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0621833640211 + y: 0.336508937675 + z: 0.0 + orientation: + x: 0.0 + y: 0.000635639197206 + z: 0.0 + w: 0.999999797981 + covariance: [9.070598577447228, -7.931084137326388e-08, 0.0, 0.0, -8.678226849145769e-05, 0.0, -7.931084137326391e-08, 9.070598330232837, 0.0, 0.0, 1.9335342194866675e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.678226849145773e-05, 1.9335342194866678e-05, 0.0, 0.0, 9.001373717743919, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 327 + stamp: + secs: 1624875222 + nsecs: 601474047 + frame_id: "/world" +pose: + pose: + position: + x: -0.0623837086329 + y: 0.337075790511 + z: 0.0 + orientation: + x: 0.0 + y: 0.000451844646753 + z: 0.0 + w: 0.999999897918 + covariance: [8.302296091171941, -7.775308406371298e-08, 0.0, 0.0, -5.8226418642442735e-05, 0.0, -7.7753084063713e-08, 8.302295846732251, 0.0, 0.0, -9.216887637069781e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.822641864244275e-05, -9.21688763706978e-06, 0.0, 0.0, 8.236104343427451, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 328 + stamp: + secs: 1624875222 + nsecs: 681476116 + frame_id: "/world" +pose: + pose: + position: + x: -0.0625364672207 + y: 0.33751475565 + z: 0.0 + orientation: + x: 0.0 + y: 0.000429052156149 + z: 0.0 + w: 0.999999907957 + covariance: [8.909126762059172, -7.482994955748239e-08, 0.0, 0.0, -4.204084262683748e-05, 0.0, -7.482994955748243e-08, 8.909126525466055, 0.0, 0.0, -2.0212838297227278e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.204084262683748e-05, -2.0212838297227278e-05, 0.0, 0.0, 8.845828868368212, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 329 + stamp: + secs: 1624875222 + nsecs: 761483908 + frame_id: "/world" +pose: + pose: + position: + x: -0.0627955663497 + y: 0.338256560486 + z: 0.0 + orientation: + x: 0.0 + y: 0.000465092037453 + z: 0.0 + w: 0.999999891845 + covariance: [9.204780727098646, -1.0711861295766547e-07, 0.0, 0.0, -3.619643051856173e-05, 0.0, -1.0711861295766551e-07, 9.204780386750997, 0.0, 0.0, -2.318316722172198e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.619643051856173e-05, -2.3183167221721975e-05, 0.0, 0.0, 9.113979317800156, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 330 + stamp: + secs: 1624875222 + nsecs: 841490030 + frame_id: "/world" +pose: + pose: + position: + x: -0.0651227037236 + y: 0.320830954365 + z: 0.0 + orientation: + x: 0.0 + y: 0.000313570598697 + z: 0.0 + w: 0.999999950837 + covariance: [8.361413088388046, -1.0125985508007369e-07, 0.0, 0.0, -5.500657122677622e-05, 0.0, -1.0125985508007372e-07, 8.361412764955512, 0.0, 0.0, -1.5152068014055596e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.5006571226776216e-05, -1.5152068014055595e-05, 0.0, 0.0, 8.274720290646979, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 331 + stamp: + secs: 1624875222 + nsecs: 921515942 + frame_id: "/world" +pose: + pose: + position: + x: -0.0646208761191 + y: 0.32416092022 + z: 0.0 + orientation: + x: 0.0 + y: 0.000317717078799 + z: 0.0 + w: 0.999999949528 + covariance: [8.966726082088062, -1.1996895793346052e-07, 0.0, 0.0, -8.433926386177825e-05, 0.0, -1.1996895793346055e-07, 8.966725695543238, 0.0, 0.0, -9.316358500895577e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.433926386177823e-05, -9.316358500895577e-06, 0.0, 0.0, 8.862964883489235, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 332 + stamp: + secs: 1624875223 + nsecs: 1523018 + frame_id: "/world" +pose: + pose: + position: + x: -0.0658835556938 + y: 0.337906631567 + z: 0.0 + orientation: + x: 0.0 + y: 0.000587621290282 + z: 0.0 + w: 0.999999827351 + covariance: [9.229032920166272, -1.232099331002996e-07, 0.0, 0.0, -8.19285831594335e-05, 0.0, -1.2320993310029963e-07, 9.229032520551815, 0.0, 0.0, -4.058841957947574e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.192858315943351e-05, -4.058841957947574e-06, 0.0, 0.0, 9.121672774134353, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 333 + stamp: + secs: 1624875223 + nsecs: 81501961 + frame_id: "/world" +pose: + pose: + position: + x: -0.069203341017 + y: 0.34006367145 + z: 0.0 + orientation: + x: 0.0 + y: 0.000813057020261 + z: 0.0 + w: 0.999999669469 + covariance: [12.157248010754182, -1.7558846239713752e-07, 0.0, 0.0, -0.00010673123921938789, 0.0, -1.7558846239713757e-07, 12.157247438527804, 0.0, 0.0, -2.34114332405368e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010673123921938793, -2.34114332405368e-06, 0.0, 0.0, 12.003279449536482, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 334 + stamp: + secs: 1624875223 + nsecs: 161525011 + frame_id: "/world" +pose: + pose: + position: + x: -0.0693657006604 + y: 0.340799368792 + z: 0.0 + orientation: + x: 0.0 + y: 0.000722942604006 + z: 0.0 + w: 0.999999738677 + covariance: [12.093213434935516, -1.8206623153716278e-07, 0.0, 0.0, -0.00011073141573720632, 0.0, -1.8206623153716283e-07, 12.093212838747968, 0.0, 0.0, -1.895119625532828e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011073141573720637, -1.8951196255328243e-07, 0.0, 0.0, 11.932131934464108, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 335 + stamp: + secs: 1624875223 + nsecs: 241575956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0697381382869 + y: 0.34195934196 + z: 0.0 + orientation: + x: 0.0 + y: 0.000631559302689 + z: 0.0 + w: 0.999999800566 + covariance: [14.280000533580713, -3.406816818038498e-07, 0.0, 0.0, -0.00023024376023090894, 0.0, -3.4068168180384984e-07, 14.279999411712417, 0.0, 0.0, 3.2199412355009094e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00023024376023090897, 3.219941235500909e-05, 0.0, 0.0, 13.973858167995894, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 336 + stamp: + secs: 1624875223 + nsecs: 321557045 + frame_id: "/world" +pose: + pose: + position: + x: -0.0701717253844 + y: 0.343023846795 + z: 0.0 + orientation: + x: 0.0 + y: 0.000356152247158 + z: 0.0 + w: 0.999999936578 + covariance: [15.76460857596211, -4.727094483077425e-07, 0.0, 0.0, -0.00030937324363199695, 0.0, -4.727094483077426e-07, 15.764607016955637, 0.0, 0.0, 7.163994069681146e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000309373243631997, 7.163994069681149e-05, 0.0, 0.0, 15.33683048198695, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 337 + stamp: + secs: 1624875223 + nsecs: 401560068 + frame_id: "/world" +pose: + pose: + position: + x: -0.0704692251728 + y: 0.360169756152 + z: 0.0 + orientation: + x: 0.0 + y: 0.00064712384885 + z: 0.0 + w: 0.999999790615 + covariance: [13.854876294733163, -4.050278640898932e-07, 0.0, 0.0, -0.00022914265546270928, 0.0, -4.050278640898932e-07, 13.854874958395841, 0.0, 0.0, 6.314003060075603e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00022914265546270928, 6.314003060075604e-05, 0.0, 0.0, 13.483065814507711, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 338 + stamp: + secs: 1624875223 + nsecs: 481564999 + frame_id: "/world" +pose: + pose: + position: + x: -0.0705699397618 + y: 0.362710170686 + z: 0.0 + orientation: + x: 0.0 + y: 0.000621131358231 + z: 0.0 + w: 0.999999807098 + covariance: [12.967204210861714, -3.49083889458625e-07, 0.0, 0.0, -0.0001626999543611177, 0.0, -3.490838894586251e-07, 12.96720305912684, 0.0, 0.0, 5.136585646895393e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00016269995436111768, 5.136585646895394e-05, 0.0, 0.0, 12.641905057413314, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 339 + stamp: + secs: 1624875223 + nsecs: 561572075 + frame_id: "/world" +pose: + pose: + position: + x: -0.068787866807 + y: 0.372067101979 + z: 0.0 + orientation: + x: 0.0 + y: 0.000470538980241 + z: 0.0 + w: 0.999999889297 + covariance: [12.533945560971482, -3.0826397614108764e-07, 0.0, 0.0, -0.00011467459842231617, 0.0, -3.0826397614108785e-07, 12.533944544110868, 0.0, 0.0, 4.066191340006121e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011467459842231616, 4.066191340006121e-05, 0.0, 0.0, 12.242795787264717, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 340 + stamp: + secs: 1624875223 + nsecs: 641552925 + frame_id: "/world" +pose: + pose: + position: + x: -0.0694551623308 + y: 0.380344444866 + z: 0.0 + orientation: + x: -0.0 + y: -4.31660451494e-06 + z: -0.0 + w: 0.999999999991 + covariance: [10.672652883360165, -2.0899801332322308e-07, 0.0, 0.0, -5.905030165743652e-05, 0.0, -2.0899801332322324e-07, 10.672652194069105, 0.0, 0.0, 2.313907842850232e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.905030165743651e-05, 2.3139078428502313e-05, 0.0, 0.0, 10.472877705886955, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 341 + stamp: + secs: 1624875223 + nsecs: 721590996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0697592506779 + y: 0.3821145809 + z: 0.0 + orientation: + x: 0.0 + y: 0.000446846969259 + z: 0.0 + w: 0.999999900164 + covariance: [9.963280810372211, -1.762266148385074e-07, 0.0, 0.0, -3.962079761236776e-05, 0.0, -1.7622661483850754e-07, 9.96328022937786, 0.0, 0.0, 1.8463638106541703e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.962079761236775e-05, 1.84636381065417e-05, 0.0, 0.0, 9.791638843563495, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 342 + stamp: + secs: 1624875223 + nsecs: 801577091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0699514428173 + y: 0.382744144491 + z: 0.0 + orientation: + x: -0.0 + y: -7.08845780061e-05 + z: -0.0 + w: 0.999999997488 + covariance: [8.614075062602394, -1.121599225656727e-07, 0.0, 0.0, -2.2852860885762814e-05, 0.0, -1.121599225656728e-07, 8.614074692986938, 0.0, 0.0, 1.2401014936802568e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2852860885762817e-05, 1.240101493680257e-05, 0.0, 0.0, 8.502789714436894, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 343 + stamp: + secs: 1624875223 + nsecs: 881587982 + frame_id: "/world" +pose: + pose: + position: + x: -0.0700948848444 + y: 0.382116359299 + z: 0.0 + orientation: + x: -0.0 + y: -0.000121981069032 + z: -0.0 + w: 0.99999999256 + covariance: [10.212136106804353, -1.1695983749492324e-07, 0.0, 0.0, -1.7114160958084713e-05, 0.0, -1.1695983749492334e-07, 10.212135721535725, 0.0, 0.0, 1.2451135503490522e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7114160958084713e-05, 1.2451135503490518e-05, 0.0, 0.0, 10.095079544308506, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 344 + stamp: + secs: 1624875223 + nsecs: 961596966 + frame_id: "/world" +pose: + pose: + position: + x: -0.0686621355694 + y: 0.37761580648 + z: 0.0 + orientation: + x: 0.0 + y: 0.000143443260228 + z: 0.0 + w: 0.999999989712 + covariance: [8.69938758283204, -9.52651771473012e-08, 0.0, 0.0, -7.127289439481347e-06, 0.0, -9.526517714730122e-08, 8.69938726916294, 0.0, 0.0, 8.12198392334184e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.127289439481345e-06, 8.12198392334184e-06, 0.0, 0.0, 8.602372198026739, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 345 + stamp: + secs: 1624875224 + nsecs: 41613102 + frame_id: "/world" +pose: + pose: + position: + x: -0.0725725092447 + y: 0.392341001892 + z: 0.0 + orientation: + x: -0.0 + y: -0.000254005470476 + z: -0.0 + w: 0.999999967741 + covariance: [11.837529307277148, -2.0577091937207197e-07, 0.0, 0.0, -2.631822022711801e-05, 0.0, -2.057709193720721e-07, 11.837528631998463, 0.0, 0.0, 8.647397170279372e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6318220227118007e-05, 8.647397170279372e-05, 0.0, 0.0, 11.625755524119516, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 346 + stamp: + secs: 1624875224 + nsecs: 121651888 + frame_id: "/world" +pose: + pose: + position: + x: -0.0726942620622 + y: 0.392858842387 + z: 0.0 + orientation: + x: -0.0 + y: -0.000122817541876 + z: -0.0 + w: 0.999999992458 + covariance: [11.919719292399972, -1.6906101267684928e-07, 0.0, 0.0, -2.8819327098212878e-05, 0.0, -1.6906101267684944e-07, 11.919718740364443, 0.0, 0.0, 9.684947510231869e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.881932709821287e-05, 9.684947510231865e-05, 0.0, 0.0, 11.744279707719501, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 347 + stamp: + secs: 1624875224 + nsecs: 201616049 + frame_id: "/world" +pose: + pose: + position: + x: -0.0729600217053 + y: 0.393852434867 + z: 0.0 + orientation: + x: 0.0 + y: 0.000230666891688 + z: 0.0 + w: 0.999999973396 + covariance: [12.032248808643285, -2.181652894311769e-07, 0.0, 0.0, -3.7116170078336047e-05, 0.0, -2.1816528943117713e-07, 12.032248104056672, 0.0, 0.0, 0.0001294811160826817, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.711617007833606e-05, 0.0001294811160826817, 0.0, 0.0, 11.803380448987108, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 348 + stamp: + secs: 1624875224 + nsecs: 281644106 + frame_id: "/world" +pose: + pose: + position: + x: -0.0983638863424 + y: 0.425511903584 + z: 0.0 + orientation: + x: 0.0 + y: 0.000127631664554 + z: 0.0 + w: 0.999999991855 + covariance: [9.278861892401041, -1.3810404388755638e-07, 0.0, 0.0, -1.7414469365337476e-05, 0.0, -1.381040438875565e-07, 9.278861449377686, 0.0, 0.0, 7.078120159241848e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.741446936533748e-05, 7.078120159241846e-05, 0.0, 0.0, 9.131441625410883, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 349 + stamp: + secs: 1624875224 + nsecs: 361641884 + frame_id: "/world" +pose: + pose: + position: + x: -0.114701411821 + y: 0.412892094524 + z: 0.0 + orientation: + x: -0.0 + y: -2.79064920339e-05 + z: -0.0 + w: 0.999999999611 + covariance: [10.569562727004243, -1.1855939959797986e-07, 0.0, 0.0, -3.029122129510336e-05, 0.0, -1.1855939959797997e-07, 10.569562348377358, 0.0, 0.0, 5.341100264562707e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0291221295103358e-05, 5.341100264562704e-05, 0.0, 0.0, 10.442302538443398, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 350 + stamp: + secs: 1624875224 + nsecs: 441647053 + frame_id: "/world" +pose: + pose: + position: + x: -0.1285625064 + y: 0.426571812637 + z: 0.0 + orientation: + x: -0.0 + y: -0.00023764640296 + z: -0.0 + w: 0.999999971762 + covariance: [11.318156544880997, -1.6362578260493432e-07, 0.0, 0.0, -7.718662600196792e-05, 0.0, -1.6362578260493446e-07, 11.318156026426857, 0.0, 0.0, 5.207460781633408e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.718662600196793e-05, 5.207460781633405e-05, 0.0, 0.0, 11.14117313337056, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 351 + stamp: + secs: 1624875224 + nsecs: 521675110 + frame_id: "/world" +pose: + pose: + position: + x: -0.131194836122 + y: 0.442669384252 + z: 0.0 + orientation: + x: -0.0 + y: -2.17147421976e-05 + z: -0.0 + w: 0.999999999764 + covariance: [10.157720115807134, -9.423300018594445e-08, 0.0, 0.0, -4.325519143880495e-05, 0.0, -9.423300018594452e-08, 10.157719817735416, 0.0, 0.0, 2.5521526889059653e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.3255191438804965e-05, 2.552152688905965e-05, 0.0, 0.0, 10.054975559029144, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 352 + stamp: + secs: 1624875224 + nsecs: 601653099 + frame_id: "/world" +pose: + pose: + position: + x: -0.133730208068 + y: 0.465857526376 + z: 0.0 + orientation: + x: -0.0 + y: -0.000161431709407 + z: -0.0 + w: 0.99999998697 + covariance: [9.740738678569233, -1.0707704913033173e-07, 0.0, 0.0, -3.748400169940448e-05, 0.0, -1.0707704913033179e-07, 9.740738341035321, 0.0, 0.0, 1.848821102694957e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7484001699404495e-05, 1.8488211026949563e-05, 0.0, 0.0, 9.6224985210987, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 353 + stamp: + secs: 1624875224 + nsecs: 681679964 + frame_id: "/world" +pose: + pose: + position: + x: -0.142604821067 + y: 0.499071612825 + z: 0.0 + orientation: + x: 0.0 + y: 0.000183186326505 + z: 0.0 + w: 0.999999983221 + covariance: [9.553909834272904, -9.843639901154941e-08, 0.0, 0.0, -2.5689686397782692e-05, 0.0, -9.84363990115495e-08, 9.55390952460988, 0.0, 0.0, 1.81460683067981e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5689686397782702e-05, 1.8146068306798096e-05, 0.0, 0.0, 9.444147274282557, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 354 + stamp: + secs: 1624875224 + nsecs: 761667967 + frame_id: "/world" +pose: + pose: + position: + x: -0.144381117273 + y: 0.529757328938 + z: 0.0 + orientation: + x: -0.0 + y: -1.28946384761e-05 + z: -0.0 + w: 0.999999999917 + covariance: [8.471908279868877, -7.509595130723793e-08, 0.0, 0.0, -1.454712573912018e-05, 0.0, -7.5095951307238e-08, 8.471908044003355, 0.0, 0.0, 1.1212345283605675e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.454712573912019e-05, 1.121234528360567e-05, 0.0, 0.0, 8.386995271024034, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 355 + stamp: + secs: 1624875224 + nsecs: 841675043 + frame_id: "/world" +pose: + pose: + position: + x: -0.160874007077 + y: 0.547938912535 + z: 0.0 + orientation: + x: 0.0 + y: 6.47192376774e-07 + z: 0.0 + w: 1.0 + covariance: [10.161570372295873, -1.1480585178628265e-07, 0.0, 0.0, -1.5162914631454634e-05, 0.0, -1.1480585178628274e-07, 10.161570012110717, 0.0, 0.0, 9.687245826512182e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5162914631454643e-05, 9.687245826512179e-06, 0.0, 0.0, 10.030163434674373, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 356 + stamp: + secs: 1624875224 + nsecs: 921693087 + frame_id: "/world" +pose: + pose: + position: + x: -0.161143634263 + y: 0.556924835669 + z: 0.0 + orientation: + x: -0.0 + y: -2.89179919235e-06 + z: -0.0 + w: 0.999999999996 + covariance: [11.080594515691619, -1.343597891202892e-07, 0.0, 0.0, -1.363306255479795e-05, 0.0, -1.343597891202893e-07, 11.080594094437158, 0.0, 0.0, 9.868533320030194e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3633062554797957e-05, 9.868533320030194e-06, 0.0, 0.0, 10.925027509615884, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 357 + stamp: + secs: 1624875225 + nsecs: 1673937 + frame_id: "/world" +pose: + pose: + position: + x: -0.161950279257 + y: 0.552020811867 + z: 0.0 + orientation: + x: -0.0 + y: -1.32209872708e-05 + z: -0.0 + w: 0.999999999913 + covariance: [11.582470001269169, -1.6380472716747376e-07, 0.0, 0.0, -1.9671855853085452e-05, 0.0, -1.6380472716747386e-07, 11.582469487991977, 0.0, 0.0, 1.2588569852036307e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.967185585308546e-05, 1.2588569852036307e-05, 0.0, 0.0, 11.390271295181835, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 358 + stamp: + secs: 1624875225 + nsecs: 81682920 + frame_id: "/world" +pose: + pose: + position: + x: -0.162897562736 + y: 0.554161113985 + z: 0.0 + orientation: + x: 0.0 + y: 0.000220170646348 + z: 0.0 + w: 0.999999975762 + covariance: [11.869575903055772, -2.041998786732822e-07, 0.0, 0.0, -5.108365763462048e-05, 0.0, -2.0419987867328235e-07, 11.86957526341433, 0.0, 0.0, 1.5425154128382768e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.108365763462048e-05, 1.542515412838277e-05, 0.0, 0.0, 11.6265164103252, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 359 + stamp: + secs: 1624875225 + nsecs: 161689043 + frame_id: "/world" +pose: + pose: + position: + x: -0.177711297241 + y: 0.574386563885 + z: 0.0 + orientation: + x: 0.0 + y: 0.000552647289358 + z: 0.0 + w: 0.99999984729 + covariance: [10.472466484688828, -1.902887938046337e-07, 0.0, 0.0, -8.115756972827287e-05, 0.0, -1.9028879380463385e-07, 10.472465888065429, 0.0, 0.0, 1.2353087221543493e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.115756972827284e-05, 1.2353087221543495e-05, 0.0, 0.0, 10.241694015015582, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 360 + stamp: + secs: 1624875225 + nsecs: 241738081 + frame_id: "/world" +pose: + pose: + position: + x: -0.180267181783 + y: 0.576128156744 + z: 0.0 + orientation: + x: 0.0 + y: 0.000208904227602 + z: 0.0 + w: 0.99999997818 + covariance: [7.928410979047397, -9.895478466629902e-08, 0.0, 0.0, -4.6469391095149245e-05, 0.0, -9.895478466629909e-08, 7.928410668330679, 0.0, 0.0, 4.833938508812419e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.646939109514923e-05, 4.833938508812422e-06, 0.0, 0.0, 7.805878311938307, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 361 + stamp: + secs: 1624875225 + nsecs: 321708918 + frame_id: "/world" +pose: + pose: + position: + x: -0.189360453649 + y: 0.58314547212 + z: 0.0 + orientation: + x: -0.0 + y: -9.16137666315e-06 + z: -0.0 + w: 0.999999999958 + covariance: [7.930556862594567, -9.789072318594848e-08, 0.0, 0.0, -3.7780772698384546e-05, 0.0, -9.789072318594851e-08, 7.930556554572722, 0.0, 0.0, 3.162167871378955e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.778077269838456e-05, 3.1621678713789566e-06, 0.0, 0.0, 7.807071637030449, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 362 + stamp: + secs: 1624875225 + nsecs: 401716948 + frame_id: "/world" +pose: + pose: + position: + x: -0.18468946814 + y: 0.56495545159 + z: 0.0 + orientation: + x: -0.0 + y: -1.80402428233e-05 + z: -0.0 + w: 0.999999999837 + covariance: [7.200568251844677, -6.805492426616184e-08, 0.0, 0.0, -2.0753108702676678e-05, 0.0, -6.805492426616181e-08, 7.200568037417854, 0.0, 0.0, 1.755557067332549e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0753108702676688e-05, 1.7555570673325494e-06, 0.0, 0.0, 7.113284739346214, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 363 + stamp: + secs: 1624875225 + nsecs: 481729984 + frame_id: "/world" +pose: + pose: + position: + x: -0.185906801087 + y: 0.564100881368 + z: 0.0 + orientation: + x: -0.0 + y: -9.39911019981e-05 + z: -0.0 + w: 0.999999995583 + covariance: [6.9557298201287105, -4.7533123801963326e-08, 0.0, 0.0, -1.4908134027288337e-05, 0.0, -4.75331238019633e-08, 6.955729670213413, 0.0, 0.0, 2.678204183174966e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.490813402728834e-05, 2.6782041831749666e-06, 0.0, 0.0, 6.893983458533737, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 364 + stamp: + secs: 1624875225 + nsecs: 561794043 + frame_id: "/world" +pose: + pose: + position: + x: -0.190268248354 + y: 0.565948529528 + z: 0.0 + orientation: + x: -0.0 + y: -0.000241187370367 + z: -0.0 + w: 0.999999970914 + covariance: [7.524969679637956, -5.057104967667379e-08, 0.0, 0.0, -1.8298720107927982e-05, 0.0, -5.057104967667379e-08, 7.5249695200217905, 0.0, 0.0, 5.37323231982993e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8298720107927982e-05, 5.373232319829931e-06, 0.0, 0.0, 7.458508376205469, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 365 + stamp: + secs: 1624875225 + nsecs: 641766071 + frame_id: "/world" +pose: + pose: + position: + x: -0.198183678603 + y: 0.574181840895 + z: 0.0 + orientation: + x: -0.0 + y: -0.000393421950723 + z: -0.0 + w: 0.99999992261 + covariance: [8.573615207581721, -6.184656379969588e-08, 0.0, 0.0, -2.1336455382742915e-05, 0.0, -6.184656379969586e-08, 8.573615012325165, 0.0, 0.0, 7.412962511879811e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1336455382742905e-05, 7.41296251187981e-06, 0.0, 0.0, 8.491519850374287, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 0 + stamp: + secs: 1624875191 + nsecs: 558718920 + frame_id: "/world" +pose: + pose: + position: + x: 0.0571035703916 + y: 0.00315463918406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0381997500533 + z: 0.0 + w: 0.999270123188 + covariance: [0.07868257137427125, -3.1287751511516264e-18, 0.0, 0.0, -9.23949708639102e-14, 0.0, -3.1287751511525547e-18, 0.07868257137427095, 0.0, 0.0, -1.2423631706158793e-11, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.239497086387514e-14, -1.2423631706166987e-11, 0.0, 0.0, 0.07868240390552253, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 1 + stamp: + secs: 1624875193 + nsecs: 478880882 + frame_id: "/world" +pose: + pose: + position: + x: 0.0507313510621 + y: -0.00158473660589 + z: 0.0 + orientation: + x: 0.0 + y: 0.169155284695 + z: 0.0 + w: 0.985589412311 + covariance: [6.211025765057356, -2.98303555690518e-08, 0.0, 0.0, -0.0005081579330980388, 0.0, -2.983035556905237e-08, 6.211025382718034, 0.0, 0.0, 3.940367165693649e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000508157933098038, 3.940367165693634e-05, 0.0, 0.0, 3.5886318727646787, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 2 + stamp: + secs: 1624875196 + nsecs: 999176979 + frame_id: "/world" +pose: + pose: + position: + x: 0.0451611590508 + y: -0.0179105370755 + z: 0.0 + orientation: + x: -0.0 + y: -0.106772826692 + z: -0.0 + w: 0.994283442224 + covariance: [5.0096076248019, -5.15621462510236e-07, 0.0, 0.0, -0.00016191940413651335, 0.0, -5.156214625102602e-07, 5.009609915240668, 0.0, 0.0, 0.001596169422619378, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00016191940413651403, 0.0015961694226193882, 0.0, 0.0, 3.3309570652863276, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 3 + stamp: + secs: 1624875197 + nsecs: 79178095 + frame_id: "/world" +pose: + pose: + position: + x: 0.0377764644393 + y: -0.00306683378159 + z: 0.0 + orientation: + x: 0.0 + y: 0.0477493565262 + z: 0.0 + w: 0.998859348933 + covariance: [2.5081276200271065, -3.22240183167372e-08, 0.0, 0.0, -8.619376352622657e-06, 0.0, -3.2224018316738664e-08, 2.5081279015124385, 0.0, 0.0, 0.00015168363099147067, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.619376352622693e-06, 0.00015168363099147156, 0.0, 0.0, 2.3807376271643834, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 4 + stamp: + secs: 1624875197 + nsecs: 159200907 + frame_id: "/world" +pose: + pose: + position: + x: -0.0131433378256 + y: 0.00821948006192 + z: 0.0 + orientation: + x: -0.0 + y: -0.0180341339558 + z: -0.0 + w: 0.999837371782 + covariance: [2.309908363639027, -2.5193417265201506e-09, 0.0, 0.0, -1.3347482328264928e-06, 0.0, -2.5193417265202883e-09, 2.309908427964202, 0.0, 0.0, 2.1799391351520684e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3347482328264964e-06, 2.179939135152077e-05, 0.0, 0.0, 2.2962705407554513, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 5 + stamp: + secs: 1624875197 + nsecs: 239206076 + frame_id: "/world" +pose: + pose: + position: + x: 0.000543487927512 + y: 0.0350821055088 + z: 0.0 + orientation: + x: 0.0 + y: 0.00454061773021 + z: 0.0 + w: 0.999989691342 + covariance: [2.291761457426893, 4.8219680025796345e-09, 0.0, 0.0, -1.94605367740911e-06, 0.0, 4.821968002579627e-09, 2.291761562228787, 0.0, 0.0, 9.542539842601038e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9460536774091117e-06, 9.542539842601044e-06, 0.0, 0.0, 2.288012008504549, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 6 + stamp: + secs: 1624875197 + nsecs: 319202900 + frame_id: "/world" +pose: + pose: + position: + x: 0.00424691416398 + y: 0.0387103438511 + z: 0.0 + orientation: + x: -0.0 + y: -0.00136584271109 + z: -0.0 + w: 0.999999067236 + covariance: [2.4280312608077583, 5.522600448733182e-09, 0.0, 0.0, -8.314918566420752e-07, 0.0, 5.522600448733187e-09, 2.4280313495129335, 0.0, 0.0, 3.421093060758751e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.314918566420768e-07, 3.4210930607587543e-06, 0.0, 0.0, 2.425943938710083, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 7 + stamp: + secs: 1624875197 + nsecs: 399205923 + frame_id: "/world" +pose: + pose: + position: + x: 0.0127919068701 + y: 0.010687868281 + z: 0.0 + orientation: + x: 0.0 + y: 0.000341635110456 + z: 0.0 + w: 0.999999941643 + covariance: [2.0640181003276816, 2.092001718790631e-09, 0.0, 0.0, -1.5428550176788322e-07, 0.0, 2.092001718790636e-09, 2.064018131511998, 0.0, 0.0, 6.26587794232119e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5428550176788338e-07, 6.265877942321197e-07, 0.0, 0.0, 2.063300495209764, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 8 + stamp: + secs: 1624875197 + nsecs: 479234934 + frame_id: "/world" +pose: + pose: + position: + x: 0.0125905766811 + y: 0.0106400234545 + z: 0.0 + orientation: + x: 0.0 + y: 0.000363135692089 + z: 0.0 + w: 0.999999934066 + covariance: [2.639570295779254, 4.030463239533952e-09, 0.0, 0.0, -3.1032599801913304e-06, 0.0, 4.030463239533951e-09, 2.6395703537779887, 0.0, 0.0, 4.231594465888948e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.103259980191329e-06, 4.2315944658889475e-06, 0.0, 0.0, 2.6380171399151124, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 9 + stamp: + secs: 1624875197 + nsecs: 559230089 + frame_id: "/world" +pose: + pose: + position: + x: -0.0103437313582 + y: 0.0443447242232 + z: 0.0 + orientation: + x: -0.0 + y: -2.09887935548e-06 + z: -0.0 + w: 0.999999999998 + covariance: [3.024753363075542, 6.320434768351333e-09, 0.0, 0.0, -3.865234278879151e-06, 0.0, 6.320434768351334e-09, 3.0247534561500014, 0.0, 0.0, 4.822653440249248e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.865234278879149e-06, 4.8226534402492505e-06, 0.0, 0.0, 3.021844583449544, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 10 + stamp: + secs: 1624875197 + nsecs: 639235020 + frame_id: "/world" +pose: + pose: + position: + x: -0.000817313549928 + y: 0.0492779640487 + z: 0.0 + orientation: + x: -0.0 + y: -3.36054114594e-05 + z: -0.0 + w: 0.999999999435 + covariance: [2.356746611094799, 3.6820830805114973e-09, 0.0, 0.0, -1.8950899977606846e-06, 0.0, 3.682083080511501e-09, 2.356746666017282, 0.0, 0.0, 1.331039239378389e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8950899977606844e-06, 1.3310392393783887e-06, 0.0, 0.0, 2.354826895956701, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 11 + stamp: + secs: 1624875197 + nsecs: 719242096 + frame_id: "/world" +pose: + pose: + position: + x: -0.000249524194457 + y: 0.0511987066654 + z: 0.0 + orientation: + x: 0.0 + y: 0.000109412086021 + z: 0.0 + w: 0.999999994014 + covariance: [2.771210290960315, 1.5399705991262075e-09, 0.0, 0.0, -1.3010764903438397e-06, 0.0, 1.5399705991262088e-09, 2.7712103140325426, 0.0, 0.0, 3.4059294247831616e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3010764903438399e-06, 3.405929424783159e-07, 0.0, 0.0, 2.7703507271527275, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 12 + stamp: + secs: 1624875197 + nsecs: 799248934 + frame_id: "/world" +pose: + pose: + position: + x: 0.00030803333952 + y: 0.0434742855753 + z: 0.0 + orientation: + x: -0.0 + y: -0.00139369046807 + z: -0.0 + w: 0.999999028813 + covariance: [2.8301534924817684, 4.2018939779731815e-09, 0.0, 0.0, -2.3891358723818905e-06, 0.0, 4.201893977973179e-09, 2.8301535569435914, 0.0, 0.0, 1.2034743398134568e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.389135872381889e-06, 1.2034743398134557e-05, 0.0, 0.0, 2.827303718254319, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 13 + stamp: + secs: 1624875197 + nsecs: 879255056 + frame_id: "/world" +pose: + pose: + position: + x: 0.00243272400426 + y: -0.00289064506479 + z: 0.0 + orientation: + x: -0.0 + y: -0.000631090799384 + z: -0.0 + w: 0.999999800862 + covariance: [2.412518466694912, 5.663303596419025e-09, 0.0, 0.0, -1.0548273114510905e-06, 0.0, 5.663303596419018e-09, 2.412518557060573, 0.0, 0.0, 9.252846578140756e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0548273114510894e-06, 9.252846578140756e-06, 0.0, 0.0, 2.408212355571004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 14 + stamp: + secs: 1624875197 + nsecs: 959276915 + frame_id: "/world" +pose: + pose: + position: + x: 0.00648756359306 + y: -0.00677924013791 + z: 0.0 + orientation: + x: 0.0 + y: 0.000575936445333 + z: 0.0 + w: 0.999999834149 + covariance: [2.0657306614961364, 4.5214192777843376e-09, 0.0, 0.0, 2.4588679716224843e-07, 0.0, 4.521419277784338e-09, 2.0657307344434694, 0.0, 0.0, 2.784540917356526e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 2.458867971622487e-07, 2.7845409173565203e-06, 0.0, 0.0, 2.0619193009595698, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 15 + stamp: + secs: 1624875198 + nsecs: 39307117 + frame_id: "/world" +pose: + pose: + position: + x: 0.0117163591387 + y: 0.00605252899946 + z: 0.0 + orientation: + x: 0.0 + y: 0.000304343618596 + z: 0.0 + w: 0.999999953687 + covariance: [2.146005064704313, 3.1565177059080497e-09, 0.0, 0.0, 8.270026924315442e-08, 0.0, 3.1565177059080485e-09, 2.1460051157429993, 0.0, 0.0, 7.604544669657427e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 8.27002692431543e-08, 7.604544669657414e-07, 0.0, 0.0, 2.1431432590691086, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 16 + stamp: + secs: 1624875198 + nsecs: 119277000 + frame_id: "/world" +pose: + pose: + position: + x: -0.000830828838583 + y: -0.0151919619832 + z: 0.0 + orientation: + x: 0.0 + y: 7.64437083169e-05 + z: 0.0 + w: 0.999999997078 + covariance: [2.490115747923964, 5.6119789052339174e-09, 0.0, 0.0, -1.053042834164992e-05, 0.0, 5.611978905233925e-09, 2.490115838937755, 0.0, 0.0, 1.0135261941182732e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.053042834164991e-05, 1.0135261941182715e-06, 0.0, 0.0, 2.4845445357063447, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 17 + stamp: + secs: 1624875198 + nsecs: 199289083 + frame_id: "/world" +pose: + pose: + position: + x: 0.00608578348289 + y: 0.0355035563829 + z: 0.0 + orientation: + x: -0.0 + y: -0.00362889972962 + z: -0.0 + w: 0.999993415522 + covariance: [2.6139474494902166, 6.503246709211537e-09, 0.0, 0.0, -9.149965389149485e-06, 0.0, 6.503246709211527e-09, 2.613947556012985, 0.0, 0.0, 2.758495337887368e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.149965389149485e-06, 2.7584953378873694e-06, 0.0, 0.0, 2.6069391815521645, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 18 + stamp: + secs: 1624875198 + nsecs: 279292107 + frame_id: "/world" +pose: + pose: + position: + x: 0.0057862759564 + y: 0.0354299542607 + z: 0.0 + orientation: + x: -0.0 + y: -0.000113806703477 + z: -0.0 + w: 0.999999993524 + covariance: [3.5629365969266176, 8.511129411512787e-09, 0.0, 0.0, -5.161293433126136e-06, 0.0, 8.511129411512782e-09, 3.562936738284586, 0.0, 0.0, 4.214092510970849e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.161293433126138e-06, 4.2140925109708515e-06, 0.0, 0.0, 3.5529071707948505, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 19 + stamp: + secs: 1624875198 + nsecs: 359307051 + frame_id: "/world" +pose: + pose: + position: + x: 0.00443116249069 + y: 0.044019377542 + z: 0.0 + orientation: + x: 0.0 + y: 0.000330729206714 + z: 0.0 + w: 0.999999945309 + covariance: [3.0388732862699315, 2.6405539809565164e-09, 0.0, 0.0, -9.377688714154101e-07, 0.0, 2.640553980956514e-09, 3.0388733303159587, 0.0, 0.0, 1.179743040237987e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.377688714154109e-07, 1.1797430402379878e-06, 0.0, 0.0, 3.0356510790043285, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 20 + stamp: + secs: 1624875198 + nsecs: 439341068 + frame_id: "/world" +pose: + pose: + position: + x: 0.00487458581203 + y: 0.0414893885128 + z: 0.0 + orientation: + x: -0.0 + y: -5.71892453277e-05 + z: -0.0 + w: 0.999999998365 + covariance: [2.671211977417257, 4.774715948597809e-10, 0.0, 0.0, -1.4077558481331576e-07, 0.0, 4.774715948597801e-10, 2.671211985395572, 0.0, 0.0, 1.9441207304537183e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4077558481331592e-07, 1.9441207304537212e-07, 0.0, 0.0, 2.6706159013387882, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 21 + stamp: + secs: 1624875198 + nsecs: 519325018 + frame_id: "/world" +pose: + pose: + position: + x: 0.0154616264841 + y: 0.0162499846793 + z: 0.0 + orientation: + x: -0.0 + y: -0.00228571508401 + z: -0.0 + w: 0.99999738775 + covariance: [2.814422923123372, 3.211664870708064e-10, 0.0, 0.0, -9.55297304870148e-10, 0.0, 3.211664870708062e-10, 2.8144229285088986, 0.0, 0.0, 1.4029059611017004e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.552973048701933e-10, 1.4029059611017023e-07, 0.0, 0.0, 2.8139877949403083, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 22 + stamp: + secs: 1624875198 + nsecs: 599320889 + frame_id: "/world" +pose: + pose: + position: + x: 0.00925735035741 + y: 0.0194015825112 + z: 0.0 + orientation: + x: 0.0 + y: 0.000222849544262 + z: 0.0 + w: 0.999999975169 + covariance: [3.4446712527437735, 1.3437496944213191e-09, 0.0, 0.0, -6.103195180550493e-07, 0.0, 1.343749694421319e-09, 3.444671275334528, 0.0, 0.0, 6.486316161245206e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.103195180550495e-07, 6.486316161245209e-07, 0.0, 0.0, 3.4426348987163298, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 23 + stamp: + secs: 1624875198 + nsecs: 679327011 + frame_id: "/world" +pose: + pose: + position: + x: 0.00984220473555 + y: 0.0198808960337 + z: 0.0 + orientation: + x: 0.0 + y: 0.000111783694732 + z: 0.0 + w: 0.999999993752 + covariance: [3.9097553635596474, 1.6941249213005866e-09, 0.0, 0.0, -7.00012148333959e-07, 0.0, 1.6941249213005853e-09, 3.9097553920792247, 0.0, 0.0, 8.1507404019409e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.000121483339592e-07, 8.150740401940902e-07, 0.0, 0.0, 3.9070711390157964, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 24 + stamp: + secs: 1624875198 + nsecs: 759337902 + frame_id: "/world" +pose: + pose: + position: + x: 0.00665082160728 + y: 0.0122950704865 + z: 0.0 + orientation: + x: 0.0 + y: 0.000351481241339 + z: 0.0 + w: 0.99999993823 + covariance: [6.6796998779875505, 1.7460354444889173e-09, 0.0, 0.0, -6.315717337636585e-07, 0.0, 1.7460354444889163e-09, 6.679699907393659, 0.0, 0.0, 1.0018533043681738e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.315717337636585e-07, 1.001853304368174e-06, 0.0, 0.0, 6.67689229937404, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 25 + stamp: + secs: 1624875198 + nsecs: 839340925 + frame_id: "/world" +pose: + pose: + position: + x: 0.00656131283759 + y: 0.0125133306089 + z: 0.0 + orientation: + x: -0.0 + y: -5.14725098349e-05 + z: -0.0 + w: 0.999999998675 + covariance: [5.387434231009007, 3.98167033673926e-10, 0.0, 0.0, -1.4294278264433425e-07, 0.0, 3.981670336739258e-10, 5.38743423771501, 0.0, 0.0, 2.3542011596029729e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.429427826443343e-07, 2.3542011596029752e-07, 0.0, 0.0, 5.38679284692647, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 26 + stamp: + secs: 1624875198 + nsecs: 919356108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0160066391523 + y: 0.0395563881879 + z: 0.0 + orientation: + x: 0.0 + y: 0.00109229560804 + z: 0.0 + w: 0.999999403445 + covariance: [5.4422867159825, 3.9994871656092023e-10, 0.0, 0.0, 6.007764198111168e-08, 0.0, 3.999487165609199e-10, 5.442286722737885, 0.0, 0.0, -5.529028106468523e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 6.007764198111167e-08, -5.529028106468525e-07, 0.0, 0.0, 5.441405981137459, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 27 + stamp: + secs: 1624875198 + nsecs: 999368906 + frame_id: "/world" +pose: + pose: + position: + x: -0.0166945672149 + y: 0.0395493916865 + z: 0.0 + orientation: + x: 0.0 + y: 0.000177400966647 + z: 0.0 + w: 0.999999984264 + covariance: [5.109337181940768, 6.726220234085786e-10, 0.0, 0.0, -2.49758378141529e-07, 0.0, 6.726220234085784e-10, 5.109337193292254, 0.0, 0.0, -3.1305949533610187e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4975837814152896e-07, -3.1305949533610187e-07, 0.0, 0.0, 5.107978719315799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 28 + stamp: + secs: 1624875199 + nsecs: 79366922 + frame_id: "/world" +pose: + pose: + position: + x: -0.0164363631371 + y: 0.0388459344108 + z: 0.0 + orientation: + x: 0.0 + y: 0.000197205335015 + z: 0.0 + w: 0.999999980555 + covariance: [5.7516503046821015, 1.1223317941243867e-09, 0.0, 0.0, -1.1372293472360282e-06, 0.0, 1.122331794124386e-09, 5.751650323615462, 0.0, 0.0, -1.9020327776458715e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1372293472360278e-06, -1.902032777645871e-07, 0.0, 0.0, 5.749389146998635, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 29 + stamp: + secs: 1624875199 + nsecs: 159377098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0215932483286 + y: 0.0412181976543 + z: 0.0 + orientation: + x: 0.0 + y: 0.000111532776806 + z: 0.0 + w: 0.99999999378 + covariance: [5.183216023943926, 1.277800157654343e-09, 0.0, 0.0, -2.1977935560395288e-06, 0.0, 1.2778001576543428e-09, 5.183216045511387, 0.0, 0.0, 8.667208147067255e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1977935560395288e-06, 8.667208147067252e-07, 0.0, 0.0, 5.18058626469247, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 30 + stamp: + secs: 1624875199 + nsecs: 239401102 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181430761731 + y: 0.0460998360207 + z: 0.0 + orientation: + x: 0.0 + y: 0.00152025696064 + z: 0.0 + w: 0.999998844409 + covariance: [6.229618965950779, 3.6573356537969314e-09, 0.0, 0.0, -6.94017505485088e-06, 0.0, 3.657335653796928e-09, 6.229619027903319, 0.0, 0.0, 2.5709755827457238e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.94017505485088e-06, 2.5709755827457246e-06, 0.0, 0.0, 6.2218008521219215, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 31 + stamp: + secs: 1624875199 + nsecs: 319385052 + frame_id: "/world" +pose: + pose: + position: + x: -0.0200555592676 + y: 0.0464923871201 + z: 0.0 + orientation: + x: 0.0 + y: 0.000329085060907 + z: 0.0 + w: 0.999999945852 + covariance: [6.094364670457917, 4.620258576101434e-10, 0.0, 0.0, -7.35171280082033e-07, 0.0, 4.62025857610143e-10, 6.094364678269983, 0.0, 0.0, 4.216738498793818e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.351712800820328e-07, 4.216738498793821e-07, 0.0, 0.0, 6.093382005283262, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 32 + stamp: + secs: 1624875199 + nsecs: 399395943 + frame_id: "/world" +pose: + pose: + position: + x: -0.0204530749926 + y: 0.046592377435 + z: 0.0 + orientation: + x: 0.0 + y: 0.000436204332822 + z: 0.0 + w: 0.999999904863 + covariance: [5.6315466847125615, 1.0279844172009881e-09, 0.0, 0.0, -4.5241419325170345e-06, 0.0, 1.0279844172009877e-09, 5.63154670216875, 0.0, 0.0, 8.404521412827395e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.5241419325170345e-06, 8.404521412827396e-07, 0.0, 0.0, 5.629272692341887, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 33 + stamp: + secs: 1624875199 + nsecs: 479382038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0481946387613 + y: 0.0779569295575 + z: 0.0 + orientation: + x: 0.0 + y: 0.00159488830561 + z: 0.0 + w: 0.999998728165 + covariance: [5.914592729255935, 1.0677418681924041e-10, 0.0, 0.0, 8.203175573547793e-08, 0.0, 1.0677418681924036e-10, 5.914592731064431, 0.0, 0.0, 5.67256028978185e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 8.203175573547784e-08, 5.672560289781845e-08, 0.0, 0.0, 5.9143626984012565, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 34 + stamp: + secs: 1624875199 + nsecs: 559417963 + frame_id: "/world" +pose: + pose: + position: + x: -0.0425653970002 + y: 0.0719048334437 + z: 0.0 + orientation: + x: 0.0 + y: 0.000240983283111 + z: 0.0 + w: 0.999999970964 + covariance: [6.003188840221803, 6.569169194160187e-10, 0.0, 0.0, -5.742061837434037e-06, 0.0, 6.569169194160184e-10, 6.003188851462168, 0.0, 0.0, 1.6363338419308646e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.742061837434035e-06, 1.6363338419308648e-06, 0.0, 0.0, 6.001594705419577, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 35 + stamp: + secs: 1624875199 + nsecs: 639408112 + frame_id: "/world" +pose: + pose: + position: + x: -0.0410990112364 + y: 0.0719800145286 + z: 0.0 + orientation: + x: 0.0 + y: 0.00010879362278 + z: 0.0 + w: 0.999999994082 + covariance: [5.6101639750458965, 1.7785246406660766e-09, 0.0, 0.0, -8.752189204696292e-06, 0.0, 1.7785246406660758e-09, 5.610164006001768, 0.0, 0.0, 4.729550871230466e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.752189204696287e-06, 4.729550871230465e-06, 0.0, 0.0, 5.605519588565328, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 36 + stamp: + secs: 1624875199 + nsecs: 719436884 + frame_id: "/world" +pose: + pose: + position: + x: -0.0401195417187 + y: 0.0670482200886 + z: 0.0 + orientation: + x: 0.0 + y: 0.000216858671016 + z: 0.0 + w: 0.999999976486 + covariance: [6.386812761949125, 3.2593319426826862e-09, 0.0, 0.0, -8.504556696385084e-06, 0.0, 3.259331942682683e-09, 6.386812819296, 0.0, 0.0, 6.7749615507815e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.504556696385078e-06, 6.774961550781498e-06, 0.0, 0.0, 6.377946141106799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 37 + stamp: + secs: 1624875199 + nsecs: 799429893 + frame_id: "/world" +pose: + pose: + position: + x: -0.0405598251814 + y: 0.0713097189718 + z: 0.0 + orientation: + x: -0.0 + y: -9.9057749344e-05 + z: -0.0 + w: 0.999999995094 + covariance: [5.709537982053482, 2.8313268758692186e-09, 0.0, 0.0, -5.529404307570489e-06, 0.0, 2.8313268758692157e-09, 5.709538032151099, 0.0, 0.0, 4.01521949535703e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.5294043075704864e-06, 4.01521949535703e-06, 0.0, 0.0, 5.701616842724652, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 38 + stamp: + secs: 1624875199 + nsecs: 879475117 + frame_id: "/world" +pose: + pose: + position: + x: -0.0398704875567 + y: 0.069053248926 + z: 0.0 + orientation: + x: -0.0 + y: -0.000203556136595 + z: -0.0 + w: 0.999999979282 + covariance: [5.526693142097694, 4.255586883963843e-12, 0.0, 0.0, -4.5132191059445734e-06, 0.0, 4.255586883964016e-12, 5.526693142182814, 0.0, 0.0, 2.505823886958543e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.513219105944572e-06, 2.5058238869586035e-08, 0.0, 0.0, 5.526573460970315, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 39 + stamp: + secs: 1624875199 + nsecs: 959450006 + frame_id: "/world" +pose: + pose: + position: + x: -0.0401841519955 + y: 0.0694097698125 + z: 0.0 + orientation: + x: 0.0 + y: 0.000263672124928 + z: 0.0 + w: 0.999999965239 + covariance: [7.551207693363225, 1.303695306552285e-09, 0.0, 0.0, -1.1850278072583487e-05, 0.0, 1.3036953065522837e-09, 7.55120771663648, 0.0, 0.0, 3.893577771968103e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1850278072583487e-05, 3.893577771968102e-06, 0.0, 0.0, 7.54731873547034, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 40 + stamp: + secs: 1624875200 + nsecs: 39455891 + frame_id: "/world" +pose: + pose: + position: + x: -0.0407192884216 + y: 0.0658396173474 + z: 0.0 + orientation: + x: 0.0 + y: 0.000311074769417 + z: 0.0 + w: 0.999999951616 + covariance: [6.470666822640793, 2.044422661463085e-09, 0.0, 0.0, -1.546516645505898e-05, 0.0, 2.044422661463084e-09, 6.470666859806525, 0.0, 0.0, 5.6520764732966e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5465166455058987e-05, 5.652076473296599e-06, 0.0, 0.0, 6.464229080615675, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 41 + stamp: + secs: 1624875200 + nsecs: 119455099 + frame_id: "/world" +pose: + pose: + position: + x: -0.040758059268 + y: 0.0663591785889 + z: 0.0 + orientation: + x: 0.0 + y: 0.000198145056707 + z: 0.0 + w: 0.999999980369 + covariance: [6.686734301054004, 2.421447987331603e-09, 0.0, 0.0, -1.261190725114951e-05, 0.0, 2.4214479873316006e-09, 6.686734345648609, 0.0, 0.0, 5.240250269802687e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2611907251149507e-05, 5.240250269802685e-06, 0.0, 0.0, 6.6787868837067785, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 42 + stamp: + secs: 1624875200 + nsecs: 199474096 + frame_id: "/world" +pose: + pose: + position: + x: -0.0427481375305 + y: 0.0686205314535 + z: 0.0 + orientation: + x: 0.0 + y: 0.00157855889501 + z: 0.0 + w: 0.999998754075 + covariance: [10.214724526931724, 6.949799205207544e-09, 0.0, 0.0, -2.1709971848932613e-05, 0.0, 6.949799205207538e-09, 10.214724656235257, 0.0, 0.0, 1.0564532821262854e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1709971848932606e-05, 1.0564532821262852e-05, 0.0, 0.0, 10.191059131040246, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 43 + stamp: + secs: 1624875200 + nsecs: 279485941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0461353770764 + y: 0.0750991273274 + z: 0.0 + orientation: + x: 0.0 + y: 2.27294127948e-05 + z: 0.0 + w: 0.999999999742 + covariance: [7.115794788594093, 4.2698502284685e-09, 0.0, 0.0, -9.3485773612271e-06, 0.0, 4.269850228468494e-09, 7.1157948684045875, 0.0, 0.0, 4.810212745920324e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.348577361227091e-06, 4.810212745920324e-06, 0.0, 0.0, 7.100861313674602, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 44 + stamp: + secs: 1624875200 + nsecs: 359488964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0459706816566 + y: 0.0755646189941 + z: 0.0 + orientation: + x: 0.0 + y: 7.35061542155e-05 + z: 0.0 + w: 0.999999997298 + covariance: [6.360788862154457, 4.739118446556409e-09, 0.0, 0.0, -6.2154256363205885e-06, 0.0, 4.739118446556404e-09, 6.360788951102866, 0.0, 0.0, 2.763714303121092e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.215425636320585e-06, 2.7637143031210908e-06, 0.0, 0.0, 6.343620944125503, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 45 + stamp: + secs: 1624875200 + nsecs: 439486027 + frame_id: "/world" +pose: + pose: + position: + x: -0.0483345495148 + y: 0.0697634574365 + z: 0.0 + orientation: + x: 0.0 + y: 0.000132947803853 + z: 0.0 + w: 0.999999991162 + covariance: [7.2569149361046295, 5.028048368338878e-09, 0.0, 0.0, -8.0228583800563e-06, 0.0, 5.028048368338874e-09, 7.256915030947338, 0.0, 0.0, 7.82658008949263e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.022858380056298e-06, 7.826580089492628e-06, 0.0, 0.0, 7.238246444032866, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 46 + stamp: + secs: 1624875200 + nsecs: 519515038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0496301948357 + y: 0.0671863823306 + z: 0.0 + orientation: + x: -0.0 + y: -0.000319716627028 + z: -0.0 + w: 0.999999948891 + covariance: [6.9499229082950436, 4.078389832455526e-09, 0.0, 0.0, -7.431792576628252e-06, 0.0, 4.078389832455522e-09, 6.949922986097483, 0.0, 0.0, 9.352490053057065e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.431792576628253e-06, 9.352490053057066e-06, 0.0, 0.0, 6.934322640108893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 47 + stamp: + secs: 1624875200 + nsecs: 599533081 + frame_id: "/world" +pose: + pose: + position: + x: -0.0425630466069 + y: 0.0675904847021 + z: 0.0 + orientation: + x: -0.0 + y: -0.00328455842331 + z: -0.0 + w: 0.999994605823 + covariance: [10.41146376244531, 6.7002540307274514e-09, 0.0, 0.0, -1.0803901104319076e-05, 0.0, 6.700254030727445e-09, 10.411463891579308, 0.0, 0.0, 1.6023921031234494e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0803901104319076e-05, 1.6023921031234497e-05, 0.0, 0.0, 10.385138575876372, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 48 + stamp: + secs: 1624875200 + nsecs: 679512978 + frame_id: "/world" +pose: + pose: + position: + x: -0.0423935786645 + y: 0.067603029362 + z: 0.0 + orientation: + x: -0.0 + y: -0.00192923746179 + z: -0.0 + w: 0.99999813902 + covariance: [12.955510211644269, 1.2736549722631535e-08, 0.0, 0.0, -1.5241051401261352e-05, 0.0, 1.273654972263152e-08, 12.955510459888291, 0.0, 0.0, 2.667368822936255e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5241051401261348e-05, 2.667368822936256e-05, 0.0, 0.0, 12.903634646722526, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 49 + stamp: + secs: 1624875200 + nsecs: 759515047 + frame_id: "/world" +pose: + pose: + position: + x: -0.0426505379339 + y: 0.0669876828773 + z: 0.0 + orientation: + x: 0.0 + y: 0.000479012840667 + z: 0.0 + w: 0.999999885273 + covariance: [7.665896446027593, 5.664701585346499e-09, 0.0, 0.0, -4.744013488578911e-06, 0.0, 5.6647015853464936e-09, 7.665896557102793, 0.0, 0.0, 8.955189757203733e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.7440134885789085e-06, 8.955189757203735e-06, 0.0, 0.0, 7.642131971376101, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 50 + stamp: + secs: 1624875200 + nsecs: 839543104 + frame_id: "/world" +pose: + pose: + position: + x: -0.0588304358 + y: 0.0450252071109 + z: 0.0 + orientation: + x: 0.0 + y: 0.000648590300192 + z: 0.0 + w: 0.999999789665 + covariance: [8.614999654858222, 8.844085364858197e-09, 0.0, 0.0, -2.158894333484208e-05, 0.0, 8.844085364858187e-09, 8.614999829907196, 0.0, 0.0, 8.814683423298341e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1588943334842082e-05, 8.814683423298341e-06, 0.0, 0.0, 8.576365638627754, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 51 + stamp: + secs: 1624875200 + nsecs: 919538975 + frame_id: "/world" +pose: + pose: + position: + x: -0.0541823858418 + y: 0.0456605715875 + z: 0.0 + orientation: + x: 0.0 + y: 3.15360215819e-05 + z: 0.0 + w: 0.999999999503 + covariance: [6.222070375101623, 3.780578819864481e-09, 0.0, 0.0, -1.4664955078387986e-05, 0.0, 3.780578819864477e-09, 6.222070450273468, 0.0, 0.0, 2.821905098360188e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4664955078387976e-05, 2.8219050983601865e-06, 0.0, 0.0, 6.2051154114586975, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 52 + stamp: + secs: 1624875200 + nsecs: 999536991 + frame_id: "/world" +pose: + pose: + position: + x: -0.0531786683828 + y: 0.0403424874 + z: 0.0 + orientation: + x: 0.0 + y: 0.00018449922361 + z: 0.0 + w: 0.99999998298 + covariance: [6.1055759692069005, 3.671988605059042e-09, 0.0, 0.0, -1.5659576223284996e-05, 0.0, 3.6719886050590356e-09, 6.105576042573671, 0.0, 0.0, 1.6348478455932145e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.565957622328498e-05, 1.6348478455932128e-06, 0.0, 0.0, 6.0885095263994256, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 53 + stamp: + secs: 1624875201 + nsecs: 79540968 + frame_id: "/world" +pose: + pose: + position: + x: -0.0531232260178 + y: 0.040599696997 + z: 0.0 + orientation: + x: 0.0 + y: 0.000356252945077 + z: 0.0 + w: 0.999999936542 + covariance: [6.071049224299907, 3.5584730208169503e-09, 0.0, 0.0, -2.593993779712038e-05, 0.0, 3.5584730208169457e-09, 6.071049295600293, 0.0, 0.0, 1.6868381304866595e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.593993779712037e-05, 1.6868381304866578e-06, 0.0, 0.0, 6.053941826995994, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 54 + stamp: + secs: 1624875201 + nsecs: 159560919 + frame_id: "/world" +pose: + pose: + position: + x: -0.0515965310653 + y: 0.0372558069186 + z: 0.0 + orientation: + x: 0.0 + y: 0.000153353581862 + z: 0.0 + w: 0.999999988241 + covariance: [6.5554202023576496, 3.6009724891127334e-09, 0.0, 0.0, -2.918246218302701e-05, 0.0, 3.60097248911273e-09, 6.555420274714578, 0.0, 0.0, 2.02520348314083e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9182462183027015e-05, 2.0252034831408287e-06, 0.0, 0.0, 6.537423109724161, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 55 + stamp: + secs: 1624875201 + nsecs: 239557981 + frame_id: "/world" +pose: + pose: + position: + x: -0.0513899309889 + y: 0.0373689933971 + z: 0.0 + orientation: + x: -0.0 + y: -0.000125327146441 + z: -0.0 + w: 0.999999992147 + covariance: [6.725715574215326, 4.257847881272714e-09, 0.0, 0.0, -2.457511235408253e-05, 0.0, 4.257847881272708e-09, 6.725715660107967, 0.0, 0.0, 2.1564461196761063e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.457511235408253e-05, 2.156446119676106e-06, 0.0, 0.0, 6.703552068996524, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 56 + stamp: + secs: 1624875201 + nsecs: 319547892 + frame_id: "/world" +pose: + pose: + position: + x: -0.051178895169 + y: 0.0373314905614 + z: 0.0 + orientation: + x: -0.0 + y: -0.000187392577384 + z: -0.0 + w: 0.999999982442 + covariance: [6.2532333187585785, 3.7711712382568555e-09, 0.0, 0.0, -1.8484614504827182e-05, 0.0, 3.77117123825685e-09, 6.253233396060383, 0.0, 0.0, 8.419443995651417e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8484614504827182e-05, 8.41944399565142e-06, 0.0, 0.0, 6.232703571208513, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 57 + stamp: + secs: 1624875201 + nsecs: 399564028 + frame_id: "/world" +pose: + pose: + position: + x: -0.0516672682339 + y: 0.0372582110394 + z: 0.0 + orientation: + x: -0.0 + y: -0.000300714830524 + z: -0.0 + w: 0.999999954785 + covariance: [7.215885306648708, 4.0094139766186375e-09, 0.0, 0.0, -1.718400311106283e-05, 0.0, 4.009413976618631e-09, 7.215885391289209, 0.0, 0.0, 1.2279148296227846e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7184003111062826e-05, 1.227914829622785e-05, 0.0, 0.0, 7.192954859396255, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 58 + stamp: + secs: 1624875201 + nsecs: 479578972 + frame_id: "/world" +pose: + pose: + position: + x: -0.0463846568532 + y: 0.034449837009 + z: 0.0 + orientation: + x: -0.0 + y: -0.000291602490515 + z: -0.0 + w: 0.999999957484 + covariance: [6.942387329995982, 3.766883062573668e-09, 0.0, 0.0, -1.1645312894036364e-05, 0.0, 3.766883062573661e-09, 6.942387411624815, 0.0, 0.0, 9.320348310627923e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1645312894036365e-05, 9.320348310627926e-06, 0.0, 0.0, 6.919730340593574, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 59 + stamp: + secs: 1624875201 + nsecs: 559628963 + frame_id: "/world" +pose: + pose: + position: + x: -0.050313590207 + y: 0.0364422263493 + z: 0.0 + orientation: + x: -0.0 + y: -6.74025135932e-05 + z: -0.0 + w: 0.999999997728 + covariance: [8.269295601044773, 5.0633002183049215e-09, 0.0, 0.0, -1.1141725625904428e-05, 0.0, 5.063300218304914e-09, 8.269295712758788, 0.0, 0.0, 8.279242589009173e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1141725625904422e-05, 8.279242589009175e-06, 0.0, 0.0, 8.237591680135125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 60 + stamp: + secs: 1624875201 + nsecs: 639638901 + frame_id: "/world" +pose: + pose: + position: + x: -0.0523279458706 + y: 0.0403151390048 + z: 0.0 + orientation: + x: -0.0 + y: -0.000128558494539 + z: -0.0 + w: 0.999999991736 + covariance: [7.267469987131691, 3.6127629809668462e-09, 0.0, 0.0, -5.862203389266706e-06, 0.0, 3.612762980966841e-09, 7.267470067813911, 0.0, 0.0, 8.23287470678679e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.862203389266702e-06, 8.232874706786788e-06, 0.0, 0.0, 7.244150463127268, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 61 + stamp: + secs: 1624875201 + nsecs: 719597101 + frame_id: "/world" +pose: + pose: + position: + x: -0.0535547339662 + y: 0.0474406387205 + z: 0.0 + orientation: + x: -0.0 + y: -0.000438105913474 + z: -0.0 + w: 0.999999904032 + covariance: [5.932813146227613, 3.2084962805613845e-09, 0.0, 0.0, -5.00333159799478e-06, 0.0, 3.2084962805613787e-09, 5.932813219124593, 0.0, 0.0, 8.906028565149512e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.003331597994779e-06, 8.906028565149518e-06, 0.0, 0.0, 5.911171074567173, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 62 + stamp: + secs: 1624875201 + nsecs: 799607038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0534598721022 + y: 0.0532877116065 + z: 0.0 + orientation: + x: -0.0 + y: -0.000521656647675 + z: -0.0 + w: 0.999999863937 + covariance: [6.504336092489083, 2.4571207468102597e-09, 0.0, 0.0, -8.757518636529195e-06, 0.0, 2.457120746810255e-09, 6.504336149267221, 0.0, 0.0, 9.047287197048292e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.757518636529195e-06, 9.047287197048297e-06, 0.0, 0.0, 6.487246933962467, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 63 + stamp: + secs: 1624875201 + nsecs: 879617929 + frame_id: "/world" +pose: + pose: + position: + x: -0.0527863464063 + y: 0.0472389370468 + z: 0.0 + orientation: + x: -0.0 + y: -0.000355931574645 + z: -0.0 + w: 0.999999936656 + covariance: [7.315057613172727, 2.52588729759037e-09, 0.0, 0.0, -1.2806505930258351e-05, 0.0, 2.5258872975903656e-09, 7.315057673794041, 0.0, 0.0, 1.0020109497711254e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2806505930258356e-05, 1.0020109497711258e-05, 0.0, 0.0, 7.296463017625533, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 64 + stamp: + secs: 1624875201 + nsecs: 959651947 + frame_id: "/world" +pose: + pose: + position: + x: -0.0553721450718 + y: 0.0476276052518 + z: 0.0 + orientation: + x: 0.0 + y: 0.000713764730188 + z: 0.0 + w: 0.99999974527 + covariance: [6.420786851635034, 2.983555028656739e-09, 0.0, 0.0, -1.3479858205933047e-05, 0.0, 2.983555028656736e-09, 6.420786926876147, 0.0, 0.0, 8.319095440942036e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3479858205933052e-05, 8.31909544094204e-06, 0.0, 0.0, 6.396988071389522, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 65 + stamp: + secs: 1624875202 + nsecs: 39701939 + frame_id: "/world" +pose: + pose: + position: + x: -0.0505424992249 + y: 0.0420103651189 + z: 0.0 + orientation: + x: 0.0 + y: 0.000309630907703 + z: 0.0 + w: 0.999999952064 + covariance: [8.006214149031438, 2.6478274883291098e-09, 0.0, 0.0, -1.0734195141003443e-05, 0.0, 2.6478274883291065e-09, 8.006214216856797, 0.0, 0.0, 6.2795935048806865e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0734195141003446e-05, 6.279593504880689e-06, 0.0, 0.0, 7.984505363981563, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 66 + stamp: + secs: 1624875202 + nsecs: 119643927 + frame_id: "/world" +pose: + pose: + position: + x: -0.0501580458513 + y: 0.0387541645839 + z: 0.0 + orientation: + x: -0.0 + y: -0.000165980270893 + z: -0.0 + w: 0.999999986225 + covariance: [7.190651972500992, 2.6737075389517348e-09, 0.0, 0.0, -7.304364844612394e-06, 0.0, 2.673707538951729e-09, 7.190652042550782, 0.0, 0.0, 4.073280221994479e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.3043648446123945e-06, 4.07328022199448e-06, 0.0, 0.0, 7.167706080416155, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 67 + stamp: + secs: 1624875202 + nsecs: 199664116 + frame_id: "/world" +pose: + pose: + position: + x: -0.0412033955723 + y: 0.0354541879602 + z: 0.0 + orientation: + x: -0.0 + y: -0.000245033191543 + z: -0.0 + w: 0.999999969979 + covariance: [9.362953637872767, 3.773190564883567e-09, 0.0, 0.0, -7.224994950347488e-06, 0.0, 3.77319056488356e-09, 9.362953737826746, 0.0, 0.0, 5.095413983632075e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.22499495034749e-06, 5.0954139836320754e-06, 0.0, 0.0, 9.329693527137698, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 68 + stamp: + secs: 1624875202 + nsecs: 279659033 + frame_id: "/world" +pose: + pose: + position: + x: -0.041060057264 + y: 0.0355073582177 + z: 0.0 + orientation: + x: -0.0 + y: -0.000232948524201 + z: -0.0 + w: 0.999999972867 + covariance: [7.584865306773077, 2.8058400145141555e-09, 0.0, 0.0, -3.5006137706194913e-06, 0.0, 2.80584001451415e-09, 7.584865381725598, 0.0, 0.0, 3.375959124298395e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5006137706194925e-06, 3.3759591242983956e-06, 0.0, 0.0, 7.559479723969458, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 69 + stamp: + secs: 1624875202 + nsecs: 359657049 + frame_id: "/world" +pose: + pose: + position: + x: -0.0404039482262 + y: 0.0353750873619 + z: 0.0 + orientation: + x: -0.0 + y: -0.000711396476627 + z: -0.0 + w: 0.999999746957 + covariance: [8.585839850028128, 5.0895258425857525e-09, 0.0, 0.0, -1.1252708141983462e-05, 0.0, 5.089525842585744e-09, 8.585839987085345, 0.0, 0.0, 3.910806942593836e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1252708141983459e-05, 3.910806942593835e-06, 0.0, 0.0, 8.538218282212535, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 70 + stamp: + secs: 1624875202 + nsecs: 439666033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0315414354144 + y: 0.0273564513163 + z: 0.0 + orientation: + x: 0.0 + y: 0.000257207155249 + z: 0.0 + w: 0.999999966922 + covariance: [8.11543617546917, 4.401690257225256e-09, 0.0, 0.0, -1.70659158640482e-05, 0.0, 4.401690257225249e-09, 8.115436294504347, 0.0, 0.0, 2.622992345158675e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7065915864048196e-05, 2.6229923451586745e-06, 0.0, 0.0, 8.073323166893184, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 71 + stamp: + secs: 1624875202 + nsecs: 519676924 + frame_id: "/world" +pose: + pose: + position: + x: -0.0294889970601 + y: 0.0257043828476 + z: 0.0 + orientation: + x: -0.0 + y: -6.40165308618e-05 + z: -0.0 + w: 0.999999997951 + covariance: [6.617887252911708, 1.7419234692831292e-09, 0.0, 0.0, -7.674655692363111e-06, 0.0, 1.741923469283126e-09, 6.617887300099801, 0.0, 0.0, 1.0195600230152164e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.674655692363113e-06, 1.0195600230152166e-06, 0.0, 0.0, 6.601034847664122, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 72 + stamp: + secs: 1624875202 + nsecs: 599704027 + frame_id: "/world" +pose: + pose: + position: + x: -0.0310287804562 + y: 0.032576968248 + z: 0.0 + orientation: + x: -0.0 + y: -0.000200619858165 + z: -0.0 + w: 0.999999979876 + covariance: [9.04002268348431, 4.7064963420103356e-09, 0.0, 0.0, -2.1525093731838417e-05, 0.0, 4.706496342010327e-09, 9.0400228116581, 0.0, 0.0, 2.1265924271434974e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1525093731838414e-05, 2.126592427143498e-06, 0.0, 0.0, 8.99286459162443, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 73 + stamp: + secs: 1624875202 + nsecs: 679709911 + frame_id: "/world" +pose: + pose: + position: + x: -0.0305069613398 + y: 0.0324097247277 + z: 0.0 + orientation: + x: -0.0 + y: -0.000114345083341 + z: -0.0 + w: 0.999999993463 + covariance: [6.855397335203499, 2.7201766585751314e-09, 0.0, 0.0, -1.1168567830275513e-05, 0.0, 2.720176658575126e-09, 6.855397409411203, 0.0, 0.0, 8.955674969571368e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1168567830275501e-05, 8.955674969571367e-07, 0.0, 0.0, 6.827569170775016, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 74 + stamp: + secs: 1624875202 + nsecs: 759694099 + frame_id: "/world" +pose: + pose: + position: + x: -0.0310316035302 + y: 0.0345282069086 + z: 0.0 + orientation: + x: 0.0 + y: 0.000195885261343 + z: 0.0 + w: 0.999999980814 + covariance: [5.441105053245132, 2.130878965044021e-09, 0.0, 0.0, -1.421210686898517e-05, 0.0, 2.130878965044017e-09, 5.441105111549796, 0.0, 0.0, 1.0340587415476813e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4212106868985169e-05, 1.0340587415476815e-06, 0.0, 0.0, 5.41866101270838, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 75 + stamp: + secs: 1624875202 + nsecs: 839711905 + frame_id: "/world" +pose: + pose: + position: + x: -0.0305541396172 + y: 0.0344195586272 + z: 0.0 + orientation: + x: 0.0 + y: 9.25634465759e-06 + z: 0.0 + w: 0.999999999957 + covariance: [5.869253540416316, 1.9139537695164687e-09, 0.0, 0.0, -1.3593708831389645e-05, 0.0, 1.913953769516465e-09, 5.869253592960617, 0.0, 0.0, 8.124658923472049e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3593708831389643e-05, 8.124658923472047e-07, 0.0, 0.0, 5.848584138772741, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 76 + stamp: + secs: 1624875202 + nsecs: 919713974 + frame_id: "/world" +pose: + pose: + position: + x: -0.03053133315 + y: 0.0340973068443 + z: 0.0 + orientation: + x: -0.0 + y: -3.17773453847e-05 + z: -0.0 + w: 0.999999999495 + covariance: [5.584295297516872, 1.6968167117566195e-09, 0.0, 0.0, -1.081074008616176e-05, 0.0, 1.6968167117566166e-09, 5.584295344138557, 0.0, 0.0, -6.84372826467214e-09, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0810740086161763e-05, -6.8437282646722635e-09, 0.0, 0.0, 5.56551204844527, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 77 + stamp: + secs: 1624875202 + nsecs: 999722004 + frame_id: "/world" +pose: + pose: + position: + x: -0.0300844483032 + y: 0.0339645587905 + z: 0.0 + orientation: + x: -0.0 + y: -0.000250040778057 + z: -0.0 + w: 0.99999996874 + covariance: [5.905692866487078, 9.938148436884699e-10, 0.0, 0.0, -5.804361844078661e-06, 0.0, 9.938148436884678e-10, 5.9056928937699285, 0.0, 0.0, -1.3304189995864723e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.804361844078661e-06, -1.3304189995864726e-07, 0.0, 0.0, 5.894575719238433, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 78 + stamp: + secs: 1624875203 + nsecs: 79710960 + frame_id: "/world" +pose: + pose: + position: + x: -0.0299044154196 + y: 0.0339138416884 + z: 0.0 + orientation: + x: -0.0 + y: -0.000222228545996 + z: -0.0 + w: 0.999999975307 + covariance: [7.761056264466418, 2.3078668151540547e-09, 0.0, 0.0, -1.0423470741179285e-05, 0.0, 2.3078668151540506e-09, 7.761056327672684, 0.0, 0.0, -2.7298197904686335e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0423470741179285e-05, -2.729819790468634e-07, 0.0, 0.0, 7.734622557341135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 79 + stamp: + secs: 1624875203 + nsecs: 159800053 + frame_id: "/world" +pose: + pose: + position: + x: -0.0297611656214 + y: 0.0338090250099 + z: 0.0 + orientation: + x: 0.0 + y: 2.67723357009e-05 + z: 0.0 + w: 0.999999999642 + covariance: [6.034103821854874, 9.463211493416067e-10, 0.0, 0.0, -3.9484974617219715e-06, 0.0, 9.463211493416046e-10, 6.034103847766836, 0.0, 0.0, 2.452769377681723e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.948497461721972e-06, 2.452769377681724e-07, 0.0, 0.0, 6.02314969427104, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 80 + stamp: + secs: 1624875203 + nsecs: 239774942 + frame_id: "/world" +pose: + pose: + position: + x: -0.029368901455 + y: 0.0334426816863 + z: 0.0 + orientation: + x: 0.0 + y: 0.000253896548854 + z: 0.0 + w: 0.999999967768 + covariance: [5.625516576044126, 1.2095090240363255e-09, 0.0, 0.0, -3.5803580570908878e-06, 0.0, 1.2095090240363234e-09, 5.625516609284909, 0.0, 0.0, 2.5886630369631993e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.580358057090885e-06, 2.588663036963198e-06, 0.0, 0.0, 5.611147056000919, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 81 + stamp: + secs: 1624875203 + nsecs: 319755077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0288780504803 + y: 0.0330889115738 + z: 0.0 + orientation: + x: 0.0 + y: 0.000415168978518 + z: 0.0 + w: 0.999999913817 + covariance: [5.924961329181338, 1.326524476628111e-09, 0.0, 0.0, -4.744901958268899e-06, 0.0, 1.3265244766281084e-09, 5.9249613659268405, 0.0, 0.0, 3.6999389075925713e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.744901958268899e-06, 3.699938907592571e-06, 0.0, 0.0, 5.908836708098841, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 82 + stamp: + secs: 1624875203 + nsecs: 399734974 + frame_id: "/world" +pose: + pose: + position: + x: -0.0286848291693 + y: 0.0331075847418 + z: 0.0 + orientation: + x: 0.0 + y: 0.000231751579465 + z: 0.0 + w: 0.999999973146 + covariance: [5.23112092223076, 1.096030935197983e-09, 0.0, 0.0, -4.797796254306697e-06, 0.0, 1.0960309351979815e-09, 5.231120952943339, 0.0, 0.0, 2.491463821786028e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.797796254306702e-06, 2.49146382178603e-06, 0.0, 0.0, 5.217371473415765, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 83 + stamp: + secs: 1624875203 + nsecs: 479762077 + frame_id: "/world" +pose: + pose: + position: + x: -0.02845367271 + y: 0.0333090233077 + z: 0.0 + orientation: + x: 0.0 + y: 0.000300143343316 + z: 0.0 + w: 0.999999954957 + covariance: [5.413716993569173, 1.473620758809044e-09, 0.0, 0.0, -4.8548570901298915e-06, 0.0, 1.4736207588090415e-09, 5.413717035367392, 0.0, 0.0, 2.691387981209663e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.854857090129894e-06, 2.691387981209665e-06, 0.0, 0.0, 5.3946013215805015, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 84 + stamp: + secs: 1624875203 + nsecs: 559773922 + frame_id: "/world" +pose: + pose: + position: + x: -0.0286786877919 + y: 0.0322730615972 + z: 0.0 + orientation: + x: 0.0 + y: 0.000191329093887 + z: 0.0 + w: 0.999999981697 + covariance: [5.46024166863482, 1.2571694277641156e-09, 0.0, 0.0, -3.0228783784282006e-06, 0.0, 1.2571694277641133e-09, 5.460241704522545, 0.0, 0.0, 1.8095042841270815e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0228783784282014e-06, 1.8095042841270815e-06, 0.0, 0.0, 5.443551179157265, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 85 + stamp: + secs: 1624875203 + nsecs: 639780045 + frame_id: "/world" +pose: + pose: + position: + x: -0.0285804901255 + y: 0.0320538731409 + z: 0.0 + orientation: + x: 0.0 + y: 3.80904171056e-05 + z: 0.0 + w: 0.999999999275 + covariance: [5.464698330818288, 5.787897154344925e-10, 0.0, 0.0, -1.2347589206645487e-06, 0.0, 5.787897154344914e-10, 5.464698347376211, 0.0, 0.0, 7.254049284124402e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2347589206645485e-06, 7.254049284124404e-07, 0.0, 0.0, 5.456947512630863, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 86 + stamp: + secs: 1624875203 + nsecs: 719793081 + frame_id: "/world" +pose: + pose: + position: + x: -0.0312620632254 + y: 0.03965032995 + z: 0.0 + orientation: + x: 0.0 + y: 9.28232736998e-05 + z: 0.0 + w: 0.999999995692 + covariance: [6.341274492878904, 1.2595089286139686e-09, 0.0, 0.0, -1.9505573017544705e-06, 0.0, 1.259508928613967e-09, 6.341274529095355, 0.0, 0.0, 8.615374796298428e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.95055730175447e-06, 8.615374796298425e-07, 0.0, 0.0, 6.323898076354738, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 87 + stamp: + secs: 1624875203 + nsecs: 799793005 + frame_id: "/world" +pose: + pose: + position: + x: -0.0309824971152 + y: 0.0342561548439 + z: 0.0 + orientation: + x: 0.0 + y: 0.000525987038703 + z: 0.0 + w: 0.999999861669 + covariance: [8.866954138062416, 2.9572405819855988e-09, 0.0, 0.0, -3.727007742265527e-06, 0.0, 2.957240581985595e-09, 8.866954223266472, 0.0, 0.0, 1.1790096818246053e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.727007742265527e-06, 1.1790096818246047e-06, 0.0, 0.0, 8.825477446385875, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 88 + stamp: + secs: 1624875203 + nsecs: 879812002 + frame_id: "/world" +pose: + pose: + position: + x: -0.0344277784667 + y: 0.0324848852501 + z: 0.0 + orientation: + x: 0.0 + y: 0.00173008697801 + z: 0.0 + w: 0.999998503398 + covariance: [5.825984817613667, 1.6098280014090348e-09, 0.0, 0.0, -2.1750407431437753e-05, 0.0, 1.6098280014090317e-09, 5.825984864197565, 0.0, 0.0, 1.4004888115707307e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1750407431437746e-05, 1.4004888115707307e-06, 0.0, 0.0, 5.802810211083719, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 89 + stamp: + secs: 1624875203 + nsecs: 959813118 + frame_id: "/world" +pose: + pose: + position: + x: -0.0339046833964 + y: 0.0324200086486 + z: 0.0 + orientation: + x: 0.0 + y: 0.000332632996013 + z: 0.0 + w: 0.999999944678 + covariance: [7.026391437656853, 8.422161757013851e-10, 0.0, 0.0, -1.3891188929663267e-05, 0.0, 8.422161757013834e-10, 7.026391462072635, 0.0, 0.0, 8.410228800457056e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3891188929663267e-05, 8.410228800457062e-07, 0.0, 0.0, 7.014174459658646, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 90 + stamp: + secs: 1624875204 + nsecs: 39814949 + frame_id: "/world" +pose: + pose: + position: + x: -0.0336694499778 + y: 0.0322203083101 + z: 0.0 + orientation: + x: 0.0 + y: 0.000176068595634 + z: 0.0 + w: 0.9999999845 + covariance: [6.87808014913502, 1.0619422557336054e-09, 0.0, 0.0, -2.0859047988894462e-05, 0.0, 1.0619422557336041e-09, 6.878080180358597, 0.0, 0.0, 1.1814973694538239e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.085904798889447e-05, 1.1814973694538234e-06, 0.0, 0.0, 6.861769548881901, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 91 + stamp: + secs: 1624875204 + nsecs: 119822979 + frame_id: "/world" +pose: + pose: + position: + x: -0.0331388001079 + y: 0.0320591442278 + z: 0.0 + orientation: + x: -0.0 + y: -0.000380136448881 + z: -0.0 + w: 0.999999927748 + covariance: [6.83999897315463, 1.70999389722201e-09, 0.0, 0.0, -2.226458264929575e-05, 0.0, 1.7099938972220078e-09, 6.839999024844985, 0.0, 0.0, 4.699388931989947e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2264582649295753e-05, 4.699388931989949e-06, 0.0, 0.0, 6.811786733087152, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 92 + stamp: + secs: 1624875204 + nsecs: 199822903 + frame_id: "/world" +pose: + pose: + position: + x: -0.0330533062238 + y: 0.0322120337701 + z: 0.0 + orientation: + x: 0.0 + y: 1.37573412709e-05 + z: 0.0 + w: 0.999999999905 + covariance: [6.829071707050904, 1.9389714085807932e-09, 0.0, 0.0, -1.9747852705332995e-05, 0.0, 1.938971408580791e-09, 6.829071767346333, 0.0, 0.0, 5.662282439739642e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.974785270533299e-05, 5.662282439739644e-06, 0.0, 0.0, 6.795282992848959, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 93 + stamp: + secs: 1624875204 + nsecs: 279830933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0328404383099 + y: 0.0318653501373 + z: 0.0 + orientation: + x: 0.0 + y: 0.000299350995417 + z: 0.0 + w: 0.999999955194 + covariance: [5.434728805439111, 1.2578021596983645e-09, 0.0, 0.0, -1.0448880064291836e-05, 0.0, 1.2578021596983635e-09, 5.43472884534197, 0.0, 0.0, 2.9077871775893866e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0448880064291828e-05, 2.9077871775893853e-06, 0.0, 0.0, 5.411911148495045, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 94 + stamp: + secs: 1624875204 + nsecs: 359844923 + frame_id: "/world" +pose: + pose: + position: + x: -0.0335537017199 + y: 0.0314645405542 + z: 0.0 + orientation: + x: 0.0 + y: 0.00120113288654 + z: 0.0 + w: 0.99999927864 + covariance: [9.278534859387115, 3.4715907434055907e-09, 0.0, 0.0, -2.301446301365134e-05, 0.0, 3.4715907434055874e-09, 9.278534971923774, 0.0, 0.0, 7.438017746864878e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3014463013651325e-05, 7.438017746864875e-06, 0.0, 0.0, 9.213334843220945, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 95 + stamp: + secs: 1624875204 + nsecs: 439868927 + frame_id: "/world" +pose: + pose: + position: + x: -0.033367072921 + y: 0.0316673289145 + z: 0.0 + orientation: + x: 0.0 + y: 0.00025451472398 + z: 0.0 + w: 0.999999967611 + covariance: [6.363887353467459, 1.4998048217274868e-09, 0.0, 0.0, -8.359749404400713e-06, 0.0, 1.4998048217274839e-09, 6.3638874025750125, 0.0, 0.0, 2.9289939249842745e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.35974940440071e-06, 2.9289939249842767e-06, 0.0, 0.0, 6.335020018787344, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 96 + stamp: + secs: 1624875204 + nsecs: 519855022 + frame_id: "/world" +pose: + pose: + position: + x: -0.0348338648711 + y: 0.0262302465414 + z: 0.0 + orientation: + x: -0.0 + y: -8.7332841064e-06 + z: -0.0 + w: 0.999999999962 + covariance: [6.162852344399239, 1.8041247970330366e-09, 0.0, 0.0, -6.443740038064464e-06, 0.0, 1.8041247970330351e-09, 6.162852404269027, 0.0, 0.0, 2.1338095757298696e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.4437400380644605e-06, 2.13380957572987e-06, 0.0, 0.0, 6.126690969693305, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 97 + stamp: + secs: 1624875204 + nsecs: 599853039 + frame_id: "/world" +pose: + pose: + position: + x: -0.0344394330311 + y: 0.0306871365431 + z: 0.0 + orientation: + x: -0.0 + y: -0.00015656970843 + z: -0.0 + w: 0.999999987743 + covariance: [6.066727496954406, 7.696880628126793e-11, 0.0, 0.0, -3.151326723928853e-07, 0.0, 7.696880628126779e-11, 6.066727499454091, 0.0, 0.0, 3.1317780592323037e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.151326723928846e-07, 3.1317780592323074e-07, 0.0, 0.0, 6.065212696692563, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 98 + stamp: + secs: 1624875204 + nsecs: 679869890 + frame_id: "/world" +pose: + pose: + position: + x: -0.0338299908124 + y: 0.0280517457894 + z: 0.0 + orientation: + x: 0.0 + y: 0.000265507918146 + z: 0.0 + w: 0.999999964753 + covariance: [6.056477543677572, 4.6285874399291374e-10, 0.0, 0.0, -1.1113497452578978e-06, 0.0, 4.628587439929135e-10, 6.056477559120053, 0.0, 0.0, 2.281488279929027e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1113497452578984e-06, 2.2814882799290287e-07, 0.0, 0.0, 6.046925064952447, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 99 + stamp: + secs: 1624875204 + nsecs: 759877920 + frame_id: "/world" +pose: + pose: + position: + x: -0.0329866515592 + y: 0.0278088847168 + z: 0.0 + orientation: + x: -0.0 + y: -0.000169404419646 + z: -0.0 + w: 0.999999985651 + covariance: [7.128632962916727, 5.875731598299246e-10, 0.0, 0.0, -8.703472016663721e-07, 0.0, 5.875731598299242e-10, 7.128632982592892, 0.0, 0.0, 1.7938865387042096e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.703472016663729e-07, 1.7938865387042101e-07, 0.0, 0.0, 7.1162835418361885, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 100 + stamp: + secs: 1624875204 + nsecs: 839879036 + frame_id: "/world" +pose: + pose: + position: + x: -0.0311154973658 + y: 0.027280179285 + z: 0.0 + orientation: + x: 0.0 + y: 0.000121586685539 + z: 0.0 + w: 0.999999992608 + covariance: [7.576464061595512, 1.4464416789095634e-09, 0.0, 0.0, -3.3697787239758352e-06, 0.0, 1.446441678909562e-09, 7.576464110183898, 0.0, 0.0, 2.2518517835426142e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.369778723975836e-06, 2.2518517835426136e-07, 0.0, 0.0, 7.54527150915445, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 101 + stamp: + secs: 1624875204 + nsecs: 919888020 + frame_id: "/world" +pose: + pose: + position: + x: -0.0308621440054 + y: 0.0268758312756 + z: 0.0 + orientation: + x: 0.0 + y: 0.000149921646249 + z: 0.0 + w: 0.999999988762 + covariance: [6.496061590876407, 1.475563981339107e-09, 0.0, 0.0, -3.479437316583001e-06, 0.0, 1.4755639813391061e-09, 6.496061640511535, 0.0, 0.0, 6.167050714684449e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.4794373165830004e-06, 6.167050714684446e-07, 0.0, 0.0, 6.463676017441984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 102 + stamp: + secs: 1624875204 + nsecs: 999908924 + frame_id: "/world" +pose: + pose: + position: + x: -0.0307967662538 + y: 0.0270356381578 + z: 0.0 + orientation: + x: 0.0 + y: 0.000307910440141 + z: 0.0 + w: 0.999999952596 + covariance: [6.720865926229536, 1.8921470645116818e-09, 0.0, 0.0, -2.952371215782367e-06, 0.0, 1.8921470645116805e-09, 6.720865989957784, 0.0, 0.0, 8.940032764606197e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9523712157823677e-06, 8.940032764606198e-07, 0.0, 0.0, 6.678675916608024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 103 + stamp: + secs: 1624875205 + nsecs: 79900026 + frame_id: "/world" +pose: + pose: + position: + x: -0.0292694340565 + y: 0.0251408741667 + z: 0.0 + orientation: + x: 0.0 + y: 0.000418195119105 + z: 0.0 + w: 0.999999912556 + covariance: [8.191414216495252, 3.0740713481742043e-09, 0.0, 0.0, -2.96976797078951e-06, 0.0, 3.074071348174201e-09, 8.19141432010717, 0.0, 0.0, 1.1726953217447635e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.96976797078951e-06, 1.1726953217447633e-06, 0.0, 0.0, 8.121821190221294, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 104 + stamp: + secs: 1624875205 + nsecs: 159907103 + frame_id: "/world" +pose: + pose: + position: + x: -0.0269060014989 + y: 0.026547691892 + z: 0.0 + orientation: + x: 0.0 + y: 0.000231854374674 + z: 0.0 + w: 0.999999973122 + covariance: [6.1535880854239755, 1.9336092367815195e-09, 0.0, 0.0, -3.220015025013334e-05, 0.0, 1.933609236781518e-09, 6.153588151035405, 0.0, 0.0, 1.9864591427352405e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.220015025013334e-05, 1.986459142735241e-06, 0.0, 0.0, 6.108501135787327, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 105 + stamp: + secs: 1624875205 + nsecs: 239924908 + frame_id: "/world" +pose: + pose: + position: + x: -0.0242349737454 + y: 0.0299399329082 + z: 0.0 + orientation: + x: 0.0 + y: 0.000238124672259 + z: 0.0 + w: 0.999999971648 + covariance: [7.203672141420394, 2.2880239048294966e-09, 0.0, 0.0, -0.0001009262665111744, 0.0, 2.288023904829495e-09, 7.203672225130319, 0.0, 0.0, 7.537435938692662e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010092626651117442, 7.537435938692666e-06, 0.0, 0.0, 7.141758772228517, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 106 + stamp: + secs: 1624875205 + nsecs: 319930077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0223290701719 + y: 0.0276953029368 + z: 0.0 + orientation: + x: -0.0 + y: -0.000471969629237 + z: -0.0 + w: 0.999999888622 + covariance: [10.806865863833153, 5.2640177746204485e-09, 0.0, 0.0, -0.00037112542953827207, 0.0, 5.26401777462044e-09, 10.80686616490761, 0.0, 0.0, 3.528434041076206e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003711254295382722, 3.528434041076207e-05, 0.0, 0.0, 10.54184294164946, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 107 + stamp: + secs: 1624875205 + nsecs: 399924994 + frame_id: "/world" +pose: + pose: + position: + x: -0.0216832806074 + y: 0.0271965744313 + z: 0.0 + orientation: + x: 0.0 + y: 0.000205124659965 + z: 0.0 + w: 0.999999978962 + covariance: [8.97139692562157, 3.0656500575536066e-09, 0.0, 0.0, -0.00022050763383345907, 0.0, 3.065650057553599e-09, 8.971397161569744, 0.0, 0.0, 2.2888386818543097e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00022050763383345916, 2.2888386818543107e-05, 0.0, 0.0, 8.745768335711709, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 108 + stamp: + secs: 1624875205 + nsecs: 479929924 + frame_id: "/world" +pose: + pose: + position: + x: -0.0210443530108 + y: 0.0277616442926 + z: 0.0 + orientation: + x: 0.0 + y: 0.000153249999882 + z: 0.0 + w: 0.999999988257 + covariance: [5.86173182967955, 7.861113392175589e-10, 0.0, 0.0, -5.512350526599892e-05, 0.0, 7.861113392175561e-10, 5.861731902653673, 0.0, 0.0, 7.169261987220312e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.512350526599894e-05, 7.1692619872203145e-06, 0.0, 0.0, 5.788777134408452, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 109 + stamp: + secs: 1624875205 + nsecs: 559950113 + frame_id: "/world" +pose: + pose: + position: + x: -0.0208444492588 + y: 0.0276181965 + z: 0.0 + orientation: + x: 0.0 + y: 0.000392762305429 + z: 0.0 + w: 0.999999922869 + covariance: [7.09019489749804, 5.179923228793742e-10, 0.0, 0.0, -3.9885929219927276e-05, 0.0, 5.179923228793714e-10, 7.090194975326228, 0.0, 0.0, 7.532119953578711e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.98859292199273e-05, 7.532119953578708e-06, 0.0, 0.0, 7.008358385653565, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 110 + stamp: + secs: 1624875205 + nsecs: 639954090 + frame_id: "/world" +pose: + pose: + position: + x: -0.0206806629598 + y: 0.0275006543784 + z: 0.0 + orientation: + x: 0.0 + y: 0.000455728624985 + z: 0.0 + w: 0.999999896156 + covariance: [10.556672854473353, 5.012310378477949e-10, 0.0, 0.0, -4.199546505392787e-05, 0.0, 5.01231037847791e-10, 10.55667296162493, 0.0, 0.0, 8.925405966031005e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.199546505392788e-05, 8.925405966031003e-06, 0.0, 0.0, 10.441121106658985, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 111 + stamp: + secs: 1624875205 + nsecs: 719960928 + frame_id: "/world" +pose: + pose: + position: + x: -0.0201304147827 + y: 0.0269101588137 + z: 0.0 + orientation: + x: 0.0 + y: 0.000280804740835 + z: 0.0 + w: 0.999999960574 + covariance: [9.85797649490676, 2.402902249834455e-10, 0.0, 0.0, -2.2618123392973962e-05, 0.0, 2.402902249834429e-10, 9.857976566732077, 0.0, 0.0, 5.864265210453712e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2618123392973976e-05, 5.864265210453712e-06, 0.0, 0.0, 9.77888075198424, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 112 + stamp: + secs: 1624875205 + nsecs: 799964905 + frame_id: "/world" +pose: + pose: + position: + x: -0.0185306526293 + y: 0.0269073240406 + z: 0.0 + orientation: + x: 0.0 + y: 0.000812724337519 + z: 0.0 + w: 0.99999966974 + covariance: [7.738198538605972, 7.249260886670068e-11, 0.0, 0.0, -1.0474687892030071e-05, 0.0, 7.249260886669899e-11, 7.738198587390989, 0.0, 0.0, 4.128636377329034e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0474687892030074e-05, 4.128636377329034e-06, 0.0, 0.0, 7.682603119075294, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 113 + stamp: + secs: 1624875205 + nsecs: 879976034 + frame_id: "/world" +pose: + pose: + position: + x: -0.0182633788442 + y: 0.0267188952873 + z: 0.0 + orientation: + x: 0.0 + y: 0.000863213571145 + z: 0.0 + w: 0.999999627431 + covariance: [11.070022413868518, 7.153691185359882e-12, 0.0, 0.0, -1.3921550056593933e-05, 0.0, 7.153691185356287e-12, 11.070022518909951, 0.0, 0.0, 7.983309001994027e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3921550056593933e-05, 7.983309001994027e-06, 0.0, 0.0, 10.947340005863346, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 114 + stamp: + secs: 1624875205 + nsecs: 959978104 + frame_id: "/world" +pose: + pose: + position: + x: -0.0179925526894 + y: 0.0264395674232 + z: 0.0 + orientation: + x: 0.0 + y: 0.000297724259556 + z: 0.0 + w: 0.99999995568 + covariance: [8.935835657880686, -3.351546096070136e-11, 0.0, 0.0, -1.3086072063318267e-05, 0.0, -3.351546096070319e-11, 8.935835711129798, 0.0, 0.0, 6.720498236479114e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3086072063318273e-05, 6.720498236479113e-06, 0.0, 0.0, 8.872679935561846, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 115 + stamp: + secs: 1624875206 + nsecs: 40004015 + frame_id: "/world" +pose: + pose: + position: + x: -0.0177912468211 + y: 0.0262907982793 + z: 0.0 + orientation: + x: 0.0 + y: 0.000482243739872 + z: 0.0 + w: 0.99999988372 + covariance: [9.202620087947007, -2.040298736356732e-10, 0.0, 0.0, -2.7929718638954978e-05, 0.0, -2.0402987363567531e-10, 9.202620151291894, 0.0, 0.0, 1.2985170437266609e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.792971863895498e-05, 1.2985170437266607e-05, 0.0, 0.0, 9.125579044959062, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 116 + stamp: + secs: 1624875206 + nsecs: 119993925 + frame_id: "/world" +pose: + pose: + position: + x: -0.0231749157567 + y: 0.0148295463433 + z: 0.0 + orientation: + x: 0.0 + y: 0.000210898587134 + z: 0.0 + w: 0.999999977761 + covariance: [8.328273322338097, -1.9411843869317915e-10, 0.0, 0.0, -1.8351649506314005e-05, 0.0, -1.9411843869318034e-10, 8.32827336247226, 0.0, 0.0, 9.157278128081366e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8351649506314005e-05, 9.157278128081368e-06, 0.0, 0.0, 8.278600801984739, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 117 + stamp: + secs: 1624875206 + nsecs: 199990988 + frame_id: "/world" +pose: + pose: + position: + x: -0.02299716388 + y: 0.0146507323071 + z: 0.0 + orientation: + x: 0.0 + y: 0.000137752994957 + z: 0.0 + w: 0.999999990512 + covariance: [11.479037152282265, -5.326957207623402e-10, 0.0, 0.0, -3.0612832973066205e-05, 0.0, -5.326957207623424e-10, 11.479037224144653, 0.0, 0.0, 1.6274931424681063e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0612832973066205e-05, 1.6274931424681066e-05, 0.0, 0.0, 11.388309687778115, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 118 + stamp: + secs: 1624875206 + nsecs: 280004025 + frame_id: "/world" +pose: + pose: + position: + x: -0.022917166196 + y: 0.0143090039172 + z: 0.0 + orientation: + x: -0.0 + y: -0.000197716386214 + z: -0.0 + w: 0.999999980454 + covariance: [7.383230190583339, -2.0653715315980992e-10, 0.0, 0.0, -9.237221517089571e-06, 0.0, -2.0653715315981072e-10, 7.383230215031422, 0.0, 0.0, 5.080690483860106e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.237221517089575e-06, 5.080690483860107e-06, 0.0, 0.0, 7.351908948740117, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 119 + stamp: + secs: 1624875206 + nsecs: 360021114 + frame_id: "/world" +pose: + pose: + position: + x: -0.0235232108536 + y: 0.0158795092608 + z: 0.0 + orientation: + x: -0.0 + y: -0.000425189601446 + z: -0.0 + w: 0.999999909607 + covariance: [7.678720028429655, -3.4915910139598493e-10, 0.0, 0.0, -8.80679168635356e-06, 0.0, -3.491591013959859e-10, 7.678720060991857, 0.0, 0.0, 6.367947421803017e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.806791686353562e-06, 6.3679474218030165e-06, 0.0, 0.0, 7.635973878069887, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 120 + stamp: + secs: 1624875206 + nsecs: 440040112 + frame_id: "/world" +pose: + pose: + position: + x: -0.0238875755032 + y: 0.0163535814918 + z: 0.0 + orientation: + x: -0.0 + y: -0.000193245896964 + z: -0.0 + w: 0.999999981328 + covariance: [8.630624022829892, -5.181334676670465e-10, 0.0, 0.0, -9.732494808975448e-06, 0.0, -5.181334676670477e-10, 8.630624065599063, 0.0, 0.0, 7.1840100509190324e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.732494808975448e-06, 7.184010050919034e-06, 0.0, 0.0, 8.5734533583517, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 121 + stamp: + secs: 1624875206 + nsecs: 520046949 + frame_id: "/world" +pose: + pose: + position: + x: -0.0254625677107 + y: 0.0207340946087 + z: 0.0 + orientation: + x: -0.0 + y: -8.47632319135e-05 + z: -0.0 + w: 0.999999996408 + covariance: [8.14783907993573, -5.967051723548534e-10, 0.0, 0.0, -8.515532928792829e-06, 0.0, -5.967051723548549e-10, 8.147839125450133, 0.0, 0.0, 5.569756398519332e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.515532928792827e-06, 5.569756398519331e-06, 0.0, 0.0, 8.085831564385149, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 122 + stamp: + secs: 1624875206 + nsecs: 600040913 + frame_id: "/world" +pose: + pose: + position: + x: -0.021514677121 + y: 0.0187254136152 + z: 0.0 + orientation: + x: 0.0 + y: 0.00225126106215 + z: 0.0 + w: 0.999997465909 + covariance: [7.975982974818024, -6.538310641181409e-10, 0.0, 0.0, -9.480824641322966e-06, 0.0, -6.538310641181426e-10, 7.975983022230691, 0.0, 0.0, 3.992503314234489e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.480824641322962e-06, 3.992503314234488e-06, 0.0, 0.0, 7.910270910791598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 123 + stamp: + secs: 1624875206 + nsecs: 680032969 + frame_id: "/world" +pose: + pose: + position: + x: -0.0214332402648 + y: 0.0186503914779 + z: 0.0 + orientation: + x: 0.0 + y: 0.00109258343587 + z: 0.0 + w: 0.999999403131 + covariance: [9.820008198748663, -6.017148079178165e-10, 0.0, 0.0, -8.819377412104283e-06, 0.0, -6.01714807917818e-10, 9.82000824163459, 0.0, 0.0, 3.003885479144821e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.819377412104281e-06, 3.0038854791448207e-06, 0.0, 0.0, 9.760124273115483, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 124 + stamp: + secs: 1624875206 + nsecs: 760040998 + frame_id: "/world" +pose: + pose: + position: + x: -0.0213392366995 + y: 0.0185613358439 + z: 0.0 + orientation: + x: 0.0 + y: 0.000148826674158 + z: 0.0 + w: 0.999999988925 + covariance: [8.532483141096968, -4.0879358899993684e-10, 0.0, 0.0, -1.894427229134666e-05, 0.0, -4.0879358899993777e-10, 8.532483169269126, 0.0, 0.0, 1.5196607487185151e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8944272291346646e-05, 1.5196607487185151e-06, 0.0, 0.0, 8.49256512212595, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 125 + stamp: + secs: 1624875206 + nsecs: 840075016 + frame_id: "/world" +pose: + pose: + position: + x: -0.022211740611 + y: 0.0110155061012 + z: 0.0 + orientation: + x: 0.0 + y: 6.38691002156e-05 + z: 0.0 + w: 0.99999999796 + covariance: [6.73333915865292, -2.8941716487302587e-10, 0.0, 0.0, -2.2630467187315206e-05, 0.0, -2.894171648730264e-10, 6.7333391771564335, 0.0, 0.0, 1.2945879349312284e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2630467187315203e-05, 1.2945879349312292e-06, 0.0, 0.0, 6.706249565606263, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 126 + stamp: + secs: 1624875206 + nsecs: 920078039 + frame_id: "/world" +pose: + pose: + position: + x: -0.0222409799199 + y: 0.011048094896 + z: 0.0 + orientation: + x: 0.0 + y: 9.28344478399e-05 + z: 0.0 + w: 0.999999995691 + covariance: [10.22751085925785, -1.6334099452560162e-10, 0.0, 0.0, -1.247852279036819e-05, 0.0, -1.6334099452560193e-10, 10.227510869979964, 0.0, 0.0, 7.350963471726725e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2478522790368189e-05, 7.350963471726732e-07, 0.0, 0.0, 10.211947692085849, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 127 + stamp: + secs: 1624875207 + nsecs: 104904 + frame_id: "/world" +pose: + pose: + position: + x: -0.0183395444905 + y: 0.0110053078331 + z: 0.0 + orientation: + x: -0.0 + y: -0.00358757292589 + z: -0.0 + w: 0.99999356464 + covariance: [12.81100767248707, -3.230994829911428e-10, 0.0, 0.0, -2.7258830938452172e-05, 0.0, -3.2309948299114347e-10, 12.81100769210501, 0.0, 0.0, 8.811132966162442e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7258830938452172e-05, 8.81113296616245e-07, 0.0, 0.0, 12.781542415719633, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 128 + stamp: + secs: 1624875207 + nsecs: 80077887 + frame_id: "/world" +pose: + pose: + position: + x: -0.0201559870744 + y: 0.010116690955 + z: 0.0 + orientation: + x: 0.0 + y: 0.000552887390014 + z: 0.0 + w: 0.999999847158 + covariance: [7.643226268295187, -2.6787770368641524e-10, 0.0, 0.0, -1.8029266264096224e-05, 0.0, -2.6787770368641565e-10, 7.643226283168313, 0.0, 0.0, 7.387093667500837e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8029266264096224e-05, 7.38709366750084e-07, 0.0, 0.0, 7.619748712073523, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 129 + stamp: + secs: 1624875207 + nsecs: 160103083 + frame_id: "/world" +pose: + pose: + position: + x: -0.019890488839 + y: 0.00981281935897 + z: 0.0 + orientation: + x: 0.0 + y: 0.000456716680405 + z: 0.0 + w: 0.999999895705 + covariance: [10.993790329377488, -1.1168245493714973e-09, 0.0, 0.0, -6.56008768809389e-05, 0.0, -1.1168245493714992e-09, 10.993790385932353, 0.0, 0.0, 3.1642822402349156e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.56008768809389e-05, 3.1642822402349147e-06, 0.0, 0.0, 10.900266042348838, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 130 + stamp: + secs: 1624875207 + nsecs: 240103006 + frame_id: "/world" +pose: + pose: + position: + x: -0.0238166654646 + y: 0.00592690260835 + z: 0.0 + orientation: + x: 0.0 + y: 1.01483706241e-05 + z: 0.0 + w: 0.999999999949 + covariance: [8.905482690895404, -5.729012074319021e-10, 0.0, 0.0, -3.888408878470981e-05, 0.0, -5.729012074319029e-10, 8.905482719060785, 0.0, 0.0, 1.6182550412668407e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.888408878470981e-05, 1.6182550412668405e-06, 0.0, 0.0, 8.858027632986373, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 131 + stamp: + secs: 1624875207 + nsecs: 320096970 + frame_id: "/world" +pose: + pose: + position: + x: -0.0258947856358 + y: 0.00892641423116 + z: 0.0 + orientation: + x: -0.0 + y: -0.000100854807115 + z: -0.0 + w: 0.999999994914 + covariance: [6.288933456265454, -3.3948807458945774e-10, 0.0, 0.0, -2.2723695803208398e-05, 0.0, -3.394880745894584e-10, 6.288933470868127, 0.0, 0.0, 3.3120770133021176e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2723695803208388e-05, 3.3120770133021154e-06, 0.0, 0.0, 6.263307048161116, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 132 + stamp: + secs: 1624875207 + nsecs: 400090933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0360834561166 + y: 0.0372382845405 + z: 0.0 + orientation: + x: -0.0 + y: -0.000666450150041 + z: -0.0 + w: 0.999999777922 + covariance: [5.705664648001188, -5.313545047623679e-10, 0.0, 0.0, -1.7958555214087125e-05, 0.0, -5.313545047623684e-10, 5.7056646658099766, 0.0, 0.0, 5.669117945246423e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7958555214087108e-05, 5.669117945246422e-06, 0.0, 0.0, 5.672717428431745, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 133 + stamp: + secs: 1624875207 + nsecs: 480118990 + frame_id: "/world" +pose: + pose: + position: + x: -0.0360490640703 + y: 0.0372176913577 + z: 0.0 + orientation: + x: -0.0 + y: -0.000281060902256 + z: -0.0 + w: 0.999999960502 + covariance: [5.957510824137518, -5.195197088393778e-10, 0.0, 0.0, -1.5001535609129928e-05, 0.0, -5.195197088393785e-10, 5.957510839726995, 0.0, 0.0, 3.875282449489996e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5001535609129927e-05, 3.875282449489996e-06, 0.0, 0.0, 5.927841350932035, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 134 + stamp: + secs: 1624875207 + nsecs: 560122967 + frame_id: "/world" +pose: + pose: + position: + x: -0.0366406302709 + y: 0.0351606397213 + z: 0.0 + orientation: + x: -0.0 + y: -0.000236692768377 + z: -0.0 + w: 0.999999971988 + covariance: [6.033058423264003, -5.170912782605844e-10, 0.0, 0.0, -1.3854620546779711e-05, 0.0, -5.170912782605851e-10, 6.033058437211719, 0.0, 0.0, 3.252686842327282e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3854620546779714e-05, 3.2526868423272822e-06, 0.0, 0.0, 6.005653877230422, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 135 + stamp: + secs: 1624875207 + nsecs: 640136957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0384632552582 + y: 0.0345702256041 + z: 0.0 + orientation: + x: 0.0 + y: 0.000863074610267 + z: 0.0 + w: 0.999999627551 + covariance: [9.689200976815654, -2.660925842750344e-10, 0.0, 0.0, -7.550591718626467e-06, 0.0, -2.6609258427503487e-10, 9.689200984259601, 0.0, 0.0, 2.178103893207592e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.550591718626467e-06, 2.178103893207592e-06, 0.0, 0.0, 9.674701616492486, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 136 + stamp: + secs: 1624875207 + nsecs: 720158100 + frame_id: "/world" +pose: + pose: + position: + x: -0.0404778604266 + y: 0.0367839579424 + z: 0.0 + orientation: + x: 0.0 + y: 0.000254643735561 + z: 0.0 + w: 0.999999967578 + covariance: [7.001891423411357, -3.23176831610324e-10, 0.0, 0.0, -1.795920422672207e-05, 0.0, -3.231768316103242e-10, 7.00189143120808, 0.0, 0.0, 1.974130987031971e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.795920422672207e-05, 1.974130987031973e-06, 0.0, 0.0, 6.985950053697888, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 137 + stamp: + secs: 1624875207 + nsecs: 800143957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0450964605258 + y: 0.040806684497 + z: 0.0 + orientation: + x: 0.0 + y: 0.00232913625657 + z: 0.0 + w: 0.999997287558 + covariance: [10.510877782204364, -2.076340233955924e-09, 0.0, 0.0, -8.00646540196225e-05, 0.0, -2.0763402339559254e-09, 10.510877821392738, 0.0, 0.0, 1.2210489596696603e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.00646540196225e-05, 1.2210489596696601e-05, 0.0, 0.0, 10.424165580689714, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 138 + stamp: + secs: 1624875207 + nsecs: 880158901 + frame_id: "/world" +pose: + pose: + position: + x: -0.0443288234939 + y: 0.0406363418229 + z: 0.0 + orientation: + x: 0.0 + y: 0.000153555078819 + z: 0.0 + w: 0.99999998821 + covariance: [7.184856518238638, -8.144800798626029e-10, 0.0, 0.0, -2.4880795707733304e-05, 0.0, -8.144800798626031e-10, 7.184856533130897, 0.0, 0.0, 4.464352201265126e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4880795707733304e-05, 4.464352201265126e-06, 0.0, 0.0, 7.151370626209529, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 139 + stamp: + secs: 1624875207 + nsecs: 960160017 + frame_id: "/world" +pose: + pose: + position: + x: -0.0526354817788 + y: 0.0474527919433 + z: 0.0 + orientation: + x: 0.0 + y: 0.000327659393797 + z: 0.0 + w: 0.99999994632 + covariance: [8.414681718629161, -1.7281776678117347e-09, 0.0, 0.0, -3.0223011044057723e-05, 0.0, -1.7281776678117362e-09, 8.414681747443408, 0.0, 0.0, 7.044490213201151e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0223011044057726e-05, 7.044490213201149e-06, 0.0, 0.0, 8.347601133476493, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 140 + stamp: + secs: 1624875208 + nsecs: 40215015 + frame_id: "/world" +pose: + pose: + position: + x: -0.0553558911999 + y: 0.0502097829513 + z: 0.0 + orientation: + x: 0.0 + y: 0.00165480937865 + z: 0.0 + w: 0.999998630802 + covariance: [11.56664359102057, -3.3696835843063628e-09, 0.0, 0.0, -4.113028078024783e-05, 0.0, -3.369683584306365e-09, 11.566643644822836, 0.0, 0.0, 1.110562042255045e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.113028078024783e-05, 1.1105620422550447e-05, 0.0, 0.0, 11.439108764556975, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 141 + stamp: + secs: 1624875208 + nsecs: 120165110 + frame_id: "/world" +pose: + pose: + position: + x: -0.0551797716369 + y: 0.0501227728728 + z: 0.0 + orientation: + x: 0.0 + y: 0.000273577753018 + z: 0.0 + w: 0.999999962578 + covariance: [9.091310172058941, -1.8182472738716208e-09, 0.0, 0.0, -2.1650833548456706e-05, 0.0, -1.8182472738716216e-09, 9.091310200522797, 0.0, 0.0, 5.562749142179315e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1650833548456716e-05, 5.562749142179315e-06, 0.0, 0.0, 9.022889177870363, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 142 + stamp: + secs: 1624875208 + nsecs: 200181961 + frame_id: "/world" +pose: + pose: + position: + x: -0.0552709062456 + y: 0.0507339983546 + z: 0.0 + orientation: + x: 0.0 + y: 0.000575468756623 + z: 0.0 + w: 0.999999834418 + covariance: [9.276684570630454, -2.4033385069307367e-09, 0.0, 0.0, -3.335837582235645e-05, 0.0, -2.4033385069307375e-09, 9.276684605680192, 0.0, 0.0, 1.5054715593458989e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.335837582235646e-05, 1.5054715593458992e-05, 0.0, 0.0, 9.19032093760843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 143 + stamp: + secs: 1624875208 + nsecs: 280190945 + frame_id: "/world" +pose: + pose: + position: + x: -0.0544202290469 + y: 0.0503397642511 + z: 0.0 + orientation: + x: 0.0 + y: 0.000252221257907 + z: 0.0 + w: 0.999999968192 + covariance: [9.360418777008881, -2.9545815324364646e-09, 0.0, 0.0, -4.250625397628083e-05, 0.0, -2.9545815324364666e-09, 9.360418815970595, 0.0, 0.0, 2.067052061121112e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.2506253976280845e-05, 2.0670520611211117e-05, 0.0, 0.0, 9.262036482546465, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 144 + stamp: + secs: 1624875208 + nsecs: 360193968 + frame_id: "/world" +pose: + pose: + position: + x: -0.0543521798515 + y: 0.0504648476661 + z: 0.0 + orientation: + x: 0.0 + y: 0.000306479606011 + z: 0.0 + w: 0.999999953035 + covariance: [6.956891337061026, -2.184692733035926e-09, 0.0, 0.0, -2.4828292566667605e-05, 0.0, -2.184692733035928e-09, 6.956891362423589, 0.0, 0.0, 2.4001264991781627e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4828292566667602e-05, 2.400126499178161e-05, 0.0, 0.0, 6.8911058066548945, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 145 + stamp: + secs: 1624875208 + nsecs: 440201998 + frame_id: "/world" +pose: + pose: + position: + x: -0.0544863146037 + y: 0.0505573716899 + z: 0.0 + orientation: + x: 0.0 + y: 4.89497561093e-05 + z: 0.0 + w: 0.999999998802 + covariance: [6.3379402900262285, -1.9243962892352826e-09, 0.0, 0.0, -1.3211349242841948e-05, 0.0, -1.9243962892352834e-09, 6.33794031003906, 0.0, 0.0, 2.1927071135828655e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3211349242841945e-05, 2.1927071135828638e-05, 0.0, 0.0, 6.2854992499142135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 146 + stamp: + secs: 1624875208 + nsecs: 520207882 + frame_id: "/world" +pose: + pose: + position: + x: -0.0572166093362 + y: 0.0530239796646 + z: 0.0 + orientation: + x: 0.0 + y: 1.8103978634e-05 + z: 0.0 + w: 0.999999999836 + covariance: [7.270214796795777, -2.160880361900544e-09, 0.0, 0.0, -9.266504436621238e-06, 0.0, -2.1608803619005447e-09, 7.270214817945915, 0.0, 0.0, 1.9360993188032343e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.266504436621242e-06, 1.9360993188032336e-05, 0.0, 0.0, 7.214602775603673, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 147 + stamp: + secs: 1624875208 + nsecs: 600233078 + frame_id: "/world" +pose: + pose: + position: + x: -0.0651464908429 + y: 0.0508651812408 + z: 0.0 + orientation: + x: 0.0 + y: 0.00381077668692 + z: 0.0 + w: 0.999992738964 + covariance: [10.664326310339947, -2.7135641842194643e-09, 0.0, 0.0, -1.729462176451981e-05, 0.0, -2.7135641842194643e-09, 10.6643263359864, 0.0, 0.0, 2.1190106916090973e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.729462176451981e-05, 2.1190106916090966e-05, 0.0, 0.0, 10.596697903073082, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 148 + stamp: + secs: 1624875208 + nsecs: 680222988 + frame_id: "/world" +pose: + pose: + position: + x: -0.0654167497932 + y: 0.054391774801 + z: 0.0 + orientation: + x: 0.0 + y: 0.00164429440374 + z: 0.0 + w: 0.999998648147 + covariance: [11.31098166278718, -3.893899353379644e-09, 0.0, 0.0, -5.6203801083069885e-05, 0.0, -3.8938993533796434e-09, 11.31098169599069, 0.0, 0.0, 2.243541792665306e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.620380108306988e-05, 2.2435417926653053e-05, 0.0, 0.0, 11.221391980960899, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 149 + stamp: + secs: 1624875208 + nsecs: 760227919 + frame_id: "/world" +pose: + pose: + position: + x: -0.0651100630841 + y: 0.0554773485993 + z: 0.0 + orientation: + x: -0.0 + y: -0.000212752722516 + z: -0.0 + w: 0.999999977368 + covariance: [7.375336531115688, -2.6883671592726957e-09, 0.0, 0.0, -3.9750448029968936e-05, 0.0, -2.6883671592726957e-09, 7.375336551289715, 0.0, 0.0, 1.4480670800563622e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.975044802996893e-05, 1.4480670800563623e-05, 0.0, 0.0, 7.318176852081374, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 150 + stamp: + secs: 1624875208 + nsecs: 840251923 + frame_id: "/world" +pose: + pose: + position: + x: -0.0615147137741 + y: 0.0882993852084 + z: 0.0 + orientation: + x: -0.0 + y: -0.000115641229226 + z: -0.0 + w: 0.999999993314 + covariance: [6.444195421309092, -2.111913802922803e-09, 0.0, 0.0, -2.4380538396054102e-05, 0.0, -2.1119138029228034e-09, 6.444195435419457, 0.0, 0.0, 1.5206329567741403e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4380538396054102e-05, 1.5206329567741397e-05, 0.0, 0.0, 6.402979747310026, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 151 + stamp: + secs: 1624875208 + nsecs: 920262098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0615041036046 + y: 0.0883452176358 + z: 0.0 + orientation: + x: 0.0 + y: 0.000310944405874 + z: 0.0 + w: 0.999999951657 + covariance: [10.057743802426893, -4.327438059930475e-09, 0.0, 0.0, -3.732313134892521e-05, 0.0, -4.327438059930475e-09, 10.057743828329142, 0.0, 0.0, 3.0997557089915934e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7323131348925194e-05, 3.099755708991593e-05, 0.0, 0.0, 9.980547035736576, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 152 + stamp: + secs: 1624875209 + nsecs: 256062 + frame_id: "/world" +pose: + pose: + position: + x: -0.0610032104159 + y: 0.0875508175815 + z: 0.0 + orientation: + x: 0.0 + y: 0.000246998957053 + z: 0.0 + w: 0.999999969496 + covariance: [9.668568063690339, -4.5540118692694894e-09, 0.0, 0.0, -2.7412292922728392e-05, 0.0, -4.5540118692694894e-09, 9.668568088950678, 0.0, 0.0, 3.384598896537729e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7412292922728375e-05, 3.384598896537727e-05, 0.0, 0.0, 9.592053431541236, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 153 + stamp: + secs: 1624875209 + nsecs: 80250978 + frame_id: "/world" +pose: + pose: + position: + x: -0.0607190040854 + y: 0.0877525656858 + z: 0.0 + orientation: + x: 0.0 + y: 0.000187608347056 + z: 0.0 + w: 0.999999982402 + covariance: [10.829891628162668, -8.723509355725396e-09, 0.0, 0.0, -2.9244149988576057e-05, 0.0, -8.723509355725396e-09, 10.8298916733383, 0.0, 0.0, 5.528366695424347e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9244149988576043e-05, 5.528366695424345e-05, 0.0, 0.0, 10.69172281797864, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 154 + stamp: + secs: 1624875209 + nsecs: 160267115 + frame_id: "/world" +pose: + pose: + position: + x: -0.0594469015574 + y: 0.0886398935282 + z: 0.0 + orientation: + x: 0.0 + y: 0.000214720585167 + z: 0.0 + w: 0.999999976948 + covariance: [10.010611843861962, -8.468860250703496e-09, 0.0, 0.0, -1.8300827745647474e-05, 0.0, -8.468860250703494e-09, 10.01061188658705, 0.0, 0.0, 4.7364027458434726e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.830082774564747e-05, 4.7364027458434726e-05, 0.0, 0.0, 9.878805610649094, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 155 + stamp: + secs: 1624875209 + nsecs: 240287066 + frame_id: "/world" +pose: + pose: + position: + x: -0.0604745511625 + y: 0.0924090287805 + z: 0.0 + orientation: + x: 0.0 + y: 8.78225883622e-05 + z: 0.0 + w: 0.999999996144 + covariance: [12.80434032108651, -1.688850015847961e-08, 0.0, 0.0, -2.1904852286412305e-05, 0.0, -1.6888500158479602e-08, 12.804340404734388, 0.0, 0.0, 7.754159436377395e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.19048522864123e-05, 7.754159436377395e-05, 0.0, 0.0, 12.545740678017884, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 156 + stamp: + secs: 1624875209 + nsecs: 320278883 + frame_id: "/world" +pose: + pose: + position: + x: -0.0603786195445 + y: 0.0924717947534 + z: 0.0 + orientation: + x: 0.0 + y: 0.000426240048495 + z: 0.0 + w: 0.99999990916 + covariance: [10.802833164367774, -1.4614279978323301e-08, 0.0, 0.0, -4.5920883165877596e-05, 0.0, -1.46142799783233e-08, 10.802833234315777, 0.0, 0.0, 6.320762016939585e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.59208831658776e-05, 6.320762016939585e-05, 0.0, 0.0, 10.583016365207628, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 157 + stamp: + secs: 1624875209 + nsecs: 400302887 + frame_id: "/world" +pose: + pose: + position: + x: -0.0603684543081 + y: 0.0927046492324 + z: 0.0 + orientation: + x: 0.0 + y: 0.000382750001533 + z: 0.0 + w: 0.999999926751 + covariance: [8.891945530766213, -9.10448914324255e-09, 0.0, 0.0, -4.045154341645455e-05, 0.0, -9.104489143242546e-09, 8.891945572249433, 0.0, 0.0, 3.814444648384035e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.045154341645456e-05, 3.814444648384033e-05, 0.0, 0.0, 8.758903866925792, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 158 + stamp: + secs: 1624875209 + nsecs: 480295897 + frame_id: "/world" +pose: + pose: + position: + x: -0.0604552656782 + y: 0.0930407149434 + z: 0.0 + orientation: + x: 0.0 + y: 0.000526804038043 + z: 0.0 + w: 0.999999861239 + covariance: [8.27481597652931, -8.846627106911211e-09, 0.0, 0.0, -3.5538308257135635e-05, 0.0, -8.846627106911208e-09, 8.274816014043816, 0.0, 0.0, 3.490905476503107e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5538308257135635e-05, 3.490905476503105e-05, 0.0, 0.0, 8.151559137518946, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 159 + stamp: + secs: 1624875209 + nsecs: 560297012 + frame_id: "/world" +pose: + pose: + position: + x: -0.062989646739 + y: 0.0912434997297 + z: 0.0 + orientation: + x: 0.0 + y: 0.000235879799356 + z: 0.0 + w: 0.99999997218 + covariance: [7.302326756051475, -6.19145260638392e-09, 0.0, 0.0, -1.996946638609721e-05, 0.0, -6.191452606383917e-09, 7.3023267810786665, 0.0, 0.0, 2.160437960340071e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9969466386097207e-05, 2.16043796034007e-05, 0.0, 0.0, 7.218331216544132, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 160 + stamp: + secs: 1624875209 + nsecs: 640314102 + frame_id: "/world" +pose: + pose: + position: + x: -0.0627360525981 + y: 0.087607233021 + z: 0.0 + orientation: + x: 0.0 + y: 0.000173554798832 + z: 0.0 + w: 0.999999984939 + covariance: [9.489713802108358, -9.945644842733902e-09, 0.0, 0.0, -3.529895192066662e-05, 0.0, -9.945644842733899e-09, 9.489713840144862, 0.0, 0.0, 2.6941102784009624e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.52989519206666e-05, 2.6941102784009614e-05, 0.0, 0.0, 9.35942431815401, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 161 + stamp: + secs: 1624875209 + nsecs: 720318079 + frame_id: "/world" +pose: + pose: + position: + x: -0.0624946181861 + y: 0.0876490447431 + z: 0.0 + orientation: + x: 0.0 + y: 0.000118113702905 + z: 0.0 + w: 0.999999993025 + covariance: [8.469670997390306, -8.923686266727836e-09, 0.0, 0.0, -6.609165932169292e-05, 0.0, -8.923686266727841e-09, 8.4696710282805, 0.0, 0.0, 2.5624229299049516e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.609165932169295e-05, 2.5624229299049512e-05, 0.0, 0.0, 8.358388332576164, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 162 + stamp: + secs: 1624875209 + nsecs: 800335884 + frame_id: "/world" +pose: + pose: + position: + x: -0.065358309703 + y: 0.0905268229374 + z: 0.0 + orientation: + x: 0.0 + y: 0.000213824915872 + z: 0.0 + w: 0.999999977139 + covariance: [7.3735958497492495, -8.14568635085441e-09, 0.0, 0.0, -6.812098707268805e-05, 0.0, -8.14568635085441e-09, 7.373595872928433, 0.0, 0.0, 2.1479980188348905e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.812098707268806e-05, 2.1479980188348882e-05, 0.0, 0.0, 7.2802707248444385, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 163 + stamp: + secs: 1624875209 + nsecs: 880331039 + frame_id: "/world" +pose: + pose: + position: + x: -0.0653767133668 + y: 0.0905572157374 + z: 0.0 + orientation: + x: 0.0 + y: 0.000236269492658 + z: 0.0 + w: 0.999999972088 + covariance: [7.689630015032313, -7.4289025777378055e-09, 0.0, 0.0, -4.908285900278426e-05, 0.0, -7.428902577737806e-09, 7.689630033577643, 0.0, 0.0, 1.5843121828872007e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.9082859002784284e-05, 1.5843121828872e-05, 0.0, 0.0, 7.6085707989065785, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 164 + stamp: + secs: 1624875209 + nsecs: 960352898 + frame_id: "/world" +pose: + pose: + position: + x: -0.0670162056739 + y: 0.0874722199536 + z: 0.0 + orientation: + x: 0.0 + y: 0.000390085451194 + z: 0.0 + w: 0.999999923917 + covariance: [7.111465754217329, -5.677159263083617e-09, 0.0, 0.0, -2.8267402206741476e-05, 0.0, -5.677159263083617e-09, 7.111465766897703, 0.0, 0.0, 9.425595974755963e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.826740220674149e-05, 9.425595974755963e-06, 0.0, 0.0, 7.051897635863527, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 165 + stamp: + secs: 1624875210 + nsecs: 40358067 + frame_id: "/world" +pose: + pose: + position: + x: -0.0683245467124 + y: 0.089681841905 + z: 0.0 + orientation: + x: 0.0 + y: 0.000419909152 + z: 0.0 + w: 0.999999911838 + covariance: [6.925473396705636, -5.005882106299054e-09, 0.0, 0.0, -1.53238213264763e-05, 0.0, -5.005882106299055e-09, 6.925473406898262, 0.0, 0.0, 5.8766165250506775e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.532382132647631e-05, 5.876616525050679e-06, 0.0, 0.0, 6.87411547391299, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 166 + stamp: + secs: 1624875210 + nsecs: 120342016 + frame_id: "/world" +pose: + pose: + position: + x: -0.0684397127554 + y: 0.0902278537722 + z: 0.0 + orientation: + x: 0.0 + y: 0.000144791187036 + z: 0.0 + w: 0.999999989518 + covariance: [6.862953133410115, -4.63513766301716e-09, 0.0, 0.0, -7.241722078945175e-06, 0.0, -4.635137663017162e-09, 6.862953142270873, 0.0, 0.0, 4.77291941305279e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.241722078945181e-06, 4.772919413052791e-06, 0.0, 0.0, 6.815860796681202, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 167 + stamp: + secs: 1624875210 + nsecs: 200356007 + frame_id: "/world" +pose: + pose: + position: + x: -0.0677525044481 + y: 0.0889268876701 + z: 0.0 + orientation: + x: -0.0 + y: -1.14545539705e-05 + z: -0.0 + w: 0.999999999934 + covariance: [6.836365786305241, -3.919793638864386e-09, 0.0, 0.0, -3.952543710669084e-06, 0.0, -3.919793638864387e-09, 6.83636579351851, 0.0, 0.0, 3.5675286337656294e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.952543710669086e-06, 3.567528633765631e-06, 0.0, 0.0, 6.7966104291124765, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 168 + stamp: + secs: 1624875210 + nsecs: 280395031 + frame_id: "/world" +pose: + pose: + position: + x: -0.0714763939681 + y: 0.095146207254 + z: 0.0 + orientation: + x: 0.0 + y: 8.7584788216e-05 + z: 0.0 + w: 0.999999996164 + covariance: [7.485099594920979, -6.390798971192872e-09, 0.0, 0.0, -6.639135518791788e-06, 0.0, -6.3907989711928714e-09, 7.48509960627185, 0.0, 0.0, 3.7842497939865835e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.6391355187917894e-06, 3.784249793986585e-06, 0.0, 0.0, 7.420083096725052, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 169 + stamp: + secs: 1624875210 + nsecs: 360379934 + frame_id: "/world" +pose: + pose: + position: + x: -0.071602836204 + y: 0.0953587522889 + z: 0.0 + orientation: + x: 0.0 + y: 0.000274675491297 + z: 0.0 + w: 0.999999962277 + covariance: [8.55679124053225, -8.141745785440454e-09, 0.0, 0.0, -2.0959806192574718e-05, 0.0, -8.141745785440454e-09, 8.556791254658915, 0.0, 0.0, 4.154940994838682e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0959806192574718e-05, 4.154940994838682e-06, 0.0, 0.0, 8.473613248795557, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 170 + stamp: + secs: 1624875210 + nsecs: 440391064 + frame_id: "/world" +pose: + pose: + position: + x: -0.0711354918431 + y: 0.0952215252298 + z: 0.0 + orientation: + x: 0.0 + y: 0.000383220672044 + z: 0.0 + w: 0.999999926571 + covariance: [9.05918491275364, -1.050486771455722e-08, 0.0, 0.0, -3.1632183983337616e-05, 0.0, -1.050486771455722e-08, 9.059184930353958, 0.0, 0.0, 4.385646597549421e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.1632183983337616e-05, 4.38564659754942e-06, 0.0, 0.0, 8.95109047363674, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 171 + stamp: + secs: 1624875210 + nsecs: 520392895 + frame_id: "/world" +pose: + pose: + position: + x: -0.0667549295503 + y: 0.0890447175755 + z: 0.0 + orientation: + x: 0.0 + y: 0.000434150283879 + z: 0.0 + w: 0.999999905757 + covariance: [8.303769751011767, -8.218010551480477e-09, 0.0, 0.0, -2.5779486358916677e-05, 0.0, -8.218010551480478e-09, 8.303769764444755, 0.0, 0.0, 2.743818685313314e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5779486358916684e-05, 2.743818685313314e-06, 0.0, 0.0, 8.21865392177844, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 172 + stamp: + secs: 1624875210 + nsecs: 600393057 + frame_id: "/world" +pose: + pose: + position: + x: -0.0674290962046 + y: 0.088755640247 + z: 0.0 + orientation: + x: 0.0 + y: 0.0011758505508 + z: 0.0 + w: 0.999999308688 + covariance: [10.076142307101978, -1.3508287019896064e-08, 0.0, 0.0, -4.7142227165322045e-05, 0.0, -1.3508287019896067e-08, 10.076142328195358, 0.0, 0.0, 1.0826392415117583e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.714222716532206e-05, 1.0826392415117583e-05, 0.0, 0.0, 9.935810645707686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 173 + stamp: + secs: 1624875210 + nsecs: 680402040 + frame_id: "/world" +pose: + pose: + position: + x: -0.0674034307709 + y: 0.0890357717124 + z: 0.0 + orientation: + x: 0.0 + y: 0.000489899473388 + z: 0.0 + w: 0.999999879999 + covariance: [8.672944899474247, -1.1712703917852664e-08, 0.0, 0.0, -3.7493503307593155e-05, 0.0, -1.1712703917852672e-08, 8.672944916993467, 0.0, 0.0, 1.4679560750997515e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.749350330759315e-05, 1.4679560750997512e-05, 0.0, 0.0, 8.550548291018433, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 174 + stamp: + secs: 1624875210 + nsecs: 760426044 + frame_id: "/world" +pose: + pose: + position: + x: -0.0673770112846 + y: 0.0891953754435 + z: 0.0 + orientation: + x: 0.0 + y: 4.54551881465e-05 + z: 0.0 + w: 0.999999998967 + covariance: [9.113720900893439, -1.2388407213461635e-08, 0.0, 0.0, -3.908800295388416e-05, 0.0, -1.238840721346164e-08, 9.113720918659928, 0.0, 0.0, 2.483939448705318e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.908800295388416e-05, 2.4839394487053176e-05, 0.0, 0.0, 8.984777905137644, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 175 + stamp: + secs: 1624875210 + nsecs: 840425968 + frame_id: "/world" +pose: + pose: + position: + x: -0.0674566766479 + y: 0.0893492280149 + z: 0.0 + orientation: + x: -0.0 + y: -0.000235322323551 + z: -0.0 + w: 0.999999972312 + covariance: [9.300419255278255, -1.246551271484872e-08, 0.0, 0.0, -5.356387839918151e-05, 0.0, -1.2465512714848722e-08, 9.300419271982928, 0.0, 0.0, 3.297414300914958e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.356387839918151e-05, 3.297414300914957e-05, 0.0, 0.0, 9.173773395124538, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 176 + stamp: + secs: 1624875210 + nsecs: 920433044 + frame_id: "/world" +pose: + pose: + position: + x: -0.0638156349384 + y: 0.0818455869704 + z: 0.0 + orientation: + x: -0.0 + y: -0.000206646121422 + z: -0.0 + w: 0.999999978649 + covariance: [7.605881132950513, -9.31348367917051e-09, 0.0, 0.0, -6.804986645719923e-05, 0.0, -9.313483679170508e-09, 7.605881143219903, 0.0, 0.0, 2.2430080416189723e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.804986645719924e-05, 2.2430080416189712e-05, 0.0, 0.0, 7.515838436333252, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 177 + stamp: + secs: 1624875211 + nsecs: 436068 + frame_id: "/world" +pose: + pose: + position: + x: -0.064186282155 + y: 0.0822708672163 + z: 0.0 + orientation: + x: -0.0 + y: -0.000122115935874 + z: -0.0 + w: 0.999999992544 + covariance: [8.638644725224129, -1.3486303518595844e-08, 0.0, 0.0, -0.00012285533659181174, 0.0, -1.3486303518595847e-08, 8.638644733516214, 0.0, 0.0, 2.4591075403114134e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00012285533659181177, 2.4591075403114127e-05, 0.0, 0.0, 8.517966277357287, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 178 + stamp: + secs: 1624875211 + nsecs: 80427885 + frame_id: "/world" +pose: + pose: + position: + x: -0.0637543846903 + y: 0.0776712156116 + z: 0.0 + orientation: + x: 0.0 + y: 7.48996434248e-05 + z: 0.0 + w: 0.999999997195 + covariance: [7.418650957254186, -1.0702184035525193e-08, 0.0, 0.0, -8.510055884810425e-05, 0.0, -1.0702184035525195e-08, 7.4186509596888435, 0.0, 0.0, 1.4614374068271068e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.510055884810426e-05, 1.461437406827106e-05, 0.0, 0.0, 7.327320228172534, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 179 + stamp: + secs: 1624875211 + nsecs: 160442114 + frame_id: "/world" +pose: + pose: + position: + x: -0.0630875910835 + y: 0.0773653023406 + z: 0.0 + orientation: + x: 0.0 + y: 0.000347446088569 + z: 0.0 + w: 0.999999939641 + covariance: [7.019364780815247, -6.7875760100892094e-09, 0.0, 0.0, -4.539524417851057e-05, 0.0, -6.7875760100892094e-09, 7.019364780993988, 0.0, 0.0, 8.076754804054044e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.539524417851056e-05, 8.076754804054041e-06, 0.0, 0.0, 6.962794868444836, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 180 + stamp: + secs: 1624875211 + nsecs: 240469933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0642331875532 + y: 0.076568113063 + z: 0.0 + orientation: + x: 0.0 + y: 9.13823647978e-05 + z: 0.0 + w: 0.999999995825 + covariance: [6.357795103094717, -6.694679566469338e-09, 0.0, 0.0, -3.402567840340567e-05, 0.0, -6.694679566469336e-09, 6.357795101581306, 0.0, 0.0, 2.1800961834300426e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.4025678403405663e-05, 2.1800961834300433e-05, 0.0, 0.0, 6.305662526158487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 181 + stamp: + secs: 1624875211 + nsecs: 320468903 + frame_id: "/world" +pose: + pose: + position: + x: -0.0645004623899 + y: 0.077265903976 + z: 0.0 + orientation: + x: -0.0 + y: -9.59026191496e-05 + z: -0.0 + w: 0.999999995401 + covariance: [5.702690780131673, -2.5318813949345575e-09, 0.0, 0.0, -1.2118174604145205e-05, 0.0, -2.5318813949345562e-09, 5.702690779467333, 0.0, 0.0, 9.700088651426859e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2118174604145204e-05, 9.70008865142686e-06, 0.0, 0.0, 5.683396900564999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 182 + stamp: + secs: 1624875211 + nsecs: 400448084 + frame_id: "/world" +pose: + pose: + position: + x: -0.0645826760565 + y: 0.0771475139003 + z: 0.0 + orientation: + x: 0.0 + y: 0.000303085342513 + z: 0.0 + w: 0.99999995407 + covariance: [7.018584979011878, -8.212112739794991e-09, 0.0, 0.0, -3.5220012967455816e-05, 0.0, -8.212112739794991e-09, 7.018584976360864, 0.0, 0.0, 3.5110445353036324e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5220012967455816e-05, 3.5110445353036324e-05, 0.0, 0.0, 6.963614993620737, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 183 + stamp: + secs: 1624875211 + nsecs: 480456114 + frame_id: "/world" +pose: + pose: + position: + x: -0.0643831748144 + y: 0.0768258878274 + z: 0.0 + orientation: + x: 0.0 + y: 4.89784353536e-05 + z: 0.0 + w: 0.999999998801 + covariance: [6.864417457991509, -2.8573479680107383e-09, 0.0, 0.0, -1.2300159802557137e-05, 0.0, -2.8573479680107383e-09, 6.864417457064917, 0.0, 0.0, 1.2280548069307739e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.230015980255714e-05, 1.2280548069307744e-05, 0.0, 0.0, 6.845246434296592, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 184 + stamp: + secs: 1624875211 + nsecs: 560573101 + frame_id: "/world" +pose: + pose: + position: + x: -0.0649794715692 + y: 0.0768045894808 + z: 0.0 + orientation: + x: 0.0 + y: 0.000212989428076 + z: 0.0 + w: 0.999999977318 + covariance: [7.4841116676868715, -7.339861321770579e-09, 0.0, 0.0, -1.9357700827015586e-05, 0.0, -7.339861321770579e-09, 7.484111665392173, 0.0, 0.0, 1.904769186943216e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9357700827015586e-05, 1.9047691869432164e-05, 0.0, 0.0, 7.439263816441796, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 185 + stamp: + secs: 1624875211 + nsecs: 640486956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0648074791914 + y: 0.0766106779714 + z: 0.0 + orientation: + x: -0.0 + y: -0.000161030510187 + z: -0.0 + w: 0.999999987035 + covariance: [5.989265237225309, -5.52587946952387e-09, 0.0, 0.0, -9.08409999846855e-06, 0.0, -5.525879469523868e-09, 5.989265235526431, 0.0, 0.0, 8.608091294726054e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.084099998468556e-06, 8.608091294726053e-06, 0.0, 0.0, 5.956288848118261, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 186 + stamp: + secs: 1624875211 + nsecs: 720500946 + frame_id: "/world" +pose: + pose: + position: + x: -0.06432153393 + y: 0.0766283152631 + z: 0.0 + orientation: + x: -0.0 + y: -0.000255399250316 + z: -0.0 + w: 0.999999967386 + covariance: [5.627704734517555, -5.7457789778686906e-09, 0.0, 0.0, -5.22541915643028e-06, 0.0, -5.745778977868692e-09, 5.627704732773036, 0.0, 0.0, 4.739498755996313e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.225419156430284e-06, 4.739498755996313e-06, 0.0, 0.0, 5.593799080770885, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 187 + stamp: + secs: 1624875211 + nsecs: 800513029 + frame_id: "/world" +pose: + pose: + position: + x: -0.069799277725 + y: 0.0715132442794 + z: 0.0 + orientation: + x: -0.0 + y: -0.000254574243191 + z: -0.0 + w: 0.999999967596 + covariance: [7.629737818371217, -8.12008128216028e-09, 0.0, 0.0, -3.39688834672889e-06, 0.0, -8.120081282160282e-09, 7.629737815921798, 0.0, 0.0, 4.131786451610706e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.3968883467288934e-06, 4.131786451610706e-06, 0.0, 0.0, 7.581799449730552, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 188 + stamp: + secs: 1624875211 + nsecs: 880506992 + frame_id: "/world" +pose: + pose: + position: + x: -0.06987227977 + y: 0.0717055801752 + z: 0.0 + orientation: + x: 0.0 + y: 1.94774869033e-05 + z: 0.0 + w: 0.99999999981 + covariance: [6.018412320845307, -4.990293452156556e-09, 0.0, 0.0, -5.508165891128941e-07, 0.0, -4.990293452156553e-09, 6.01841231934603, 0.0, 0.0, 1.5435554707059462e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.508165891128954e-07, 1.543555470705947e-06, 0.0, 0.0, 5.988742694261827, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 189 + stamp: + secs: 1624875211 + nsecs: 960508108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0669018728837 + y: 0.0791102968702 + z: 0.0 + orientation: + x: -0.0 + y: -0.00059346157969 + z: -0.0 + w: 0.999999823902 + covariance: [9.771800860085188, -1.8389608020280644e-08, 0.0, 0.0, -3.6674869413038455e-06, 0.0, -1.8389608020280634e-08, 9.771800854573405, 0.0, 0.0, 1.7259784561202466e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.667486941303848e-06, 1.7259784561202472e-06, 0.0, 0.0, 9.661639963851176, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 190 + stamp: + secs: 1624875212 + nsecs: 40527105 + frame_id: "/world" +pose: + pose: + position: + x: -0.0671447183967 + y: 0.079399946683 + z: 0.0 + orientation: + x: -0.0 + y: -3.90408158208e-05 + z: -0.0 + w: 0.999999999238 + covariance: [7.710339744536157, -9.835623115991636e-09, 0.0, 0.0, -3.24815815483313e-06, 0.0, -9.835623115991633e-09, 7.710339741587531, 0.0, 0.0, 1.6691145724359905e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.2481581548331324e-06, 1.669114572435992e-06, 0.0, 0.0, 7.65080483000845, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 191 + stamp: + secs: 1624875212 + nsecs: 120568991 + frame_id: "/world" +pose: + pose: + position: + x: -0.0758594753221 + y: 0.0872578798723 + z: 0.0 + orientation: + x: -0.0 + y: -0.000149951582123 + z: -0.0 + w: 0.999999988757 + covariance: [7.847142885542752, -1.5925789664681933e-08, 0.0, 0.0, -5.544850122826242e-06, 0.0, -1.5925789664681926e-08, 7.847142880765877, 0.0, 0.0, 4.184621565051354e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.544850122826244e-06, 4.184621565051354e-06, 0.0, 0.0, 7.749210885877313, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 192 + stamp: + secs: 1624875212 + nsecs: 200536013 + frame_id: "/world" +pose: + pose: + position: + x: -0.0759416085309 + y: 0.0872348201874 + z: 0.0 + orientation: + x: -0.0 + y: -0.000292642815665 + z: -0.0 + w: 0.99999995718 + covariance: [7.878560736461114, -1.4856280130385023e-08, 0.0, 0.0, -4.244562309125132e-06, 0.0, -1.4856280130385023e-08, 7.87856073200425, 0.0, 0.0, 3.3108096362936802e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.244562309125135e-06, 3.31080963629368e-06, 0.0, 0.0, 7.786076746013208, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 193 + stamp: + secs: 1624875212 + nsecs: 280575037 + frame_id: "/world" +pose: + pose: + position: + x: -0.0775036085623 + y: 0.0869906630496 + z: 0.0 + orientation: + x: -0.0 + y: -0.000545294707222 + z: -0.0 + w: 0.999999851327 + covariance: [8.732594000411433, -1.4350898977398843e-08, 0.0, 0.0, -3.5345396629335378e-06, 0.0, -1.4350898977398838e-08, 8.73259399610498, 0.0, 0.0, 2.3678581949538095e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.5345396629335403e-06, 2.3678581949538095e-06, 0.0, 0.0, 8.642373290940599, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 194 + stamp: + secs: 1624875212 + nsecs: 360554934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0778189843595 + y: 0.0863698777112 + z: 0.0 + orientation: + x: 0.0 + y: 0.000124553522561 + z: 0.0 + w: 0.999999992243 + covariance: [10.24555232525173, -1.1502041446210719e-08, 0.0, 0.0, -2.5648865468341656e-06, 0.0, -1.1502041446210714e-08, 10.245552321799526, 0.0, 0.0, 1.3813577441011839e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5648865468341677e-06, 1.3813577441011836e-06, 0.0, 0.0, 10.172859585276301, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 195 + stamp: + secs: 1624875212 + nsecs: 440562963 + frame_id: "/world" +pose: + pose: + position: + x: -0.0776955143081 + y: 0.0861583779382 + z: 0.0 + orientation: + x: -0.0 + y: -0.000168175241485 + z: -0.0 + w: 0.999999985859 + covariance: [8.70460367646707, -1.1975699814116953e-08, 0.0, 0.0, -1.0043856042590402e-05, 0.0, -1.1975699814116948e-08, 8.704603672856893, 0.0, 0.0, 4.367428621493413e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0043856042590404e-05, 4.367428621493414e-06, 0.0, 0.0, 8.62766703186998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 196 + stamp: + secs: 1624875212 + nsecs: 520561934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0808357936979 + y: 0.0862559911165 + z: 0.0 + orientation: + x: 0.0 + y: 2.19509910757e-05 + z: 0.0 + w: 0.999999999759 + covariance: [9.126050209809087, -1.6537893620885326e-08, 0.0, 0.0, -5.935532451307784e-05, 0.0, -1.6537893620885316e-08, 9.126050204105518, 0.0, 0.0, 8.087059506122517e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.9355324513077824e-05, 8.087059506122517e-06, 0.0, 0.0, 9.018702109496472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 197 + stamp: + secs: 1624875212 + nsecs: 600589991 + frame_id: "/world" +pose: + pose: + position: + x: -0.0809388919923 + y: 0.0862151587872 + z: 0.0 + orientation: + x: 0.0 + y: 0.000427455809521 + z: 0.0 + w: 0.999999908641 + covariance: [9.29817844910503, -1.682952189869451e-08, 0.0, 0.0, -7.652153184415569e-05, 0.0, -1.68295218986945e-08, 9.298178441557086, 0.0, 0.0, 6.783944558095843e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.652153184415568e-05, 6.783944558095842e-06, 0.0, 0.0, 9.188536938469428, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 198 + stamp: + secs: 1624875212 + nsecs: 680576086 + frame_id: "/world" +pose: + pose: + position: + x: -0.0713131929553 + y: 0.116745653512 + z: 0.0 + orientation: + x: 0.0 + y: 0.000939986672819 + z: 0.0 + w: 0.999999558212 + covariance: [9.387291382714812, -1.9170396898759667e-08, 0.0, 0.0, -7.218758421624446e-05, 0.0, -1.917039689875965e-08, 9.38729137205671, 0.0, 0.0, 3.3376697624209957e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.218758421624448e-05, 3.337669762420995e-06, 0.0, 0.0, 9.260994940418831, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 199 + stamp: + secs: 1624875212 + nsecs: 760581970 + frame_id: "/world" +pose: + pose: + position: + x: -0.0579986693468 + y: 0.149519463176 + z: 0.0 + orientation: + x: -0.0 + y: -0.0002732363916 + z: -0.0 + w: 0.999999962671 + covariance: [7.6071584708632205, -1.0049798267121434e-08, 0.0, 0.0, -3.0744621536203854e-05, 0.0, -1.0049798267121424e-08, 7.607158464785454, 0.0, 0.0, 2.545238347039986e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.074462153620387e-05, 2.545238347039986e-06, 0.0, 0.0, 7.54034389335982, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 200 + stamp: + secs: 1624875212 + nsecs: 840596914 + frame_id: "/world" +pose: + pose: + position: + x: -0.0577952461299 + y: 0.149435779042 + z: 0.0 + orientation: + x: 0.0 + y: 0.000179467567357 + z: 0.0 + w: 0.999999983896 + covariance: [7.078118173935857, -7.146757430017939e-09, 0.0, 0.0, -1.5832717685819557e-05, 0.0, -7.1467574300179334e-09, 7.078118169256378, 0.0, 0.0, 4.198799868785044e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.583271768581956e-05, 4.198799868785042e-06, 0.0, 0.0, 7.030285934585913, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 201 + stamp: + secs: 1624875212 + nsecs: 920597076 + frame_id: "/world" +pose: + pose: + position: + x: -0.0578009970043 + y: 0.149609655869 + z: 0.0 + orientation: + x: 0.0 + y: 0.000262431496371 + z: 0.0 + w: 0.999999965565 + covariance: [8.348856566186615, -8.692600371925538e-09, 0.0, 0.0, -1.3016588632481835e-05, 0.0, -8.692600371925529e-09, 8.348856560153576, 0.0, 0.0, 6.073665122454936e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.301658863248184e-05, 6.073665122454934e-06, 0.0, 0.0, 8.290332987134356, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 202 + stamp: + secs: 1624875213 + nsecs: 623941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0648869930524 + y: 0.160999763656 + z: 0.0 + orientation: + x: 0.0 + y: 0.000234148330999 + z: 0.0 + w: 0.999999972587 + covariance: [8.919674194181892, -7.362258567208585e-09, 0.0, 0.0, -8.462262243431985e-06, 0.0, -7.362258567208581e-09, 8.919674188935844, 0.0, 0.0, 4.636671727199689e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.462262243431985e-06, 4.636671727199688e-06, 0.0, 0.0, 8.86981569140546, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 203 + stamp: + secs: 1624875213 + nsecs: 80640078 + frame_id: "/world" +pose: + pose: + position: + x: -0.0647752764025 + y: 0.16098855974 + z: 0.0 + orientation: + x: 0.0 + y: 0.000488176127738 + z: 0.0 + w: 0.999999880842 + covariance: [8.234187322371552, -6.444380642916246e-09, 0.0, 0.0, -5.345311455713887e-06, 0.0, -6.444380642916245e-09, 8.234187317666569, 0.0, 0.0, 5.140830687999859e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.345311455713886e-06, 5.140830687999858e-06, 0.0, 0.0, 8.190513262141277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 204 + stamp: + secs: 1624875213 + nsecs: 160621881 + frame_id: "/world" +pose: + pose: + position: + x: -0.0705581773746 + y: 0.161421436993 + z: 0.0 + orientation: + x: 0.0 + y: 0.00104183367585 + z: 0.0 + w: 0.999999457291 + covariance: [10.031348006822641, -1.6156466171781172e-08, 0.0, 0.0, -8.653257854433093e-06, 0.0, -1.615646617178117e-08, 10.031347994808012, 0.0, 0.0, 1.2333083724765819e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.653257854433094e-06, 1.2333083724765812e-05, 0.0, 0.0, 9.919402779650355, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 205 + stamp: + secs: 1624875213 + nsecs: 240650892 + frame_id: "/world" +pose: + pose: + position: + x: -0.0705621981038 + y: 0.161997072552 + z: 0.0 + orientation: + x: 0.0 + y: 0.000257737977977 + z: 0.0 + w: 0.999999966786 + covariance: [8.633364646300727, -1.2561903373294521e-08, 0.0, 0.0, -5.040840997634153e-06, 0.0, -1.2561903373294518e-08, 8.633364636904728, 0.0, 0.0, 7.280615555894683e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.040840997634155e-06, 7.280615555894682e-06, 0.0, 0.0, 8.54517457326381, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 206 + stamp: + secs: 1624875213 + nsecs: 320632935 + frame_id: "/world" +pose: + pose: + position: + x: -0.0705395638366 + y: 0.1621737086 + z: 0.0 + orientation: + x: 0.0 + y: 0.000284109382729 + z: 0.0 + w: 0.999999959641 + covariance: [7.402117573237779, -9.388767758297917e-09, 0.0, 0.0, -2.307859054577666e-05, 0.0, -9.388767758297914e-09, 7.4021175660526275, 0.0, 0.0, 3.367538906358908e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.307859054577666e-05, 3.367538906358907e-06, 0.0, 0.0, 7.335451702126676, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 207 + stamp: + secs: 1624875213 + nsecs: 400671005 + frame_id: "/world" +pose: + pose: + position: + x: -0.0711542463663 + y: 0.163794971352 + z: 0.0 + orientation: + x: 0.0 + y: 0.00034020950977 + z: 0.0 + w: 0.999999942129 + covariance: [7.0301245930688765, -9.166323482665889e-09, 0.0, 0.0, -3.4402365537248386e-05, 0.0, -9.166323482665884e-09, 7.03012458554465, 0.0, 0.0, 1.8499606629865858e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.440236553724838e-05, 1.849960662986586e-05, 0.0, 0.0, 6.965510207803547, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 208 + stamp: + secs: 1624875213 + nsecs: 480642080 + frame_id: "/world" +pose: + pose: + position: + x: -0.0716241204004 + y: 0.164955517122 + z: 0.0 + orientation: + x: 0.0 + y: 0.000353962823428 + z: 0.0 + w: 0.999999937355 + covariance: [7.576573663103091, -1.3140628778653531e-08, 0.0, 0.0, -3.087238690686374e-05, 0.0, -1.3140628778653525e-08, 7.576573652532563, 0.0, 0.0, 3.519497411489785e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0872386906863724e-05, 3.519497411489785e-05, 0.0, 0.0, 7.486612417483455, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 209 + stamp: + secs: 1624875213 + nsecs: 560708046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0723380269443 + y: 0.16484187283 + z: 0.0 + orientation: + x: 0.0 + y: 0.00023973300423 + z: 0.0 + w: 0.999999971264 + covariance: [7.097437063621975, -1.2364473481157014e-08, 0.0, 0.0, -1.9917081570284157e-05, 0.0, -1.2364473481157015e-08, 7.097437053879413, 0.0, 0.0, 2.4619189382050904e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.991708157028416e-05, 2.4619189382050907e-05, 0.0, 0.0, 7.013417502939897, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 210 + stamp: + secs: 1624875213 + nsecs: 640685081 + frame_id: "/world" +pose: + pose: + position: + x: -0.071282826782 + y: 0.166313499119 + z: 0.0 + orientation: + x: 0.0 + y: 0.000293504440394 + z: 0.0 + w: 0.999999956928 + covariance: [6.931548511338607, -1.0368231532850616e-08, 0.0, 0.0, -2.1644661942517233e-05, 0.0, -1.036823153285062e-08, 6.9315485032239375, 0.0, 0.0, 1.6292756017375845e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1644661942517233e-05, 1.6292756017375845e-05, 0.0, 0.0, 6.861697131767378, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 211 + stamp: + secs: 1624875213 + nsecs: 720674992 + frame_id: "/world" +pose: + pose: + position: + x: -0.0723260602155 + y: 0.167181440478 + z: 0.0 + orientation: + x: 0.0 + y: 0.00051335349927 + z: 0.0 + w: 0.999999868234 + covariance: [8.315660940669705, -1.5851307717467587e-08, 0.0, 0.0, -3.373104286803557e-05, 0.0, -1.5851307717467587e-08, 8.315660928135447, 0.0, 0.0, 1.7965777550723394e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.373104286803557e-05, 1.79657775507234e-05, 0.0, 0.0, 8.209987807966966, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 212 + stamp: + secs: 1624875213 + nsecs: 800682068 + frame_id: "/world" +pose: + pose: + position: + x: -0.0720948682338 + y: 0.167177811442 + z: 0.0 + orientation: + x: 0.0 + y: 0.000408927183225 + z: 0.0 + w: 0.999999916389 + covariance: [8.042541534872605, -1.3028974166532547e-08, 0.0, 0.0, -2.2143840030303365e-05, 0.0, -1.3028974166532549e-08, 8.042541524488833, 0.0, 0.0, 1.0812601021668432e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.214384003030336e-05, 1.0812601021668436e-05, 0.0, 0.0, 7.955717133152418, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 213 + stamp: + secs: 1624875213 + nsecs: 880716085 + frame_id: "/world" +pose: + pose: + position: + x: -0.0721589565579 + y: 0.167539762148 + z: 0.0 + orientation: + x: 0.0 + y: 0.000777605646838 + z: 0.0 + w: 0.999999697665 + covariance: [7.235041764609234, -1.1360508841562784e-08, 0.0, 0.0, -2.5941810592570596e-05, 0.0, -1.1360508841562786e-08, 7.235041755373409, 0.0, 0.0, 8.388891521316225e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5941810592570593e-05, 8.388891521316227e-06, 0.0, 0.0, 7.159882041222513, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 214 + stamp: + secs: 1624875213 + nsecs: 960705042 + frame_id: "/world" +pose: + pose: + position: + x: -0.0833136080689 + y: 0.169621916406 + z: 0.0 + orientation: + x: 0.0 + y: 0.000730085307521 + z: 0.0 + w: 0.999999733488 + covariance: [6.422854024090663, -9.47456809806743e-09, 0.0, 0.0, -2.7797808391550835e-05, 0.0, -9.474568098067428e-09, 6.422854016077826, 0.0, 0.0, 1.089654587259185e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7797808391550835e-05, 1.0896545872591851e-05, 0.0, 0.0, 6.36000794116683, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 215 + stamp: + secs: 1624875214 + nsecs: 40746927 + frame_id: "/world" +pose: + pose: + position: + x: -0.0853692628428 + y: 0.165002383264 + z: 0.0 + orientation: + x: 0.0 + y: 7.95615894148e-05 + z: 0.0 + w: 0.999999996835 + covariance: [6.173798708885196, -6.043684493518283e-09, 0.0, 0.0, -1.627244899805488e-05, 0.0, -6.043684493518281e-09, 6.173798703708926, 0.0, 0.0, 1.0615924189579503e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6272448998054878e-05, 1.0615924189579503e-05, 0.0, 0.0, 6.133939320081568, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 216 + stamp: + secs: 1624875214 + nsecs: 120723009 + frame_id: "/world" +pose: + pose: + position: + x: -0.0858497143164 + y: 0.168644290446 + z: 0.0 + orientation: + x: 0.0 + y: 0.000105303577777 + z: 0.0 + w: 0.999999994456 + covariance: [7.201397251355295, -7.60361395904355e-09, 0.0, 0.0, -2.6371231478352432e-05, 0.0, -7.603613959043549e-09, 7.201397244774411, 0.0, 0.0, 1.4847577926534025e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6371231478352435e-05, 1.4847577926534027e-05, 0.0, 0.0, 7.152450338109883, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 217 + stamp: + secs: 1624875214 + nsecs: 200736046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0873026961873 + y: 0.175522808034 + z: 0.0 + orientation: + x: 0.0 + y: 0.00036557771304 + z: 0.0 + w: 0.999999933176 + covariance: [6.954186404432193, -7.516672994898602e-09, 0.0, 0.0, -2.705774191496975e-05, 0.0, -7.516672994898602e-09, 6.954186397688143, 0.0, 0.0, 1.1156201207398514e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7057741914969752e-05, 1.1156201207398519e-05, 0.0, 0.0, 6.906910004070561, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 218 + stamp: + secs: 1624875214 + nsecs: 280742884 + frame_id: "/world" +pose: + pose: + position: + x: -0.087849504203 + y: 0.17574086119 + z: 0.0 + orientation: + x: 0.0 + y: 0.00040789584873 + z: 0.0 + w: 0.99999991681 + covariance: [6.883217077200282, -9.069085994059954e-09, 0.0, 0.0, -2.9377994316860625e-05, 0.0, -9.069085994059956e-09, 6.883217068691036, 0.0, 0.0, 8.409153391552481e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9377994316860632e-05, 8.409153391552483e-06, 0.0, 0.0, 6.826775430291029, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 219 + stamp: + secs: 1624875214 + nsecs: 360743999 + frame_id: "/world" +pose: + pose: + position: + x: -0.0849075036231 + y: 0.169626006906 + z: 0.0 + orientation: + x: 0.0 + y: 0.000379879219878 + z: 0.0 + w: 0.999999927846 + covariance: [6.327685835379596, -9.659520574996085e-09, 0.0, 0.0, -2.403627997404706e-05, 0.0, -9.659520574996083e-09, 6.327685825909227, 0.0, 0.0, 5.130116088891365e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.403627997404707e-05, 5.130116088891366e-06, 0.0, 0.0, 6.267785633032007, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 220 + stamp: + secs: 1624875214 + nsecs: 440733910 + frame_id: "/world" +pose: + pose: + position: + x: -0.093073931488 + y: 0.180145433713 + z: 0.0 + orientation: + x: 0.0 + y: 0.000829922225438 + z: 0.0 + w: 0.999999655614 + covariance: [6.684211507133398, -1.1660600464829524e-08, 0.0, 0.0, -3.8704605391856824e-05, 0.0, -1.166060046482952e-08, 6.684211495077529, 0.0, 0.0, 7.088137941801111e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.870460539185681e-05, 7.088137941801112e-06, 0.0, 0.0, 6.612586045996174, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 221 + stamp: + secs: 1624875214 + nsecs: 520785093 + frame_id: "/world" +pose: + pose: + position: + x: -0.0898115509783 + y: 0.181822797695 + z: 0.0 + orientation: + x: 0.0 + y: 0.000829707072535 + z: 0.0 + w: 0.999999655793 + covariance: [9.136042911479162, -2.2165644567657484e-08, 0.0, 0.0, -7.328082899920067e-05, 0.0, -2.2165644567657487e-08, 9.136042886661265, 0.0, 0.0, 1.280241099352802e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.328082899920066e-05, 1.2802410993528019e-05, 0.0, 0.0, 8.99939211557021, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 222 + stamp: + secs: 1624875214 + nsecs: 600769043 + frame_id: "/world" +pose: + pose: + position: + x: -0.103917106926 + y: 0.181960351119 + z: 0.0 + orientation: + x: -0.0 + y: -2.85271429829e-05 + z: -0.0 + w: 0.999999999593 + covariance: [10.552910178638335, -2.9735360297131915e-08, 0.0, 0.0, -8.29202674532419e-05, 0.0, -2.9735360297131922e-08, 10.552910143827223, 0.0, 0.0, 1.912897647529078e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.292026745324187e-05, 1.9128976475290773e-05, 0.0, 0.0, 10.369912452414017, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 223 + stamp: + secs: 1624875214 + nsecs: 680773020 + frame_id: "/world" +pose: + pose: + position: + x: -0.104719793881 + y: 0.18390407822 + z: 0.0 + orientation: + x: -0.0 + y: -0.000136087246792 + z: -0.0 + w: 0.99999999074 + covariance: [7.2259631628785215, -1.391144485402281e-08, 0.0, 0.0, -3.2988945820694154e-05, 0.0, -1.3911444854022804e-08, 7.225963146170574, 0.0, 0.0, 9.379850034910949e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.2988945820694134e-05, 9.379850034910949e-06, 0.0, 0.0, 7.139685980961851, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 224 + stamp: + secs: 1624875214 + nsecs: 760782957 + frame_id: "/world" +pose: + pose: + position: + x: -0.105323608951 + y: 0.184749930815 + z: 0.0 + orientation: + x: -0.0 + y: -8.66277929886e-05 + z: -0.0 + w: 0.999999996248 + covariance: [8.460527200753432, -1.9084367865443872e-08, 0.0, 0.0, -3.722438723849808e-05, 0.0, -1.9084367865443862e-08, 8.460527177149128, 0.0, 0.0, 1.0024231523641681e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7224387238498044e-05, 1.002423152364168e-05, 0.0, 0.0, 8.342061808764793, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 225 + stamp: + secs: 1624875214 + nsecs: 840786934 + frame_id: "/world" +pose: + pose: + position: + x: -0.089179154332 + y: 0.180729840886 + z: 0.0 + orientation: + x: -0.0 + y: -5.86484887011e-05 + z: -0.0 + w: 0.99999999828 + covariance: [10.175795349446027, -2.7422100781661933e-08, 0.0, 0.0, -4.2925435548907616e-05, 0.0, -2.7422100781661933e-08, 10.175795314883526, 0.0, 0.0, 1.1623574824058533e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.292543554890759e-05, 1.1623574824058531e-05, 0.0, 0.0, 10.00490842184004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 226 + stamp: + secs: 1624875214 + nsecs: 920783043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0892034550565 + y: 0.185644424114 + z: 0.0 + orientation: + x: 0.0 + y: 6.89057287964e-05 + z: 0.0 + w: 0.999999997626 + covariance: [8.694645054429799, -1.9291802490223264e-08, 0.0, 0.0, -2.5207430520559193e-05, 0.0, -1.929180249022326e-08, 8.694645029863874, 0.0, 0.0, 7.0136754417558656e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.520743052055917e-05, 7.013675441755865e-06, 0.0, 0.0, 8.573040791503782, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 227 + stamp: + secs: 1624875215 + nsecs: 812054 + frame_id: "/world" +pose: + pose: + position: + x: -0.0692859455338 + y: 0.175968165307 + z: 0.0 + orientation: + x: 0.0 + y: 0.000289926973335 + z: 0.0 + w: 0.999999957971 + covariance: [9.122802880677016, -2.0127738741381074e-08, 0.0, 0.0, -2.1009483621896143e-05, 0.0, -2.0127738741381078e-08, 9.122802854860147, 0.0, 0.0, 1.0434202668741034e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1009483621896126e-05, 1.0434202668741038e-05, 0.0, 0.0, 8.994885705044343, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 228 + stamp: + secs: 1624875215 + nsecs: 80796003 + frame_id: "/world" +pose: + pose: + position: + x: -0.0693670350149 + y: 0.178630879479 + z: 0.0 + orientation: + x: -0.0 + y: -6.24720852563e-05 + z: -0.0 + w: 0.999999998049 + covariance: [6.921281090889064, -1.4396441097046827e-08, 0.0, 0.0, -1.0034800439044679e-05, 0.0, -1.4396441097046828e-08, 6.921281072363581, 0.0, 0.0, 8.570002956868368e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0034800439044676e-05, 8.570002956868372e-06, 0.0, 0.0, 6.828054173649712, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 229 + stamp: + secs: 1624875215 + nsecs: 160804033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0680991217795 + y: 0.182955643138 + z: 0.0 + orientation: + x: -0.0 + y: -0.000238154606255 + z: -0.0 + w: 0.999999971641 + covariance: [7.532012779516534, -1.5179267476655746e-08, 0.0, 0.0, -1.010344533920927e-05, 0.0, -1.5179267476655746e-08, 7.5320127599523445, 0.0, 0.0, 7.248149568053789e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0103445339209269e-05, 7.248149568053793e-06, 0.0, 0.0, 7.432477337032703, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 230 + stamp: + secs: 1624875215 + nsecs: 240817070 + frame_id: "/world" +pose: + pose: + position: + x: -0.06768948149 + y: 0.181727257745 + z: 0.0 + orientation: + x: -0.0 + y: -8.31215717628e-05 + z: -0.0 + w: 0.999999996545 + covariance: [7.079308258404116, -1.2491824864445397e-08, 0.0, 0.0, -8.454311562052424e-06, 0.0, -1.2491824864445394e-08, 7.079308242279739, 0.0, 0.0, 4.436373262341629e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.454311562052422e-06, 4.43637326234163e-06, 0.0, 0.0, 6.996222819773897, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 231 + stamp: + secs: 1624875215 + nsecs: 320810080 + frame_id: "/world" +pose: + pose: + position: + x: -0.0678321027153 + y: 0.182100173058 + z: 0.0 + orientation: + x: -0.0 + y: -8.17249299571e-05 + z: -0.0 + w: 0.999999996661 + covariance: [7.001409948483838, -2.1334273635005022e-08, 0.0, 0.0, -2.420035685976731e-05, 0.0, -2.133427363500501e-08, 7.00140992075839, 0.0, 0.0, 4.0663728383639155e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.420035685976731e-05, 4.0663728383639155e-06, 0.0, 0.0, 6.855908692482383, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 232 + stamp: + secs: 1624875215 + nsecs: 400810003 + frame_id: "/world" +pose: + pose: + position: + x: -0.0702898923592 + y: 0.179636317373 + z: 0.0 + orientation: + x: 0.0 + y: 0.00026451883317 + z: 0.0 + w: 0.999999965015 + covariance: [6.918183614892453, -1.5680651424760227e-08, 0.0, 0.0, -2.3962171432877516e-05, 0.0, -1.5680651424760227e-08, 6.9181835943545975, 0.0, 0.0, 7.41120457741965e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3962171432877516e-05, 7.411204577419648e-06, 0.0, 0.0, 6.809885553088628, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 233 + stamp: + secs: 1624875215 + nsecs: 480870008 + frame_id: "/world" +pose: + pose: + position: + x: -0.072869049958 + y: 0.182944160456 + z: 0.0 + orientation: + x: 0.0 + y: 7.63720893022e-05 + z: 0.0 + w: 0.999999997084 + covariance: [6.324297376790609, -8.857872966108384e-09, 0.0, 0.0, -1.5109569696840433e-05, 0.0, -8.857872966108389e-09, 6.32429736513081, 0.0, 0.0, 6.091541416912843e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5109569696840437e-05, 6.091541416912842e-06, 0.0, 0.0, 6.262718931966723, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 234 + stamp: + secs: 1624875215 + nsecs: 560842037 + frame_id: "/world" +pose: + pose: + position: + x: -0.0801351259881 + y: 0.198435365887 + z: 0.0 + orientation: + x: -0.0 + y: -0.000526503977966 + z: -0.0 + w: 0.999999861397 + covariance: [8.922812897519085, -1.9311535858546602e-08, 0.0, 0.0, -2.0643857851260102e-05, 0.0, -1.9311535858546592e-08, 8.922812871923158, 0.0, 0.0, 7.624203649478449e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0643857851260106e-05, 7.624203649478447e-06, 0.0, 0.0, 8.788686058638094, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 235 + stamp: + secs: 1624875215 + nsecs: 640876055 + frame_id: "/world" +pose: + pose: + position: + x: -0.0822161578556 + y: 0.204696964678 + z: 0.0 + orientation: + x: -0.0 + y: -0.000306685175375 + z: -0.0 + w: 0.999999952972 + covariance: [8.282462202687308, -1.714493950505576e-08, 0.0, 0.0, -1.039792959820423e-05, 0.0, -1.714493950505575e-08, 8.282462179915303, 0.0, 0.0, 5.6168048359505954e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0397929598204232e-05, 5.616804835950592e-06, 0.0, 0.0, 8.161598641643915, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 236 + stamp: + secs: 1624875215 + nsecs: 720865965 + frame_id: "/world" +pose: + pose: + position: + x: -0.0824719411358 + y: 0.205160378314 + z: 0.0 + orientation: + x: -0.0 + y: -6.53654353642e-05 + z: -0.0 + w: 0.999999997864 + covariance: [8.053357955855681, -1.6213014861423923e-08, 0.0, 0.0, -1.3216067415351139e-05, 0.0, -1.6213014861423913e-08, 8.05335793436216, 0.0, 0.0, 1.3953999474251822e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3216067415351139e-05, 1.3953999474251822e-05, 0.0, 0.0, 7.9379440300495965, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 237 + stamp: + secs: 1624875215 + nsecs: 800864935 + frame_id: "/world" +pose: + pose: + position: + x: -0.0817246502126 + y: 0.204838827232 + z: 0.0 + orientation: + x: -0.0 + y: -0.000123840630337 + z: -0.0 + w: 0.999999992332 + covariance: [7.967669027501841, -1.5917215340978075e-08, 0.0, 0.0, -4.816103588147254e-05, 0.0, -1.5917215340978058e-08, 7.967669006681341, 0.0, 0.0, 3.259431957985921e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.816103588147253e-05, 3.259431957985922e-05, 0.0, 0.0, 7.855939127837616, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 238 + stamp: + secs: 1624875215 + nsecs: 880872965 + frame_id: "/world" +pose: + pose: + position: + x: -0.0841595572119 + y: 0.204396081064 + z: 0.0 + orientation: + x: 0.0 + y: 0.000782956770013 + z: 0.0 + w: 0.999999693489 + covariance: [11.295244529755667, -3.353820734858239e-08, 0.0, 0.0, -0.00012877180555740823, 0.0, -3.3538207348582356e-08, 11.295244486342456, 0.0, 0.0, 7.582298744622546e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001287718055574082, 7.582298744622546e-05, 0.0, 0.0, 11.076067919676046, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 239 + stamp: + secs: 1624875215 + nsecs: 960884094 + frame_id: "/world" +pose: + pose: + position: + x: -0.0762371581739 + y: 0.203564343287 + z: 0.0 + orientation: + x: 0.0 + y: 0.000308785444784 + z: 0.0 + w: 0.999999952326 + covariance: [10.207248478064878, -2.81305590270521e-08, 0.0, 0.0, -9.28728562946896e-05, 0.0, -2.8130559027052072e-08, 10.207248441942596, 0.0, 0.0, 5.8174931612398725e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.287285629468959e-05, 5.817493161239873e-05, 0.0, 0.0, 10.029647963725582, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 240 + stamp: + secs: 1624875216 + nsecs: 40893078 + frame_id: "/world" +pose: + pose: + position: + x: -0.0765307572537 + y: 0.204309917706 + z: 0.0 + orientation: + x: 0.0 + y: 9.90109735276e-05 + z: 0.0 + w: 0.999999995098 + covariance: [9.77400360542105, -2.664108482344855e-08, 0.0, 0.0, -6.65281155365077e-05, 0.0, -2.664108482344852e-08, 9.774003572168384, 0.0, 0.0, 6.182507380086254e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.65281155365077e-05, 6.182507380086256e-05, 0.0, 0.0, 9.612158850073065, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 241 + stamp: + secs: 1624875216 + nsecs: 120889902 + frame_id: "/world" +pose: + pose: + position: + x: -0.0768735992097 + y: 0.204430380213 + z: 0.0 + orientation: + x: -0.0 + y: -1.90922775432e-05 + z: -0.0 + w: 0.999999999818 + covariance: [8.583784364056452, -2.8180185673649873e-08, 0.0, 0.0, -0.0003986035959806228, 0.0, -2.8180185673649853e-08, 8.58378430020021, 0.0, 0.0, 5.3765307839326686e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003986035959806228, 5.37653078393267e-05, 0.0, 0.0, 8.44338396474089, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 242 + stamp: + secs: 1624875216 + nsecs: 200895071 + frame_id: "/world" +pose: + pose: + position: + x: -0.0771014332471 + y: 0.205966462631 + z: 0.0 + orientation: + x: -0.0 + y: -7.38826374412e-05 + z: -0.0 + w: 0.999999997271 + covariance: [8.157353289267837, -2.978092838430008e-08, 0.0, 0.0, -0.0005099466735926385, 0.0, -2.9780928384300076e-08, 8.157353124138496, 0.0, 0.0, 3.577475013106243e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005099466735926386, 3.577475013106243e-05, 0.0, 0.0, 8.039496389566333, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 243 + stamp: + secs: 1624875216 + nsecs: 280922890 + frame_id: "/world" +pose: + pose: + position: + x: -0.0771120487011 + y: 0.206413023395 + z: 0.0 + orientation: + x: -0.0 + y: -0.000259371856646 + z: -0.0 + w: 0.999999966363 + covariance: [10.002130332056675, -4.9903375807177675e-08, 0.0, 0.0, -0.0006410122173492962, 0.0, -4.9903375807177656e-08, 10.002129920365604, 0.0, 0.0, 3.611470784482266e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006410122173492965, 3.6114707844822666e-05, 0.0, 0.0, 9.833901689096908, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 244 + stamp: + secs: 1624875216 + nsecs: 360913038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0772224726454 + y: 0.206653320222 + z: 0.0 + orientation: + x: -0.0 + y: -0.000299326535766 + z: -0.0 + w: 0.999999955202 + covariance: [11.014674918960633, -6.199898823109156e-08, 0.0, 0.0, -0.0005682377841076578, 0.0, -6.199898823109153e-08, 11.01467431744622, 0.0, 0.0, 2.959795270263138e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000568237784107658, 2.9597952702631386e-05, 0.0, 0.0, 10.82100237977306, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 245 + stamp: + secs: 1624875216 + nsecs: 440912962 + frame_id: "/world" +pose: + pose: + position: + x: -0.0773691649227 + y: 0.206974469261 + z: 0.0 + orientation: + x: -0.0 + y: -0.000291476782334 + z: -0.0 + w: 0.999999957521 + covariance: [13.517252672322762, -1.0419011178155853e-07, 0.0, 0.0, -0.0006362957407247301, 0.0, -1.0419011178155847e-07, 13.51725155350965, 0.0, 0.0, 3.236071373516475e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006362957407247303, 3.2360713735164756e-05, 0.0, 0.0, 13.208832200211956, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 246 + stamp: + secs: 1624875216 + nsecs: 520924091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0781167069823 + y: 0.207950819792 + z: 0.0 + orientation: + x: -0.0 + y: -0.000143154373108 + z: -0.0 + w: 0.999999989753 + covariance: [10.981321832541372, -5.470247676784251e-08, 0.0, 0.0, -0.0002775134245648622, 0.0, -5.470247676784248e-08, 10.981321227035048, 0.0, 0.0, 1.4174777027517648e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00027751342456486234, 1.4174777027517653e-05, 0.0, 0.0, 10.820405514566726, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 247 + stamp: + secs: 1624875216 + nsecs: 600929976 + frame_id: "/world" +pose: + pose: + position: + x: -0.0778757894352 + y: 0.207673050582 + z: 0.0 + orientation: + x: -0.0 + y: -0.000269589423827 + z: -0.0 + w: 0.999999963661 + covariance: [10.071998475394977, -4.934524640145978e-08, 0.0, 0.0, -0.00016843772118466793, 0.0, -4.9345246401459724e-08, 10.071997902593427, 0.0, 0.0, 9.661341144285265e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00016843772118466801, 9.661341144285268e-06, 0.0, 0.0, 9.929646323857067, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 248 + stamp: + secs: 1624875216 + nsecs: 680928946 + frame_id: "/world" +pose: + pose: + position: + x: -0.0779916201598 + y: 0.207761631199 + z: 0.0 + orientation: + x: -0.0 + y: -0.000183126006852 + z: -0.0 + w: 0.999999983232 + covariance: [8.658442759065007, -3.8421403867474165e-08, 0.0, 0.0, -9.129444642909187e-05, 0.0, -3.8421403867474125e-08, 8.658442298862264, 0.0, 0.0, 6.966613448000476e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.12944464290919e-05, 6.9666134480004794e-06, 0.0, 0.0, 8.548433609782766, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 249 + stamp: + secs: 1624875216 + nsecs: 760956049 + frame_id: "/world" +pose: + pose: + position: + x: -0.0785439589966 + y: 0.207871054893 + z: 0.0 + orientation: + x: 0.0 + y: 0.000184197613972 + z: 0.0 + w: 0.999999983036 + covariance: [10.288600246792885, -5.898878481274031e-08, 0.0, 0.0, -9.94799189545387e-05, 0.0, -5.898878481274026e-08, 10.28859952439211, 0.0, 0.0, 9.483566173321246e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.947991895453871e-05, 9.48356617332125e-06, 0.0, 0.0, 10.12047153338736, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 250 + stamp: + secs: 1624875216 + nsecs: 840961933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0786430998767 + y: 0.208136136922 + z: 0.0 + orientation: + x: 0.0 + y: 0.000415623691727 + z: 0.0 + w: 0.999999913628 + covariance: [9.82541404356982, -6.138238887762508e-08, 0.0, 0.0, -7.418029401897698e-05, 0.0, -6.138238887762504e-08, 9.825413282388824, 0.0, 0.0, 7.721090943839061e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.4180294018977e-05, 7.721090943839066e-06, 0.0, 0.0, 9.649337251893343, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 251 + stamp: + secs: 1624875216 + nsecs: 920965910 + frame_id: "/world" +pose: + pose: + position: + x: -0.0710886181621 + y: 0.193346139946 + z: 0.0 + orientation: + x: 0.0 + y: 0.00031536525661 + z: 0.0 + w: 0.999999950272 + covariance: [8.580407269000688, -4.294360685880467e-08, 0.0, 0.0, -4.3878713075743766e-05, 0.0, -4.2943606858804645e-08, 8.580406732984887, 0.0, 0.0, 4.198434439531331e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.387871307574377e-05, 4.198434439531333e-06, 0.0, 0.0, 8.45584499574792, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 252 + stamp: + secs: 1624875217 + nsecs: 967979 + frame_id: "/world" +pose: + pose: + position: + x: -0.0711723845535 + y: 0.193519139037 + z: 0.0 + orientation: + x: 0.0 + y: 0.000583140763051 + z: 0.0 + w: 0.999999829973 + covariance: [9.107942063281394, -5.604046909911982e-08, 0.0, 0.0, -5.339444979252549e-05, 0.0, -5.6040469099119794e-08, 9.107941359531921, 0.0, 0.0, 4.224172520014586e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.3394449792525495e-05, 4.224172520014588e-06, 0.0, 0.0, 8.943696597380555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 253 + stamp: + secs: 1624875217 + nsecs: 80965996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0713557592873 + y: 0.194476320684 + z: 0.0 + orientation: + x: 0.0 + y: 0.000332028896375 + z: 0.0 + w: 0.999999944878 + covariance: [8.325292567118916, -3.680349576281345e-08, 0.0, 0.0, -3.1942036760394e-05, 0.0, -3.680349576281344e-08, 8.325292103425962, 0.0, 0.0, -2.095235482467815e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.1942036760394005e-05, -2.0952354824678134e-06, 0.0, 0.0, 8.215846932942707, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 254 + stamp: + secs: 1624875217 + nsecs: 160985947 + frame_id: "/world" +pose: + pose: + position: + x: -0.0705437733416 + y: 0.193901909063 + z: 0.0 + orientation: + x: 0.0 + y: 0.00044065986736 + z: 0.0 + w: 0.999999902909 + covariance: [8.070908514493489, -3.735243506577884e-08, 0.0, 0.0, -2.3765324240303986e-05, 0.0, -3.735243506577881e-08, 8.070908041568277, 0.0, 0.0, -7.325724102760076e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.376532424030398e-05, -7.325724102760076e-06, 0.0, 0.0, 7.958077855876616, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 255 + stamp: + secs: 1624875217 + nsecs: 240998983 + frame_id: "/world" +pose: + pose: + position: + x: -0.0711096054423 + y: 0.194113269291 + z: 0.0 + orientation: + x: 0.0 + y: 0.000462113919288 + z: 0.0 + w: 0.999999893225 + covariance: [7.250968341485635, -2.9541164792756137e-08, 0.0, 0.0, -3.21668550390345e-05, 0.0, -2.9541164792756127e-08, 7.2509679658158985, 0.0, 0.0, -4.715246348850721e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.21668550390345e-05, -4.715246348850721e-06, 0.0, 0.0, 7.160038936259018, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 256 + stamp: + secs: 1624875217 + nsecs: 320986986 + frame_id: "/world" +pose: + pose: + position: + x: -0.0740608059027 + y: 0.193076114188 + z: 0.0 + orientation: + x: 0.0 + y: 0.00255703927391 + z: 0.0 + w: 0.99999673077 + covariance: [10.813950000712696, -8.211102920691755e-08, 0.0, 0.0, -0.00010907693165059327, 0.0, -8.211102920691752e-08, 10.813948948289525, 0.0, 0.0, -7.707375989738151e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010907693165059326, -7.707375989738153e-06, 0.0, 0.0, 10.555743890752712, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 257 + stamp: + secs: 1624875217 + nsecs: 400985956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0733098963242 + y: 0.192683563383 + z: 0.0 + orientation: + x: 0.0 + y: 8.28152422876e-05 + z: 0.0 + w: 0.999999996571 + covariance: [7.996261853373878, -3.3978878272566894e-08, 0.0, 0.0, -5.1724216304315884e-05, 0.0, -3.3978878272566894e-08, 7.996261416828459, 0.0, 0.0, -2.5411523044369742e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.172421630431591e-05, -2.5411523044369755e-06, 0.0, 0.0, 7.887507946806881, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 258 + stamp: + secs: 1624875217 + nsecs: 481002092 + frame_id: "/world" +pose: + pose: + position: + x: -0.0735056622766 + y: 0.192910565253 + z: 0.0 + orientation: + x: 0.0 + y: 0.000456368430197 + z: 0.0 + w: 0.999999895864 + covariance: [8.823758891213725, -4.152704343844352e-08, 0.0, 0.0, -8.989248246013898e-05, 0.0, -4.152704343844352e-08, 8.823758354523276, 0.0, 0.0, 2.3793741897539935e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.9892482460139e-05, 2.3793741897539935e-06, 0.0, 0.0, 8.688933414844476, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 259 + stamp: + secs: 1624875217 + nsecs: 561059952 + frame_id: "/world" +pose: + pose: + position: + x: -0.0735628327053 + y: 0.192983282535 + z: 0.0 + orientation: + x: 0.0 + y: 0.000489181602761 + z: 0.0 + w: 0.999999880351 + covariance: [10.338104997014975, -4.2540338812940544e-08, 0.0, 0.0, -9.64300943761516e-05, 0.0, -4.2540338812940544e-08, 10.338104446226831, 0.0, 0.0, 5.437668859777262e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.64300943761516e-05, 5.437668859777264e-06, 0.0, 0.0, 10.198893415608241, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 260 + stamp: + secs: 1624875217 + nsecs: 641020060 + frame_id: "/world" +pose: + pose: + position: + x: -0.073501632657 + y: 0.193199024642 + z: 0.0 + orientation: + x: 0.0 + y: 5.46996537933e-06 + z: 0.0 + w: 0.999999999985 + covariance: [7.15587338856007, -1.6040999336633896e-08, 0.0, 0.0, -3.4085915681170736e-05, 0.0, -1.6040999336633893e-08, 7.155873180185478, 0.0, 0.0, -1.4234560138120365e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.408591568117076e-05, -1.423456013812036e-06, 0.0, 0.0, 7.102549302951419, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 261 + stamp: + secs: 1624875217 + nsecs: 721040964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0735809764698 + y: 0.193304418351 + z: 0.0 + orientation: + x: 0.0 + y: 0.000589080555863 + z: 0.0 + w: 0.999999826492 + covariance: [9.395919765437254, -3.0840242435139395e-08, 0.0, 0.0, -4.805620213144637e-05, 0.0, -3.084024243513939e-08, 9.395919361050545, 0.0, 0.0, -9.412537405080078e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.805620213144638e-05, -9.412537405080074e-06, 0.0, 0.0, 9.292732976778227, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 262 + stamp: + secs: 1624875217 + nsecs: 801040888 + frame_id: "/world" +pose: + pose: + position: + x: -0.073941906964 + y: 0.193433295555 + z: 0.0 + orientation: + x: 0.0 + y: 0.00064389215744 + z: 0.0 + w: 0.999999792701 + covariance: [9.384755038713685, -2.3350302411623642e-08, 0.0, 0.0, -2.995817734421835e-05, 0.0, -2.335030241162364e-08, 9.384754731833834, 0.0, 0.0, -5.061409624911385e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.995817734421835e-05, -5.061409624911382e-06, 0.0, 0.0, 9.305386835907047, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 263 + stamp: + secs: 1624875217 + nsecs: 881045103 + frame_id: "/world" +pose: + pose: + position: + x: -0.0739314188873 + y: 0.193544804767 + z: 0.0 + orientation: + x: 0.0 + y: 0.000160694904816 + z: 0.0 + w: 0.999999987089 + covariance: [7.624593171211033, -2.0663525022636383e-08, 0.0, 0.0, -2.044212842754458e-05, 0.0, -2.066352502263637e-08, 7.624592899448356, 0.0, 0.0, 8.636084713587245e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0442128427544578e-05, 8.636084713587245e-06, 0.0, 0.0, 7.55280758487379, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 264 + stamp: + secs: 1624875217 + nsecs: 961061954 + frame_id: "/world" +pose: + pose: + position: + x: -0.0740591336377 + y: 0.193782994226 + z: 0.0 + orientation: + x: 0.0 + y: 0.000108455971801 + z: 0.0 + w: 0.999999994119 + covariance: [8.627422411624515, -2.6230894241286296e-08, 0.0, 0.0, -1.750486033238176e-05, 0.0, -2.623089424128628e-08, 8.627422067688535, 0.0, 0.0, 1.82934148964345e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7504860332381752e-05, 1.82934148964345e-05, 0.0, 0.0, 8.535036864974364, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 265 + stamp: + secs: 1624875218 + nsecs: 41070938 + frame_id: "/world" +pose: + pose: + position: + x: -0.0753028980105 + y: 0.193404576171 + z: 0.0 + orientation: + x: -0.0 + y: -0.00061186885264 + z: -0.0 + w: 0.999999812808 + covariance: [9.094654635986014, -3.3240974919239453e-08, 0.0, 0.0, -0.00012461398222083297, 0.0, -3.324097491923945e-08, 9.094654205685858, 0.0, 0.0, 2.4789047394140237e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00012461398222083297, 2.478904739414023e-05, 0.0, 0.0, 8.978228705639838, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 266 + stamp: + secs: 1624875218 + nsecs: 121073961 + frame_id: "/world" +pose: + pose: + position: + x: -0.0754449458858 + y: 0.194833654317 + z: 0.0 + orientation: + x: 0.0 + y: 0.000197109958401 + z: 0.0 + w: 0.999999980574 + covariance: [8.330074557819467, -3.001476665892087e-08, 0.0, 0.0, -0.00016485918450811796, 0.0, -3.001476665892087e-08, 8.33007417551943, 0.0, 0.0, 2.3960274139643614e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000164859184508118, 2.396027413964361e-05, 0.0, 0.0, 8.227990768605043, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 267 + stamp: + secs: 1624875218 + nsecs: 201076984 + frame_id: "/world" +pose: + pose: + position: + x: -0.0759118745626 + y: 0.195745567144 + z: 0.0 + orientation: + x: -0.0 + y: -6.59592990123e-05 + z: -0.0 + w: 0.999999997825 + covariance: [7.327704901376255, -2.6084732823767373e-08, 0.0, 0.0, -0.00011741474359054761, 0.0, -2.608473282376737e-08, 7.327704574665999, 0.0, 0.0, 1.7785520035399776e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011741474359054768, 1.7785520035399776e-05, 0.0, 0.0, 7.241429703045467, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 268 + stamp: + secs: 1624875218 + nsecs: 281084061 + frame_id: "/world" +pose: + pose: + position: + x: -0.0738467308377 + y: 0.193359792155 + z: 0.0 + orientation: + x: 0.0 + y: 0.00034586754658 + z: 0.0 + w: 0.999999940188 + covariance: [7.018014500577828, -2.5451591883811218e-08, 0.0, 0.0, -8.919214049875191e-05, 0.0, -2.54515918838112e-08, 7.018014185854238, 0.0, 0.0, 1.3066386328457822e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.919214049875195e-05, 1.3066386328457824e-05, 0.0, 0.0, 6.935620431445128, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 269 + stamp: + secs: 1624875218 + nsecs: 361097097 + frame_id: "/world" +pose: + pose: + position: + x: -0.0740785627151 + y: 0.194017322215 + z: 0.0 + orientation: + x: 0.0 + y: 0.00029005961951 + z: 0.0 + w: 0.999999957933 + covariance: [8.361069188797261, -3.626403645072564e-08, 0.0, 0.0, -9.489769699789062e-05, 0.0, -3.6264036450725645e-08, 8.361068745808327, 0.0, 0.0, 1.7507706051600368e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.489769699789069e-05, 1.7507706051600368e-05, 0.0, 0.0, 8.245361834015096, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 270 + stamp: + secs: 1624875218 + nsecs: 441102982 + frame_id: "/world" +pose: + pose: + position: + x: -0.0728079456502 + y: 0.194969973359 + z: 0.0 + orientation: + x: 0.0 + y: 7.64712010315e-05 + z: 0.0 + w: 0.999999997076 + covariance: [8.072882392695224, -3.282223975353038e-08, 0.0, 0.0, -6.0914482785624365e-05, 0.0, -3.2822239753530384e-08, 8.07288199511429, 0.0, 0.0, 1.3575200328214039e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.0914482785624385e-05, 1.3575200328214035e-05, 0.0, 0.0, 7.968176159104928, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 271 + stamp: + secs: 1624875218 + nsecs: 521126986 + frame_id: "/world" +pose: + pose: + position: + x: -0.0717671446558 + y: 0.196325472143 + z: 0.0 + orientation: + x: 0.0 + y: 0.000214319098157 + z: 0.0 + w: 0.999999977034 + covariance: [7.25583491421633, -2.902689737845941e-08, 0.0, 0.0, -3.437239861125081e-05, 0.0, -2.90268973784594e-08, 7.25583456474495, 0.0, 0.0, 7.982299237936582e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.437239861125082e-05, 7.982299237936582e-06, 0.0, 0.0, 7.163019346392249, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 272 + stamp: + secs: 1624875218 + nsecs: 601133108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0720212572152 + y: 0.196897220124 + z: 0.0 + orientation: + x: 0.0 + y: 0.000304404034902 + z: 0.0 + w: 0.999999953669 + covariance: [6.987702176077731, -2.4047338689459946e-08, 0.0, 0.0, -3.040860966759905e-05, 0.0, -2.4047338689459933e-08, 6.9877018876057155, 0.0, 0.0, 4.6253771302696255e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.040860966759905e-05, 4.625377130269624e-06, 0.0, 0.0, 6.910132221979082, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 273 + stamp: + secs: 1624875218 + nsecs: 681112051 + frame_id: "/world" +pose: + pose: + position: + x: -0.0694311861681 + y: 0.199042852965 + z: 0.0 + orientation: + x: 0.0 + y: 0.000439360501519 + z: 0.0 + w: 0.999999903481 + covariance: [10.52845167223315, -4.988753176115606e-08, 0.0, 0.0, -6.414564722610465e-05, 0.0, -4.988753176115604e-08, 10.5284510751653, 0.0, 0.0, 7.032544147790344e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.414564722610464e-05, 7.032544147790342e-06, 0.0, 0.0, 10.367305238313572, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 274 + stamp: + secs: 1624875218 + nsecs: 761149883 + frame_id: "/world" +pose: + pose: + position: + x: -0.0695017287372 + y: 0.199124378304 + z: 0.0 + orientation: + x: 0.0 + y: 0.00111705049823 + z: 0.0 + w: 0.999999376099 + covariance: [13.151906513711381, -8.010994155709197e-08, 0.0, 0.0, -9.467886054862818e-05, 0.0, -8.010994155709193e-08, 13.151905556722282, 0.0, 0.0, 1.4520155925487143e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.467886054862818e-05, 1.4520155925487143e-05, 0.0, 0.0, 12.892813279390033, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 275 + stamp: + secs: 1624875218 + nsecs: 841130018 + frame_id: "/world" +pose: + pose: + position: + x: -0.0674099096352 + y: 0.203546158019 + z: 0.0 + orientation: + x: 0.0 + y: 0.00118979708159 + z: 0.0 + w: 0.999999292191 + covariance: [12.607157755943353, -7.45734646585869e-08, 0.0, 0.0, -9.341264596315865e-05, 0.0, -7.457346465858686e-08, 12.607156867977118, 0.0, 0.0, 2.4420744321264315e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.341264596315863e-05, 2.4420744321264318e-05, 0.0, 0.0, 12.36415856730339, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 276 + stamp: + secs: 1624875218 + nsecs: 921133995 + frame_id: "/world" +pose: + pose: + position: + x: -0.0669108475445 + y: 0.208514292271 + z: 0.0 + orientation: + x: 0.0 + y: 0.000596826066144 + z: 0.0 + w: 0.999999821899 + covariance: [12.355731507886965, -7.528370095599327e-08, 0.0, 0.0, -0.00010844492064665254, 0.0, -7.528370095599323e-08, 12.3557306192903, 0.0, 0.0, 4.784009077483298e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010844492064665256, 4.7840090774832986e-05, 0.0, 0.0, 12.107320830736933, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 277 + stamp: + secs: 1624875219 + nsecs: 1161098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0661537165748 + y: 0.198108247862 + z: 0.0 + orientation: + x: -0.0 + y: -0.000174348856785 + z: -0.0 + w: 0.999999984801 + covariance: [12.22479598487514, -7.309020100441114e-08, 0.0, 0.0, -8.739379553326301e-05, 0.0, -7.30902010044111e-08, 12.224795131192797, 0.0, 0.0, 5.742905848030394e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.7393795533263e-05, 5.7429058480303935e-05, 0.0, 0.0, 11.982368134693477, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 278 + stamp: + secs: 1624875219 + nsecs: 81146002 + frame_id: "/world" +pose: + pose: + position: + x: -0.0658554706011 + y: 0.198584234012 + z: 0.0 + orientation: + x: -0.0 + y: -0.000154829181964 + z: -0.0 + w: 0.999999988014 + covariance: [10.571167036304933, -5.8679331012110356e-08, 0.0, 0.0, -7.749369862664033e-05, 0.0, -5.867933101211031e-08, 10.57116635838467, 0.0, 0.0, 4.3288269085015984e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.749369862664033e-05, 4.328826908501598e-05, 0.0, 0.0, 10.375138584803809, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 279 + stamp: + secs: 1624875219 + nsecs: 161160946 + frame_id: "/world" +pose: + pose: + position: + x: -0.0643369663885 + y: 0.200017156198 + z: 0.0 + orientation: + x: 0.0 + y: 1.49401369366e-05 + z: 0.0 + w: 0.999999999888 + covariance: [9.921298481416102, -5.0382191481317835e-08, 0.0, 0.0, -7.748393667590503e-05, 0.0, -5.03821914813178e-08, 9.921297905746158, 0.0, 0.0, 3.0566145148218225e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.748393667590501e-05, 3.056614514821823e-05, 0.0, 0.0, 9.752280913178812, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 280 + stamp: + secs: 1624875219 + nsecs: 241172075 + frame_id: "/world" +pose: + pose: + position: + x: -0.0627982066071 + y: 0.203470437513 + z: 0.0 + orientation: + x: -0.0 + y: -6.89773984416e-06 + z: -0.0 + w: 0.999999999976 + covariance: [8.615649356317522, -3.713176874290918e-08, 0.0, 0.0, -4.827414490925237e-05, 0.0, -3.7131768742909156e-08, 8.615648935484392, 0.0, 0.0, 1.674690488917234e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.827414490925237e-05, 1.6746904889172343e-05, 0.0, 0.0, 8.489960703891967, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 281 + stamp: + secs: 1624875219 + nsecs: 321162939 + frame_id: "/world" +pose: + pose: + position: + x: -0.0620207075825 + y: 0.201427997997 + z: 0.0 + orientation: + x: -0.0 + y: -0.000385735626732 + z: -0.0 + w: 0.999999925604 + covariance: [9.093899309763037, -3.9747814137448744e-08, 0.0, 0.0, -3.714709362600195e-05, 0.0, -3.974781413744872e-08, 9.093898862236838, 0.0, 0.0, 1.232864177119996e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.714709362600196e-05, 1.232864177119996e-05, 0.0, 0.0, 8.958229319171652, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 282 + stamp: + secs: 1624875219 + nsecs: 401169062 + frame_id: "/world" +pose: + pose: + position: + x: -0.0627016398558 + y: 0.202507233488 + z: 0.0 + orientation: + x: -0.0 + y: -0.000178235283866 + z: -0.0 + w: 0.999999984116 + covariance: [9.273258024434625, -3.2226544360011053e-08, 0.0, 0.0, -2.31053974590957e-05, 0.0, -3.2226544360011034e-08, 9.273257662877443, 0.0, 0.0, 8.988899075860161e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3105397459095702e-05, 8.98889907586016e-06, 0.0, 0.0, 9.162352990129731, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 283 + stamp: + secs: 1624875219 + nsecs: 481182098 + frame_id: "/world" +pose: + pose: + position: + x: -0.076788567603 + y: 0.215261627972 + z: 0.0 + orientation: + x: -0.0 + y: -0.000395999028047 + z: -0.0 + w: 0.999999921592 + covariance: [10.623031446769232, -4.734637020830244e-08, 0.0, 0.0, -6.257813702540697e-05, 0.0, -4.734637020830238e-08, 10.623030919895715, 0.0, 0.0, 2.5982940913045773e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.257813702540698e-05, 2.5982940913045777e-05, 0.0, 0.0, 10.459343808487144, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 284 + stamp: + secs: 1624875219 + nsecs: 561197996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0770545901087 + y: 0.215248928592 + z: 0.0 + orientation: + x: -0.0 + y: -0.000385957212274 + z: -0.0 + w: 0.999999925519 + covariance: [11.347055784832865, -5.699059664907306e-08, 0.0, 0.0, -9.925739267572298e-05, 0.0, -5.6990596649072984e-08, 11.347055158503052, 0.0, 0.0, 3.796053238229741e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.9257392675723e-05, 3.796053238229742e-05, 0.0, 0.0, 11.150041878077687, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 285 + stamp: + secs: 1624875219 + nsecs: 641206980 + frame_id: "/world" +pose: + pose: + position: + x: -0.0768111316067 + y: 0.215257321056 + z: 0.0 + orientation: + x: -0.0 + y: -5.78035553547e-06 + z: -0.0 + w: 0.999999999983 + covariance: [11.74502667205212, -6.940397740555302e-08, 0.0, 0.0, -0.0001291903314135608, 0.0, -6.940397740555294e-08, 11.745025918660197, 0.0, 0.0, 4.042653245196555e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001291903314135608, 4.0426532451965557e-05, 0.0, 0.0, 11.504877487458026, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 286 + stamp: + secs: 1624875219 + nsecs: 721213102 + frame_id: "/world" +pose: + pose: + position: + x: -0.074646751398 + y: 0.216503072913 + z: 0.0 + orientation: + x: 0.0 + y: 0.000231611187284 + z: 0.0 + w: 0.999999973178 + covariance: [10.38527055916001, -5.463187063247553e-08, 0.0, 0.0, -9.66061428024148e-05, 0.0, -5.4631870632475466e-08, 10.385269971034074, 0.0, 0.0, 2.6943394580166456e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.660614280241478e-05, 2.6943394580166456e-05, 0.0, 0.0, 10.19495303337412, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 287 + stamp: + secs: 1624875219 + nsecs: 801218987 + frame_id: "/world" +pose: + pose: + position: + x: -0.0717184225034 + y: 0.217198491915 + z: 0.0 + orientation: + x: 0.0 + y: 0.000202147492672 + z: 0.0 + w: 0.999999979568 + covariance: [8.787168550401717, -4.341781650933562e-08, 0.0, 0.0, -0.00010156766251325926, 0.0, -4.341781650933558e-08, 8.787168086897607, 0.0, 0.0, 1.657342077423873e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001015676625132593, 1.657342077423873e-05, 0.0, 0.0, 8.634772342350946, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 288 + stamp: + secs: 1624875219 + nsecs: 881227016 + frame_id: "/world" +pose: + pose: + position: + x: -0.07143330459 + y: 0.217660622277 + z: 0.0 + orientation: + x: 0.0 + y: 0.000182283108325 + z: 0.0 + w: 0.999999983386 + covariance: [9.177177619798558, -4.398261145721746e-08, 0.0, 0.0, -0.00011487105366307066, 0.0, -4.3982611457217416e-08, 9.177177150969671, 0.0, 0.0, 8.949808836103109e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011487105366307063, 8.94980883610311e-06, 0.0, 0.0, 9.021889336017365, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 289 + stamp: + secs: 1624875219 + nsecs: 961224079 + frame_id: "/world" +pose: + pose: + position: + x: -0.0715475580445 + y: 0.218014033597 + z: 0.0 + orientation: + x: 0.0 + y: 0.000125916545563 + z: 0.0 + w: 0.999999992073 + covariance: [8.365152253812875, -3.417756215952177e-08, 0.0, 0.0, -7.750100890301545e-05, 0.0, -3.417756215952174e-08, 8.36515188859061, 0.0, 0.0, 2.3748631564285077e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.750100890301547e-05, 2.374863156428509e-06, 0.0, 0.0, 8.242895920507419, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 290 + stamp: + secs: 1624875220 + nsecs: 41249990 + frame_id: "/world" +pose: + pose: + position: + x: -0.0720047512954 + y: 0.218850738628 + z: 0.0 + orientation: + x: -0.0 + y: -4.92954542739e-05 + z: -0.0 + w: 0.999999998785 + covariance: [8.997507063183953, -4.102451533519235e-08, 0.0, 0.0, -6.880044134927916e-05, 0.0, -4.1024515335192304e-08, 8.997506623102202, 0.0, 0.0, -8.167304594680262e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.880044134927918e-05, -8.167304594680259e-07, 0.0, 0.0, 8.848597786547051, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 291 + stamp: + secs: 1624875220 + nsecs: 121259928 + frame_id: "/world" +pose: + pose: + position: + x: -0.0721686014235 + y: 0.219146517992 + z: 0.0 + orientation: + x: -0.0 + y: -0.000139155276807 + z: -0.0 + w: 0.999999990318 + covariance: [9.271139815657374, -4.2343872854059057e-08, 0.0, 0.0, -5.5188266583444915e-05, 0.0, -4.234387285405901e-08, 9.271139359875566, 0.0, 0.0, -2.6552945396054304e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.5188266583444936e-05, -2.65529453960543e-06, 0.0, 0.0, 9.114975886371479, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 292 + stamp: + secs: 1624875220 + nsecs: 201256037 + frame_id: "/world" +pose: + pose: + position: + x: -0.0718220592027 + y: 0.218488241328 + z: 0.0 + orientation: + x: 0.0 + y: 3.2995553866e-05 + z: 0.0 + w: 0.999999999456 + covariance: [7.602024617032422, -2.734789511692596e-08, 0.0, 0.0, -2.5550631347189332e-05, 0.0, -2.734789511692593e-08, 7.6020243220714585, 0.0, 0.0, -1.177968641877924e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5550631347189353e-05, -1.1779686418779243e-07, 0.0, 0.0, 7.49924596947639, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 293 + stamp: + secs: 1624875220 + nsecs: 281263113 + frame_id: "/world" +pose: + pose: + position: + x: -0.0712888725813 + y: 0.223814171085 + z: 0.0 + orientation: + x: 0.0 + y: 0.000130942751508 + z: 0.0 + w: 0.999999991427 + covariance: [9.681889735098974, -4.163573870452355e-08, 0.0, 0.0, -2.2908900962838973e-05, 0.0, -4.163573870452352e-08, 9.681889285517661, 0.0, 0.0, 3.951885896489254e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.290890096283899e-05, 3.9518858964892554e-06, 0.0, 0.0, 9.523400036501505, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 294 + stamp: + secs: 1624875220 + nsecs: 361269951 + frame_id: "/world" +pose: + pose: + position: + x: -0.0726052737164 + y: 0.234727297927 + z: 0.0 + orientation: + x: 0.0 + y: 0.000206472345828 + z: 0.0 + w: 0.999999978685 + covariance: [9.553055273398044, -3.893251606375478e-08, 0.0, 0.0, -1.4034929662285234e-05, 0.0, -3.8932516063754754e-08, 9.553054852757818, 0.0, 0.0, 5.61857685075771e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4034929662285237e-05, 5.61857685075771e-06, 0.0, 0.0, 9.402612654330817, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 295 + stamp: + secs: 1624875220 + nsecs: 441265106 + frame_id: "/world" +pose: + pose: + position: + x: -0.0726474043033 + y: 0.234996353904 + z: 0.0 + orientation: + x: 0.0 + y: 9.36454480508e-05 + z: 0.0 + w: 0.999999995615 + covariance: [8.50090983079158, -3.246955919734679e-08, 0.0, 0.0, -7.326284562873288e-06, 0.0, -3.246955919734677e-08, 8.500909479821486, 0.0, 0.0, 4.956381254904585e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.326284562873288e-06, 4.956381254904585e-06, 0.0, 0.0, 8.372949248044295, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 296 + stamp: + secs: 1624875220 + nsecs: 521287918 + frame_id: "/world" +pose: + pose: + position: + x: -0.0729943309088 + y: 0.23582706102 + z: 0.0 + orientation: + x: -0.0 + y: -1.30779801469e-05 + z: -0.0 + w: 0.999999999914 + covariance: [8.128276850280972, -2.844047862160623e-08, 0.0, 0.0, -7.5874848420642556e-06, 0.0, -2.844047862160622e-08, 8.12827654289553, 0.0, 0.0, 7.932965591298843e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.587484842064256e-06, 7.93296559129884e-06, 0.0, 0.0, 8.014250790747472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 297 + stamp: + secs: 1624875220 + nsecs: 601290941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0732659313619 + y: 0.23642002511 + z: 0.0 + orientation: + x: 0.0 + y: 3.75127242809e-05 + z: 0.0 + w: 0.999999999296 + covariance: [8.87917922892542, -3.2764135549598024e-08, 0.0, 0.0, -1.5137085226646413e-05, 0.0, -3.276413554959801e-08, 8.879178875351515, 0.0, 0.0, 1.308095526666226e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5137085226646408e-05, 1.3080955266662254e-05, 0.0, 0.0, 8.745936214569962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 298 + stamp: + secs: 1624875220 + nsecs: 681297064 + frame_id: "/world" +pose: + pose: + position: + x: -0.075086980399 + y: 0.237000355091 + z: 0.0 + orientation: + x: -0.0 + y: -0.000751901882724 + z: -0.0 + w: 0.999999717322 + covariance: [9.211720105672967, -3.4263124733625136e-08, 0.0, 0.0, -1.755717071301511e-05, 0.0, -3.426312473362512e-08, 9.211719736513142, 0.0, 0.0, 9.50151371488263e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.75571707130151e-05, 9.501513714882625e-06, 0.0, 0.0, 9.070436688158658, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 299 + stamp: + secs: 1624875220 + nsecs: 761307001 + frame_id: "/world" +pose: + pose: + position: + x: -0.070929977494 + y: 0.255808966128 + z: 0.0 + orientation: + x: -0.0 + y: -0.000674604029729 + z: -0.0 + w: 0.999999772455 + covariance: [10.594095706625978, -4.4324270265251736e-08, 0.0, 0.0, -2.2576916200853522e-05, 0.0, -4.432427026525172e-08, 10.594095229632122, 0.0, 0.0, 9.04094679080549e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.257691620085351e-05, 9.040946790805483e-06, 0.0, 0.0, 10.408978138168878, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 300 + stamp: + secs: 1624875220 + nsecs: 841311932 + frame_id: "/world" +pose: + pose: + position: + x: -0.0727491934797 + y: 0.262049096337 + z: 0.0 + orientation: + x: -0.0 + y: -5.9910058681e-05 + z: -0.0 + w: 0.999999998205 + covariance: [8.813818601162096, -2.6048104533383155e-08, 0.0, 0.0, -1.2990675402021717e-05, 0.0, -2.6048104533383148e-08, 8.813818321091093, 0.0, 0.0, 5.31532630859073e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2990675402021706e-05, 5.315326308590725e-06, 0.0, 0.0, 8.70342074397333, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 301 + stamp: + secs: 1624875220 + nsecs: 921345949 + frame_id: "/world" +pose: + pose: + position: + x: -0.0711052889164 + y: 0.251609534713 + z: 0.0 + orientation: + x: 0.0 + y: 0.000173789865681 + z: 0.0 + w: 0.999999984899 + covariance: [9.193030022726505, -3.2756047592365766e-08, 0.0, 0.0, -1.36197935204652e-05, 0.0, -3.2756047592365766e-08, 9.193029671031834, 0.0, 0.0, 7.1591352079674625e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.361979352046519e-05, 7.159135207967457e-06, 0.0, 0.0, 9.051896069181568, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 302 + stamp: + secs: 1624875221 + nsecs: 1331091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0592449768436 + y: 0.235018724583 + z: 0.0 + orientation: + x: -0.0 + y: -0.000266488875058 + z: -0.0 + w: 0.999999964492 + covariance: [10.589157523688257, -4.3825067054416714e-08, 0.0, 0.0, -0.00010115739732500836, 0.0, -4.3825067054416714e-08, 10.589157054727071, 0.0, 0.0, 1.631850162034697e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010115739732500835, 1.6318501620346963e-05, 0.0, 0.0, 10.39923982102686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 303 + stamp: + secs: 1624875221 + nsecs: 81346989 + frame_id: "/world" +pose: + pose: + position: + x: -0.0536614095382 + y: 0.235402038444 + z: 0.0 + orientation: + x: -0.0 + y: -0.00236744516077 + z: -0.0 + w: 0.999997197598 + covariance: [13.20599930894818, -6.783865873831547e-08, 0.0, 0.0, -0.0002384734454696985, 0.0, -6.783865873831544e-08, 13.205998585903346, 0.0, 0.0, 2.971943719324929e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00023847344546969846, 2.971943719324928e-05, 0.0, 0.0, 12.913457218447107, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 304 + stamp: + secs: 1624875221 + nsecs: 161340952 + frame_id: "/world" +pose: + pose: + position: + x: -0.0538962988469 + y: 0.236116010946 + z: 0.0 + orientation: + x: -0.0 + y: -0.00100530844421 + z: -0.0 + w: 0.999999494677 + covariance: [12.650461234989749, -6.411402021095188e-08, 0.0, 0.0, -0.00018535896888577152, 0.0, -6.411402021095186e-08, 12.650460553317677, 0.0, 0.0, 2.4340514178983595e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00018535896888577152, 2.434051417898359e-05, 0.0, 0.0, 12.372767223409843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 305 + stamp: + secs: 1624875221 + nsecs: 241344929 + frame_id: "/world" +pose: + pose: + position: + x: -0.0536383330733 + y: 0.236931333487 + z: 0.0 + orientation: + x: -0.0 + y: -0.000701410157294 + z: -0.0 + w: 0.999999754012 + covariance: [12.377294950360772, -6.069947663492311e-08, 0.0, 0.0, -0.00012163074883410307, 0.0, -6.06994766349231e-08, 12.377294306162609, 0.0, 0.0, 1.79364450027553e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00012163074883410303, 1.7936445002755293e-05, 0.0, 0.0, 12.112533524784709, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 306 + stamp: + secs: 1624875221 + nsecs: 321346045 + frame_id: "/world" +pose: + pose: + position: + x: -0.0536554947827 + y: 0.239286932473 + z: 0.0 + orientation: + x: -0.0 + y: -0.000348627135337 + z: -0.0 + w: 0.99999993923 + covariance: [9.411509389404097, -4.2711330009659203e-08, 0.0, 0.0, -7.358250703066844e-05, 0.0, -4.27113300096592e-08, 9.411508937183084, 0.0, 0.0, 1.221903006584815e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.358250703066844e-05, 1.2219030065848146e-05, 0.0, 0.0, 9.222137811386139, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 307 + stamp: + secs: 1624875221 + nsecs: 401350975 + frame_id: "/world" +pose: + pose: + position: + x: -0.0533866924688 + y: 0.240158749231 + z: 0.0 + orientation: + x: -0.0 + y: -0.000272706715172 + z: -0.0 + w: 0.999999962816 + covariance: [10.739087741075585, -5.534297059715468e-08, 0.0, 0.0, -8.608015229910287e-05, 0.0, -5.534297059715467e-08, 10.739087156914952, 0.0, 0.0, 1.590892186313663e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.608015229910287e-05, 1.5908921863136626e-05, 0.0, 0.0, 10.491741905622405, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 308 + stamp: + secs: 1624875221 + nsecs: 481375933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0530613186788 + y: 0.239965662586 + z: 0.0 + orientation: + x: -0.0 + y: -5.84929934313e-05 + z: -0.0 + w: 0.999999998289 + covariance: [10.010615997144525, -4.667614202681969e-08, 0.0, 0.0, -6.400298860236924e-05, 0.0, -4.667614202681969e-08, 10.010615505443253, 0.0, 0.0, 1.142446762689816e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.400298860236923e-05, 1.142446762689816e-05, 0.0, 0.0, 9.799066153229566, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 309 + stamp: + secs: 1624875221 + nsecs: 561382055 + frame_id: "/world" +pose: + pose: + position: + x: -0.056245623567 + y: 0.240409892602 + z: 0.0 + orientation: + x: 0.0 + y: 0.000136528891498 + z: 0.0 + w: 0.99999999068 + covariance: [8.662293267654768, -3.4032732155981744e-08, 0.0, 0.0, -4.0456701609814416e-05, 0.0, -3.403273215598173e-08, 8.662292909687734, 0.0, 0.0, 6.387462571790146e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.04567016098144e-05, 6.387462571790147e-06, 0.0, 0.0, 8.505435215953325, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 310 + stamp: + secs: 1624875221 + nsecs: 641385078 + frame_id: "/world" +pose: + pose: + position: + x: -0.055026054056 + y: 0.239821517809 + z: 0.0 + orientation: + x: 0.0 + y: 0.0011154343428 + z: 0.0 + w: 0.999999377903 + covariance: [10.316525756160136, -4.7162077503369026e-08, 0.0, 0.0, -4.2237093001117274e-05, 0.0, -4.7162077503369006e-08, 10.316525260833734, 0.0, 0.0, 7.475397411887995e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.2237093001117253e-05, 7.4753974118879985e-06, 0.0, 0.0, 10.096902462494842, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 311 + stamp: + secs: 1624875221 + nsecs: 721393108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0664853345228 + y: 0.253445552848 + z: 0.0 + orientation: + x: 0.0 + y: 0.00073651497299 + z: 0.0 + w: 0.999999728773 + covariance: [11.187840483848712, -4.824688525893254e-08, 0.0, 0.0, -3.332323161105107e-05, 0.0, -4.8246885258932514e-08, 11.187839977713123, 0.0, 0.0, 9.19003044157396e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.332323161105104e-05, 9.190030441573961e-06, 0.0, 0.0, 10.960235765545233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 312 + stamp: + secs: 1624875221 + nsecs: 801403046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0668622717009 + y: 0.25205467772 + z: 0.0 + orientation: + x: 0.0 + y: 0.000237664867538 + z: 0.0 + w: 0.999999971758 + covariance: [10.177532056051241, -4.022729371885413e-08, 0.0, 0.0, -3.2018324997053226e-05, 0.0, -4.0227293718854116e-08, 10.177531634607694, 0.0, 0.0, 8.504240279623335e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.2018324997053206e-05, 8.504240279623337e-06, 0.0, 0.0, 9.984561749360495, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 313 + stamp: + secs: 1624875221 + nsecs: 881417036 + frame_id: "/world" +pose: + pose: + position: + x: -0.0600659748247 + y: 0.246663544113 + z: 0.0 + orientation: + x: 0.0 + y: 0.000582431041317 + z: 0.0 + w: 0.999999830387 + covariance: [9.774800945853986, -3.711984697297885e-08, 0.0, 0.0, -3.595502409487876e-05, 0.0, -3.7119846972978847e-08, 9.774800557449955, 0.0, 0.0, 7.330526081496382e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.595502409487874e-05, 7.33052608149638e-06, 0.0, 0.0, 9.594688777466903, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 314 + stamp: + secs: 1624875221 + nsecs: 961416006 + frame_id: "/world" +pose: + pose: + position: + x: -0.0596399885306 + y: 0.24795227566 + z: 0.0 + orientation: + x: 0.0 + y: 0.00113154067508 + z: 0.0 + w: 0.999999359808 + covariance: [10.904800740104848, -4.2977867807664004e-08, 0.0, 0.0, -4.081380480631859e-05, 0.0, -4.297786780766399e-08, 10.904800291537686, 0.0, 0.0, 1.704507240998722e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.0813804806318563e-05, 1.7045072409987217e-05, 0.0, 0.0, 10.694594802513745, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 315 + stamp: + secs: 1624875222 + nsecs: 41435957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0591758065669 + y: 0.246451996662 + z: 0.0 + orientation: + x: 0.0 + y: 0.00134868706419 + z: 0.0 + w: 0.999999090521 + covariance: [11.496171173540553, -5.8849942566826576e-08, 0.0, 0.0, -0.0005725543263537358, 0.0, -5.884994256682658e-08, 11.496170660445632, 0.0, 0.0, 0.0001654029510830258, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005725543263537357, 0.00016540295108302577, 0.0, 0.0, 11.274220378623415, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 316 + stamp: + secs: 1624875222 + nsecs: 121417046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0591004761452 + y: 0.24654899964 + z: 0.0 + orientation: + x: 0.0 + y: 0.00193986465015 + z: 0.0 + w: 0.999998118461 + covariance: [13.824733287216592, -1.666415658197411e-07, 0.0, 0.0, -0.001385455952048602, 0.0, -1.6664156581974107e-07, 13.82473231613377, 0.0, 0.0, 0.0003984495864671929, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013854559520486017, 0.0003984495864671929, 0.0, 0.0, 13.526799061739172, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 317 + stamp: + secs: 1624875222 + nsecs: 201446056 + frame_id: "/world" +pose: + pose: + position: + x: -0.0591476830716 + y: 0.246629142032 + z: 0.0 + orientation: + x: 0.0 + y: 0.00207492094838 + z: 0.0 + w: 0.999997847349 + covariance: [15.437371282982902, -3.5403020691835286e-07, 0.0, 0.0, -0.0018575264665381867, 0.0, -3.540302069183529e-07, 15.437369587334238, 0.0, 0.0, 0.000546249143501606, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0018575264665381857, 0.0005462491435016058, 0.0, 0.0, 15.044858949644388, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 318 + stamp: + secs: 1624875222 + nsecs: 281445026 + frame_id: "/world" +pose: + pose: + position: + x: -0.0504233944545 + y: 0.250729831219 + z: 0.0 + orientation: + x: 0.0 + y: 0.00208477917832 + z: 0.0 + w: 0.999997826846 + covariance: [16.534322161770856, -6.043501605233961e-07, 0.0, 0.0, -0.0020915792990233843, 0.0, -6.043501605233961e-07, 16.534319515995215, 0.0, 0.0, 0.0006095480062201463, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0020915792990233843, 0.0006095480062201464, 0.0, 0.0, 16.02991449919313, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 319 + stamp: + secs: 1624875222 + nsecs: 361438990 + frame_id: "/world" +pose: + pose: + position: + x: -0.0609054952185 + y: 0.252065156756 + z: 0.0 + orientation: + x: 0.0 + y: 0.000547785612473 + z: 0.0 + w: 0.999999849965 + covariance: [12.020084629209812, -3.462388906653031e-07, 0.0, 0.0, -0.00093886135925317, 0.0, -3.4623889066530304e-07, 12.020083157606487, 0.0, 0.0, 0.00027080710646441435, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009388613592531698, 0.0002708071064644144, 0.0, 0.0, 11.758772170703857, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 320 + stamp: + secs: 1624875222 + nsecs: 441457987 + frame_id: "/world" +pose: + pose: + position: + x: -0.0607080397196 + y: 0.252552282636 + z: 0.0 + orientation: + x: 0.0 + y: 0.000528031086589 + z: 0.0 + w: 0.999999860592 + covariance: [10.4830671020409, -2.85456081483138e-07, 0.0, 0.0, -0.0005561659855871397, 0.0, -2.8545608148313795e-07, 10.4830659285497, 0.0, 0.0, 0.00016538441248811097, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005561659855871394, 0.00016538441248811092, 0.0, 0.0, 10.291805740225081, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 321 + stamp: + secs: 1624875222 + nsecs: 521461964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0610870571733 + y: 0.253108929571 + z: 0.0 + orientation: + x: 0.0 + y: 0.000857994669415 + z: 0.0 + w: 0.999999631923 + covariance: [11.29946131128637, -3.9370440172523635e-07, 0.0, 0.0, -0.0005076594351132391, 0.0, -3.937044017252362e-07, 11.299459740256259, 0.0, 0.0, 0.00015937073491753586, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005076594351132388, 0.0001593707349175358, 0.0, 0.0, 11.064424747932069, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 322 + stamp: + secs: 1624875222 + nsecs: 601474047 + frame_id: "/world" +pose: + pose: + position: + x: -0.0606089013993 + y: 0.252236784796 + z: 0.0 + orientation: + x: 0.0 + y: 0.000897797158663 + z: 0.0 + w: 0.99999959698 + covariance: [11.724653639762375, -4.6685217929769756e-07, 0.0, 0.0, -0.00044656304487221497, 0.0, -4.6685217929769745e-07, 11.724651812590029, 0.0, 0.0, 0.00015948824538911197, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004465630448722147, 0.00015948824538911195, 0.0, 0.0, 11.461935086232751, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 323 + stamp: + secs: 1624875222 + nsecs: 681476116 + frame_id: "/world" +pose: + pose: + position: + x: -0.0601387769358 + y: 0.252443860586 + z: 0.0 + orientation: + x: 0.0 + y: 0.000509241831921 + z: 0.0 + w: 0.999999870336 + covariance: [10.385755804789264, -3.790915765193058e-07, 0.0, 0.0, -0.00028564095726039756, 0.0, -3.7909157651930594e-07, 10.385754340835263, 0.0, 0.0, 0.00011015463375678269, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00028564095726039756, 0.00011015463375678269, 0.0, 0.0, 10.179192691118548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 324 + stamp: + secs: 1624875222 + nsecs: 761483908 + frame_id: "/world" +pose: + pose: + position: + x: -0.0622200172223 + y: 0.251474850039 + z: 0.0 + orientation: + x: 0.0 + y: 0.000789638704392 + z: 0.0 + w: 0.999999688235 + covariance: [13.080361735067147, -6.051320192430684e-07, 0.0, 0.0, -0.00033579563710160135, 0.0, -6.051320192430685e-07, 13.080359421365893, 0.0, 0.0, 0.0001291052566652159, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00033579563710160135, 0.0001291052566652159, 0.0, 0.0, 12.759088390109579, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 325 + stamp: + secs: 1624875222 + nsecs: 841490030 + frame_id: "/world" +pose: + pose: + position: + x: -0.0618264327942 + y: 0.251893898201 + z: 0.0 + orientation: + x: 0.0 + y: 0.000536155519812 + z: 0.0 + w: 0.999999856269 + covariance: [12.622442022787533, -6.121197181912751e-07, 0.0, 0.0, -0.00028262046868592945, 0.0, -6.121197181912753e-07, 12.62243969623539, 0.0, 0.0, 9.711090625868903e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00028262046868592945, 9.711090625868906e-05, 0.0, 0.0, 12.298949632987824, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 326 + stamp: + secs: 1624875222 + nsecs: 921515942 + frame_id: "/world" +pose: + pose: + position: + x: -0.0628755340785 + y: 0.26062154256 + z: 0.0 + orientation: + x: 0.0 + y: 0.000171495676441 + z: 0.0 + w: 0.999999985295 + covariance: [9.447383291421087, -3.218982453511612e-07, 0.0, 0.0, -0.00014162101324229854, 0.0, -3.218982453511612e-07, 9.44738207172136, 0.0, 0.0, 4.100605533635475e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001416210132422985, 4.100605533635474e-05, 0.0, 0.0, 9.27608852370007, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 327 + stamp: + secs: 1624875223 + nsecs: 1523018 + frame_id: "/world" +pose: + pose: + position: + x: -0.0629538630759 + y: 0.260729326902 + z: 0.0 + orientation: + x: 0.0 + y: 0.000406282159971 + z: 0.0 + w: 0.999999917467 + covariance: [9.457066931986384, -2.9718296322205495e-07, 0.0, 0.0, -0.00011387180645084524, 0.0, -2.971829632220549e-07, 9.457065809084687, 0.0, 0.0, 2.9267677432943057e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011387180645084529, 2.926767743294306e-05, 0.0, 0.0, 9.298762089156346, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 328 + stamp: + secs: 1624875223 + nsecs: 81501961 + frame_id: "/world" +pose: + pose: + position: + x: -0.0658162474073 + y: 0.268168244138 + z: 0.0 + orientation: + x: 0.0 + y: 0.000790822111937 + z: 0.0 + w: 0.9999996873 + covariance: [12.383931199655388, -4.751437959604132e-07, 0.0, 0.0, -0.0001474677443072151, 0.0, -4.751437959604131e-07, 12.383929415412899, 0.0, 0.0, 0.00011706662155272503, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00014746774430721515, 0.00011706662155272501, 0.0, 0.0, 12.131842990062141, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 329 + stamp: + secs: 1624875223 + nsecs: 161525011 + frame_id: "/world" +pose: + pose: + position: + x: -0.0660812208708 + y: 0.268154185222 + z: 0.0 + orientation: + x: 0.0 + y: 0.000641663722076 + z: 0.0 + w: 0.999999794134 + covariance: [10.614012751091435, -3.4173234633695316e-07, 0.0, 0.0, -9.314679198754315e-05, 0.0, -3.417323463369532e-07, 10.614011478734016, 0.0, 0.0, 0.00012584301911690117, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.314679198754318e-05, 0.00012584301911690117, 0.0, 0.0, 10.431440901169568, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 330 + stamp: + secs: 1624875223 + nsecs: 241575956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0650988149081 + y: 0.266440106956 + z: 0.0 + orientation: + x: 0.0 + y: 0.000144105217008 + z: 0.0 + w: 0.999999989617 + covariance: [8.85099847052954, -2.592844877046882e-07, 0.0, 0.0, -7.57653300144453e-05, 0.0, -2.5928448770468827e-07, 8.85099751651872, 0.0, 0.0, 0.00010434834314954772, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.57653300144453e-05, 0.00010434834314954772, 0.0, 0.0, 8.710279610456578, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 331 + stamp: + secs: 1624875223 + nsecs: 321557045 + frame_id: "/world" +pose: + pose: + position: + x: -0.0659773792453 + y: 0.268783664374 + z: 0.0 + orientation: + x: -0.0 + y: -1.66429757673e-05 + z: -0.0 + w: 0.999999999862 + covariance: [7.474562098339697, -1.6639571202392258e-07, 0.0, 0.0, -4.7981117370574916e-05, 0.0, -1.6639571202392258e-07, 7.474561492714461, 0.0, 0.0, 6.345162820711044e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.79811173705749e-05, 6.345162820711045e-05, 0.0, 0.0, 7.383557193412096, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 332 + stamp: + secs: 1624875223 + nsecs: 401560068 + frame_id: "/world" +pose: + pose: + position: + x: -0.0683041799822 + y: 0.271986292311 + z: 0.0 + orientation: + x: 0.0 + y: 0.000420421613039 + z: 0.0 + w: 0.999999911623 + covariance: [7.07396942701025, -1.6854970953086886e-07, 0.0, 0.0, -3.494689746222924e-05, 0.0, -1.6854970953086891e-07, 7.0739688212193075, 0.0, 0.0, 4.7737354170659394e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.494689746222924e-05, 4.773735417065941e-05, 0.0, 0.0, 6.981143309655695, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 333 + stamp: + secs: 1624875223 + nsecs: 481564999 + frame_id: "/world" +pose: + pose: + position: + x: -0.0708530048794 + y: 0.273362413903 + z: 0.0 + orientation: + x: 0.0 + y: 0.000539809258736 + z: 0.0 + w: 0.999999854303 + covariance: [8.389616415058956, -2.1994720688932727e-07, 0.0, 0.0, -3.095835351001098e-05, 0.0, -2.1994720688932732e-07, 8.389615630366235, 0.0, 0.0, 4.3849838378338154e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0958353510010975e-05, 4.3849838378338154e-05, 0.0, 0.0, 8.267071024952523, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 334 + stamp: + secs: 1624875223 + nsecs: 561572075 + frame_id: "/world" +pose: + pose: + position: + x: -0.0789638008897 + y: 0.276809062711 + z: 0.0 + orientation: + x: 0.0 + y: 0.000405388425442 + z: 0.0 + w: 0.99999991783 + covariance: [8.98833036074085, -2.2836078790515871e-07, 0.0, 0.0, -2.2496140106223114e-05, 0.0, -2.2836078790515882e-07, 8.988329549442263, 0.0, 0.0, 3.30016905430963e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2496140106223108e-05, 3.300169054309628e-05, 0.0, 0.0, 8.859667128400028, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 335 + stamp: + secs: 1624875223 + nsecs: 641552925 + frame_id: "/world" +pose: + pose: + position: + x: -0.0789727994369 + y: 0.277196007931 + z: 0.0 + orientation: + x: 0.0 + y: 0.000569758379728 + z: 0.0 + w: 0.999999837688 + covariance: [8.287749641444654, -1.7683869816003012e-07, 0.0, 0.0, -1.1163233229755226e-05, 0.0, -1.7683869816003018e-07, 8.287749014888576, 0.0, 0.0, 1.8297468066660258e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.116323322975522e-05, 1.8297468066660247e-05, 0.0, 0.0, 8.187483565696068, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 336 + stamp: + secs: 1624875223 + nsecs: 721590996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0789995951559 + y: 0.277667994039 + z: 0.0 + orientation: + x: 0.0 + y: 0.000559018776837 + z: 0.0 + w: 0.999999843749 + covariance: [8.039848537243204, -1.598541092902315e-07, 0.0, 0.0, -9.391583902347966e-06, 0.0, -1.5985410929023152e-07, 8.039847972106827, 0.0, 0.0, 1.3529663932041463e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.391583902347961e-06, 1.352966393204146e-05, 0.0, 0.0, 7.94736133218905, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 337 + stamp: + secs: 1624875223 + nsecs: 801577091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0795914350492 + y: 0.278457276129 + z: 0.0 + orientation: + x: -0.0 + y: -4.18618530647e-05 + z: -0.0 + w: 0.999999999124 + covariance: [7.230432952037622, -1.2559554452268912e-07, 0.0, 0.0, -2.3450374558133242e-05, 0.0, -1.2559554452268918e-07, 7.230432508721251, 0.0, 0.0, 8.860111553621698e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3450374558133242e-05, 8.860111553621693e-06, 0.0, 0.0, 7.15623457607848, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 338 + stamp: + secs: 1624875223 + nsecs: 881587982 + frame_id: "/world" +pose: + pose: + position: + x: -0.076344896705 + y: 0.273440931165 + z: 0.0 + orientation: + x: -0.0 + y: -0.000120878518504 + z: -0.0 + w: 0.999999992694 + covariance: [6.976016765496476, -1.1352848485117645e-07, 0.0, 0.0, -3.0270970885673144e-05, 0.0, -1.1352848485117648e-07, 6.976016364934502, 0.0, 0.0, 6.041409156791846e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0270970885673137e-05, 6.041409156791841e-06, 0.0, 0.0, 6.9079108281391175, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 339 + stamp: + secs: 1624875223 + nsecs: 961596966 + frame_id: "/world" +pose: + pose: + position: + x: -0.0809575537586 + y: 0.277520830363 + z: 0.0 + orientation: + x: -0.0 + y: -2.93591825883e-05 + z: -0.0 + w: 0.999999999569 + covariance: [7.542058548828703, -1.28291106959609e-07, 0.0, 0.0, -2.9144793887127767e-05, 0.0, -1.2829110695960903e-07, 7.542058096112143, 0.0, 0.0, 4.850865118189671e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9144793887127774e-05, 4.850865118189668e-06, 0.0, 0.0, 7.4639089776321015, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 340 + stamp: + secs: 1624875224 + nsecs: 41613102 + frame_id: "/world" +pose: + pose: + position: + x: -0.0845112181695 + y: 0.278911001904 + z: 0.0 + orientation: + x: -0.0 + y: -0.000195859993587 + z: -0.0 + w: 0.999999980819 + covariance: [7.079962443797534, -1.1935680976334024e-07, 0.0, 0.0, -2.5682895905627326e-05, 0.0, -1.1935680976334019e-07, 7.079962023519487, 0.0, 0.0, 3.079422528545546e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.568289590562733e-05, 3.079422528545546e-05, 0.0, 0.0, 7.006204493763564, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 341 + stamp: + secs: 1624875224 + nsecs: 121651888 + frame_id: "/world" +pose: + pose: + position: + x: -0.0861482180483 + y: 0.279982870415 + z: 0.0 + orientation: + x: -0.0 + y: -0.000278540581508 + z: -0.0 + w: 0.999999961208 + covariance: [6.9574936173701705, -1.567860087988423e-07, 0.0, 0.0, -3.960186607098884e-05, 0.0, -1.5678600879884227e-07, 6.957493070956921, 0.0, 0.0, 5.706723208453386e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.960186607098883e-05, 5.706723208453385e-05, 0.0, 0.0, 6.859196196446501, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 342 + stamp: + secs: 1624875224 + nsecs: 201616049 + frame_id: "/world" +pose: + pose: + position: + x: -0.077556423474 + y: 0.266944843058 + z: 0.0 + orientation: + x: 0.0 + y: 2.21314590936e-05 + z: 0.0 + w: 0.999999999755 + covariance: [9.326236994992977, -2.833339545758425e-07, 0.0, 0.0, -6.113335317778019e-05, 0.0, -2.8333395457584246e-07, 9.326236015630434, 0.0, 0.0, 8.200671247209192e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.113335317778015e-05, 8.20067124720919e-05, 0.0, 0.0, 9.147254084555145, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 343 + stamp: + secs: 1624875224 + nsecs: 281644106 + frame_id: "/world" +pose: + pose: + position: + x: -0.0775953235325 + y: 0.26680551569 + z: 0.0 + orientation: + x: 0.0 + y: 0.000194035660082 + z: 0.0 + w: 0.999999981175 + covariance: [9.412888547140684, -2.6489978245649104e-07, 0.0, 0.0, -4.230982437243472e-05, 0.0, -2.6489978245649094e-07, 9.412887635274387, 0.0, 0.0, 6.1015311787004334e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.230982437243469e-05, 6.101531178700431e-05, 0.0, 0.0, 9.243428593176809, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 344 + stamp: + secs: 1624875224 + nsecs: 361641884 + frame_id: "/world" +pose: + pose: + position: + x: -0.0767888290382 + y: 0.263676304478 + z: 0.0 + orientation: + x: -0.0 + y: -0.000309140194416 + z: -0.0 + w: 0.999999952216 + covariance: [9.4447414653369, -2.4804753667989636e-07, 0.0, 0.0, -0.00010201279250681467, 0.0, -2.480475366798963e-07, 9.444740614765985, 0.0, 0.0, 4.742402089432809e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010201279250681469, 4.742402089432809e-05, 0.0, 0.0, 9.28472208050006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 345 + stamp: + secs: 1624875224 + nsecs: 441647053 + frame_id: "/world" +pose: + pose: + position: + x: -0.0765826620295 + y: 0.263537451233 + z: 0.0 + orientation: + x: -0.0 + y: -0.000571391003235 + z: -0.0 + w: 0.999999836756 + covariance: [9.476157168176934, -2.6707822644836313e-07, 0.0, 0.0, -0.000165206074526183, 0.0, -2.670782264483631e-07, 9.476156251790414, 0.0, 0.0, 4.056859774027124e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00016520607452618299, 4.056859774027124e-05, 0.0, 0.0, 9.301906205851333, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 346 + stamp: + secs: 1624875224 + nsecs: 521675110 + frame_id: "/world" +pose: + pose: + position: + x: -0.0758445284269 + y: 0.268027662074 + z: 0.0 + orientation: + x: -0.0 + y: -0.00027135703475 + z: -0.0 + w: 0.999999963183 + covariance: [8.48418980402142, -2.1952052300272794e-07, 0.0, 0.0, -0.00013914730382207045, 0.0, -2.1952052300272794e-07, 8.484189048406241, 0.0, 0.0, 2.6881626961270266e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00013914730382207048, 2.6881626961270266e-05, 0.0, 0.0, 8.339044754140371, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 347 + stamp: + secs: 1624875224 + nsecs: 601653099 + frame_id: "/world" +pose: + pose: + position: + x: -0.0765596575245 + y: 0.26971748426 + z: 0.0 + orientation: + x: -0.0 + y: -8.89738266997e-05 + z: -0.0 + w: 0.999999996042 + covariance: [8.137494150467276, -2.017407405599342e-07, 0.0, 0.0, -0.00011470395610172289, 0.0, -2.017407405599341e-07, 8.13749345306914, 0.0, 0.0, 1.9141138102215134e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011470395610172289, 1.9141138102215145e-05, 0.0, 0.0, 8.0019707755978, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 348 + stamp: + secs: 1624875224 + nsecs: 681679964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0766817364409 + y: 0.269669047437 + z: 0.0 + orientation: + x: 0.0 + y: 0.000142445652347 + z: 0.0 + w: 0.999999989855 + covariance: [8.011967628619116, -1.94072604685331e-07, 0.0, 0.0, -8.325357979265529e-05, 0.0, -1.9407260468533088e-07, 8.01196695526083, 0.0, 0.0, 1.2128212650588888e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.325357979265528e-05, 1.2128212650588893e-05, 0.0, 0.0, 7.879509336318668, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 349 + stamp: + secs: 1624875224 + nsecs: 761667967 + frame_id: "/world" +pose: + pose: + position: + x: -0.0792062117799 + y: 0.271575186002 + z: 0.0 + orientation: + x: 0.0 + y: 0.000150884841275 + z: 0.0 + w: 0.999999988617 + covariance: [7.96341809737664, -1.8601162480945377e-07, 0.0, 0.0, -8.50051031386703e-05, 0.0, -1.8601162480945364e-07, 7.963417449488675, 0.0, 0.0, 8.590744768687535e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.50051031386703e-05, 8.59074476868754e-06, 0.0, 0.0, 7.834402514405279, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 350 + stamp: + secs: 1624875224 + nsecs: 841675043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0797926180461 + y: 0.272955328307 + z: 0.0 + orientation: + x: 0.0 + y: 9.03086612735e-05 + z: 0.0 + w: 0.999999995922 + covariance: [6.622993047551593, -1.1810357158625077e-07, 0.0, 0.0, -5.460635402201216e-05, 0.0, -1.1810357158625067e-07, 6.62299263444393, 0.0, 0.0, 4.4726577342637616e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.460635402201214e-05, 4.472657734263764e-06, 0.0, 0.0, 6.539559020065255, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 351 + stamp: + secs: 1624875224 + nsecs: 921693087 + frame_id: "/world" +pose: + pose: + position: + x: -0.0801173625538 + y: 0.273184670813 + z: 0.0 + orientation: + x: 0.0 + y: 2.81467591238e-05 + z: 0.0 + w: 0.999999999604 + covariance: [6.254073117015302, -9.520358802266636e-08, 0.0, 0.0, -5.1919786067113475e-05, 0.0, -9.520358802266631e-08, 6.2540727834425, 0.0, 0.0, 1.810188884517827e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.1919786067113475e-05, 1.8101888845178268e-05, 0.0, 0.0, 6.185886648173521, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 352 + stamp: + secs: 1624875225 + nsecs: 1673937 + frame_id: "/world" +pose: + pose: + position: + x: -0.0802803552942 + y: 0.273236749469 + z: 0.0 + orientation: + x: 0.0 + y: 6.76048988962e-05 + z: 0.0 + w: 0.999999997715 + covariance: [8.848710365148527, -1.4971089159280565e-07, 0.0, 0.0, -8.555006109994648e-05, 0.0, -1.497108915928056e-07, 8.848709842868717, 0.0, 0.0, 4.085114519158449e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.555006109994649e-05, 4.085114519158449e-05, 0.0, 0.0, 8.74139507381806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 353 + stamp: + secs: 1624875225 + nsecs: 81682920 + frame_id: "/world" +pose: + pose: + position: + x: -0.0804948149461 + y: 0.273300878929 + z: 0.0 + orientation: + x: 0.0 + y: 0.000172914703221 + z: 0.0 + w: 0.99999998505 + covariance: [6.842336724519776, -1.0381843960129253e-07, 0.0, 0.0, -5.57053641484502e-05, 0.0, -1.0381843960129252e-07, 6.8423363640379256, 0.0, 0.0, 2.3740500795327743e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.5705364148450214e-05, 2.3740500795327743e-05, 0.0, 0.0, 6.767250849649829, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 354 + stamp: + secs: 1624875225 + nsecs: 161689043 + frame_id: "/world" +pose: + pose: + position: + x: -0.080697452357 + y: 0.273172924826 + z: 0.0 + orientation: + x: 0.0 + y: 0.000475921310687 + z: 0.0 + w: 0.999999886749 + covariance: [6.875033344419229, -1.2914723096814106e-07, 0.0, 0.0, -6.340339293583947e-05, 0.0, -1.2914723096814103e-07, 6.8750328976581825, 0.0, 0.0, 1.8910865888437137e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.340339293583944e-05, 1.8910865888437137e-05, 0.0, 0.0, 6.780880384479232, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 355 + stamp: + secs: 1624875225 + nsecs: 241738081 + frame_id: "/world" +pose: + pose: + position: + x: -0.0811792404278 + y: 0.274038105969 + z: 0.0 + orientation: + x: 0.0 + y: 0.000406250681459 + z: 0.0 + w: 0.99999991748 + covariance: [6.878889327722448, -1.2669622887342905e-07, 0.0, 0.0, -4.8937157700628336e-05, 0.0, -1.2669622887342897e-07, 6.878888889965597, 0.0, 0.0, 1.166603261406046e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.89371577006283e-05, 1.1666032614060454e-05, 0.0, 0.0, 6.785228223370331, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 356 + stamp: + secs: 1624875225 + nsecs: 321708918 + frame_id: "/world" +pose: + pose: + position: + x: -0.081290157309 + y: 0.274129920264 + z: 0.0 + orientation: + x: 0.0 + y: 0.000175432914357 + z: 0.0 + w: 0.999999984612 + covariance: [6.872725412388043, -1.1479064907772075e-07, 0.0, 0.0, -3.089473794218805e-05, 0.0, -1.1479064907772063e-07, 6.872725015998508, 0.0, 0.0, 6.932822901736759e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.089473794218803e-05, 6.9328229017367525e-06, 0.0, 0.0, 6.786480666878184, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 357 + stamp: + secs: 1624875225 + nsecs: 401716948 + frame_id: "/world" +pose: + pose: + position: + x: -0.0819274671055 + y: 0.275088848837 + z: 0.0 + orientation: + x: 0.0 + y: 3.78640856774e-05 + z: 0.0 + w: 0.999999999283 + covariance: [6.868403867536371, -1.068095313394706e-07, 0.0, 0.0, -2.260686327234516e-05, 0.0, -1.068095313394705e-07, 6.868403498883822, 0.0, 0.0, 7.0568498404089995e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.260686327234515e-05, 7.056849840408999e-06, 0.0, 0.0, 6.786891177317468, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 358 + stamp: + secs: 1624875225 + nsecs: 481729984 + frame_id: "/world" +pose: + pose: + position: + x: -0.083971233837 + y: 0.276180505559 + z: 0.0 + orientation: + x: -0.0 + y: -0.000120043432073 + z: -0.0 + w: 0.999999992795 + covariance: [7.49638630007811, -1.0382691253786488e-07, 0.0, 0.0, -2.3590393921623978e-05, 0.0, -1.038269125378648e-07, 7.496385942027027, 0.0, 0.0, 1.3113625398517515e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3590393921623978e-05, 1.3113625398517515e-05, 0.0, 0.0, 7.416190651549669, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 359 + stamp: + secs: 1624875225 + nsecs: 561794043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0870949474083 + y: 0.279499687286 + z: 0.0 + orientation: + x: -0.0 + y: -0.00033572549703 + z: -0.0 + w: 0.999999943644 + covariance: [7.743542939720301, -1.0618535263907891e-07, 0.0, 0.0, -2.6263680468436355e-05, 0.0, -1.0618535263907892e-07, 7.743542574120819, 0.0, 0.0, 1.6012183226435375e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6263680468436368e-05, 1.601218322643538e-05, 0.0, 0.0, 7.66051471679914, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 360 + stamp: + secs: 1624875225 + nsecs: 641766071 + frame_id: "/world" +pose: + pose: + position: + x: -0.0886857555693 + y: 0.278736807462 + z: 0.0 + orientation: + x: -0.0 + y: -0.000150325983202 + z: -0.0 + w: 0.999999988701 + covariance: [8.704594408969355, -1.5208053742057257e-07, 0.0, 0.0, -9.876621199402392e-05, 0.0, -1.5208053742057252e-07, 8.704593887865917, 0.0, 0.0, 4.783137197249906e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.876621199402391e-05, 4.783137197249905e-05, 0.0, 0.0, 8.584728607752771, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 0 + stamp: + secs: 1624875191 + nsecs: 558718920 + frame_id: "/world" +pose: + pose: + position: + x: 0.0923602762393 + y: 0.0102065644464 + z: 0.0 + orientation: + x: 0.0 + y: 0.0471156780545 + z: 0.0 + w: 0.998889439769 + covariance: [0.09798037559877427, -1.68358732288718e-18, 0.0, 0.0, 6.128441227512958e-14, 0.0, -1.6835873228864287e-18, 0.09798037559877444, 0.0, 0.0, -3.650189795357495e-12, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 6.128441227513547e-14, -3.650189795357921e-12, 0.0, 0.0, 0.09798030470902293, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 1 + stamp: + secs: 1624875191 + nsecs: 638731003 + frame_id: "/world" +pose: + pose: + position: + x: 0.106228830275 + y: 0.0151566925105 + z: 0.0 + orientation: + x: 0.0 + y: 0.0194111332989 + z: 0.0 + w: 0.999811586202 + covariance: [3.3783884747177413, -1.221265690512682e-12, 0.0, 0.0, -1.5340831415739486e-07, 0.0, -1.2212656905126821e-12, 3.3783884747199147, 0.0, 0.0, 3.4118344821480353e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.534083141573949e-07, 3.4118344821480353e-07, 0.0, 0.0, 3.3783362523957123, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 2 + stamp: + secs: 1624875192 + nsecs: 838804007 + frame_id: "/world" +pose: + pose: + position: + x: 0.100935010492 + y: 0.0126733741331 + z: 0.0 + orientation: + x: 0.0 + y: 0.179613772122 + z: 0.0 + w: 0.983737207218 + covariance: [9.761567366508634, -2.332133514151258e-06, 0.0, 0.0, -0.0026928580039068074, 0.0, -2.3321335141512508e-06, 9.76156342453144, 0.0, 0.0, 0.0012467757868695967, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.002692858003906807, 0.0012467757868695974, 0.0, 0.0, 5.015700300680833, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 3 + stamp: + secs: 1624875192 + nsecs: 918823004 + frame_id: "/world" +pose: + pose: + position: + x: 0.100805737952 + y: -0.0081697671005 + z: 0.0 + orientation: + x: -0.0 + y: -0.0304815183375 + z: -0.0 + w: 0.999535330561 + covariance: [4.921541774535543, -4.07231484642146e-07, 0.0, 0.0, -0.0004978060921384511, 0.0, -4.0723148464214474e-07, 4.921541082765353, 0.0, 0.0, 0.00022934761942997674, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004978060921384514, 0.00022934761942997695, 0.0, 0.0, 4.169424018901372, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 4 + stamp: + secs: 1624875192 + nsecs: 998827934 + frame_id: "/world" +pose: + pose: + position: + x: 0.103944920863 + y: -0.00805548280622 + z: 0.0 + orientation: + x: -0.0 + y: -0.00304883284805 + z: -0.0 + w: 0.999995352298 + covariance: [5.271889144134795, -1.7637610081217596e-07, 0.0, 0.0, -0.00018386063908794271, 0.0, -1.7637610081217572e-07, 5.271888843753759, 0.0, 0.0, 8.52631026464194e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00018386063908794293, 8.526310264641944e-05, 0.0, 0.0, 5.023671625077126, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 5 + stamp: + secs: 1624875193 + nsecs: 78836918 + frame_id: "/world" +pose: + pose: + position: + x: 0.103897258579 + y: -0.00823029979625 + z: 0.0 + orientation: + x: -0.0 + y: -0.000273336546512 + z: -0.0 + w: 0.999999962644 + covariance: [5.3513290480510705, -8.153367698449751e-08, 0.0, 0.0, -6.84088287689448e-05, 0.0, -8.153367698449742e-08, 5.351328909000762, 0.0, 0.0, 3.221944839149563e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.840882876894486e-05, 3.2219448391495636e-05, 0.0, 0.0, 5.269717394378887, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 6 + stamp: + secs: 1624875193 + nsecs: 158852100 + frame_id: "/world" +pose: + pose: + position: + x: 0.109411745971 + y: -0.00685425519602 + z: 0.0 + orientation: + x: 0.0 + y: 3.07364402165e-05 + z: 0.0 + w: 0.999999999528 + covariance: [5.999784545432195, -2.120025043194099e-08, 0.0, 0.0, -1.953346366418934e-05, 0.0, -2.1200250431940956e-08, 5.999784509300183, 0.0, 0.0, 9.265053252169039e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9533463664189353e-05, 9.265053252169042e-06, 0.0, 0.0, 5.974651556951078, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 7 + stamp: + secs: 1624875193 + nsecs: 238862991 + frame_id: "/world" +pose: + pose: + position: + x: 0.101107083214 + y: 0.00826885647459 + z: 0.0 + orientation: + x: -0.0 + y: -5.99540288784e-05 + z: -0.0 + w: 0.999999998203 + covariance: [6.214448445083825, -6.3418131396398505e-09, 0.0, 0.0, -5.9679674251992935e-06, 0.0, -6.34181313963984e-09, 6.214448434277419, 0.0, 0.0, 2.8448986712828544e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.9679674251992986e-06, 2.8448986712828565e-06, 0.0, 0.0, 6.206589940699875, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 8 + stamp: + secs: 1624875193 + nsecs: 318856001 + frame_id: "/world" +pose: + pose: + position: + x: 0.0926784332314 + y: 0.0275423287747 + z: 0.0 + orientation: + x: -0.0 + y: -9.06341683549e-06 + z: -0.0 + w: 0.999999999959 + covariance: [5.5820660642535, -3.757411128092938e-09, 0.0, 0.0, -2.6227490863092643e-06, 0.0, -3.757411128092934e-09, 5.582066057836703, 0.0, 0.0, 1.178963372560483e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.622749086309266e-06, 1.1789633725604836e-06, 0.0, 0.0, 5.579323898143024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 9 + stamp: + secs: 1624875193 + nsecs: 398844004 + frame_id: "/world" +pose: + pose: + position: + x: 0.0926559298547 + y: 0.0274670273806 + z: 0.0 + orientation: + x: -0.0 + y: -0.000116353083093 + z: -0.0 + w: 0.999999993231 + covariance: [8.033322705860774, -6.021354819196237e-09, 0.0, 0.0, -2.7705522978739985e-06, 0.0, -6.021354819196235e-09, 8.033322695562301, 0.0, 0.0, 1.2836501094260473e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7705522978740006e-06, 1.2836501094260479e-06, 0.0, 0.0, 8.0305848008468, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 10 + stamp: + secs: 1624875193 + nsecs: 478880882 + frame_id: "/world" +pose: + pose: + position: + x: 0.0926154340635 + y: 0.0273305404609 + z: 0.0 + orientation: + x: -0.0 + y: -0.000147346206854 + z: -0.0 + w: 0.999999989145 + covariance: [6.38240668427666, -8.782873068207728e-09, 0.0, 0.0, -7.565640638865887e-06, 0.0, -8.782873068207726e-09, 6.382406669222914, 0.0, 0.0, 1.3463373568663217e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.565640638865889e-06, 1.346337356866321e-06, 0.0, 0.0, 6.379890838924413, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 11 + stamp: + secs: 1624875193 + nsecs: 558876991 + frame_id: "/world" +pose: + pose: + position: + x: 0.0994538135161 + y: 0.0269118320679 + z: 0.0 + orientation: + x: -0.0 + y: -0.000246817175358 + z: -0.0 + w: 0.999999969541 + covariance: [5.622733027742423, -2.8557583546671916e-09, 0.0, 0.0, -2.7108959258121925e-06, 0.0, -2.855758354667191e-09, 5.622733022845247, 0.0, 0.0, 4.7139564391582756e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.710895925812192e-06, 4.713956439158275e-07, 0.0, 0.0, 5.621947685414704, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 12 + stamp: + secs: 1624875193 + nsecs: 638890028 + frame_id: "/world" +pose: + pose: + position: + x: 0.0785011235269 + y: 0.0705717263013 + z: 0.0 + orientation: + x: -0.0 + y: -8.70945188234e-06 + z: -0.0 + w: 0.999999999962 + covariance: [5.430779826872614, -1.0068993121418857e-09, 0.0, 0.0, -1.037791514702723e-06, 0.0, -1.0068993121418855e-09, 5.430779825144899, 0.0, 0.0, 1.8532259785351084e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0377915147027228e-06, 1.853225978535108e-07, 0.0, 0.0, 5.430512230033345, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 13 + stamp: + secs: 1624875193 + nsecs: 718884945 + frame_id: "/world" +pose: + pose: + position: + x: 0.0785048670066 + y: 0.0705838882405 + z: 0.0 + orientation: + x: -0.0 + y: -4.60666550946e-07 + z: -0.0 + w: 1.0 + covariance: [5.1053931692575185, -1.1060481354697719e-10, 0.0, 0.0, -4.616188987285344e-08, 0.0, -1.1060481354697714e-10, 5.105393169068048, 0.0, 0.0, 9.621007357360895e-09, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.616188987285342e-08, 9.621007357360897e-09, 0.0, 0.0, 5.105360142705162, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 14 + stamp: + secs: 1624875193 + nsecs: 798893929 + frame_id: "/world" +pose: + pose: + position: + x: 0.078504911107 + y: 0.0705840315188 + z: 0.0 + orientation: + x: 0.0 + y: 4.75983779315e-08 + z: 0.0 + w: 1.0 + covariance: [5.587932491517596, -3.3136259306765436e-11, 0.0, 0.0, -1.2806364405573206e-08, 0.0, -3.3136259306765404e-11, 5.587932491460832, 0.0, 0.0, 2.7212772508994753e-09, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2806364405573197e-08, 2.7212772508994753e-09, 0.0, 0.0, 5.587922553201984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 15 + stamp: + secs: 1624875193 + nsecs: 878897905 + frame_id: "/world" +pose: + pose: + position: + x: 0.0538617571471 + y: 0.0671889231217 + z: 0.0 + orientation: + x: -0.0 + y: -1.8942193014e-05 + z: -0.0 + w: 0.999999999821 + covariance: [5.4216312089425465, -1.0303624768745532e-10, 0.0, 0.0, -8.50993809552369e-08, 0.0, -1.0303624768745528e-10, 5.421631208765199, 0.0, 0.0, 1.2327275937115075e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.509938095523691e-08, 1.2327275937115072e-08, 0.0, 0.0, 5.421604783330392, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 16 + stamp: + secs: 1624875193 + nsecs: 958905935 + frame_id: "/world" +pose: + pose: + position: + x: 0.0538758894214 + y: 0.0672342117071 + z: 0.0 + orientation: + x: 0.0 + y: 4.25045697237e-05 + z: 0.0 + w: 0.999999999097 + covariance: [5.68333865483073, -1.7366002722022156e-10, 0.0, 0.0, -2.0948165892337386e-07, 0.0, -1.7366002722022148e-10, 5.683338654530382, 0.0, 0.0, 1.3140815808171763e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.094816589233738e-07, 1.314081580817176e-07, 0.0, 0.0, 5.683287345219091, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 17 + stamp: + secs: 1624875194 + nsecs: 38930893 + frame_id: "/world" +pose: + pose: + position: + x: 0.0595394034332 + y: 0.0570295160187 + z: 0.0 + orientation: + x: -0.0 + y: -7.34677248385e-05 + z: -0.0 + w: 0.999999997301 + covariance: [4.280835092259253, -3.0707018058059495e-10, 0.0, 0.0, 1.970729167793004e-07, 0.0, -3.0707018058059495e-10, 4.280835091729262, 0.0, 0.0, -1.0952617060021471e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 1.9707291677930037e-07, -1.0952617060021471e-07, 0.0, 0.0, 4.280746026906948, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 18 + stamp: + secs: 1624875194 + nsecs: 118920088 + frame_id: "/world" +pose: + pose: + position: + x: 0.0675043409015 + y: 0.0498450790251 + z: 0.0 + orientation: + x: 0.0 + y: 0.000111318370813 + z: 0.0 + w: 0.999999993804 + covariance: [5.327032038943727, -2.639789327478965e-09, 0.0, 0.0, -5.161797321980913e-07, 0.0, -2.6397893274789646e-09, 5.327032034385305, 0.0, 0.0, 5.205595664688122e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.161797321980913e-07, 5.205595664688122e-07, 0.0, 0.0, 5.326234122702448, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 19 + stamp: + secs: 1624875194 + nsecs: 198947906 + frame_id: "/world" +pose: + pose: + position: + x: 0.0674847777806 + y: 0.0497835592123 + z: 0.0 + orientation: + x: 0.0 + y: 7.17136789194e-07 + z: 0.0 + w: 1.0 + covariance: [8.558755355148655, -2.884998113982955e-10, 0.0, 0.0, -1.7821660783935503e-07, 0.0, -2.8849981139829534e-10, 8.55875535465037, 0.0, 0.0, 5.5952146772781216e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.782166078393551e-07, 5.59521467727812e-08, 0.0, 0.0, 8.55865879479844, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 20 + stamp: + secs: 1624875194 + nsecs: 278934002 + frame_id: "/world" +pose: + pose: + position: + x: 0.0675316962731 + y: 0.0499314799019 + z: 0.0 + orientation: + x: 0.0 + y: 0.000169527658975 + z: 0.0 + w: 0.99999998563 + covariance: [11.94159815152312, -5.851750930727207e-09, 0.0, 0.0, -5.203939135255632e-07, 0.0, -5.851750930727206e-09, 11.941598141419338, 0.0, 0.0, 1.5487880808267688e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.203939135255633e-07, 1.5487880808267686e-06, 0.0, 0.0, 11.939785460265538, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 21 + stamp: + secs: 1624875194 + nsecs: 358956099 + frame_id: "/world" +pose: + pose: + position: + x: 0.0738006844331 + y: 0.0428194080969 + z: 0.0 + orientation: + x: 0.0 + y: 0.00025398323796 + z: 0.0 + w: 0.999999967746 + covariance: [8.404940348216137, -1.2126182579968446e-08, 0.0, 0.0, -9.8494106908807e-07, 0.0, -1.2126182579968449e-08, 8.40494032728229, 0.0, 0.0, 2.7923417810837023e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.849410690880697e-07, 2.792341781083703e-06, 0.0, 0.0, 8.401082983993494, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 22 + stamp: + secs: 1624875194 + nsecs: 438956976 + frame_id: "/world" +pose: + pose: + position: + x: 0.0857788322256 + y: 0.0344530994906 + z: 0.0 + orientation: + x: 0.0 + y: 0.00079361661975 + z: 0.0 + w: 0.999999685086 + covariance: [8.03516676316282, -3.276671147477185e-08, 0.0, 0.0, -4.0991748637689757e-07, 0.0, -3.2766711474771855e-08, 8.035166706604493, 0.0, 0.0, 4.270134767721572e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.0991748637689757e-07, 4.2701347677215726e-06, 0.0, 0.0, 8.024418421515472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 23 + stamp: + secs: 1624875194 + nsecs: 518964052 + frame_id: "/world" +pose: + pose: + position: + x: 0.0858718776551 + y: 0.0346991921308 + z: 0.0 + orientation: + x: 0.0 + y: 0.000862017474509 + z: 0.0 + w: 0.999999628463 + covariance: [11.547410163055522, -7.119596866563642e-08, 0.0, 0.0, -2.1826942286276045e-06, 0.0, -7.119596866563645e-08, 11.547410040170385, 0.0, 0.0, 6.197743156203284e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.182694228627604e-06, 6.197743156203285e-06, 0.0, 0.0, 11.523544281498362, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 24 + stamp: + secs: 1624875194 + nsecs: 598967075 + frame_id: "/world" +pose: + pose: + position: + x: 0.0936511263592 + y: 0.0326972470274 + z: 0.0 + orientation: + x: -0.0 + y: -0.000140938366677 + z: -0.0 + w: 0.999999990068 + covariance: [12.435649151939524, -4.8506064157546624e-08, 0.0, 0.0, -2.5176564293010083e-06, 0.0, -4.850606415754664e-08, 12.43564906821802, 0.0, 0.0, 3.984861027034668e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5176564293010083e-06, 3.984861027034668e-06, 0.0, 0.0, 12.419328352581724, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 25 + stamp: + secs: 1624875194 + nsecs: 678981066 + frame_id: "/world" +pose: + pose: + position: + x: 0.0926630059323 + y: 0.0342065219345 + z: 0.0 + orientation: + x: -0.0 + y: -0.000602588046255 + z: -0.0 + w: 0.999999818444 + covariance: [10.288525559314532, -6.407655931856736e-08, 0.0, 0.0, -1.7004928987329004e-05, 0.0, -6.407655931856738e-08, 10.28852544868748, 0.0, 0.0, 3.5619107880050186e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7004928987328997e-05, 3.5619107880050186e-06, 0.0, 0.0, 10.266148686642602, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 26 + stamp: + secs: 1624875194 + nsecs: 758990049 + frame_id: "/world" +pose: + pose: + position: + x: 0.0958160523547 + y: 0.0375334779637 + z: 0.0 + orientation: + x: -0.0 + y: -0.000516380659031 + z: -0.0 + w: 0.999999866675 + covariance: [13.258872642643166, -4.1982354748534495e-08, 0.0, 0.0, -1.0945092164106599e-05, 0.0, -4.19823547485345e-08, 13.258872570167195, 0.0, 0.0, 2.4419252497304425e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0945092164106594e-05, 2.4419252497304425e-06, 0.0, 0.0, 13.244259878918678, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 27 + stamp: + secs: 1624875194 + nsecs: 838989973 + frame_id: "/world" +pose: + pose: + position: + x: 0.10024628264 + y: 0.0384475877581 + z: 0.0 + orientation: + x: 0.0 + y: 2.14260702054e-05 + z: 0.0 + w: 0.99999999977 + covariance: [11.791392581508845, -2.4987127073826576e-11, 0.0, 0.0, -1.1889959091327181e-05, 0.0, -2.498712707381298e-11, 11.791392581417597, 0.0, 0.0, -3.9161165813748645e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.188995909132719e-05, -3.9161165813748634e-07, 0.0, 0.0, 11.790830454602801, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 28 + stamp: + secs: 1624875194 + nsecs: 918983936 + frame_id: "/world" +pose: + pose: + position: + x: 0.10043076449 + y: 0.0389117980548 + z: 0.0 + orientation: + x: 0.0 + y: 0.000712056549991 + z: 0.0 + w: 0.999999746488 + covariance: [14.391105036010831, -3.841462381136573e-08, 0.0, 0.0, -3.928763713252007e-06, 0.0, -3.841462381136577e-08, 14.391104969656933, 0.0, 0.0, 7.093134515030945e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.928763713251996e-06, 7.093134515030945e-06, 0.0, 0.0, 14.37684844843973, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 29 + stamp: + secs: 1624875194 + nsecs: 999001026 + frame_id: "/world" +pose: + pose: + position: + x: 0.100463929418 + y: 0.0389948212032 + z: 0.0 + orientation: + x: 0.0 + y: 0.000616593692666 + z: 0.0 + w: 0.999999809906 + covariance: [16.246732901488745, -4.00499245089968e-08, 0.0, 0.0, -5.065990433110867e-06, 0.0, -4.004992450899684e-08, 16.246732832295766, 0.0, 0.0, 5.43142683544828e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.065990433110857e-06, 5.43142683544828e-06, 0.0, 0.0, 16.231681515682247, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 30 + stamp: + secs: 1624875195 + nsecs: 79009056 + frame_id: "/world" +pose: + pose: + position: + x: 0.117074898757 + y: 0.0273119354461 + z: 0.0 + orientation: + x: 0.0 + y: 0.000726209905823 + z: 0.0 + w: 0.99999973631 + covariance: [12.99652753846536, -6.617811358430674e-08, 0.0, 0.0, -6.583459508262642e-06, 0.0, -6.617811358430678e-08, 12.996527424317543, 0.0, 0.0, 4.231063476536652e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.58345950826264e-06, 4.2310634765366524e-05, 0.0, 0.0, 12.97026813870605, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 31 + stamp: + secs: 1624875195 + nsecs: 159003973 + frame_id: "/world" +pose: + pose: + position: + x: 0.123965068032 + y: 0.0219874593042 + z: 0.0 + orientation: + x: -0.0 + y: -6.70025875379e-05 + z: -0.0 + w: 0.999999997755 + covariance: [11.721553602035723, -1.1665902622807822e-07, 0.0, 0.0, -0.0005234001037202219, 0.0, -1.1665902622807827e-07, 11.721553375099017, 0.0, 0.0, 8.477867714905602e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005234001037202218, 8.477867714905605e-05, 0.0, 0.0, 11.675765946784095, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 32 + stamp: + secs: 1624875195 + nsecs: 239037991 + frame_id: "/world" +pose: + pose: + position: + x: 0.125837677132 + y: 0.0202839057683 + z: 0.0 + orientation: + x: 0.0 + y: 0.000205528803547 + z: 0.0 + w: 0.999999978879 + covariance: [11.163788964673563, -1.28097141237304e-07, 0.0, 0.0, -0.0007246089553224255, 0.0, -1.2809714123730406e-07, 11.163788631090362, 0.0, 0.0, 7.053211099724943e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007246089553224254, 7.053211099724943e-05, 0.0, 0.0, 11.115171224695358, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 33 + stamp: + secs: 1624875195 + nsecs: 319048882 + frame_id: "/world" +pose: + pose: + position: + x: 0.128681412985 + y: 0.0190470572541 + z: 0.0 + orientation: + x: 0.0 + y: 0.000142986278536 + z: 0.0 + w: 0.999999989777 + covariance: [12.234386353390223, -1.256210524485577e-07, 0.0, 0.0, -0.0006438389687046262, 0.0, -1.256210524485577e-07, 12.234385983408236, 0.0, 0.0, 5.6955245336934415e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000643838968704626, 5.69552453369344e-05, 0.0, 0.0, 12.187237448094653, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 34 + stamp: + secs: 1624875195 + nsecs: 399015903 + frame_id: "/world" +pose: + pose: + position: + x: 0.133516401742 + y: 0.01834091075 + z: 0.0 + orientation: + x: 0.0 + y: 0.000274624270913 + z: 0.0 + w: 0.999999962291 + covariance: [11.384942356749447, -1.2004475773224755e-07, 0.0, 0.0, -0.0004743544307640542, 0.0, -1.2004475773224752e-07, 11.384941955636222, 0.0, 0.0, 3.980462363767507e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00047435443076405406, 3.980462363767507e-05, 0.0, 0.0, 11.340113943861333, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 35 + stamp: + secs: 1624875195 + nsecs: 479039907 + frame_id: "/world" +pose: + pose: + position: + x: 0.133627009334 + y: 0.0184568747502 + z: 0.0 + orientation: + x: 0.0 + y: 0.000533345427639 + z: 0.0 + w: 0.999999857771 + covariance: [14.104941688125463, -1.6026802996780641e-07, 0.0, 0.0, -0.0005082364915181532, 0.0, -1.6026802996780641e-07, 14.104941112476386, 0.0, 0.0, 4.1636817166548484e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005082364915181528, 4.163681716654848e-05, 0.0, 0.0, 14.045249172976062, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 36 + stamp: + secs: 1624875195 + nsecs: 559047937 + frame_id: "/world" +pose: + pose: + position: + x: 0.133137311073 + y: 0.017543011601 + z: 0.0 + orientation: + x: 0.0 + y: 0.000863868411507 + z: 0.0 + w: 0.999999626866 + covariance: [13.861505992540131, -1.8532274661421006e-07, 0.0, 0.0, -0.0004299813962681654, 0.0, -1.8532274661421006e-07, 13.861505280527654, 0.0, 0.0, 3.4971087347398704e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004299813962681652, 3.497108734739871e-05, 0.0, 0.0, 13.79203483655907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 37 + stamp: + secs: 1624875195 + nsecs: 639061928 + frame_id: "/world" +pose: + pose: + position: + x: 0.141514468711 + y: 0.0172445122426 + z: 0.0 + orientation: + x: 0.0 + y: 0.000438169789138 + z: 0.0 + w: 0.999999904004 + covariance: [15.923212680117791, -2.741551551352535e-07, 0.0, 0.0, -0.00046239252707326817, 0.0, -2.741551551352535e-07, 15.92321158202501, 0.0, 0.0, 4.833088641004912e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00046239252707326795, 4.8330886410049134e-05, 0.0, 0.0, 15.819529721629205, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 38 + stamp: + secs: 1624875195 + nsecs: 719072104 + frame_id: "/world" +pose: + pose: + position: + x: 0.143341677066 + y: 0.0172740598609 + z: 0.0 + orientation: + x: 0.0 + y: 0.000224591267408 + z: 0.0 + w: 0.999999974779 + covariance: [12.860688262042643, -1.1933845222506693e-07, 0.0, 0.0, -0.00019114928503589614, 0.0, -1.1933845222506693e-07, 12.860687781892347, 0.0, 0.0, 2.1405434259072677e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00019114928503589609, 2.140543425907269e-05, 0.0, 0.0, 12.815326309844025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 39 + stamp: + secs: 1624875195 + nsecs: 799081087 + frame_id: "/world" +pose: + pose: + position: + x: 0.14353207764 + y: 0.0174389928946 + z: 0.0 + orientation: + x: 0.0 + y: 0.00070330755904 + z: 0.0 + w: 0.999999752679 + covariance: [15.202066579054204, -2.0887267119126797e-07, 0.0, 0.0, -0.0004424151665574836, 0.0, -2.0887267119126802e-07, 15.20206570232755, 0.0, 0.0, 3.875581374294658e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004424151665574836, 3.875581374294659e-05, 0.0, 0.0, 15.121504109270616, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 40 + stamp: + secs: 1624875195 + nsecs: 879076004 + frame_id: "/world" +pose: + pose: + position: + x: 0.146670272927 + y: 0.0107045652454 + z: 0.0 + orientation: + x: 0.0 + y: 0.000380539869486 + z: 0.0 + w: 0.999999927595 + covariance: [14.431701006178075, -2.1497967307642454e-07, 0.0, 0.0, -0.0005316951105715222, 0.0, -2.1497967307642457e-07, 14.431700068961634, 0.0, 0.0, 3.662213962975546e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005316951105715222, 3.6622139629755474e-05, 0.0, 0.0, 14.347598525158562, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 41 + stamp: + secs: 1624875195 + nsecs: 959115028 + frame_id: "/world" +pose: + pose: + position: + x: 0.14920209322 + y: 0.0136058659801 + z: 0.0 + orientation: + x: -0.0 + y: -0.000428230986806 + z: -0.0 + w: 0.999999908309 + covariance: [14.038913481019597, -2.2454647634295275e-07, 0.0, 0.0, -0.0005572131352480672, 0.0, -2.2454647634295282e-07, 14.038912467664078, 0.0, 0.0, 3.18887320401775e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005572131352480672, 3.1888732040177516e-05, 0.0, 0.0, 13.949752630931266, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 42 + stamp: + secs: 1624875196 + nsecs: 39123058 + frame_id: "/world" +pose: + pose: + position: + x: 0.144082229624 + y: 0.0168413207326 + z: 0.0 + orientation: + x: 0.0 + y: 0.000717558851491 + z: 0.0 + w: 0.999999742555 + covariance: [16.058091546489273, -3.1606365622028067e-07, 0.0, 0.0, -0.0007235687289401172, 0.0, -3.160636562202809e-07, 16.0580900732218, 0.0, 0.0, 3.994852342311296e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007235687289401173, 3.9948523423112976e-05, 0.0, 0.0, 15.930966005076563, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 43 + stamp: + secs: 1624875196 + nsecs: 119092941 + frame_id: "/world" +pose: + pose: + position: + x: 0.139326615318 + y: 0.0198317446396 + z: 0.0 + orientation: + x: 0.0 + y: 0.00199561467528 + z: 0.0 + w: 0.999998008759 + covariance: [17.467884848420596, -3.952930781774462e-07, 0.0, 0.0, -0.0007633068010739801, 0.0, -3.952930781774464e-07, 17.467882985993025, 0.0, 0.0, 0.00010359489904426507, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007633068010739802, 0.00010359489904426506, 0.0, 0.0, 17.308724919883417, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 44 + stamp: + secs: 1624875196 + nsecs: 199114084 + frame_id: "/world" +pose: + pose: + position: + x: 0.13952550793 + y: 0.0198845923684 + z: 0.0 + orientation: + x: 0.0 + y: 0.00204794269602 + z: 0.0 + w: 0.999997902963 + covariance: [18.439726576033575, -4.986633553552704e-07, 0.0, 0.0, -0.0009147705309012214, 0.0, -4.986633553552708e-07, 18.439724223182616, 0.0, 0.0, 0.00020270710324363472, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009147705309012214, 0.00020270710324363477, 0.0, 0.0, 18.239838067424422, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 45 + stamp: + secs: 1624875196 + nsecs: 279119015 + frame_id: "/world" +pose: + pose: + position: + x: 0.139707603414 + y: 0.0200213585105 + z: 0.0 + orientation: + x: 0.0 + y: 0.00136595863608 + z: 0.0 + w: 0.999999067078 + covariance: [19.091298585670543, -5.876256001825093e-07, 0.0, 0.0, -0.0011410945491818767, 0.0, -5.876256001825097e-07, 19.091295801612027, 0.0, 0.0, 0.00025092394933717886, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011410945491818769, 0.0002509239493371789, 0.0, 0.0, 18.85736887489047, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 46 + stamp: + secs: 1624875196 + nsecs: 359162092 + frame_id: "/world" +pose: + pose: + position: + x: 0.139887987632 + y: 0.0201511016678 + z: 0.0 + orientation: + x: 0.0 + y: 0.000999115940855 + z: 0.0 + w: 0.999999500884 + covariance: [19.523517897021115, -6.605494005611074e-07, 0.0, 0.0, -0.0012489939951052802, 0.0, -6.605494005611077e-07, 19.523514755748078, 0.0, 0.0, 0.0002661802417557854, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0012489939951052806, 0.00026618024175578545, 0.0, 0.0, 19.262155712364446, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 47 + stamp: + secs: 1624875196 + nsecs: 439135075 + frame_id: "/world" +pose: + pose: + position: + x: 0.140037745192 + y: 0.0202586944643 + z: 0.0 + orientation: + x: 0.0 + y: 0.000661512925184 + z: 0.0 + w: 0.9999997812 + covariance: [16.533660365674166, -4.979519512436703e-07, 0.0, 0.0, -0.0008499581130913744, 0.0, -4.979519512436703e-07, 16.533657992123327, 0.0, 0.0, 0.0001803588036703954, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000849958113091374, 0.0001803588036703954, 0.0, 0.0, 16.33560569564781, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 48 + stamp: + secs: 1624875196 + nsecs: 519134998 + frame_id: "/world" +pose: + pose: + position: + x: 0.140226081868 + y: 0.0203575050588 + z: 0.0 + orientation: + x: 0.0 + y: 0.000922251193892 + z: 0.0 + w: 0.999999574726 + covariance: [17.826293724994162, -6.078119842687434e-07, 0.0, 0.0, -0.0008690414369380767, 0.0, -6.078119842687435e-07, 17.826290824146437, 0.0, 0.0, 0.00018754007921662361, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008690414369380764, 0.00018754007921662361, 0.0, 0.0, 17.584277635608522, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 49 + stamp: + secs: 1624875196 + nsecs: 599162102 + frame_id: "/world" +pose: + pose: + position: + x: 0.140407228226 + y: 0.0204837717124 + z: 0.0 + orientation: + x: 0.0 + y: 0.00125888264072 + z: 0.0 + w: 0.999999207607 + covariance: [18.700467673194723, -6.915965121453265e-07, 0.0, 0.0, -0.0008684985095493674, 0.0, -6.915965121453266e-07, 18.700464367954186, 0.0, 0.0, 0.0001807869998388871, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008684985095493673, 0.0001807869998388871, 0.0, 0.0, 18.423957994958453, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 50 + stamp: + secs: 1624875196 + nsecs: 679157019 + frame_id: "/world" +pose: + pose: + position: + x: 0.140583254706 + y: 0.0206027267884 + z: 0.0 + orientation: + x: 0.0 + y: 0.00151443092167 + z: 0.0 + w: 0.999998853249 + covariance: [19.279179205722286, -7.457009737905874e-07, 0.0, 0.0, -0.000841424757964681, 0.0, -7.457009737905877e-07, 19.27917563522086, 0.0, 0.0, 0.00016349958328449064, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008414247579646807, 0.00016349958328449061, 0.0, 0.0, 18.979042502063347, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 51 + stamp: + secs: 1624875196 + nsecs: 759172916 + frame_id: "/world" +pose: + pose: + position: + x: 0.140786776954 + y: 0.0207406176665 + z: 0.0 + orientation: + x: 0.0 + y: 0.000959624898986 + z: 0.0 + w: 0.99999953956 + covariance: [19.68298297510194, -8.3834072684779e-07, 0.0, 0.0, -0.0007964582157048356, 0.0, -8.383407268477903e-07, 19.682978955568043, 0.0, 0.0, 0.00015066792184781664, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007964582157048354, 0.00015066792184781661, 0.0, 0.0, 19.340727552084306, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 52 + stamp: + secs: 1624875196 + nsecs: 839164019 + frame_id: "/world" +pose: + pose: + position: + x: 0.140947994582 + y: 0.0208486786768 + z: 0.0 + orientation: + x: -0.0 + y: -0.000120712371078 + z: -0.0 + w: 0.999999992714 + covariance: [19.917912954718776, -8.291545340631593e-07, 0.0, 0.0, -0.0007066290086477547, 0.0, -8.291545340631598e-07, 19.917908979234966, 0.0, 0.0, 0.00014538395146126328, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007066290086477547, 0.0001453839514612633, 0.0, 0.0, 19.575672262973608, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 53 + stamp: + secs: 1624875196 + nsecs: 919202089 + frame_id: "/world" +pose: + pose: + position: + x: 0.141107047099 + y: 0.0209538851665 + z: 0.0 + orientation: + x: -0.0 + y: -0.000736401992221 + z: -0.0 + w: 0.999999728856 + covariance: [20.06858349721397, -8.176886577947344e-07, 0.0, 0.0, -0.0006357201047859182, 0.0, -8.176886577947349e-07, 20.06857957950005, 0.0, 0.0, 0.0001473125375187703, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006357201047859181, 0.00014731253751877033, 0.0, 0.0, 19.7275453021173, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 54 + stamp: + secs: 1624875196 + nsecs: 999176979 + frame_id: "/world" +pose: + pose: + position: + x: 0.141266142782 + y: 0.0210592860901 + z: 0.0 + orientation: + x: -0.0 + y: -0.000715708263153 + z: -0.0 + w: 0.999999743881 + covariance: [20.165408486877897, -8.053279113955774e-07, 0.0, 0.0, -0.0005471150894265668, 0.0, -8.053279113955776e-07, 20.165404629895736, 0.0, 0.0, 0.00013294675078253927, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005471150894265666, 0.00013294675078253927, 0.0, 0.0, 19.825245545039923, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 55 + stamp: + secs: 1624875197 + nsecs: 79178095 + frame_id: "/world" +pose: + pose: + position: + x: 0.14142428373 + y: 0.0211638210578 + z: 0.0 + orientation: + x: -0.0 + y: -0.000353548246362 + z: -0.0 + w: 0.999999937502 + covariance: [20.227672292280456, -7.932845574122855e-07, 0.0, 0.0, -0.0004728082189797432, 0.0, -7.932845574122861e-07, 20.227668495607926, 0.0, 0.0, 0.0001267920636793975, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004728082189797431, 0.00012679206367939748, 0.0, 0.0, 19.887989013538697, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 56 + stamp: + secs: 1624875197 + nsecs: 159200907 + frame_id: "/world" +pose: + pose: + position: + x: 0.141594741206 + y: 0.0212753506445 + z: 0.0 + orientation: + x: -0.0 + y: -6.76449389378e-05 + z: -0.0 + w: 0.999999997712 + covariance: [20.28128806878443, -8.130048304096402e-07, 0.0, 0.0, -0.0004521442336915954, 0.0, -8.130048304096411e-07, 20.281284186131984, 0.0, 0.0, 0.00015644666486459326, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004521442336915952, 0.0001564466648645932, 0.0, 0.0, 19.9282464029608, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 57 + stamp: + secs: 1624875197 + nsecs: 239206076 + frame_id: "/world" +pose: + pose: + position: + x: 0.141757922801 + y: 0.0213821728233 + z: 0.0 + orientation: + x: 0.0 + y: 0.000100234296303 + z: 0.0 + w: 0.999999994977 + covariance: [20.318488430962933, -8.305005835321762e-07, 0.0, 0.0, -0.00047323493888882536, 0.0, -8.305005835321772e-07, 20.318484490667295, 0.0, 0.0, 0.0002645001342582306, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00047323493888882514, 0.0002645001342582306, 0.0, 0.0, 19.954056956637363, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 58 + stamp: + secs: 1624875197 + nsecs: 319202900 + frame_id: "/world" +pose: + pose: + position: + x: 0.141885753279 + y: 0.0214638379438 + z: 0.0 + orientation: + x: -0.0 + y: -0.000105138157686 + z: -0.0 + w: 0.999999994473 + covariance: [20.31855932608583, -7.907370504615866e-07, 0.0, 0.0, -0.0005286214312372513, 0.0, -7.907370504615875e-07, 20.318555620812315, 0.0, 0.0, 0.0003846592589271845, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005286214312372512, 0.00038465925892718447, 0.0, 0.0, 19.97060307224003, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 59 + stamp: + secs: 1624875197 + nsecs: 399205923 + frame_id: "/world" +pose: + pose: + position: + x: 0.142049518958 + y: 0.0215638838068 + z: 0.0 + orientation: + x: -0.0 + y: -0.000629823219446 + z: -0.0 + w: 0.999999801661 + covariance: [20.358369335834745, -8.616529964809776e-07, 0.0, 0.0, -0.0006788210205087805, 0.0, -8.616529964809783e-07, 20.358365395167198, 0.0, 0.0, 0.0005369916770487256, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006788210205087806, 0.0005369916770487255, 0.0, 0.0, 19.98130103917718, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 60 + stamp: + secs: 1624875197 + nsecs: 479234934 + frame_id: "/world" +pose: + pose: + position: + x: 0.142190503903 + y: 0.0216459835056 + z: 0.0 + orientation: + x: -0.0 + y: -0.00107641820801 + z: -0.0 + w: 0.999999420662 + covariance: [20.367806244239233, -8.734268459692813e-07, 0.0, 0.0, -0.000753518235739007, 0.0, -8.734268459692821e-07, 20.367802340598757, 0.0, 0.0, 0.0005784198370693017, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000753518235739007, 0.0005784198370693016, 0.0, 0.0, 19.988461444487548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 61 + stamp: + secs: 1624875197 + nsecs: 559230089 + frame_id: "/world" +pose: + pose: + position: + x: 0.142322909546 + y: 0.0217199394621 + z: 0.0 + orientation: + x: -0.0 + y: -0.00131297481155 + z: -0.0 + w: 0.999999138048 + covariance: [20.36701957027534, -8.673118416178904e-07, 0.0, 0.0, -0.0007722214911405818, 0.0, -8.673118416178912e-07, 20.367015777216455, 0.0, 0.0, 0.0005611447942552138, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007722214911405816, 0.0005611447942552135, 0.0, 0.0, 19.992903013618772, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 62 + stamp: + secs: 1624875197 + nsecs: 639235020 + frame_id: "/world" +pose: + pose: + position: + x: 0.142451625064 + y: 0.0217917332905 + z: 0.0 + orientation: + x: -0.0 + y: -0.00105127595096 + z: -0.0 + w: 0.999999447409 + covariance: [20.36216942499042, -8.517444533814034e-07, 0.0, 0.0, -0.0007175647258071141, 0.0, -8.517444533814038e-07, 20.362165766160654, 0.0, 0.0, 0.0005026144805265393, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000717564725807114, 0.0005026144805265392, 0.0, 0.0, 19.995507103665037, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 63 + stamp: + secs: 1624875197 + nsecs: 719242096 + frame_id: "/world" +pose: + pose: + position: + x: 0.142571663678 + y: 0.0218588780235 + z: 0.0 + orientation: + x: -0.0 + y: -0.000645311738444 + z: -0.0 + w: 0.999999791786 + covariance: [20.34987289751099, -8.189060578965417e-07, 0.0, 0.0, -0.0006278500928641375, 0.0, -8.189060578965421e-07, 20.34986943316976, 0.0, 0.0, 0.0004377728798368377, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006278500928641374, 0.0004377728798368376, 0.0, 0.0, 19.997101054204084, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 64 + stamp: + secs: 1624875197 + nsecs: 799248934 + frame_id: "/world" +pose: + pose: + position: + x: 0.142688849732 + y: 0.0219248698359 + z: 0.0 + orientation: + x: -0.0 + y: -0.000280792392206 + z: -0.0 + w: 0.999999960578 + covariance: [20.341124545621714, -7.96310661401607e-07, 0.0, 0.0, -0.0005577252295113297, 0.0, -7.963106614016076e-07, 20.34112123351684, 0.0, 0.0, 0.00042234935121777793, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005577252295113294, 0.00042234935121777777, 0.0, 0.0, 19.998088273231176, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 65 + stamp: + secs: 1624875197 + nsecs: 879255056 + frame_id: "/world" +pose: + pose: + position: + x: 0.142793076844 + y: 0.0219887265856 + z: 0.0 + orientation: + x: -0.0 + y: -0.000224043332258 + z: -0.0 + w: 0.999999974902 + covariance: [20.33489782431552, -7.861004940141888e-07, 0.0, 0.0, -0.0005118636245879254, 0.0, -7.861004940141894e-07, 20.33489464523732, 0.0, 0.0, 0.0005830942323055184, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005118636245879253, 0.0005830942323055183, 0.0, 0.0, 19.998861023595147, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 66 + stamp: + secs: 1624875197 + nsecs: 959276915 + frame_id: "/world" +pose: + pose: + position: + x: 0.142900331804 + y: 0.0220587522923 + z: 0.0 + orientation: + x: -0.0 + y: -0.000740923061719 + z: -0.0 + w: 0.999999725516 + covariance: [20.350226117329473, -8.333193308432022e-07, 0.0, 0.0, -0.0005192153474078675, 0.0, -8.333193308432027e-07, 20.350222893297303, 0.0, 0.0, 0.0008200331606486834, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005192153474078673, 0.0008200331606486831, 0.0, 0.0, 19.999716574405923, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 67 + stamp: + secs: 1624875198 + nsecs: 39307117 + frame_id: "/world" +pose: + pose: + position: + x: 0.143006468766 + y: 0.022125868666 + z: 0.0 + orientation: + x: -0.0 + y: -0.000970210638807 + z: -0.0 + w: 0.999999529346 + covariance: [20.36291538043386, -8.725479623433077e-07, 0.0, 0.0, -0.0005188517449850574, 0.0, -8.725479623433083e-07, 20.362912138303546, 0.0, 0.0, 0.0008873823144354131, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005188517449850573, 0.0008873823144354127, 0.0, 0.0, 19.99991289590822, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 68 + stamp: + secs: 1624875198 + nsecs: 119277000 + frame_id: "/world" +pose: + pose: + position: + x: 0.143047754079 + y: 0.0221518498366 + z: 0.0 + orientation: + x: -0.0 + y: -0.000555098603107 + z: -0.0 + w: 0.999999845933 + covariance: [20.28292523308736, -6.79334735246807e-07, 0.0, 0.0, -0.00039173198278479173, 0.0, -6.793347352468075e-07, 20.282922738108446, 0.0, 0.0, 0.0006800408290099104, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00039173198278479157, 0.0006800408290099102, 0.0, 0.0, 19.999888820804404, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 69 + stamp: + secs: 1624875198 + nsecs: 199289083 + frame_id: "/world" +pose: + pose: + position: + x: 0.14314235281 + y: 0.0222112887021 + z: 0.0 + orientation: + x: 0.0 + y: 7.35061802635e-05 + z: 0.0 + w: 0.999999997298 + covariance: [20.2935137438148, -7.079472860429985e-07, 0.0, 0.0, -0.0003614262141310263, 0.0, -7.07947286042999e-07, 20.293511227717723, 0.0, 0.0, 0.0006425054350370249, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00036142621413102615, 0.0006425054350370246, 0.0, 0.0, 19.999800219541292, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 70 + stamp: + secs: 1624875198 + nsecs: 279292107 + frame_id: "/world" +pose: + pose: + position: + x: 0.143235230776 + y: 0.0222703422694 + z: 0.0 + orientation: + x: 0.0 + y: 0.000664175354051 + z: 0.0 + w: 0.999999779436 + covariance: [20.302983224378547, -7.315285352248396e-07, 0.0, 0.0, -0.00032523364683389067, 0.0, -7.315285352248401e-07, 20.302980699171652, 0.0, 0.0, 0.0006036385673005004, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003252336468338906, 0.0006036385673005002, 0.0, 0.0, 19.999924522135384, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 71 + stamp: + secs: 1624875198 + nsecs: 359307051 + frame_id: "/world" +pose: + pose: + position: + x: 0.143323933658 + y: 0.0223279709658 + z: 0.0 + orientation: + x: 0.0 + y: 0.000948688613795 + z: 0.0 + w: 0.999999549995 + covariance: [20.308833819192348, -7.444235801270228e-07, 0.0, 0.0, -0.00028376405270847305, 0.0, -7.444235801270234e-07, 20.308831314932092, 0.0, 0.0, 0.0005682152508496066, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000283764052708473, 0.0005682152508496064, 0.0, 0.0, 20.000073850339614, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 72 + stamp: + secs: 1624875198 + nsecs: 439341068 + frame_id: "/world" +pose: + pose: + position: + x: 0.143411465918 + y: 0.0223857061656 + z: 0.0 + orientation: + x: 0.0 + y: 0.000837314373215 + z: 0.0 + w: 0.999999649452 + covariance: [20.314229396100586, -7.539933640705781e-07, 0.0, 0.0, -0.00023982258255056613, 0.0, -7.539933640705786e-07, 20.31422691499952, 0.0, 0.0, 0.0005241687662868368, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00023982258255056613, 0.0005241687662868367, 0.0, 0.0, 20.00008596997165, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 73 + stamp: + secs: 1624875198 + nsecs: 519325018 + frame_id: "/world" +pose: + pose: + position: + x: 0.143496210896 + y: 0.0224434667567 + z: 0.0 + orientation: + x: 0.0 + y: 0.000664026573213 + z: 0.0 + w: 0.999999779534 + covariance: [20.319555693571086, -7.619935731798487e-07, 0.0, 0.0, -0.00020095955058760563, 0.0, -7.619935731798491e-07, 20.31955323644494, 0.0, 0.0, 0.0005089743705799907, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00020095955058760566, 0.0005089743705799906, 0.0, 0.0, 20.000126516201448, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 74 + stamp: + secs: 1624875198 + nsecs: 599320889 + frame_id: "/world" +pose: + pose: + position: + x: 0.143577493004 + y: 0.0225008440471 + z: 0.0 + orientation: + x: 0.0 + y: 0.000630998815394 + z: 0.0 + w: 0.99999980092 + covariance: [20.324101840368563, -7.669170633226726e-07, 0.0, 0.0, -0.0001686965467288735, 0.0, -7.669170633226732e-07, 20.32409941318292, 0.0, 0.0, 0.0005060442686293259, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00016869654672887354, 0.0005060442686293258, 0.0, 0.0, 20.000177737584465, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 75 + stamp: + secs: 1624875198 + nsecs: 679327011 + frame_id: "/world" +pose: + pose: + position: + x: 0.143657896801 + y: 0.0225576892355 + z: 0.0 + orientation: + x: 0.0 + y: 0.000590281761934 + z: 0.0 + w: 0.999999825784 + covariance: [20.32828028464122, -7.69615317552232e-07, 0.0, 0.0, -0.00014645793606303973, 0.0, -7.696153175522325e-07, 20.32827788743366, 0.0, 0.0, 0.000477053252916665, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00014645793606303973, 0.000477053252916665, 0.0, 0.0, 20.00017891990006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 76 + stamp: + secs: 1624875198 + nsecs: 759337902 + frame_id: "/world" +pose: + pose: + position: + x: 0.143754612422 + y: 0.0226268839057 + z: 0.0 + orientation: + x: 0.0 + y: 0.000564916960703 + z: 0.0 + w: 0.999999840434 + covariance: [20.361066318191718, -8.356014061968462e-07, 0.0, 0.0, -0.0001176395451562261, 0.0, -8.356014061968467e-07, 20.36106375448946, 0.0, 0.0, 0.0004541630641877266, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00011763954515622609, 0.0004541630641877266, 0.0, 0.0, 20.000202812611093, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 77 + stamp: + secs: 1624875198 + nsecs: 839340925 + frame_id: "/world" +pose: + pose: + position: + x: 0.143834701164 + y: 0.0226849845262 + z: 0.0 + orientation: + x: 0.0 + y: 0.000533222734573 + z: 0.0 + w: 0.999999857837 + covariance: [20.3592225033623, -8.206716051496224e-07, 0.0, 0.0, -8.260524684840447e-05, 0.0, -8.206716051496228e-07, 20.359220009245007, 0.0, 0.0, 0.0003906988256370396, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.260524684840446e-05, 0.00039069882563703967, 0.0, 0.0, 20.000214836540547, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 78 + stamp: + secs: 1624875198 + nsecs: 919356108 + frame_id: "/world" +pose: + pose: + position: + x: 0.143926323399 + y: 0.0227512540892 + z: 0.0 + orientation: + x: 0.0 + y: 0.000553968229217 + z: 0.0 + w: 0.99999984656 + covariance: [20.37967933711851, -8.545160280897374e-07, 0.0, 0.0, -6.292732656093559e-05, 0.0, -8.545160280897377e-07, 20.379676763692263, 0.0, 0.0, 0.0003425974117212289, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.292732656093556e-05, 0.0003425974117212288, 0.0, 0.0, 20.00023766467564, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 79 + stamp: + secs: 1624875198 + nsecs: 999368906 + frame_id: "/world" +pose: + pose: + position: + x: 0.144017384225 + y: 0.0228173805027 + z: 0.0 + orientation: + x: 0.0 + y: 0.000623743366926 + z: 0.0 + w: 0.999999805472 + covariance: [20.39671755568844, -8.789709215831281e-07, 0.0, 0.0, -4.688774330701036e-05, 0.0, -8.789709215831286e-07, 20.39671492765556, 0.0, 0.0, 0.00029673148818289136, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.6887743307010344e-05, 0.00029673148818289136, 0.0, 0.0, 20.000320013731834, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 80 + stamp: + secs: 1624875199 + nsecs: 79366922 + frame_id: "/world" +pose: + pose: + position: + x: 0.144090699598 + y: 0.0228698908812 + z: 0.0 + orientation: + x: 0.0 + y: 0.000685648223919 + z: 0.0 + w: 0.999999764943 + covariance: [20.37866153408463, -8.276995283889596e-07, 0.0, 0.0, -5.335144749536147e-05, 0.0, -8.276995283889601e-07, 20.378659072374326, 0.0, 0.0, 0.00024923652980300747, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.335144749536144e-05, 0.00024923652980300747, 0.0, 0.0, 20.00038724806373, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 81 + stamp: + secs: 1624875199 + nsecs: 159377098 + frame_id: "/world" +pose: + pose: + position: + x: 0.144173239643 + y: 0.0229279310786 + z: 0.0 + orientation: + x: 0.0 + y: 0.000632426822125 + z: 0.0 + w: 0.999999800018 + covariance: [20.38348553249298, -8.257557065149534e-07, 0.0, 0.0, -7.338298447920312e-05, 0.0, -8.257557065149539e-07, 20.383483089967083, 0.0, 0.0, 0.00021924395496224398, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.338298447920311e-05, 0.00021924395496224395, 0.0, 0.0, 20.00027828356472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 82 + stamp: + secs: 1624875199 + nsecs: 239401102 + frame_id: "/world" +pose: + pose: + position: + x: 0.14425336991 + y: 0.0229832314532 + z: 0.0 + orientation: + x: 0.0 + y: 0.000659002414231 + z: 0.0 + w: 0.999999782858 + covariance: [20.383477448338375, -8.133858008411231e-07, 0.0, 0.0, -9.392154114910258e-05, 0.0, -8.133858008411234e-07, 20.383475053046514, 0.0, 0.0, 0.00018647890533262756, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.392154114910258e-05, 0.0001864789053326275, 0.0, 0.0, 20.00045605378125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 83 + stamp: + secs: 1624875199 + nsecs: 319385052 + frame_id: "/world" +pose: + pose: + position: + x: 0.141909054139 + y: 0.0495358066692 + z: 0.0 + orientation: + x: 0.0 + y: 0.000555859648123 + z: 0.0 + w: 0.99999984551 + covariance: [16.95057869599822, -5.898166458347048e-07, 0.0, 0.0, -8.399647319616842e-05, 0.0, -5.898166458347049e-07, 16.95057696620987, 0.0, 0.0, 0.00011821696428369132, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.399647319616843e-05, 0.0001182169642836913, 0.0, 0.0, 16.666908443257825, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 84 + stamp: + secs: 1624875199 + nsecs: 399395943 + frame_id: "/world" +pose: + pose: + position: + x: 0.136716080555 + y: 0.0506369886303 + z: 0.0 + orientation: + x: 0.0 + y: 0.000722263912807 + z: 0.0 + w: 0.999999739167 + covariance: [18.137664388385783, -6.748234341930772e-07, 0.0, 0.0, -0.0002753889822473472, 0.0, -6.748234341930774e-07, 18.13766241754715, 0.0, 0.0, 0.00013386184047531303, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00027538898224734715, 0.000133861840475313, 0.0, 0.0, 17.809335818804918, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 85 + stamp: + secs: 1624875199 + nsecs: 479382038 + frame_id: "/world" +pose: + pose: + position: + x: 0.136778762808 + y: 0.0507476970571 + z: 0.0 + orientation: + x: 0.0 + y: 0.00135618767375 + z: 0.0 + w: 0.999999080377 + covariance: [18.935434551390905, -7.365456441090824e-07, 0.0, 0.0, -0.0005077408016643635, 0.0, -7.365456441090829e-07, 18.935432401570843, 0.0, 0.0, 0.0001610613648677671, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005077408016643635, 0.00016106136486776704, 0.0, 0.0, 18.57351218602324, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 86 + stamp: + secs: 1624875199 + nsecs: 559417963 + frame_id: "/world" +pose: + pose: + position: + x: 0.136822723178 + y: 0.0508833219849 + z: 0.0 + orientation: + x: 0.0 + y: 0.000938055702979 + z: 0.0 + w: 0.999999560026 + covariance: [19.450792719680763, -7.53727295154971e-07, 0.0, 0.0, -0.000577371670883137, 0.0, -7.537272951549714e-07, 19.450790518637092, 0.0, 0.0, 0.00017103977813274446, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000577371670883137, 0.00017103977813274446, 0.0, 0.0, 19.07625559751021, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 87 + stamp: + secs: 1624875199 + nsecs: 639408112 + frame_id: "/world" +pose: + pose: + position: + x: 0.136872936301 + y: 0.0510384230948 + z: 0.0 + orientation: + x: 0.0 + y: 0.000601856820511 + z: 0.0 + w: 0.999999818884 + covariance: [19.809919088780774, -8.067133398834359e-07, 0.0, 0.0, -0.0006001632559252081, 0.0, -8.067133398834366e-07, 19.809916730194875, 0.0, 0.0, 0.00017231537923247193, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006001632559252082, 0.00017231537923247193, 0.0, 0.0, 19.404691388899124, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 88 + stamp: + secs: 1624875199 + nsecs: 719436884 + frame_id: "/world" +pose: + pose: + position: + x: 0.132887213177 + y: 0.0470177234791 + z: 0.0 + orientation: + x: 0.0 + y: 0.000627512079701 + z: 0.0 + w: 0.999999803114 + covariance: [20.069610286791267, -8.885753582303269e-07, 0.0, 0.0, -0.0005851457632625704, 0.0, -8.885753582303276e-07, 20.069607685629272, 0.0, 0.0, 0.00016984415956720938, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005851457632625704, 0.00016984415956720935, 0.0, 0.0, 19.617310685326085, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 89 + stamp: + secs: 1624875199 + nsecs: 799429893 + frame_id: "/world" +pose: + pose: + position: + x: 0.132562062672 + y: 0.0520253851008 + z: 0.0 + orientation: + x: 0.0 + y: 0.00407400486071 + z: 0.0 + w: 0.999991701208 + covariance: [20.23380959857869, -9.294569341386509e-07, 0.0, 0.0, -0.0005452496681331392, 0.0, -9.294569341386513e-07, 20.233806875250462, 0.0, 0.0, 0.00016415063795676012, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005452496681331392, 0.0001641506379567601, 0.0, 0.0, 19.754427500391532, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 90 + stamp: + secs: 1624875199 + nsecs: 879475117 + frame_id: "/world" +pose: + pose: + position: + x: 0.121547632064 + y: 0.0509946755823 + z: 0.0 + orientation: + x: 0.0 + y: 0.00318364158447 + z: 0.0 + w: 0.9999949322 + covariance: [20.37631917098131, -1.0212608759918213e-06, 0.0, 0.0, -0.0005539397117804976, 0.0, -1.0212608759918217e-06, 20.376316179883293, 0.0, 0.0, 0.0002018382607891089, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005539397117804976, 0.00020183826078910887, 0.0, 0.0, 19.84267822117244, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 91 + stamp: + secs: 1624875199 + nsecs: 959450006 + frame_id: "/world" +pose: + pose: + position: + x: 0.12158075261 + y: 0.0509715981335 + z: 0.0 + orientation: + x: 0.0 + y: 0.00227346912016 + z: 0.0 + w: 0.999997415666 + covariance: [20.41799497305208, -9.82270426879905e-07, 0.0, 0.0, -0.0005228471120172287, 0.0, -9.822704268799054e-07, 20.417992099959818, 0.0, 0.0, 0.00021828156546889178, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005228471120172286, 0.00021828156546889178, 0.0, 0.0, 19.899557739403164, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 92 + stamp: + secs: 1624875200 + nsecs: 39455891 + frame_id: "/world" +pose: + pose: + position: + x: 0.121547976497 + y: 0.0511037800197 + z: 0.0 + orientation: + x: 0.0 + y: 0.00196052138408 + z: 0.0 + w: 0.999998078176 + covariance: [20.442306513511518, -9.493325045360411e-07, 0.0, 0.0, -0.0004925604820264288, 0.0, -9.493325045360413e-07, 20.442303740387263, 0.0, 0.0, 0.00021737921281768095, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004925604820264286, 0.00021737921281768092, 0.0, 0.0, 19.936237442945394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 93 + stamp: + secs: 1624875200 + nsecs: 119455099 + frame_id: "/world" +pose: + pose: + position: + x: 0.121515012847 + y: 0.0512354170601 + z: 0.0 + orientation: + x: 0.0 + y: 0.00162574104812 + z: 0.0 + w: 0.999998678482 + covariance: [20.456379704349164, -9.214570249253112e-07, 0.0, 0.0, -0.00044642246865207226, 0.0, -9.214570249253114e-07, 20.456377016140028, 0.0, 0.0, 0.00020413876628912943, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004464224686520721, 0.0002041387662891294, 0.0, 0.0, 19.959382592198143, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 94 + stamp: + secs: 1624875200 + nsecs: 199474096 + frame_id: "/world" +pose: + pose: + position: + x: 0.121478211388 + y: 0.0513811820333 + z: 0.0 + orientation: + x: 0.0 + y: 0.00135395360797 + z: 0.0 + w: 0.999999083404 + covariance: [20.48651833413105, -9.372676546501918e-07, 0.0, 0.0, -0.0004116031073087455, 0.0, -9.372676546501919e-07, 20.486515603418432, 0.0, 0.0, 0.00019009258019696742, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004116031073087453, 0.00019009258019696745, 0.0, 0.0, 19.97444988044076, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 95 + stamp: + secs: 1624875200 + nsecs: 279485941 + frame_id: "/world" +pose: + pose: + position: + x: 0.121448317916 + y: 0.0514987865547 + z: 0.0 + orientation: + x: 0.0 + y: 0.00112345450848 + z: 0.0 + w: 0.999999368925 + covariance: [20.46789104583358, -8.749523420403631e-07, 0.0, 0.0, -0.000353534113093661, 0.0, -8.749523420403631e-07, 20.467888498870906, 0.0, 0.0, 0.00016055136695435488, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00035353411309366076, 0.0001605513669543549, 0.0, 0.0, 19.983843439932297, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 96 + stamp: + secs: 1624875200 + nsecs: 359488964 + frame_id: "/world" +pose: + pose: + position: + x: 0.121413436452 + y: 0.0516361311299 + z: 0.0 + orientation: + x: 0.0 + y: 0.000837012034883 + z: 0.0 + w: 0.999999649705 + covariance: [20.482199009551895, -8.77100922394175e-07, 0.0, 0.0, -0.00031418219768844887, 0.0, -8.771009223941751e-07, 20.48219645855932, 0.0, 0.0, 0.0001373184018254589, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00031418219768844865, 0.00013731840182545894, 0.0, 0.0, 19.98971298834571, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 97 + stamp: + secs: 1624875200 + nsecs: 439486027 + frame_id: "/world" +pose: + pose: + position: + x: 0.121378148004 + y: 0.0517697730301 + z: 0.0 + orientation: + x: 0.0 + y: 0.000888955512696 + z: 0.0 + w: 0.999999604879 + covariance: [20.493234367864176, -8.770178403814583e-07, 0.0, 0.0, -0.0003270221443344637, 0.0, -8.770178403814584e-07, 20.493231818637675, 0.0, 0.0, 0.00012299771878578416, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00032702214433446354, 0.00012299771878578416, 0.0, 0.0, 19.994018061104093, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 98 + stamp: + secs: 1624875200 + nsecs: 519515038 + frame_id: "/world" +pose: + pose: + position: + x: 0.121341750968 + y: 0.0518991242364 + z: 0.0 + orientation: + x: 0.0 + y: 0.00117934041324 + z: 0.0 + w: 0.999999304578 + covariance: [20.50215575021878, -8.757902551106705e-07, 0.0, 0.0, -0.0003656689730582504, 0.0, -8.757902551106706e-07, 20.50215320562888, 0.0, 0.0, 0.00012297359509308873, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003656689730582504, 0.00012297359509308873, 0.0, 0.0, 19.996767288196896, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 99 + stamp: + secs: 1624875200 + nsecs: 599533081 + frame_id: "/world" +pose: + pose: + position: + x: 0.121304589169 + y: 0.0520275664187 + z: 0.0 + orientation: + x: 0.0 + y: 0.00125222446583 + z: 0.0 + w: 0.999999215967 + covariance: [20.50938035416286, -8.728256964963638e-07, 0.0, 0.0, -0.0003673074331506511, 0.0, -8.728256964963637e-07, 20.50937781969439, 0.0, 0.0, 0.00012540453762082302, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003673074331506511, 0.00012540453762082307, 0.0, 0.0, 19.99848201169108, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 100 + stamp: + secs: 1624875200 + nsecs: 679512978 + frame_id: "/world" +pose: + pose: + position: + x: 0.121267179299 + y: 0.0521558643234 + z: 0.0 + orientation: + x: 0.0 + y: 0.00127903445653 + z: 0.0 + w: 0.999999182035 + covariance: [20.5154822566628, -8.683666415819488e-07, 0.0, 0.0, -0.0003410433886378491, 0.0, -8.683666415819487e-07, 20.515479736687713, 0.0, 0.0, 0.00012116962595663264, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003410433886378491, 0.0001211696259566327, 0.0, 0.0, 19.999667355471157, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 101 + stamp: + secs: 1624875200 + nsecs: 759515047 + frame_id: "/world" +pose: + pose: + position: + x: 0.121229878821 + y: 0.0522837048447 + z: 0.0 + orientation: + x: 0.0 + y: 0.00135517214142 + z: 0.0 + w: 0.999999081754 + covariance: [20.52022177075076, -8.617317548946696e-07, 0.0, 0.0, -0.00030210520404500837, 0.0, -8.617317548946697e-07, 20.52021927126484, 0.0, 0.0, 0.00011039216688662572, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003021052040450083, 0.00011039216688662577, 0.0, 0.0, 20.000561265631394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 102 + stamp: + secs: 1624875200 + nsecs: 839543104 + frame_id: "/world" +pose: + pose: + position: + x: 0.121192525614 + y: 0.0524075971347 + z: 0.0 + orientation: + x: 0.0 + y: 0.00132556341149 + z: 0.0 + w: 0.99999912144 + covariance: [20.524637751702436, -8.549595344128645e-07, 0.0, 0.0, -0.0003226634400215518, 0.0, -8.549595344128647e-07, 20.5246352716741, 0.0, 0.0, 9.784118176448694e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00032266344002155164, 9.7841181764487e-05, 0.0, 0.0, 20.000862754511942, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 103 + stamp: + secs: 1624875200 + nsecs: 919538975 + frame_id: "/world" +pose: + pose: + position: + x: 0.121154158033 + y: 0.0525273777005 + z: 0.0 + orientation: + x: 0.0 + y: 0.0011614302884 + z: 0.0 + w: 0.99999932554 + covariance: [20.529019839311136, -8.486999516294216e-07, 0.0, 0.0, -0.00036384307942375477, 0.0, -8.486999516294215e-07, 20.529017376443072, 0.0, 0.0, 0.00010017497766786806, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00036384307942375466, 0.0001001749776678681, 0.0, 0.0, 20.000950695779736, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 104 + stamp: + secs: 1624875200 + nsecs: 999536991 + frame_id: "/world" +pose: + pose: + position: + x: 0.121116162924 + y: 0.0526428186384 + z: 0.0 + orientation: + x: 0.0 + y: 0.00116659700703 + z: 0.0 + w: 0.999999319525 + covariance: [20.526100369796236, -8.312887463841209e-07, 0.0, 0.0, -0.0003595310074937897, 0.0, -8.312887463841209e-07, 20.52609795739026, 0.0, 0.0, 0.0001043279769048286, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003595310074937896, 0.00010432797690482864, 0.0, 0.0, 20.001367829448714, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 105 + stamp: + secs: 1624875201 + nsecs: 79540968 + frame_id: "/world" +pose: + pose: + position: + x: 0.121071522212 + y: 0.0527780162248 + z: 0.0 + orientation: + x: 0.0 + y: 0.00142266406348 + z: 0.0 + w: 0.999998988013 + covariance: [20.56159165332065, -8.72141936784241e-07, 0.0, 0.0, -0.0003507361703321775, 0.0, -8.721419367842411e-07, 20.561589122174148, 0.0, 0.0, 0.00010585026492241899, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003507361703321773, 0.00010585026492241902, 0.0, 0.0, 20.001846078902247, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 106 + stamp: + secs: 1624875201 + nsecs: 159560919 + frame_id: "/world" +pose: + pose: + position: + x: 0.121036520622 + y: 0.0528761730428 + z: 0.0 + orientation: + x: 0.0 + y: 0.00110768681814 + z: 0.0 + w: 0.999999386515 + covariance: [20.532667544819827, -8.158066033953628e-07, 0.0, 0.0, -0.00039463553761946696, 0.0, -8.158066033953629e-07, 20.532665175161615, 0.0, 0.0, 0.00010117938872685844, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003946355376194668, 0.00010117938872685846, 0.0, 0.0, 20.001124009982018, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 107 + stamp: + secs: 1624875201 + nsecs: 239557981 + frame_id: "/world" +pose: + pose: + position: + x: 0.120994784894 + y: 0.0529852884194 + z: 0.0 + orientation: + x: 0.0 + y: 0.000532206997831 + z: 0.0 + w: 0.999999858378 + covariance: [20.545011197950508, -8.225992075702254e-07, 0.0, 0.0, -0.0004935555376261311, 0.0, -8.225992075702255e-07, 20.54500880236531, 0.0, 0.0, 0.00010256070166428799, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000493555537626131, 0.00010256070166428797, 0.0, 0.0, 20.000526962703912, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 108 + stamp: + secs: 1624875201 + nsecs: 319547892 + frame_id: "/world" +pose: + pose: + position: + x: 0.120951002602 + y: 0.0530870411698 + z: 0.0 + orientation: + x: -0.0 + y: -4.81469695837e-05 + z: -0.0 + w: 0.999999998841 + covariance: [20.55127633872474, -8.219017851504633e-07, 0.0, 0.0, -0.0005900040451021643, 0.0, -8.219017851504636e-07, 20.551273940299104, 0.0, 0.0, 0.0001410217484815958, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005900040451021641, 0.00014102174848159578, 0.0, 0.0, 20.000063578010177, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 109 + stamp: + secs: 1624875201 + nsecs: 399564028 + frame_id: "/world" +pose: + pose: + position: + x: 0.120904794305 + y: 0.0531818526263 + z: 0.0 + orientation: + x: -0.0 + y: -0.000532272429985 + z: -0.0 + w: 0.999999858343 + covariance: [20.554467418053747, -8.192996639125734e-07, 0.0, 0.0, -0.0006672691520352934, 0.0, -8.192996639125735e-07, 20.554465025119296, 0.0, 0.0, 0.00018679941431243362, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006672691520352933, 0.00018679941431243353, 0.0, 0.0, 19.99997668749489, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 110 + stamp: + secs: 1624875201 + nsecs: 479578972 + frame_id: "/world" +pose: + pose: + position: + x: 0.120860502577 + y: 0.0532700962768 + z: 0.0 + orientation: + x: -0.0 + y: -0.000835642008155 + z: -0.0 + w: 0.999999650851 + covariance: [20.548265554592305, -8.029738340131802e-07, 0.0, 0.0, -0.000689576697186586, 0.0, -8.029738340131802e-07, 20.54826320607622, 0.0, 0.0, 0.00019310537298939336, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006895766971865859, 0.00019310537298939325, 0.0, 0.0, 20.000094943631595, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 111 + stamp: + secs: 1624875201 + nsecs: 559628963 + frame_id: "/world" +pose: + pose: + position: + x: 0.120800517119 + y: 0.0533552033551 + z: 0.0 + orientation: + x: -0.0 + y: -0.00101413566837 + z: -0.0 + w: 0.999999485764 + covariance: [20.612620275701786, -8.961558304354472e-07, 0.0, 0.0, -0.0011867193733151463, 0.0, -8.961558304354471e-07, 20.612617617867826, 0.0, 0.0, 0.00023352005572317502, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001186719373315146, 0.0002335200557231749, 0.0, 0.0, 20.00016910104026, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 112 + stamp: + secs: 1624875201 + nsecs: 639638901 + frame_id: "/world" +pose: + pose: + position: + x: 0.120757561627 + y: 0.0533947797635 + z: 0.0 + orientation: + x: -0.0 + y: -0.000759796971543 + z: -0.0 + w: 0.999999711354 + covariance: [20.570095136835604, -8.372757651716205e-07, 0.0, 0.0, -0.001393147436312483, 0.0, -8.372757651716205e-07, 20.570092601557036, 0.0, 0.0, 0.000238794668343114, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001393147436312483, 0.00023879466834311386, 0.0, 0.0, 20.000154618675545, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 113 + stamp: + secs: 1624875201 + nsecs: 719597101 + frame_id: "/world" +pose: + pose: + position: + x: 0.120705508316 + y: 0.0534406832492 + z: 0.0 + orientation: + x: -0.0 + y: -0.000710009446878 + z: -0.0 + w: 0.999999747943 + covariance: [20.57388705168037, -8.489514176894268e-07, 0.0, 0.0, -0.0015051303769686869, 0.0, -8.489514176894265e-07, 20.573884419359366, 0.0, 0.0, 0.00025311531138204986, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0015051303769686867, 0.0002531153113820498, 0.0, 0.0, 20.00027953883724, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 114 + stamp: + secs: 1624875201 + nsecs: 799607038 + frame_id: "/world" +pose: + pose: + position: + x: 0.120635087141 + y: 0.0534850348489 + z: 0.0 + orientation: + x: -0.0 + y: -0.000764748241207 + z: -0.0 + w: 0.99999970758 + covariance: [20.662326632897262, -9.92233106084788e-07, 0.0, 0.0, -0.001894475078499878, 0.0, -9.922331060847878e-07, 20.662323424312877, 0.0, 0.0, 0.00027477676142650834, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0018944750784998777, 0.00027477676142650823, 0.0, 0.0, 20.000270464551342, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 115 + stamp: + secs: 1624875201 + nsecs: 879617929 + frame_id: "/world" +pose: + pose: + position: + x: 0.120578093289 + y: 0.0535055433088 + z: 0.0 + orientation: + x: -0.0 + y: -0.000969806829934 + z: -0.0 + w: 0.999999529737 + covariance: [20.66345678102275, -1.0029550041486276e-06, 0.0, 0.0, -0.001982857736368122, 0.0, -1.002955004148627e-06, 20.663453418920106, 0.0, 0.0, 0.0002697869382511768, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0019828577363681216, 0.00026978693825117667, 0.0, 0.0, 20.000847005930414, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 116 + stamp: + secs: 1624875201 + nsecs: 959651947 + frame_id: "/world" +pose: + pose: + position: + x: 0.120521149387 + y: 0.0535226410165 + z: 0.0 + orientation: + x: -0.0 + y: -0.00116346614288 + z: -0.0 + w: 0.999999323173 + covariance: [20.655815530361167, -1.000040908414131e-06, 0.0, 0.0, -0.00189815955235924, 0.0, -1.0000409084141304e-06, 20.6558120863413, 0.0, 0.0, 0.00026972090240210686, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00189815955235924, 0.0002697209024021068, 0.0, 0.0, 20.000827847679272, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 117 + stamp: + secs: 1624875202 + nsecs: 39701939 + frame_id: "/world" +pose: + pose: + position: + x: 0.120467120907 + y: 0.0535370255846 + z: 0.0 + orientation: + x: -0.0 + y: -0.000634369720018 + z: -0.0 + w: 0.999999798788 + covariance: [20.629973256905284, -9.673155271427665e-07, 0.0, 0.0, -0.0016931945529843609, 0.0, -9.673155271427654e-07, 20.629969863861174, 0.0, 0.0, 0.0002678295574150905, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001693194552984361, 0.00026782955741509046, 0.0, 0.0, 20.00024568910632, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 118 + stamp: + secs: 1624875202 + nsecs: 119643927 + frame_id: "/world" +pose: + pose: + position: + x: 0.120420262058 + y: 0.0535488962732 + z: 0.0 + orientation: + x: -0.0 + y: -0.000304718665135 + z: -0.0 + w: 0.999999953573 + covariance: [20.575842392362897, -8.877112845723594e-07, 0.0, 0.0, -0.001410264117944784, 0.0, -8.877112845723584e-07, 20.57583924194194, 0.0, 0.0, 0.0002486997209908747, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014102641179447838, 0.0002486997209908747, 0.0, 0.0, 20.00025743316449, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 119 + stamp: + secs: 1624875202 + nsecs: 199664116 + frame_id: "/world" +pose: + pose: + position: + x: 0.120358251967 + y: 0.0535641751311 + z: 0.0 + orientation: + x: -0.0 + y: -0.000164725746143 + z: -0.0 + w: 0.999999986433 + covariance: [20.59645063015971, -9.234691100653328e-07, 0.0, 0.0, -0.0012553348841527674, 0.0, -9.234691100653319e-07, 20.5964473059016, 0.0, 0.0, 0.00024106008693532458, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0012553348841527674, 0.0002410600869353246, 0.0, 0.0, 20.000057268500715, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 120 + stamp: + secs: 1624875202 + nsecs: 279659033 + frame_id: "/world" +pose: + pose: + position: + x: 0.120303103064 + y: 0.0535736141478 + z: 0.0 + orientation: + x: -0.0 + y: -0.000347417288993 + z: -0.0 + w: 0.999999939651 + covariance: [20.584824280308, -9.056859323519149e-07, 0.0, 0.0, -0.0011166370057923702, 0.0, -9.056859323519143e-07, 20.584820981152653, 0.0, 0.0, 0.00021370878227058659, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011166370057923702, 0.00021370878227058667, 0.0, 0.0, 20.00039097358133, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 121 + stamp: + secs: 1624875202 + nsecs: 359657049 + frame_id: "/world" +pose: + pose: + position: + x: 0.120244801916 + y: 0.053577821311 + z: 0.0 + orientation: + x: -0.0 + y: -0.000747479852809 + z: -0.0 + w: 0.999999720637 + covariance: [20.58886987432295, -9.109353647788195e-07, 0.0, 0.0, -0.0010570717197718562, 0.0, -9.109353647788187e-07, 20.588866514334445, 0.0, 0.0, 0.0001880268862971448, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010570717197718562, 0.00018802688629714485, 0.0, 0.0, 20.000434624225903, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 122 + stamp: + secs: 1624875202 + nsecs: 439666033 + frame_id: "/world" +pose: + pose: + position: + x: 0.120188457786 + y: 0.0535779080094 + z: 0.0 + orientation: + x: -0.0 + y: -0.000589567480518 + z: -0.0 + w: 0.999999826205 + covariance: [20.581027766948495, -8.964714651582974e-07, 0.0, 0.0, -0.000992498995512358, 0.0, -8.964714651582965e-07, 20.581024427016676, 0.0, 0.0, 0.00017314750136592053, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000992498995512358, 0.00017314750136592056, 0.0, 0.0, 20.000290117678734, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 123 + stamp: + secs: 1624875202 + nsecs: 519676924 + frame_id: "/world" +pose: + pose: + position: + x: 0.120127361069 + y: 0.0535756275741 + z: 0.0 + orientation: + x: -0.0 + y: -0.00043014103218 + z: -0.0 + w: 0.999999907489 + covariance: [20.59116045090737, -9.086547511750554e-07, 0.0, 0.0, -0.0009342597521032428, 0.0, -9.086547511750548e-07, 20.591157036917558, 0.0, 0.0, 0.00016753728350060046, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009342597521032426, 0.00016753728350060043, 0.0, 0.0, 20.000257254404904, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 124 + stamp: + secs: 1624875202 + nsecs: 599704027 + frame_id: "/world" +pose: + pose: + position: + x: 0.120072774816 + y: 0.0535724348513 + z: 0.0 + orientation: + x: -0.0 + y: -0.000288826326278 + z: -0.0 + w: 0.99999995829 + covariance: [20.57375494368962, -8.763593549838349e-07, 0.0, 0.0, -0.0008285072766980759, 0.0, -8.763593549838343e-07, 20.573751629709996, 0.0, 0.0, 0.0001473236608327186, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008285072766980757, 0.00014732366083271856, 0.0, 0.0, 20.0001391218508, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 125 + stamp: + secs: 1624875202 + nsecs: 679709911 + frame_id: "/world" +pose: + pose: + position: + x: 0.120006081259 + y: 0.0535659910546 + z: 0.0 + orientation: + x: -0.0 + y: -6.38289664575e-05 + z: -0.0 + w: 0.999999997963 + covariance: [20.609868508361224, -9.237333871587829e-07, 0.0, 0.0, -0.0007911786291885359, 0.0, -9.237333871587821e-07, 20.609864990019098, 0.0, 0.0, 0.00013338325477278264, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007911786291885357, 0.00013338325477278256, 0.0, 0.0, 20.0000451033738, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 126 + stamp: + secs: 1624875202 + nsecs: 759694099 + frame_id: "/world" +pose: + pose: + position: + x: 0.119948241072 + y: 0.0535547745393 + z: 0.0 + orientation: + x: 0.0 + y: 0.000322980040157 + z: 0.0 + w: 0.999999947842 + covariance: [20.601764537953493, -9.032261256374164e-07, 0.0, 0.0, -0.0007720115681266761, 0.0, -9.032261256374153e-07, 20.60176107615262, 0.0, 0.0, 0.00011756490869710488, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000772011568126676, 0.00011756490869710483, 0.0, 0.0, 20.00003568862044, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 127 + stamp: + secs: 1624875202 + nsecs: 839711905 + frame_id: "/world" +pose: + pose: + position: + x: 0.119888416864 + y: 0.0535402290457 + z: 0.0 + orientation: + x: 0.0 + y: 0.000667256552082 + z: 0.0 + w: 0.999999777384 + covariance: [20.60060420859299, -8.929462970143382e-07, 0.0, 0.0, -0.0007440133841670558, 0.0, -8.92946297014337e-07, 20.600600767269015, 0.0, 0.0, 0.0001143278054704367, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007440133841670557, 0.00011432780547043666, 0.0, 0.0, 20.000122494343433, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 128 + stamp: + secs: 1624875202 + nsecs: 919713974 + frame_id: "/world" +pose: + pose: + position: + x: 0.119821180666 + y: 0.0535262360576 + z: 0.0 + orientation: + x: 0.0 + y: 0.000265580197239 + z: 0.0 + w: 0.999999964734 + covariance: [20.625546618950946, -9.198822948175844e-07, 0.0, 0.0, -0.0006687339434056996, 0.0, -9.198822948175833e-07, 20.625543062177414, 0.0, 0.0, 0.00012624118106263873, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006687339434056997, 0.0001262411810626387, 0.0, 0.0, 20.000012156067, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 129 + stamp: + secs: 1624875202 + nsecs: 999722004 + frame_id: "/world" +pose: + pose: + position: + x: 0.119758699192 + y: 0.0535132387019 + z: 0.0 + orientation: + x: -0.0 + y: -0.000120173040001 + z: -0.0 + w: 0.999999992779 + covariance: [20.62436231769586, -9.074161458298366e-07, 0.0, 0.0, -0.0005711213457310076, 0.0, -9.074161458298355e-07, 20.62435880182465, 0.0, 0.0, 0.00012762642053383092, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005711213457310076, 0.00012762642053383092, 0.0, 0.0, 20.000004635681705, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 130 + stamp: + secs: 1624875203 + nsecs: 79710960 + frame_id: "/world" +pose: + pose: + position: + x: 0.119701632688 + y: 0.0535010690024 + z: 0.0 + orientation: + x: -0.0 + y: -2.50620756476e-05 + z: -0.0 + w: 0.999999999686 + covariance: [20.60241544660692, -8.648368595850821e-07, 0.0, 0.0, -0.00047312066165260987, 0.0, -8.64836859585081e-07, 20.60241208982821, 0.0, 0.0, 0.0001155613275185968, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004731206616526098, 0.00011556132751859681, 0.0, 0.0, 20.00000070920627, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 131 + stamp: + secs: 1624875203 + nsecs: 159800053 + frame_id: "/world" +pose: + pose: + position: + x: 0.119634468878 + y: 0.0534867363233 + z: 0.0 + orientation: + x: 0.0 + y: 0.00020254045739 + z: 0.0 + w: 0.999999979489 + covariance: [20.62635896677506, -8.863190107921235e-07, 0.0, 0.0, -0.00040387299563251046, 0.0, -8.863190107921222e-07, 20.626355520552924, 0.0, 0.0, 0.00010512693218647135, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004038729956325103, 0.00010512693218647136, 0.0, 0.0, 20.000042920414554, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 132 + stamp: + secs: 1624875203 + nsecs: 239774942 + frame_id: "/world" +pose: + pose: + position: + x: 0.119576509395 + y: 0.05347476297 + z: 0.0 + orientation: + x: 0.0 + y: 0.000621422137744 + z: 0.0 + w: 0.999999806917 + covariance: [20.60910762023457, -8.497799600179792e-07, 0.0, 0.0, -0.00032110522113189076, 0.0, -8.49779960017978e-07, 20.60910431203065, 0.0, 0.0, 8.831362647162233e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003211052211318906, 8.831362647162233e-05, 0.0, 0.0, 20.0003305890172, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 133 + stamp: + secs: 1624875203 + nsecs: 319755077 + frame_id: "/world" +pose: + pose: + position: + x: 0.119514762011 + y: 0.0534614608507 + z: 0.0 + orientation: + x: 0.0 + y: 0.00100864157374 + z: 0.0 + w: 0.999999491321 + covariance: [20.612362375983526, -8.412975210200575e-07, 0.0, 0.0, -0.0002681712312382927, 0.0, -8.412975210200563e-07, 20.612359096821713, 0.0, 0.0, 7.060613092973237e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002681712312382926, 7.060613092973236e-05, 0.0, 0.0, 20.000611037213105, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 134 + stamp: + secs: 1624875203 + nsecs: 399734974 + frame_id: "/world" +pose: + pose: + position: + x: 0.119462040519 + y: 0.0534492565604 + z: 0.0 + orientation: + x: 0.0 + y: 0.00103512702983 + z: 0.0 + w: 0.999999464256 + covariance: [20.58025613104007, -7.85767331817555e-07, 0.0, 0.0, -0.00022444875858946414, 0.0, -7.857673318175537e-07, 20.58025306526944, 0.0, 0.0, 5.200913499342807e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00022444875858946406, 5.200913499342807e-05, 0.0, 0.0, 20.000747361967, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 135 + stamp: + secs: 1624875203 + nsecs: 479762077 + frame_id: "/world" +pose: + pose: + position: + x: 0.119400915238 + y: 0.0534351163313 + z: 0.0 + orientation: + x: 0.0 + y: 0.000974330405041 + z: 0.0 + w: 0.99999952534 + covariance: [20.58893667025932, -7.846823294663177e-07, 0.0, 0.0, -0.00019256159145454731, 0.0, -7.846823294663166e-07, 20.58893360591373, 0.0, 0.0, 4.0617861733799485e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001925615914545472, 4.061786173379949e-05, 0.0, 0.0, 20.00070278559429, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 136 + stamp: + secs: 1624875203 + nsecs: 559773922 + frame_id: "/world" +pose: + pose: + position: + x: 0.119314768146 + y: 0.0534141016422 + z: 0.0 + orientation: + x: 0.0 + y: 0.000865187358501 + z: 0.0 + w: 0.999999625725 + covariance: [20.630725217821528, -8.296877488516365e-07, 0.0, 0.0, -0.0002465889074272225, 0.0, -8.296877488516353e-07, 20.630721996412028, 0.0, 0.0, 0.0003002832100439812, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00024658890742722245, 0.00030028321004398127, 0.0, 0.0, 20.000707763113102, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 137 + stamp: + secs: 1624875203 + nsecs: 639780045 + frame_id: "/world" +pose: + pose: + position: + x: 0.119231582793 + y: 0.0533921006783 + z: 0.0 + orientation: + x: 0.0 + y: 0.000857870217731 + z: 0.0 + w: 0.999999632029 + covariance: [20.609919511280246, -7.994896676135392e-07, 0.0, 0.0, -0.00030374116501066486, 0.0, -7.994896676135381e-07, 20.609916460889036, 0.0, 0.0, 0.0005420238203003826, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00030374116501066486, 0.0005420238203003826, 0.0, 0.0, 20.00089032112281, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 138 + stamp: + secs: 1624875203 + nsecs: 719793081 + frame_id: "/world" +pose: + pose: + position: + x: 0.119134364449 + y: 0.0533525666274 + z: 0.0 + orientation: + x: 0.0 + y: 0.000986836393148 + z: 0.0 + w: 0.999999513077 + covariance: [20.633219597898638, -8.379233749155235e-07, 0.0, 0.0, -0.0005474493292306959, 0.0, -8.379233749155223e-07, 20.633216499360152, 0.0, 0.0, 0.0006728863406027545, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005474493292306959, 0.0006728863406027545, 0.0, 0.0, 20.00101819803006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 139 + stamp: + secs: 1624875203 + nsecs: 799793005 + frame_id: "/world" +pose: + pose: + position: + x: 0.119036599127 + y: 0.0532997660884 + z: 0.0 + orientation: + x: 0.0 + y: 0.000952879774964 + z: 0.0 + w: 0.99999954601 + covariance: [20.651496715330794, -8.797931215992057e-07, 0.0, 0.0, -0.0008041748914912668, 0.0, -8.797931215992047e-07, 20.651493574990738, 0.0, 0.0, 0.0007053323035125088, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000804174891491267, 0.0007053323035125089, 0.0, 0.0, 20.000982295403865, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 140 + stamp: + secs: 1624875203 + nsecs: 879812002 + frame_id: "/world" +pose: + pose: + position: + x: 0.118939111002 + y: 0.0532398796657 + z: 0.0 + orientation: + x: 0.0 + y: 0.000813758330451 + z: 0.0 + w: 0.999999668899 + covariance: [20.66306701397587, -9.151797695087174e-07, 0.0, 0.0, -0.0009924851468552205, 0.0, -9.151797695087165e-07, 20.66306384861479, 0.0, 0.0, 0.000680687747476981, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009924851468552207, 0.0006806877474769813, 0.0, 0.0, 20.00081940140776, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 141 + stamp: + secs: 1624875203 + nsecs: 959813118 + frame_id: "/world" +pose: + pose: + position: + x: 0.118834347083 + y: 0.0531684048044 + z: 0.0 + orientation: + x: 0.0 + y: 0.00107885250267 + z: 0.0 + w: 0.999999418038 + covariance: [20.694190021910522, -9.791424426480006e-07, 0.0, 0.0, -0.001132884158314052, 0.0, -9.791424426479996e-07, 20.69418672403573, 0.0, 0.0, 0.0006317936126450344, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011328841583140522, 0.0006317936126450347, 0.0, 0.0, 20.001752447522527, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 142 + stamp: + secs: 1624875204 + nsecs: 39814949 + frame_id: "/world" +pose: + pose: + position: + x: 0.118729266831 + y: 0.053097612058 + z: 0.0 + orientation: + x: 0.0 + y: 0.0012838429018 + z: 0.0 + w: 0.999999175873 + covariance: [20.720066047382264, -1.0290243281374772e-06, 0.0, 0.0, -0.0011107149620169304, 0.0, -1.029024328137476e-06, 20.720062651191565, 0.0, 0.0, 0.0005663327970072738, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011107149620169306, 0.000566332797007274, 0.0, 0.0, 20.001527802866764, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 143 + stamp: + secs: 1624875204 + nsecs: 119822979 + frame_id: "/world" +pose: + pose: + position: + x: 0.118622499237 + y: 0.0530289196768 + z: 0.0 + orientation: + x: 0.0 + y: 0.000949906430178 + z: 0.0 + w: 0.999999548839 + covariance: [20.72184843716251, -1.0478210240190688e-06, 0.0, 0.0, -0.001035391993222134, 0.0, -1.0478210240190677e-06, 20.721845067572772, 0.0, 0.0, 0.0006142742780443283, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010353919932221342, 0.0006142742780443284, 0.0, 0.0, 20.00108714001132, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 144 + stamp: + secs: 1624875204 + nsecs: 199822903 + frame_id: "/world" +pose: + pose: + position: + x: 0.11851405824 + y: 0.0529574579231 + z: 0.0 + orientation: + x: 0.0 + y: 0.000907224463778 + z: 0.0 + w: 0.999999588472 + covariance: [20.704529661082635, -1.045204540711342e-06, 0.0, 0.0, -0.0010119489049159056, 0.0, -1.045204540711341e-06, 20.704526406416544, 0.0, 0.0, 0.0006957334339221963, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010119489049159056, 0.0006957334339221964, 0.0, 0.0, 20.001435601292208, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 145 + stamp: + secs: 1624875204 + nsecs: 279830933 + frame_id: "/world" +pose: + pose: + position: + x: 0.118406232481 + y: 0.0528819047605 + z: 0.0 + orientation: + x: 0.0 + y: 0.00119972851312 + z: 0.0 + w: 0.999999280325 + covariance: [20.686784586101055, -1.039793572976869e-06, 0.0, 0.0, -0.0009900013924151081, 0.0, -1.039793572976868e-06, 20.686781441208517, 0.0, 0.0, 0.0006981810943531755, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009900013924151083, 0.0006981810943531756, 0.0, 0.0, 20.001980916377494, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 146 + stamp: + secs: 1624875204 + nsecs: 359844923 + frame_id: "/world" +pose: + pose: + position: + x: 0.118307272005 + y: 0.0528108510642 + z: 0.0 + orientation: + x: 0.0 + y: 0.00150006118663 + z: 0.0 + w: 0.999998874908 + covariance: [20.65191261956748, -1.0009205280116267e-06, 0.0, 0.0, -0.0009090878395821619, 0.0, -1.000920528011626e-06, 20.651909660587968, 0.0, 0.0, 0.0006323531300180356, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009090878395821621, 0.0006323531300180356, 0.0, 0.0, 20.002815768798506, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 147 + stamp: + secs: 1624875204 + nsecs: 439868927 + frame_id: "/world" +pose: + pose: + position: + x: 0.11817361589 + y: 0.0527158031987 + z: 0.0 + orientation: + x: 0.0 + y: 0.00208024864087 + z: 0.0 + w: 0.99999783628 + covariance: [20.711125784906045, -1.107090254295217e-06, 0.0, 0.0, -0.0008530163504760206, 0.0, -1.1070902542952161e-06, 20.711122595486813, 0.0, 0.0, 0.0005962652132033027, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008530163504760204, 0.0005962652132033028, 0.0, 0.0, 20.00422274482919, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 148 + stamp: + secs: 1624875204 + nsecs: 519855022 + frame_id: "/world" +pose: + pose: + position: + x: 0.118068661611 + y: 0.052642960491 + z: 0.0 + orientation: + x: 0.0 + y: 0.0018546788338 + z: 0.0 + w: 0.999998280082 + covariance: [20.689306722969103, -1.0750915510267178e-06, 0.0, 0.0, -0.000687095039697058, 0.0, -1.0750915510267165e-06, 20.689303672521703, 0.0, 0.0, 0.0004936941317807485, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000687095039697058, 0.0004936941317807486, 0.0, 0.0, 20.003363194045548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 149 + stamp: + secs: 1624875204 + nsecs: 599853039 + frame_id: "/world" +pose: + pose: + position: + x: 0.117964147956 + y: 0.0525658170013 + z: 0.0 + orientation: + x: 0.0 + y: 0.00126324402825 + z: 0.0 + w: 0.999999202107 + covariance: [20.67171955569221, -1.0489512191604228e-06, 0.0, 0.0, -0.0006246062842293143, 0.0, -1.0489512191604215e-06, 20.671716618093786, 0.0, 0.0, 0.0004045771236633283, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006246062842293144, 0.0004045771236633284, 0.0, 0.0, 20.00214658998543, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 150 + stamp: + secs: 1624875204 + nsecs: 679869890 + frame_id: "/world" +pose: + pose: + position: + x: 0.12525722812 + y: 0.0297316668687 + z: 0.0 + orientation: + x: 0.0 + y: 0.000251956746045 + z: 0.0 + w: 0.999999968259 + covariance: [17.109395008159577, -6.864559557731499e-07, 0.0, 0.0, -0.00041799007702874056, 0.0, -6.864559557731492e-07, 17.109393103091882, 0.0, 0.0, 0.00022428163818598814, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004179900770287406, 0.00022428163818598822, 0.0, 0.0, 16.667617382474262, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 151 + stamp: + secs: 1624875204 + nsecs: 759877920 + frame_id: "/world" +pose: + pose: + position: + x: 0.125203169074 + y: 0.0294995405908 + z: 0.0 + orientation: + x: 0.0 + y: 0.00020912137763 + z: 0.0 + w: 0.999999978134 + covariance: [18.293409877541542, -7.542411722045438e-07, 0.0, 0.0, -0.0005406439519533432, 0.0, -7.542411722045431e-07, 18.29340780161646, 0.0, 0.0, 0.00019944681677007412, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005406439519533432, 0.00019944681677007414, 0.0, 0.0, 17.808962405701955, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 152 + stamp: + secs: 1624875204 + nsecs: 839879036 + frame_id: "/world" +pose: + pose: + position: + x: 0.125146999382 + y: 0.0292735523156 + z: 0.0 + orientation: + x: 0.0 + y: 0.000261259942522 + z: 0.0 + w: 0.999999965872 + covariance: [19.083930676475628, -7.952851318347315e-07, 0.0, 0.0, -0.0006581901190890199, 0.0, -7.952851318347306e-07, 19.083928492425564, 0.0, 0.0, 0.00017179254536029757, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00065819011908902, 0.00017179254536029757, 0.0, 0.0, 18.57287140893109, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 153 + stamp: + secs: 1624875204 + nsecs: 919888020 + frame_id: "/world" +pose: + pose: + position: + x: 0.125078722659 + y: 0.0290037837618 + z: 0.0 + orientation: + x: 0.0 + y: 0.000519766475609 + z: 0.0 + w: 0.999999864921 + covariance: [19.645563911536417, -8.837934208402938e-07, 0.0, 0.0, -0.0007500754100549147, 0.0, -8.837934208402929e-07, 19.64556148623445, 0.0, 0.0, 0.00017776485978549886, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007500754100549146, 0.00017776485978549883, 0.0, 0.0, 19.07662362889442, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 154 + stamp: + secs: 1624875204 + nsecs: 999908924 + frame_id: "/world" +pose: + pose: + position: + x: 0.125020436106 + y: 0.0287781773644 + z: 0.0 + orientation: + x: 0.0 + y: 0.000788978950389 + z: 0.0 + w: 0.999999688756 + covariance: [19.97376710775799, -8.796642987546075e-07, 0.0, 0.0, -0.00071441193907949, 0.0, -8.796642987546066e-07, 19.97376469578722, 0.0, 0.0, 0.00017725117593952596, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00071441193907949, 0.00017725117593952596, 0.0, 0.0, 19.40524637815989, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 155 + stamp: + secs: 1624875205 + nsecs: 79900026 + frame_id: "/world" +pose: + pose: + position: + x: 0.124949708331 + y: 0.0285093371148 + z: 0.0 + orientation: + x: 0.0 + y: 0.000883466523382 + z: 0.0 + w: 0.999999609743 + covariance: [20.22836428843851, -9.385885070213936e-07, 0.0, 0.0, -0.0006767903794114099, 0.0, -9.385885070213926e-07, 20.22836171841337, 0.0, 0.0, 0.00019304913205087516, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006767903794114101, 0.00019304913205087519, 0.0, 0.0, 19.61778332631817, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 156 + stamp: + secs: 1624875205 + nsecs: 159907103 + frame_id: "/world" +pose: + pose: + position: + x: 0.124882357475 + y: 0.0282461165277 + z: 0.0 + orientation: + x: 0.0 + y: 0.000935547787207 + z: 0.0 + w: 0.999999562375 + covariance: [20.378426212163667, -9.542015643040949e-07, 0.0, 0.0, -0.0008160750740504691, 0.0, -9.542015643040936e-07, 20.37842359264583, 0.0, 0.0, 0.000207126917604297, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008160750740504694, 0.00020712691760429704, 0.0, 0.0, 19.75541810733098, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 157 + stamp: + secs: 1624875205 + nsecs: 239924908 + frame_id: "/world" +pose: + pose: + position: + x: 0.124819996879 + y: 0.0279852060413 + z: 0.0 + orientation: + x: 0.0 + y: 0.000784446895262 + z: 0.0 + w: 0.999999692321 + covariance: [20.44993756813879, -9.300294919757571e-07, 0.0, 0.0, -0.0011448431942365058, 0.0, -9.300294919757558e-07, 20.449934979030242, 0.0, 0.0, 0.00022108842860630635, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011448431942365058, 0.0002210884286063063, 0.0, 0.0, 19.84302940258926, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 158 + stamp: + secs: 1624875205 + nsecs: 319930077 + frame_id: "/world" +pose: + pose: + position: + x: 0.124695117105 + y: 0.027452025269 + z: 0.0 + orientation: + x: 0.0 + y: 0.000438057885498 + z: 0.0 + w: 0.999999904053 + covariance: [20.731312531915638, -1.2929834000716946e-06, 0.0, 0.0, -0.0020140072509750276, 0.0, -1.2929834000716932e-06, 20.731308768747517, 0.0, 0.0, 0.00032313575818826017, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.002014007250975028, 0.0003231357581882601, 0.0, 0.0, 19.89955760862993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 159 + stamp: + secs: 1624875205 + nsecs: 399924994 + frame_id: "/world" +pose: + pose: + position: + x: 0.134919439646 + y: 0.0239509063043 + z: 0.0 + orientation: + x: -0.0 + y: -0.00036700797512 + z: -0.0 + w: 0.999999932653 + covariance: [17.324789398944496, -1.0816249211378576e-06, 0.0, 0.0, -0.0015751630820863927, 0.0, -1.0816249211378568e-06, 17.324786164764774, 0.0, 0.0, 0.00024423656457015587, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001575163082086393, 0.00024423656457015587, 0.0, 0.0, 16.622383505714108, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 160 + stamp: + secs: 1624875205 + nsecs: 479929924 + frame_id: "/world" +pose: + pose: + position: + x: 0.134925470936 + y: 0.0236747444802 + z: 0.0 + orientation: + x: 0.0 + y: 0.000134028844377 + z: 0.0 + w: 0.999999991018 + covariance: [18.475765095449816, -1.0712091471646118e-06, 0.0, 0.0, -0.001416458405087703, 0.0, -1.0712091471646105e-06, 18.47576185049254, 0.0, 0.0, 0.0002265017347118429, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014164584050877032, 0.0002265017347118429, 0.0, 0.0, 17.77876712846188, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 161 + stamp: + secs: 1624875205 + nsecs: 559950113 + frame_id: "/world" +pose: + pose: + position: + x: 0.134933989448 + y: 0.0233830738568 + z: 0.0 + orientation: + x: 0.0 + y: 0.000519348402044 + z: 0.0 + w: 0.999999865139 + covariance: [19.233147848667578, -1.0420160515995657e-06, 0.0, 0.0, -0.0012219742943312792, 0.0, -1.0420160515995645e-06, 19.233144659038864, 0.0, 0.0, 0.00020713401709714283, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0012219742943312794, 0.0002071340170971428, 0.0, 0.0, 18.553208897901772, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 162 + stamp: + secs: 1624875205 + nsecs: 639954090 + frame_id: "/world" +pose: + pose: + position: + x: 0.134941749006 + y: 0.0230926329819 + z: 0.0 + orientation: + x: 0.0 + y: 0.0007734101017 + z: 0.0 + w: 0.999999700918 + covariance: [19.722342151211546, -1.0055129415717305e-06, 0.0, 0.0, -0.0010311421444693962, 0.0, -1.0055129415717292e-06, 19.722339046699695, 0.0, 0.0, 0.0001921411311628344, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010311421444693962, 0.0001921411311628343, 0.0, 0.0, 19.063836116569117, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 163 + stamp: + secs: 1624875205 + nsecs: 719960928 + frame_id: "/world" +pose: + pose: + position: + x: 0.136222626122 + y: 0.0198701431852 + z: 0.0 + orientation: + x: -0.0 + y: -0.000470456726306 + z: -0.0 + w: 0.999999889335 + covariance: [16.689328444008282, -6.729849913876634e-07, 0.0, 0.0, -0.0009027205156245475, 0.0, -6.729849913876625e-07, 16.68932632416469, 0.0, 0.0, 0.00012558668184269462, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009027205156245475, 0.00012558668184269462, 0.0, 0.0, 16.246185302064987, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 164 + stamp: + secs: 1624875205 + nsecs: 799964905 + frame_id: "/world" +pose: + pose: + position: + x: 0.136186005541 + y: 0.0200197897116 + z: 0.0 + orientation: + x: 0.0 + y: 6.02457732111e-05 + z: 0.0 + w: 0.999999998185 + covariance: [18.00956883448021, -7.428921226909511e-07, 0.0, 0.0, -0.0013655768875985338, 0.0, -7.4289212269095e-07, 18.009566368051296, 0.0, 0.0, 0.0001273796181134263, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013655768875985334, 0.00012737961811342633, 0.0, 0.0, 17.523125886941074, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 165 + stamp: + secs: 1624875205 + nsecs: 879976034 + frame_id: "/world" +pose: + pose: + position: + x: 0.136199618767 + y: 0.0196632243829 + z: 0.0 + orientation: + x: 0.0 + y: 0.000296154540897 + z: 0.0 + w: 0.999999956146 + covariance: [18.89113403603491, -7.776370645146904e-07, 0.0, 0.0, -0.0014994978584117417, 0.0, -7.776370645146894e-07, 18.891131328372154, 0.0, 0.0, 0.00012051937579876621, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014994978584117412, 0.00012051937579876623, 0.0, 0.0, 18.38345816163294, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 166 + stamp: + secs: 1624875205 + nsecs: 959978104 + frame_id: "/world" +pose: + pose: + position: + x: 0.136212263076 + y: 0.0193088479528 + z: 0.0 + orientation: + x: 0.0 + y: 0.00092900063922 + z: 0.0 + w: 0.999999568479 + covariance: [19.47109371290106, -7.951032069129269e-07, 0.0, 0.0, -0.0014635635198650477, 0.0, -7.951032069129261e-07, 19.47109083359036, 0.0, 0.0, 0.00012077555055186969, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014635635198650473, 0.00012077555055186965, 0.0, 0.0, 18.953373216098992, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 167 + stamp: + secs: 1624875206 + nsecs: 40004015 + frame_id: "/world" +pose: + pose: + position: + x: 0.144500854143 + y: 0.0303782297485 + z: 0.0 + orientation: + x: 0.0 + y: 0.000830909694516 + z: 0.0 + w: 0.999999654794 + covariance: [19.847553163637432, -8.015434394667041e-07, 0.0, 0.0, -0.0013434624173025251, 0.0, -8.015434394667035e-07, 19.847550167431905, 0.0, 0.0, 0.00012330897501902117, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013434624173025247, 0.00012330897501902115, 0.0, 0.0, 19.325430455041346, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 168 + stamp: + secs: 1624875206 + nsecs: 119993925 + frame_id: "/world" +pose: + pose: + position: + x: 0.144565830568 + y: 0.0300351563344 + z: 0.0 + orientation: + x: 0.0 + y: 0.000727308533362 + z: 0.0 + w: 0.999999735511 + covariance: [20.090347168710068, -8.016218414611106e-07, 0.0, 0.0, -0.001215446081938499, 0.0, -8.016218414611098e-07, 20.090344087798815, 0.0, 0.0, 0.00011863477398623279, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001215446081938499, 0.00011863477398623273, 0.0, 0.0, 19.565834815425337, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 169 + stamp: + secs: 1624875206 + nsecs: 199990988 + frame_id: "/world" +pose: + pose: + position: + x: 0.144626294674 + y: 0.0296965617277 + z: 0.0 + orientation: + x: 0.0 + y: 0.000451107921675 + z: 0.0 + w: 0.999999898251 + covariance: [20.274693459993696, -8.433961130145399e-07, 0.0, 0.0, -0.0011554063641818588, 0.0, -8.433961130145391e-07, 20.27469013480176, 0.0, 0.0, 0.0001393098927065603, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011554063641818588, 0.0001393098927065603, 0.0, 0.0, 19.7212850100616, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 170 + stamp: + secs: 1624875206 + nsecs: 280004025 + frame_id: "/world" +pose: + pose: + position: + x: 0.144672337952 + y: 0.0293565920105 + z: 0.0 + orientation: + x: 0.0 + y: 5.98916759118e-05 + z: 0.0 + w: 0.999999998206 + covariance: [20.397443232594508, -8.891848795742399e-07, 0.0, 0.0, -0.0010642705603043719, 0.0, -8.891848795742391e-07, 20.39743972524047, 0.0, 0.0, 0.0003602877297331169, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010642705603043719, 0.00036028772973311697, 0.0, 0.0, 19.820775862250958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 171 + stamp: + secs: 1624875206 + nsecs: 360021114 + frame_id: "/world" +pose: + pose: + position: + x: 0.14469833284 + y: 0.0290869537981 + z: 0.0 + orientation: + x: -0.0 + y: -0.000203996184485 + z: -0.0 + w: 0.999999979193 + covariance: [20.428378590943066, -8.508163908570661e-07, 0.0, 0.0, -0.0008902449353146382, 0.0, -8.50816390857065e-07, 20.42837528369411, 0.0, 0.0, 0.000535827442129174, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008902449353146382, 0.0005358274421291742, 0.0, 0.0, 19.885096775708906, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 172 + stamp: + secs: 1624875206 + nsecs: 440040112 + frame_id: "/world" +pose: + pose: + position: + x: 0.144725051677 + y: 0.0287900986384 + z: 0.0 + orientation: + x: -0.0 + y: -0.000109974950398 + z: -0.0 + w: 0.999999993953 + covariance: [20.470818482180707, -8.63816026561991e-07, 0.0, 0.0, -0.0007747654547182105, 0.0, -8.638160265619897e-07, 20.470815175346466, 0.0, 0.0, 0.0006277941962265235, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007747654547182103, 0.0006277941962265236, 0.0, 0.0, 19.926438668155722, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 173 + stamp: + secs: 1624875206 + nsecs: 520046949 + frame_id: "/world" +pose: + pose: + position: + x: 0.144753704639 + y: 0.0284847121044 + z: 0.0 + orientation: + x: -0.0 + y: -7.02152665021e-05 + z: -0.0 + w: 0.999999997535 + covariance: [20.49544495009412, -8.673421774967779e-07, 0.0, 0.0, -0.000667462218510679, 0.0, -8.673421774967766e-07, 20.495441672772387, 0.0, 0.0, 0.000626550982493443, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006674622185106789, 0.000626550982493443, 0.0, 0.0, 19.952856643625324, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 174 + stamp: + secs: 1624875206 + nsecs: 600040913 + frame_id: "/world" +pose: + pose: + position: + x: 0.14478159089 + y: 0.0281858166113 + z: 0.0 + orientation: + x: -0.0 + y: -0.000266271755122 + z: -0.0 + w: 0.99999996455 + covariance: [20.506056120816638, -8.603030577629318e-07, 0.0, 0.0, -0.0005750961580421128, 0.0, -8.603030577629305e-07, 20.50605290681735, 0.0, 0.0, 0.0005763081576443862, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005750961580421127, 0.0005763081576443862, 0.0, 0.0, 19.96979496960457, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 175 + stamp: + secs: 1624875206 + nsecs: 680032969 + frame_id: "/world" +pose: + pose: + position: + x: 0.144807727063 + y: 0.0278921852075 + z: 0.0 + orientation: + x: -0.0 + y: -0.000341197707182 + z: -0.0 + w: 0.999999941792 + covariance: [20.508038920334883, -8.473289196707694e-07, 0.0, 0.0, -0.000494426443540488, 0.0, -8.473289196707681e-07, 20.508035790017438, 0.0, 0.0, 0.0005241991201846846, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000494426443540488, 0.0005241991201846845, 0.0, 0.0, 19.980666779382602, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 176 + stamp: + secs: 1624875206 + nsecs: 760040998 + frame_id: "/world" +pose: + pose: + position: + x: 0.150110569626 + y: 0.0319668922732 + z: 0.0 + orientation: + x: -0.0 + y: -6.34034686518e-05 + z: -0.0 + w: 0.99999999799 + covariance: [17.01797475693596, -5.741850705340525e-07, 0.0, 0.0, -0.0002877029806271601, 0.0, -5.741850705340516e-07, 17.017972653947805, 0.0, 0.0, 0.00032765586963984933, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00028770298062716, 0.0003276558696398492, 0.0, 0.0, 16.658171689204462, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 177 + stamp: + secs: 1624875206 + nsecs: 840075016 + frame_id: "/world" +pose: + pose: + position: + x: 0.155516802459 + y: 0.031492077295 + z: 0.0 + orientation: + x: -0.0 + y: -0.00154315843964 + z: -0.0 + w: 0.99999880933 + covariance: [18.24094082609352, -6.996706231519143e-07, 0.0, 0.0, -0.0002940960861338145, 0.0, -6.99670623151913e-07, 18.24093829820398, 0.0, 0.0, 0.0003412087662451708, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002940960861338143, 0.00034120876624517063, 0.0, 0.0, 17.802668838814803, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 178 + stamp: + secs: 1624875206 + nsecs: 920078039 + frame_id: "/world" +pose: + pose: + position: + x: 0.162158988007 + y: 0.0309247990533 + z: 0.0 + orientation: + x: -0.0 + y: -0.00211224330014 + z: -0.0 + w: 0.999997769212 + covariance: [19.059037277280446, -7.805924243658962e-07, 0.0, 0.0, -0.0003022962373931249, 0.0, -7.80592424365895e-07, 19.059034488817055, 0.0, 0.0, 0.000346861701139921, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003022962373931248, 0.00034686170113992077, 0.0, 0.0, 18.56876280319795, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 179 + stamp: + secs: 1624875207 + nsecs: 104904 + frame_id: "/world" +pose: + pose: + position: + x: 0.162257934467 + y: 0.0306714083051 + z: 0.0 + orientation: + x: -0.0 + y: -0.00144103122325 + z: -0.0 + w: 0.999998961714 + covariance: [19.55580384732583, -7.65016798788938e-07, 0.0, 0.0, -0.0003166944093142392, 0.0, -7.65016798788937e-07, 19.555801137765314, 0.0, 0.0, 0.0003194790953180591, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003166944093142391, 0.000319479095318059, 0.0, 0.0, 19.073412233083978, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 180 + stamp: + secs: 1624875207 + nsecs: 80077887 + frame_id: "/world" +pose: + pose: + position: + x: 0.150963417414 + y: 0.0391685007139 + z: 0.0 + orientation: + x: 0.0 + y: 0.000475885312731 + z: 0.0 + w: 0.999999886767 + covariance: [19.907380857737156, -7.970786045663572e-07, 0.0, 0.0, -0.00036217177906811276, 0.0, -7.970786045663563e-07, 19.907378059905817, 0.0, 0.0, 0.0002971557507124828, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00036217177906811254, 0.00029715575071248264, 0.0, 0.0, 19.402416165654852, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 181 + stamp: + secs: 1624875207 + nsecs: 160103083 + frame_id: "/world" +pose: + pose: + position: + x: 0.133635387309 + y: 0.0469573447314 + z: 0.0 + orientation: + x: 0.0 + y: 0.00538388799993 + z: 0.0 + w: 0.99998550677 + covariance: [20.1522654291959, -8.42297008272025e-07, 0.0, 0.0, -0.0004290494915031747, 0.0, -8.422970082720238e-07, 20.152262495626335, 0.0, 0.0, 0.00027204797870456095, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004290494915031746, 0.00027204797870456084, 0.0, 0.0, 19.61579485972072, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 182 + stamp: + secs: 1624875207 + nsecs: 240103006 + frame_id: "/world" +pose: + pose: + position: + x: 0.138032693415 + y: 0.047119753054 + z: 0.0 + orientation: + x: 0.0 + y: 0.000544818900905 + z: 0.0 + w: 0.999999851586 + covariance: [16.882245963849385, -6.022909014358274e-07, 0.0, 0.0, -0.00034487041989283615, 0.0, -6.022909014358268e-07, 16.882243885048602, 0.0, 0.0, 0.00022912715049944928, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003448704198928362, 0.00022912715049944922, 0.0, 0.0, 16.49522596381316, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 183 + stamp: + secs: 1624875207 + nsecs: 320096970 + frame_id: "/world" +pose: + pose: + position: + x: 0.141414231251 + y: 0.0467829901983 + z: 0.0 + orientation: + x: 0.0 + y: 7.96617397215e-05 + z: 0.0 + w: 0.999999996827 + covariance: [15.313287976333497, -4.3475311950171025e-07, 0.0, 0.0, -0.00025301764396367066, 0.0, -4.347531195017098e-07, 15.313286491555447, 0.0, 0.0, 0.00020850200977323065, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002530176439636706, 0.0002085020097732306, 0.0, 0.0, 15.032731342795298, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 184 + stamp: + secs: 1624875207 + nsecs: 400090933 + frame_id: "/world" +pose: + pose: + position: + x: 0.141583542517 + y: 0.043734564388 + z: 0.0 + orientation: + x: -0.0 + y: -0.00043732706155 + z: -0.0 + w: 0.999999904373 + covariance: [14.573801107531713, -4.193649536837453e-07, 0.0, 0.0, -0.00021952256588382736, 0.0, -4.193649536837447e-07, 14.573799699996295, 0.0, 0.0, 0.0002225054373070231, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00021952256588382728, 0.0002225054373070231, 0.0, 0.0, 14.302666564369643, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 185 + stamp: + secs: 1624875207 + nsecs: 480118990 + frame_id: "/world" +pose: + pose: + position: + x: 0.142476902866 + y: 0.0424621431629 + z: 0.0 + orientation: + x: -0.0 + y: -0.00083548566371 + z: -0.0 + w: 0.999999650982 + covariance: [16.50841420032839, -5.123647910931299e-07, 0.0, 0.0, -0.00042349159441433146, 0.0, -5.12364791093129e-07, 16.508412508141117, 0.0, 0.0, 0.0002637036658399103, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004234915944143312, 0.0002637036658399104, 0.0, 0.0, 16.179727688183178, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 186 + stamp: + secs: 1624875207 + nsecs: 560122967 + frame_id: "/world" +pose: + pose: + position: + x: 0.142473613519 + y: 0.0423778617224 + z: 0.0 + orientation: + x: -0.0 + y: -0.000583356819748 + z: -0.0 + w: 0.999999829847 + covariance: [17.747423764090193, -4.2112518149873565e-07, 0.0, 0.0, -0.0004299240961578801, 0.0, -4.211251814987349e-07, 17.74742237829128, 0.0, 0.0, 0.00021303299837377475, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004299240961578798, 0.00021303299837377484, 0.0, 0.0, 17.47795690147369, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 187 + stamp: + secs: 1624875207 + nsecs: 640136957 + frame_id: "/world" +pose: + pose: + position: + x: 0.142462351175 + y: 0.0420869757409 + z: 0.0 + orientation: + x: -0.0 + y: -0.000434446499099 + z: -0.0 + w: 0.999999905628 + covariance: [18.692762159910096, -5.409575300987148e-07, 0.0, 0.0, -0.000717628413961763, 0.0, -5.409575300987138e-07, 18.692760391331834, 0.0, 0.0, 0.00023672240457309231, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007176284139617625, 0.00023672240457309231, 0.0, 0.0, 18.3527062021872, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 188 + stamp: + secs: 1624875207 + nsecs: 720158100 + frame_id: "/world" +pose: + pose: + position: + x: 0.142449918138 + y: 0.041760134266 + z: 0.0 + orientation: + x: -0.0 + y: -4.84009222737e-05 + z: -0.0 + w: 0.999999998829 + covariance: [19.35310216822201, -6.783219467092668e-07, 0.0, 0.0, -0.000944735508180052, 0.0, -6.783219467092655e-07, 19.35309994437682, 0.0, 0.0, 0.0002399228080693974, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009447355081800518, 0.00023992280806939742, 0.0, 0.0, 18.93149486390693, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 189 + stamp: + secs: 1624875207 + nsecs: 800143957 + frame_id: "/world" +pose: + pose: + position: + x: 0.142437518753 + y: 0.0414405965818 + z: 0.0 + orientation: + x: 0.0 + y: 0.000268287894111 + z: 0.0 + w: 0.999999964011 + covariance: [19.791423938206112, -7.786164937725876e-07, 0.0, 0.0, -0.0010475641464883906, 0.0, -7.786164937725861e-07, 19.791421368927285, 0.0, 0.0, 0.00022828665333003885, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010475641464883906, 0.00022828665333003885, 0.0, 0.0, 19.310260900422517, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 190 + stamp: + secs: 1624875207 + nsecs: 880158901 + frame_id: "/world" +pose: + pose: + position: + x: 0.142422428277 + y: 0.0411012694767 + z: 0.0 + orientation: + x: 0.0 + y: 0.000264937000551 + z: 0.0 + w: 0.999999964904 + covariance: [20.083991688557617, -8.630288719803566e-07, 0.0, 0.0, -0.0013706935861623126, 0.0, -8.630288719803551e-07, 20.083988798475954, 0.0, 0.0, 0.00024696579761042007, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013706935861623123, 0.00024696579761041996, 0.0, 0.0, 19.55612838453532, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 191 + stamp: + secs: 1624875207 + nsecs: 960160017 + frame_id: "/world" +pose: + pose: + position: + x: 0.14240634703 + y: 0.0407704732381 + z: 0.0 + orientation: + x: 0.0 + y: 0.000337797867003 + z: 0.0 + w: 0.999999942946 + covariance: [20.25818674406134, -9.010680177773379e-07, 0.0, 0.0, -0.0016566327614700714, 0.0, -9.010680177773366e-07, 20.258183652260563, 0.0, 0.0, 0.00026118040805964707, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0016566327614700714, 0.000261180408059647, 0.0, 0.0, 19.714967233883627, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 192 + stamp: + secs: 1624875208 + nsecs: 40215015 + frame_id: "/world" +pose: + pose: + position: + x: 0.142389429741 + y: 0.0404398063698 + z: 0.0 + orientation: + x: 0.0 + y: 0.00043439845396 + z: 0.0 + w: 0.999999905649 + covariance: [20.37153683720421, -9.29866966082794e-07, 0.0, 0.0, -0.0016905183840150782, 0.0, -9.298669660827927e-07, 20.37153358054137, 0.0, 0.0, 0.00026260552362958003, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0016905183840150782, 0.00026260552362958003, 0.0, 0.0, 19.817245115540953, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 193 + stamp: + secs: 1624875208 + nsecs: 120165110 + frame_id: "/world" +pose: + pose: + position: + x: 0.142368458862 + y: 0.0400470786113 + z: 0.0 + orientation: + x: 0.0 + y: 0.000515841728213 + z: 0.0 + w: 0.999999866954 + covariance: [20.49128239534723, -1.030495394262505e-06, 0.0, 0.0, -0.0016649046040139504, 0.0, -1.0304953942625035e-06, 20.491278720459466, 0.0, 0.0, 0.00026926356503130446, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0016649046040139502, 0.0002692635650313045, 0.0, 0.0, 19.88299368483262, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 194 + stamp: + secs: 1624875208 + nsecs: 200181961 + frame_id: "/world" +pose: + pose: + position: + x: 0.1423496766 + y: 0.0397309792451 + z: 0.0 + orientation: + x: 0.0 + y: 0.000635076089843 + z: 0.0 + w: 0.999999798339 + covariance: [20.520550999209334, -1.0134533004936717e-06, 0.0, 0.0, -0.001451856158361279, 0.0, -1.0134533004936702e-06, 20.520547353954036, 0.0, 0.0, 0.00026803327069191033, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001451856158361279, 0.0002680332706919103, 0.0, 0.0, 19.92536882711999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 195 + stamp: + secs: 1624875208 + nsecs: 280190945 + frame_id: "/world" +pose: + pose: + position: + x: 0.140560508904 + y: 0.0345101037347 + z: 0.0 + orientation: + x: 0.0 + y: 0.000797935742198 + z: 0.0 + w: 0.999999681649 + covariance: [17.069822206417484, -7.416681006146688e-07, 0.0, 0.0, -0.0009229103726266189, 0.0, -7.416681006146676e-07, 17.069819522640255, 0.0, 0.0, 0.00020446319425063952, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000922910372626619, 0.00020446319425063952, 0.0, 0.0, 16.63405670483017, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 196 + stamp: + secs: 1624875208 + nsecs: 360193968 + frame_id: "/world" +pose: + pose: + position: + x: 0.143062682233 + y: 0.0380298571903 + z: 0.0 + orientation: + x: 0.0 + y: 0.000303562297453 + z: 0.0 + w: 0.999999953925 + covariance: [18.304064213810708, -8.900881851052045e-07, 0.0, 0.0, -0.0009084884306007618, 0.0, -8.900881851052034e-07, 18.304060991753875, 0.0, 0.0, 0.0002942949159250886, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009084884306007619, 0.00029429491592508855, 0.0, 0.0, 17.78655013653313, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 197 + stamp: + secs: 1624875208 + nsecs: 440201998 + frame_id: "/world" +pose: + pose: + position: + x: 0.147859714268 + y: 0.0413242187074 + z: 0.0 + orientation: + x: -0.0 + y: -0.00129572502538 + z: -0.0 + w: 0.999999160548 + covariance: [19.04436444124868, -8.404040800969801e-07, 0.0, 0.0, -0.000748857432391046, 0.0, -8.40404080096979e-07, 19.044361414522026, 0.0, 0.0, 0.0003345063357748019, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007488574323910463, 0.00033450633577480183, 0.0, 0.0, 18.557934335634513, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 198 + stamp: + secs: 1624875208 + nsecs: 520207882 + frame_id: "/world" +pose: + pose: + position: + x: 0.147856018569 + y: 0.0409729173796 + z: 0.0 + orientation: + x: -0.0 + y: -0.000567162889524 + z: -0.0 + w: 0.999999839163 + covariance: [19.60780405715735, -9.424134574908398e-07, 0.0, 0.0, -0.0006795757624748822, 0.0, -9.424134574908385e-07, 19.60780069501504, 0.0, 0.0, 0.0004282552650167189, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006795757624748825, 0.00042825526501671886, 0.0, 0.0, 19.066705049697873, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 199 + stamp: + secs: 1624875208 + nsecs: 600233078 + frame_id: "/world" +pose: + pose: + position: + x: 0.144375333932 + y: 0.0426706811863 + z: 0.0 + orientation: + x: 0.0 + y: 0.00125564454595 + z: 0.0 + w: 0.999999211678 + covariance: [19.981063772426396, -1.0159637330053566e-06, 0.0, 0.0, -0.0005940268801615351, 0.0, -1.0159637330053549e-06, 19.9810601742661, 0.0, 0.0, 0.00045212981709114134, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005940268801615351, 0.00045212981709114134, 0.0, 0.0, 19.398597093876152, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 200 + stamp: + secs: 1624875208 + nsecs: 680222988 + frame_id: "/world" +pose: + pose: + position: + x: 0.138905891857 + y: 0.0419220004916 + z: 0.0 + orientation: + x: 0.0 + y: 0.00245519063178 + z: 0.0 + w: 0.999996986015 + covariance: [20.245982894247526, -1.1023534749048171e-06, 0.0, 0.0, -0.0005719329070006276, 0.0, -1.1023534749048152e-06, 20.245979020694673, 0.0, 0.0, 0.0004696229731888643, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005719329070006279, 0.00046962297318886434, 0.0, 0.0, 19.613903945079123, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 201 + stamp: + secs: 1624875208 + nsecs: 760227919 + frame_id: "/world" +pose: + pose: + position: + x: 0.13887442379 + y: 0.0414163769421 + z: 0.0 + orientation: + x: 0.0 + y: 0.00189510578148 + z: 0.0 + w: 0.999998204285 + covariance: [20.42360035200424, -1.1684627215715925e-06, 0.0, 0.0, -0.0005666300578947264, 0.0, -1.1684627215715904e-06, 20.42359628371323, 0.0, 0.0, 0.0004920062474365625, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005666300578947265, 0.0004920062474365625, 0.0, 0.0, 19.75259819336361, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 202 + stamp: + secs: 1624875208 + nsecs: 840251923 + frame_id: "/world" +pose: + pose: + position: + x: 0.138800305295 + y: 0.0410555848699 + z: 0.0 + orientation: + x: 0.0 + y: 0.00169437326739 + z: 0.0 + w: 0.999998564549 + covariance: [20.538392496578197, -1.2081295317323807e-06, 0.0, 0.0, -0.0005227829545212774, 0.0, -1.2081295317323786e-06, 20.538388327870518, 0.0, 0.0, 0.0005000847337855841, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005227829545212774, 0.000500084733785584, 0.0, 0.0, 19.842653763951592, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 203 + stamp: + secs: 1624875208 + nsecs: 920262098 + frame_id: "/world" +pose: + pose: + position: + x: 0.138724382208 + y: 0.0406907336721 + z: 0.0 + orientation: + x: 0.0 + y: 0.0014893058682 + z: 0.0 + w: 0.999998890983 + covariance: [20.615153582768453, -1.2368720018510262e-06, 0.0, 0.0, -0.0005423277940263922, 0.0, -1.2368720018510243e-06, 20.61514935473362, 0.0, 0.0, 0.0004959056065078365, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005423277940263922, 0.0004959056065078364, 0.0, 0.0, 19.899441380280997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 204 + stamp: + secs: 1624875209 + nsecs: 256062 + frame_id: "/world" +pose: + pose: + position: + x: 0.138647412379 + y: 0.0403218044626 + z: 0.0 + orientation: + x: 0.0 + y: 0.00130658686254 + z: 0.0 + w: 0.999999146415 + covariance: [20.666829130770097, -1.2572009798744244e-06, 0.0, 0.0, -0.0005741019089943265, 0.0, -1.2572009798744225e-06, 20.66682487210388, 0.0, 0.0, 0.00047303913506572864, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005741019089943266, 0.00047303913506572875, 0.0, 0.0, 19.93677484211388, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 205 + stamp: + secs: 1624875209 + nsecs: 80250978 + frame_id: "/world" +pose: + pose: + position: + x: 0.138570324353 + y: 0.039953263934 + z: 0.0 + orientation: + x: 0.0 + y: 0.00114508589986 + z: 0.0 + w: 0.999999344389 + covariance: [20.70224838841741, -1.2686469278436214e-06, 0.0, 0.0, -0.0005462337225038096, 0.0, -1.2686469278436195e-06, 20.702244122807723, 0.0, 0.0, 0.00042861399531021093, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005462337225038098, 0.00042861399531021093, 0.0, 0.0, 19.95960735759614, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 206 + stamp: + secs: 1624875209 + nsecs: 160267115 + frame_id: "/world" +pose: + pose: + position: + x: 0.138874080141 + y: 0.0413653345064 + z: 0.0 + orientation: + x: 0.0 + y: 0.000589949352852 + z: 0.0 + w: 0.99999982598 + covariance: [17.145508872843248, -8.350178545958933e-07, 0.0, 0.0, -0.00032808869854119394, 0.0, -8.350178545958925e-07, 17.14550607979006, 0.0, 0.0, 0.0002669698019110615, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00032808869854119405, 0.0002669698019110615, 0.0, 0.0, 16.649108196503555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 207 + stamp: + secs: 1624875209 + nsecs: 240287066 + frame_id: "/world" +pose: + pose: + position: + x: 0.13879136552 + y: 0.0409863951229 + z: 0.0 + orientation: + x: 0.0 + y: 0.000788519699119 + z: 0.0 + w: 0.999999689118 + covariance: [18.389091322749554, -9.88815640773958e-07, 0.0, 0.0, -0.0003243809586724937, 0.0, -9.88815640773957e-07, 18.389088043290556, 0.0, 0.0, 0.0002915311157111446, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00032438095867249375, 0.00029153111571114453, 0.0, 0.0, 17.796887034429552, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 208 + stamp: + secs: 1624875209 + nsecs: 320278883 + frame_id: "/world" +pose: + pose: + position: + x: 0.143344897788 + y: 0.0420706400096 + z: 0.0 + orientation: + x: 0.0 + y: 0.000246607652712 + z: 0.0 + w: 0.999999969592 + covariance: [19.272001691110155, -1.1695384689601978e-06, 0.0, 0.0, -0.0003956905864243096, 0.0, -1.169538468960196e-06, 19.27199784548348, 0.0, 0.0, 0.0003307581212088146, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00039569058642430964, 0.0003307581212088145, 0.0, 0.0, 18.56583714060859, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 209 + stamp: + secs: 1624875209 + nsecs: 400302887 + frame_id: "/world" +pose: + pose: + position: + x: 0.143884601821 + y: 0.0481189698821 + z: 0.0 + orientation: + x: 0.0 + y: 0.000164834211141 + z: 0.0 + w: 0.999999986415 + covariance: [16.49135495008524, -7.737722412203046e-07, 0.0, 0.0, -0.00028462949985883016, 0.0, -7.737722412203036e-07, 16.491352417974074, 0.0, 0.0, 0.00021982229108583135, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00028462949985883016, 0.00021982229108583127, 0.0, 0.0, 16.0175189677815, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 210 + stamp: + secs: 1624875209 + nsecs: 480295897 + frame_id: "/world" +pose: + pose: + position: + x: 0.140780270749 + y: 0.0500811189308 + z: 0.0 + orientation: + x: 0.0 + y: 0.000961539922621 + z: 0.0 + w: 0.99999953772 + covariance: [17.93446739384575, -9.18296198629834e-07, 0.0, 0.0, -0.0003290952741895451, 0.0, -9.182961986298327e-07, 17.93446441351104, 0.0, 0.0, 0.00026281302926821077, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003290952741895451, 0.00026281302926821066, 0.0, 0.0, 17.368464980704022, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 211 + stamp: + secs: 1624875209 + nsecs: 560297012 + frame_id: "/world" +pose: + pose: + position: + x: 0.138374342632 + y: 0.0514087023616 + z: 0.0 + orientation: + x: 0.0 + y: 0.00137145796054 + z: 0.0 + w: 0.999999059551 + covariance: [18.85064255408925, -9.178396940499558e-07, 0.0, 0.0, -0.00030783363454321304, 0.0, -9.178396940499544e-07, 18.850639591968864, 0.0, 0.0, 0.0002604546010937218, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003078336345432131, 0.0002604546010937217, 0.0, 0.0, 18.279727226664544, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 212 + stamp: + secs: 1624875209 + nsecs: 640314102 + frame_id: "/world" +pose: + pose: + position: + x: 0.138292013783 + y: 0.0511420379492 + z: 0.0 + orientation: + x: 0.0 + y: 0.000895138900629 + z: 0.0 + w: 0.999999599363 + covariance: [19.499934701659527, -9.801526909312594e-07, 0.0, 0.0, -0.00029852671563510377, 0.0, -9.801526909312584e-07, 19.49993155813948, 0.0, 0.0, 0.00026788973714469955, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002985267156351038, 0.0002678897371446995, 0.0, 0.0, 18.88365535606345, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 213 + stamp: + secs: 1624875209 + nsecs: 720318079 + frame_id: "/world" +pose: + pose: + position: + x: 0.147101841813 + y: 0.0560749490266 + z: 0.0 + orientation: + x: 0.0 + y: 0.000900469295823 + z: 0.0 + w: 0.999999594577 + covariance: [19.96762767330168, -1.0817621794639465e-06, 0.0, 0.0, -0.00031011505581638956, 0.0, -1.0817621794639454e-06, 19.967624225283032, 0.0, 0.0, 0.0002745467088205769, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003101150558163896, 0.0002745467088205768, 0.0, 0.0, 19.279273300304258, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 214 + stamp: + secs: 1624875209 + nsecs: 800335884 + frame_id: "/world" +pose: + pose: + position: + x: 0.147082336193 + y: 0.0558364914987 + z: 0.0 + orientation: + x: 0.0 + y: 0.000844995202109 + z: 0.0 + w: 0.999999642991 + covariance: [20.250157469090126, -1.107869457960124e-06, 0.0, 0.0, -0.000299869132492094, 0.0, -1.107869457960123e-06, 20.250153953607537, 0.0, 0.0, 0.00025284302359927395, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002998691324920941, 0.00025284302359927384, 0.0, 0.0, 19.53667393103118, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 215 + stamp: + secs: 1624875209 + nsecs: 880331039 + frame_id: "/world" +pose: + pose: + position: + x: 0.152959594596 + y: 0.0536601232637 + z: 0.0 + orientation: + x: 0.0 + y: 0.00050780744724 + z: 0.0 + w: 0.999999871066 + covariance: [14.506156160750482, -5.648690972424474e-07, 0.0, 0.0, -0.0001446679205998285, 0.0, -5.648690972424471e-07, 14.506154374859056, 0.0, 0.0, 0.00013066367766445948, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00014466792059982856, 0.0001306636776644594, 0.0, 0.0, 14.133659232141284, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 216 + stamp: + secs: 1624875209 + nsecs: 960352898 + frame_id: "/world" +pose: + pose: + position: + x: 0.152989998547 + y: 0.0533674851651 + z: 0.0 + orientation: + x: 0.0 + y: 0.00117496704699 + z: 0.0 + w: 0.999999309726 + covariance: [16.549437675306297, -7.322566810283447e-07, 0.0, 0.0, -0.00017368128217754028, 0.0, -7.322566810283444e-07, 16.549435374222103, 0.0, 0.0, 0.00017401059363862268, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00017368128217754033, 0.00017401059363862263, 0.0, 0.0, 16.062458691644157, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 217 + stamp: + secs: 1624875210 + nsecs: 40358067 + frame_id: "/world" +pose: + pose: + position: + x: 0.156187088143 + y: 0.0531801485102 + z: 0.0 + orientation: + x: 0.0 + y: 0.000954490731887 + z: 0.0 + w: 0.999999544474 + covariance: [15.204413668600539, -5.682466611532847e-07, 0.0, 0.0, -0.0001216400539467323, 0.0, -5.682466611532844e-07, 15.204411889176392, 0.0, 0.0, 0.00012994322353394918, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00012164005394673238, 0.00012994322353394918, 0.0, 0.0, 14.820026346234135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 218 + stamp: + secs: 1624875210 + nsecs: 120342016 + frame_id: "/world" +pose: + pose: + position: + x: 0.162011783802 + y: 0.0481856783237 + z: 0.0 + orientation: + x: 0.0 + y: 0.000932714554032 + z: 0.0 + w: 0.999999565022 + covariance: [17.07994953256771, -7.830405731720905e-07, 0.0, 0.0, -0.00013114090354453213, 0.0, -7.830405731720898e-07, 17.07994709239313, 0.0, 0.0, 0.00015959557636444026, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001311409035445322, 0.00015959557636444028, 0.0, 0.0, 16.54175776914888, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 219 + stamp: + secs: 1624875210 + nsecs: 200356007 + frame_id: "/world" +pose: + pose: + position: + x: 0.162061696321 + y: 0.0480184988457 + z: 0.0 + orientation: + x: 0.0 + y: 0.000609143594155 + z: 0.0 + w: 0.999999814472 + covariance: [18.221683645682734, -7.167736496024413e-07, 0.0, 0.0, -0.00010876904074787774, 0.0, -7.167736496024406e-07, 18.221681415770643, 0.0, 0.0, 0.00013617848026313241, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00010876904074787778, 0.00013617848026313244, 0.0, 0.0, 17.72366436681826, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 220 + stamp: + secs: 1624875210 + nsecs: 280395031 + frame_id: "/world" +pose: + pose: + position: + x: 0.162150619269 + y: 0.0476992718366 + z: 0.0 + orientation: + x: 0.0 + y: 0.000492739866296 + z: 0.0 + w: 0.999999878604 + covariance: [19.10285473404375, -8.335330349139783e-07, 0.0, 0.0, -0.00023556872860822886, 0.0, -8.335330349139777e-07, 19.102852152665832, 0.0, 0.0, 0.00018529571610345142, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00023556872860822884, 0.00018529571610345145, 0.0, 0.0, 18.516542689922623, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 221 + stamp: + secs: 1624875210 + nsecs: 360379934 + frame_id: "/world" +pose: + pose: + position: + x: 0.162221227273 + y: 0.0474225540458 + z: 0.0 + orientation: + x: 0.0 + y: 0.000736129933605 + z: 0.0 + w: 0.999999729056 + covariance: [19.647528091179208, -8.572786916059132e-07, 0.0, 0.0, -0.000373889757502315, 0.0, -8.572786916059124e-07, 19.647525450191317, 0.0, 0.0, 0.00023505963455554453, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00037388975750231497, 0.0002350596345555446, 0.0, 0.0, 19.040029348247423, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 222 + stamp: + secs: 1624875210 + nsecs: 440391064 + frame_id: "/world" +pose: + pose: + position: + x: 0.162278961997 + y: 0.047192465533 + z: 0.0 + orientation: + x: 0.0 + y: 0.000596045855659 + z: 0.0 + w: 0.999999822365 + covariance: [16.667668149803845, -6.021460624868289e-07, 0.0, 0.0, -0.0002987367576298679, 0.0, -6.021460624868284e-07, 16.667666303125475, 0.0, 0.0, 0.00017776426955914875, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002987367576298678, 0.00017776426955914883, 0.0, 0.0, 16.234733031463534, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 223 + stamp: + secs: 1624875210 + nsecs: 520392895 + frame_id: "/world" +pose: + pose: + position: + x: 0.162372864373 + y: 0.0468240207781 + z: 0.0 + orientation: + x: 0.0 + y: 0.00108960580451 + z: 0.0 + w: 0.999999406379 + covariance: [18.058127021315766, -7.479438002429899e-07, 0.0, 0.0, -0.0003540233190239082, 0.0, -7.479438002429893e-07, 18.058124741347577, 0.0, 0.0, 0.00020600422988593565, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00035402331902390817, 0.00020600422988593573, 0.0, 0.0, 17.51737254131063, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 224 + stamp: + secs: 1624875210 + nsecs: 600393057 + frame_id: "/world" +pose: + pose: + position: + x: 0.162441471099 + y: 0.0465440718868 + z: 0.0 + orientation: + x: 0.0 + y: 0.00145845586998 + z: 0.0 + w: 0.999998936453 + covariance: [18.959729257846348, -7.949559660621536e-07, 0.0, 0.0, -0.00035834515173682193, 0.0, -7.949559660621533e-07, 18.959726845665912, 0.0, 0.0, 0.0002138841262182181, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003583451517368219, 0.00021388412621821816, 0.0, 0.0, 18.38027400832202, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 225 + stamp: + secs: 1624875210 + nsecs: 680402040 + frame_id: "/world" +pose: + pose: + position: + x: 0.162517313156 + y: 0.0462235106813 + z: 0.0 + orientation: + x: 0.0 + y: 0.00136802216266 + z: 0.0 + w: 0.999999064257 + covariance: [19.592464692102325, -8.717059451717726e-07, 0.0, 0.0, -0.000373575414064317, 0.0, -8.717059451717722e-07, 19.592462061729666, 0.0, 0.0, 0.00023874935410166445, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000373575414064317, 0.00023874935410166456, 0.0, 0.0, 18.950307021265093, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 226 + stamp: + secs: 1624875210 + nsecs: 760426044 + frame_id: "/world" +pose: + pose: + position: + x: 0.162575486047 + y: 0.0459643264415 + z: 0.0 + orientation: + x: 0.0 + y: 0.000702089773082 + z: 0.0 + w: 0.999999753535 + covariance: [19.95343773642661, -8.481937736585343e-07, 0.0, 0.0, -0.0003619236605465665, 0.0, -8.481937736585343e-07, 19.95343519035328, 0.0, 0.0, 0.00025721600706647387, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003619236605465665, 0.000257216007066474, 0.0, 0.0, 19.32173483480294, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 227 + stamp: + secs: 1624875210 + nsecs: 840425968 + frame_id: "/world" +pose: + pose: + position: + x: 0.162636815497 + y: 0.0456501623727 + z: 0.0 + orientation: + x: 0.0 + y: 0.000166595125585 + z: 0.0 + w: 0.999999986123 + covariance: [20.220193744063224, -8.800365613479252e-07, 0.0, 0.0, -0.0005717222493846278, 0.0, -8.80036561347925e-07, 20.22019112811467, 0.0, 0.0, 0.0003427771292993325, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005717222493846278, 0.0003427771292993327, 0.0, 0.0, 19.56356591219736, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 228 + stamp: + secs: 1624875210 + nsecs: 920433044 + frame_id: "/world" +pose: + pose: + position: + x: 0.162691692558 + y: 0.0453320563465 + z: 0.0 + orientation: + x: -0.0 + y: -7.76810030644e-05 + z: -0.0 + w: 0.999999996983 + covariance: [20.37994617278429, -8.929774798661758e-07, 0.0, 0.0, -0.0009076894089653886, 0.0, -8.929774798661753e-07, 20.379943540872635, 0.0, 0.0, 0.00039368823399866566, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009076894089653884, 0.00039368823399866577, 0.0, 0.0, 19.719436055553537, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 229 + stamp: + secs: 1624875211 + nsecs: 436068 + frame_id: "/world" +pose: + pose: + position: + x: 0.16274551877 + y: 0.0450029937148 + z: 0.0 + orientation: + x: -0.0 + y: -9.97144780193e-05 + z: -0.0 + w: 0.999999995029 + covariance: [20.480496014063316, -9.058856041132462e-07, 0.0, 0.0, -0.0011939760802176312, 0.0, -9.058856041132457e-07, 20.480493346234184, 0.0, 0.0, 0.00039979228469573183, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011939760802176312, 0.00039979228469573183, 0.0, 0.0, 19.820183528789958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 230 + stamp: + secs: 1624875211 + nsecs: 80427885 + frame_id: "/world" +pose: + pose: + position: + x: 0.162808157775 + y: 0.0446078609731 + z: 0.0 + orientation: + x: 0.0 + y: 4.53566512001e-05 + z: 0.0 + w: 0.999999998971 + covariance: [20.592884344556946, -9.888825884723961e-07, 0.0, 0.0, -0.0014391195307431087, 0.0, -9.88882588472396e-07, 20.592881415695054, 0.0, 0.0, 0.00040405748692359924, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001439119530743109, 0.00040405748692359924, 0.0, 0.0, 19.884697945677527, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 231 + stamp: + secs: 1624875211 + nsecs: 160442114 + frame_id: "/world" +pose: + pose: + position: + x: 0.162857241155 + y: 0.0442969750926 + z: 0.0 + orientation: + x: 0.0 + y: 0.000229375878088 + z: 0.0 + w: 0.999999973693 + covariance: [20.60306223189616, -9.521970073120834e-07, 0.0, 0.0, -0.001357289919336116, 0.0, -9.521970073120832e-07, 20.60305939491459, 0.0, 0.0, 0.0003505554009371014, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013572899193361164, 0.0003505554009371014, 0.0, 0.0, 19.92644363336895, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 232 + stamp: + secs: 1624875211 + nsecs: 240469933 + frame_id: "/world" +pose: + pose: + position: + x: 0.162892542112 + y: 0.0440365316284 + z: 0.0 + orientation: + x: -0.0 + y: -0.000181178553751 + z: -0.0 + w: 0.999999983587 + covariance: [17.085409456668145, -6.383851168703135e-07, 0.0, 0.0, -0.000866033845387825, 0.0, -6.383851168703135e-07, 17.085407561872355, 0.0, 0.0, 0.0002790661578185712, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008660338453878252, 0.0002790661578185712, 0.0, 0.0, 16.633866352862416, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 233 + stamp: + secs: 1624875211 + nsecs: 320468903 + frame_id: "/world" +pose: + pose: + position: + x: 0.160690376494 + y: 0.048643150079 + z: 0.0 + orientation: + x: 0.0 + y: 0.000423181181184 + z: 0.0 + w: 0.999999910459 + covariance: [18.316933566747178, -7.754447592541205e-07, 0.0, 0.0, -0.0009254473597586651, 0.0, -7.754447592541205e-07, 18.31693130847225, 0.0, 0.0, 0.00044442040020472304, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009254473597586655, 0.0004444204002047231, 0.0, 0.0, 17.78606147720979, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 234 + stamp: + secs: 1624875211 + nsecs: 400448084 + frame_id: "/world" +pose: + pose: + position: + x: 0.164019139316 + y: 0.0553009029739 + z: 0.0 + orientation: + x: 0.0 + y: 0.000143222258878 + z: 0.0 + w: 0.999999989744 + covariance: [13.842310625721803, -4.5550871937325395e-07, 0.0, 0.0, -0.0005047845330959829, 0.0, -4.5550871937325385e-07, 13.842309323195375, 0.0, 0.0, 0.00031065607197886604, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005047845330959832, 0.00031065607197886626, 0.0, 0.0, 13.534258568386209, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 235 + stamp: + secs: 1624875211 + nsecs: 480456114 + frame_id: "/world" +pose: + pose: + position: + x: 0.161538348852 + y: 0.0586548441264 + z: 0.0 + orientation: + x: 0.0 + y: 0.00116773426072 + z: 0.0 + w: 0.999999318198 + covariance: [13.822182974585413, -4.5919850382532313e-07, 0.0, 0.0, -0.0004903161024629613, 0.0, -4.5919850382532313e-07, 13.822181698535834, 0.0, 0.0, 0.00032942436033468324, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004903161024629615, 0.0003294243603346834, 0.0, 0.0, 13.52191104594252, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 236 + stamp: + secs: 1624875211 + nsecs: 560573101 + frame_id: "/world" +pose: + pose: + position: + x: 0.161553724235 + y: 0.0584032445552 + z: 0.0 + orientation: + x: 0.0 + y: 0.000757716635465 + z: 0.0 + w: 0.999999712933 + covariance: [16.0140726133697, -6.103580354188074e-07, 0.0, 0.0, -0.0005938518863159128, 0.0, -6.103580354188073e-07, 16.01407095895082, 0.0, 0.0, 0.00038954479525431105, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005938518863159131, 0.0003895447952543112, 0.0, 0.0, 15.627403280388316, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 237 + stamp: + secs: 1624875211 + nsecs: 640486956 + frame_id: "/world" +pose: + pose: + position: + x: 0.161583863767 + y: 0.0580979516583 + z: 0.0 + orientation: + x: 0.0 + y: 0.000200449722067 + z: 0.0 + w: 0.99999997991 + covariance: [17.558524052630588, -7.411129632413095e-07, 0.0, 0.0, -0.0007128657059650457, 0.0, -7.411129632413093e-07, 17.558522080010544, 0.0, 0.0, 0.00040878848846817996, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007128657059650459, 0.00040878848846818, 0.0, 0.0, 17.100196475726346, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 238 + stamp: + secs: 1624875211 + nsecs: 720500946 + frame_id: "/world" +pose: + pose: + position: + x: 0.161598290596 + y: 0.0578148007595 + z: 0.0 + orientation: + x: -0.0 + y: -0.000337928936087 + z: -0.0 + w: 0.999999942902 + covariance: [18.624289932822855, -8.641194543206052e-07, 0.0, 0.0, -0.0008241260771612317, 0.0, -8.641194543206049e-07, 18.624287662313826, 0.0, 0.0, 0.0004050833338288385, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008241260771612319, 0.0004050833338288387, 0.0, 0.0, 18.100434493014724, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 239 + stamp: + secs: 1624875211 + nsecs: 800513029 + frame_id: "/world" +pose: + pose: + position: + x: 0.161611113042 + y: 0.0575594163361 + z: 0.0 + orientation: + x: -0.0 + y: -0.000591622752475 + z: -0.0 + w: 0.999999824991 + covariance: [19.31428307852607, -9.131038258270765e-07, 0.0, 0.0, -0.0007991960772466283, 0.0, -9.131038258270762e-07, 19.31428069891496, 0.0, 0.0, 0.0003645158369812062, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007991960772466284, 0.00036451583698120634, 0.0, 0.0, 18.76577942763237, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 240 + stamp: + secs: 1624875211 + nsecs: 880506992 + frame_id: "/world" +pose: + pose: + position: + x: 0.161625212478 + y: 0.0572750710021 + z: 0.0 + orientation: + x: -0.0 + y: -0.000491902651009 + z: -0.0 + w: 0.999999879016 + covariance: [19.793353778010633, -9.88644018934245e-07, 0.0, 0.0, -0.0007535076556016807, 0.0, -9.886440189342447e-07, 19.79335121960438, 0.0, 0.0, 0.0003267662447286305, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007535076556016807, 0.0003267662447286307, 0.0, 0.0, 19.20218348179904, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 241 + stamp: + secs: 1624875211 + nsecs: 960508108 + frame_id: "/world" +pose: + pose: + position: + x: 0.161640424986 + y: 0.0569569387196 + z: 0.0 + orientation: + x: -0.0 + y: -0.000304465914277 + z: -0.0 + w: 0.99999995365 + covariance: [20.13207464478425, -1.0836036387409429e-06, 0.0, 0.0, -0.0008543898852829514, 0.0, -1.0836036387409427e-06, 20.132071847500356, 0.0, 0.0, 0.00028901070437233696, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008543898852829516, 0.00028901070437233707, 0.0, 0.0, 19.486157644269653, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 242 + stamp: + secs: 1624875212 + nsecs: 40527105 + frame_id: "/world" +pose: + pose: + position: + x: 0.161656207805 + y: 0.0566152537542 + z: 0.0 + orientation: + x: -0.0 + y: -0.000368056302965 + z: -0.0 + w: 0.999999932267 + covariance: [20.371604262729324, -1.177721129451926e-06, 0.0, 0.0, -0.0009930039182592775, 0.0, -1.1777211294519256e-06, 20.371601211306846, 0.0, 0.0, 0.00025049949680957113, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009930039182592773, 0.0002504994968095713, 0.0, 0.0, 19.67002703041774, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 243 + stamp: + secs: 1624875212 + nsecs: 120568991 + frame_id: "/world" +pose: + pose: + position: + x: 0.161672669525 + y: 0.0562580822593 + z: 0.0 + orientation: + x: -0.0 + y: -0.000577547255893 + z: -0.0 + w: 0.99999983322 + covariance: [20.55034506569027, -1.2729285915735258e-06, 0.0, 0.0, -0.0010066531667928399, 0.0, -1.2729285915735256e-06, 20.550341753522574, 0.0, 0.0, 0.0002146827639892466, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010066531667928397, 0.0002146827639892467, 0.0, 0.0, 19.78864395887087, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 244 + stamp: + secs: 1624875212 + nsecs: 200536013 + frame_id: "/world" +pose: + pose: + position: + x: 0.161683998572 + y: 0.0560100557244 + z: 0.0 + orientation: + x: -0.0 + y: -0.000688550266905 + z: -0.0 + w: 0.999999762949 + covariance: [20.567223776829714, -1.1649874368647393e-06, 0.0, 0.0, -0.0008495042642364259, 0.0, -1.1649874368647391e-06, 20.567220738177532, 0.0, 0.0, 0.00016828812373319226, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008495042642364255, 0.0001682881237331923, 0.0, 0.0, 19.86500579654188, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 245 + stamp: + secs: 1624875212 + nsecs: 280575037 + frame_id: "/world" +pose: + pose: + position: + x: 0.161697544325 + y: 0.0557108135882 + z: 0.0 + orientation: + x: -0.0 + y: -0.000704176431039 + z: -0.0 + w: 0.999999752068 + covariance: [20.61819131131803, -1.1581910539994494e-06, 0.0, 0.0, -0.0007457047517328126, 0.0, -1.1581910539994491e-06, 20.61818828116052, 0.0, 0.0, 0.00013877177236456707, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007457047517328123, 0.0001387717723645671, 0.0, 0.0, 19.913609800502538, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 246 + stamp: + secs: 1624875212 + nsecs: 360554934 + frame_id: "/world" +pose: + pose: + position: + x: 0.161710373733 + y: 0.0554211434994 + z: 0.0 + orientation: + x: -0.0 + y: -0.000691328776155 + z: -0.0 + w: 0.999999761032 + covariance: [20.641839568481497, -1.1339458170675572e-06, 0.0, 0.0, -0.0006358438073505402, 0.0, -1.1339458170675567e-06, 20.641836594131092, 0.0, 0.0, 0.00011675173504920058, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00063584380735054, 0.00011675173504920062, 0.0, 0.0, 19.94525610746541, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 247 + stamp: + secs: 1624875212 + nsecs: 440562963 + frame_id: "/world" +pose: + pose: + position: + x: 0.1617219679 + y: 0.0551296756693 + z: 0.0 + orientation: + x: -0.0 + y: -0.000786489360542 + z: -0.0 + w: 0.999999690717 + covariance: [20.65578180634075, -1.1120525735330814e-06, 0.0, 0.0, -0.0005657477066797772, 0.0, -1.112052573533081e-06, 20.655778882972516, 0.0, 0.0, 0.00011556712264428747, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000565747706679777, 0.00011556712264428748, 0.0, 0.0, 19.965349296437992, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 248 + stamp: + secs: 1624875212 + nsecs: 520561934 + frame_id: "/world" +pose: + pose: + position: + x: 0.161732485442 + y: 0.0548297019487 + z: 0.0 + orientation: + x: -0.0 + y: -0.000432195456419 + z: -0.0 + w: 0.999999906604 + covariance: [20.661960248859145, -1.0901376910477697e-06, 0.0, 0.0, -0.000653239762902536, 0.0, -1.090137691047769e-06, 20.66195736975083, 0.0, 0.0, 0.00011872062373598926, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006532397629025356, 0.00011872062373598926, 0.0, 0.0, 19.977437662887287, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 249 + stamp: + secs: 1624875212 + nsecs: 600589991 + frame_id: "/world" +pose: + pose: + position: + x: 0.161742532976 + y: 0.0545234269197 + z: 0.0 + orientation: + x: 0.0 + y: 0.000292720760676 + z: 0.0 + w: 0.999999957157 + covariance: [20.66484658424566, -1.0705195861397184e-06, 0.0, 0.0, -0.0007298042164265321, 0.0, -1.0705195861397177e-06, 20.664843737703492, 0.0, 0.0, 0.00011879600002094637, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007298042164265319, 0.00011879600002094637, 0.0, 0.0, 19.985390049009897, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 250 + stamp: + secs: 1624875212 + nsecs: 680576086 + frame_id: "/world" +pose: + pose: + position: + x: 0.161752372282 + y: 0.0542253944589 + z: 0.0 + orientation: + x: 0.0 + y: 0.000487520427273 + z: 0.0 + w: 0.999999881162 + covariance: [20.66073318518629, -1.0435616363973497e-06, 0.0, 0.0, -0.0006854672125088796, 0.0, -1.0435616363973493e-06, 20.66073039570676, 0.0, 0.0, 0.00010812457128015007, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006854672125088792, 0.0001081245712801501, 0.0, 0.0, 19.990637384664122, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 251 + stamp: + secs: 1624875212 + nsecs: 760581970 + frame_id: "/world" +pose: + pose: + position: + x: 0.161758824895 + y: 0.0539241688121 + z: 0.0 + orientation: + x: 0.0 + y: 0.000621267645801 + z: 0.0 + w: 0.999999807013 + covariance: [20.6569374550032, -1.0222595519441305e-06, 0.0, 0.0, -0.00066709761786493, 0.0, -1.02225955194413e-06, 20.656934713115255, 0.0, 0.0, 0.00015028876544516723, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006670976178649299, 0.0001502887654451673, 0.0, 0.0, 19.99438031508344, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 252 + stamp: + secs: 1624875212 + nsecs: 840596914 + frame_id: "/world" +pose: + pose: + position: + x: 0.161758852826 + y: 0.0536246703924 + z: 0.0 + orientation: + x: 0.0 + y: 0.00100870163523 + z: 0.0 + w: 0.99999949126 + covariance: [20.648448702420296, -1.0002699106859015e-06, 0.0, 0.0, -0.0006668209592742035, 0.0, -1.0002699106859011e-06, 20.64844602267259, 0.0, 0.0, 0.00024818442760564244, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006668209592742033, 0.00024818442760564244, 0.0, 0.0, 19.996965668880804, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 253 + stamp: + secs: 1624875212 + nsecs: 920597076 + frame_id: "/world" +pose: + pose: + position: + x: 0.161755940648 + y: 0.0533227568686 + z: 0.0 + orientation: + x: 0.0 + y: 0.000953381316296 + z: 0.0 + w: 0.999999545532 + covariance: [20.642469655982424, -9.847425083080167e-07, 0.0, 0.0, -0.0006421708127048097, 0.0, -9.847425083080162e-07, 20.64246702869571, 0.0, 0.0, 0.00031895530683423114, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006421708127048094, 0.0003189553068342311, 0.0, 0.0, 19.998260907111515, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 254 + stamp: + secs: 1624875213 + nsecs: 623941 + frame_id: "/world" +pose: + pose: + position: + x: 0.161751777141 + y: 0.0530212290526 + z: 0.0 + orientation: + x: 0.0 + y: 0.000837590833752 + z: 0.0 + w: 0.999999649221 + covariance: [20.63647397129081, -9.697222822338338e-07, 0.0, 0.0, -0.0006029729959861455, 0.0, -9.697222822338333e-07, 20.63647139710939, 0.0, 0.0, 0.00034983574687772574, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006029729959861451, 0.00034983574687772574, 0.0, 0.0, 19.999347303770776, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 255 + stamp: + secs: 1624875213 + nsecs: 80640078 + frame_id: "/world" +pose: + pose: + position: + x: 0.161747656232 + y: 0.0527763105905 + z: 0.0 + orientation: + x: 0.0 + y: 0.0011344306991 + z: 0.0 + w: 0.999999356533 + covariance: [20.586689747348874, -8.875588111125368e-07, 0.0, 0.0, -0.0004859529206797198, 0.0, -8.875588111125365e-07, 20.586687401808568, 0.0, 0.0, 0.0003276657620349954, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004859529206797196, 0.0003276657620349954, 0.0, 0.0, 20.00129262322976, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 256 + stamp: + secs: 1624875213 + nsecs: 160621881 + frame_id: "/world" +pose: + pose: + position: + x: 0.161742075909 + y: 0.0524473886552 + z: 0.0 + orientation: + x: 0.0 + y: 0.00130157855144 + z: 0.0 + w: 0.999999152946 + covariance: [20.619169933053872, -9.279591538286531e-07, 0.0, 0.0, -0.0003852686020293624, 0.0, -9.279591538286529e-07, 20.61916749453015, 0.0, 0.0, 0.0003218792028106905, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00038526860202936226, 0.00032187920281069056, 0.0, 0.0, 20.000701039706907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 257 + stamp: + secs: 1624875213 + nsecs: 240650892 + frame_id: "/world" +pose: + pose: + position: + x: 0.161736493403 + y: 0.0521324514021 + z: 0.0 + orientation: + x: 0.0 + y: 0.0010009563412 + z: 0.0 + w: 0.999999499043 + covariance: [20.63405542345888, -9.402424523801291e-07, 0.0, 0.0, -0.0003062650195429578, 0.0, -9.402424523801291e-07, 20.634052963579546, 0.0, 0.0, 0.00029509970017957713, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00030626501954295765, 0.00029509970017957724, 0.0, 0.0, 20.001332607790257, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 258 + stamp: + secs: 1624875213 + nsecs: 320632935 + frame_id: "/world" +pose: + pose: + position: + x: 0.161731942674 + y: 0.0518730881755 + z: 0.0 + orientation: + x: 0.0 + y: 0.00102681778025 + z: 0.0 + w: 0.999999472822 + covariance: [20.59397084198086, -8.733191335974788e-07, 0.0, 0.0, -0.0003396274077230073, 0.0, -8.733191335974788e-07, 20.593968564439862, 0.0, 0.0, 0.0002460919083574306, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00033962740772300716, 0.0002460919083574307, 0.0, 0.0, 20.001707130589228, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 259 + stamp: + secs: 1624875213 + nsecs: 400671005 + frame_id: "/world" +pose: + pose: + position: + x: 0.161715084092 + y: 0.0515299025851 + z: 0.0 + orientation: + x: 0.0 + y: 0.00106514939196 + z: 0.0 + w: 0.999999432728 + covariance: [20.62284388843462, -9.143154077650458e-07, 0.0, 0.0, -0.0005003031446944228, 0.0, -9.143154077650462e-07, 20.622841530563264, 0.0, 0.0, 0.00039331779072221743, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005003031446944227, 0.0003933177907222176, 0.0, 0.0, 20.001724877854983, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 260 + stamp: + secs: 1624875213 + nsecs: 480642080 + frame_id: "/world" +pose: + pose: + position: + x: 0.161683565969 + y: 0.0511598507831 + z: 0.0 + orientation: + x: 0.0 + y: 0.000909642051876 + z: 0.0 + w: 0.999999586276 + covariance: [20.682225832388646, -9.963271420497e-07, 0.0, 0.0, -0.00031753759118988964, 0.0, -9.963271420497002e-07, 20.682223316509344, 0.0, 0.0, 0.000623411542805777, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003175375911898894, 0.0006234115428057769, 0.0, 0.0, 20.001424650525646, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 261 + stamp: + secs: 1624875213 + nsecs: 560708046 + frame_id: "/world" +pose: + pose: + position: + x: 0.161653070458 + y: 0.0508190346948 + z: 0.0 + orientation: + x: 0.0 + y: 0.000722106449865 + z: 0.0 + w: 0.999999739281 + covariance: [20.720890322343386, -1.0292022160935584e-06, 0.0, 0.0, 1.592121187500839e-05, 0.0, -1.0292022160935588e-06, 20.720887732128823, 0.0, 0.0, 0.0006840876957238006, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 1.592121187500855e-05, 0.0006840876957238004, 0.0, 0.0, 20.001214497012647, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 262 + stamp: + secs: 1624875213 + nsecs: 640685081 + frame_id: "/world" +pose: + pose: + position: + x: 0.16162266205 + y: 0.050479441103 + z: 0.0 + orientation: + x: 0.0 + y: 0.000930296455535 + z: 0.0 + w: 0.999999567274 + covariance: [20.751654723820394, -1.0478683765368218e-06, 0.0, 0.0, 0.00018277197446218616, 0.0, -1.047868376536822e-06, 20.75165207998453, 0.0, 0.0, 0.000663359615461486, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.00018277197446218635, 0.0006633596154614859, 0.0, 0.0, 20.002334105147366, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 263 + stamp: + secs: 1624875213 + nsecs: 720674992 + frame_id: "/world" +pose: + pose: + position: + x: 0.161592852215 + y: 0.0501467900562 + z: 0.0 + orientation: + x: 0.0 + y: 0.00108422367378 + z: 0.0 + w: 0.999999412229 + covariance: [20.770153592142115, -1.0489642752413406e-06, 0.0, 0.0, 0.00025080793027525654, 0.0, -1.0489642752413406e-06, 20.77015093502726, 0.0, 0.0, 0.0005998944360574351, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0002508079302752567, 0.0005998944360574351, 0.0, 0.0, 20.002028002866073, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 264 + stamp: + secs: 1624875213 + nsecs: 800682068 + frame_id: "/world" +pose: + pose: + position: + x: 0.161562872315 + y: 0.0498197911699 + z: 0.0 + orientation: + x: 0.0 + y: 0.00114536243357 + z: 0.0 + w: 0.999999344072 + covariance: [20.77956417800249, -1.037995932575425e-06, 0.0, 0.0, 0.0002606509342371993, 0.0, -1.0379959325754252e-06, 20.779561538635075, 0.0, 0.0, 0.0005315668304889258, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.00026065093423719945, 0.0005315668304889258, 0.0, 0.0, 20.002967065374335, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 265 + stamp: + secs: 1624875213 + nsecs: 880716085 + frame_id: "/world" +pose: + pose: + position: + x: 0.16153214513 + y: 0.0494920429299 + z: 0.0 + orientation: + x: 0.0 + y: 0.00195760177349 + z: 0.0 + w: 0.999998083896 + covariance: [20.786749146473156, -1.0249488287919102e-06, 0.0, 0.0, 0.00022310562218394293, 0.0, -1.0249488287919104e-06, 20.786746533426943, 0.0, 0.0, 0.00046782865883747367, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.000223105622183943, 0.00046782865883747356, 0.0, 0.0, 20.007843995442958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 266 + stamp: + secs: 1624875213 + nsecs: 960705042 + frame_id: "/world" +pose: + pose: + position: + x: 0.161500546724 + y: 0.0491624407426 + z: 0.0 + orientation: + x: 0.0 + y: 0.00224412577269 + z: 0.0 + w: 0.999997481947 + covariance: [20.793493786283165, -1.0119242689319505e-06, 0.0, 0.0, 0.00017114373365800234, 0.0, -1.011924268931951e-06, 20.793491201768695, 0.0, 0.0, 0.00041343799077500436, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.00017114373365800236, 0.0004134379907750042, 0.0, 0.0, 20.006257439376988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 267 + stamp: + secs: 1624875214 + nsecs: 40746927 + frame_id: "/world" +pose: + pose: + position: + x: 0.170558749759 + y: 0.0488838807287 + z: 0.0 + orientation: + x: 0.0 + y: 0.00151097075954 + z: 0.0 + w: 0.999998858483 + covariance: [20.740840102988617, -9.271734559476981e-07, 0.0, 0.0, 0.00012630146044822615, 0.0, -9.271734559476984e-07, 20.74083773194887, 0.0, 0.0, 0.00036270452853438346, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.00012630146044822615, 0.00036270452853438324, 0.0, 0.0, 20.00418922251163, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 268 + stamp: + secs: 1624875214 + nsecs: 120723009 + frame_id: "/world" +pose: + pose: + position: + x: 0.162768582574 + y: 0.0552152456575 + z: 0.0 + orientation: + x: 0.0 + y: 0.00232287605053 + z: 0.0 + w: 0.99999730212 + covariance: [20.737436091650157, -9.044045311653334e-07, 0.0, 0.0, 8.90451146030691e-05, 0.0, -9.044045311653337e-07, 20.737433776575756, 0.0, 0.0, 0.0003379795366924856, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 8.90451146030691e-05, 0.00033797953669248545, 0.0, 0.0, 20.00376096063892, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 269 + stamp: + secs: 1624875214 + nsecs: 200736046 + frame_id: "/world" +pose: + pose: + position: + x: 0.162800444322 + y: 0.0548295346662 + z: 0.0 + orientation: + x: 0.0 + y: 0.00191101854746 + z: 0.0 + w: 0.999998174002 + covariance: [20.698023634651317, -8.406444794470279e-07, 0.0, 0.0, 5.692005774215475e-05, 0.0, -8.406444794470283e-07, 20.698021481470928, 0.0, 0.0, 0.0002925977992148294, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 5.692005774215475e-05, 0.0002925977992148293, 0.0, 0.0, 20.004434571369686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 270 + stamp: + secs: 1624875214 + nsecs: 280742884 + frame_id: "/world" +pose: + pose: + position: + x: 0.162775848308 + y: 0.0545524826579 + z: 0.0 + orientation: + x: 0.0 + y: 0.00173090475688 + z: 0.0 + w: 0.999998501983 + covariance: [20.711298872372147, -8.416718018767334e-07, 0.0, 0.0, -7.991142563289961e-05, 0.0, -8.41671801876734e-07, 20.711296720280913, 0.0, 0.0, 0.0002662364149982742, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.99114256328996e-05, 0.000266236414998274, 0.0, 0.0, 20.003840126338233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 271 + stamp: + secs: 1624875214 + nsecs: 360743999 + frame_id: "/world" +pose: + pose: + position: + x: 0.162752347498 + y: 0.0542839216059 + z: 0.0 + orientation: + x: 0.0 + y: 0.00157063618485 + z: 0.0 + w: 0.99999876655 + covariance: [20.705737071388874, -8.225472084279941e-07, 0.0, 0.0, -0.00022869309170810162, 0.0, -8.225472084279946e-07, 20.7057349715286, 0.0, 0.0, 0.00023354223444010366, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00022869309170810162, 0.0002335422344401035, 0.0, 0.0, 20.004635311787222, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 272 + stamp: + secs: 1624875214 + nsecs: 440733910 + frame_id: "/world" +pose: + pose: + position: + x: 0.162728724549 + y: 0.0540213111932 + z: 0.0 + orientation: + x: 0.0 + y: 0.0019274199208 + z: 0.0 + w: 0.999998142524 + covariance: [20.687518847282984, -7.913050423702986e-07, 0.0, 0.0, -0.0004039840013347071, 0.0, -7.91305042370299e-07, 20.687516827659792, 0.0, 0.0, 0.00021871605049879545, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000403984001334707, 0.00021871605049879532, 0.0, 0.0, 20.007473285520295, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 273 + stamp: + secs: 1624875214 + nsecs: 520785093 + frame_id: "/world" +pose: + pose: + position: + x: 0.162704686541 + y: 0.0537602711117 + z: 0.0 + orientation: + x: 0.0 + y: 0.00154178187868 + z: 0.0 + w: 0.999998811454 + covariance: [20.66491007607508, -7.577694203742778e-07, 0.0, 0.0, -0.0005490171547236896, 0.0, -7.577694203742782e-07, 20.664908137725888, 0.0, 0.0, 0.00020904427680939992, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005490171547236894, 0.00020904427680939986, 0.0, 0.0, 20.003321126301664, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 274 + stamp: + secs: 1624875214 + nsecs: 600769043 + frame_id: "/world" +pose: + pose: + position: + x: 0.162679687954 + y: 0.0534980341522 + z: 0.0 + orientation: + x: 0.0 + y: 0.00046600185664 + z: 0.0 + w: 0.999999891421 + covariance: [20.647707116537024, -7.312971877712592e-07, 0.0, 0.0, -0.0006015375841126266, 0.0, -7.312971877712596e-07, 20.647705240192032, 0.0, 0.0, 0.00020436439829636628, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006015375841126264, 0.00020436439829636622, 0.0, 0.0, 20.001196108133147, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 275 + stamp: + secs: 1624875214 + nsecs: 680773020 + frame_id: "/world" +pose: + pose: + position: + x: 0.162653740416 + y: 0.05323508429 + z: 0.0 + orientation: + x: 0.0 + y: 4.54413180674e-05 + z: 0.0 + w: 0.999999998968 + covariance: [20.633933686151074, -7.0956090022862e-07, 0.0, 0.0, -0.0006037423883783023, 0.0, -7.095609002286201e-07, 20.633931860325248, 0.0, 0.0, 0.00019976939216298137, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006037423883783023, 0.00019976939216298135, 0.0, 0.0, 20.000751659457908, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 276 + stamp: + secs: 1624875214 + nsecs: 760782957 + frame_id: "/world" +pose: + pose: + position: + x: 0.16262772406 + y: 0.0529704696988 + z: 0.0 + orientation: + x: -0.0 + y: -1.11535060463e-05 + z: -0.0 + w: 0.999999999938 + covariance: [20.623724037120503, -6.917894069199149e-07, 0.0, 0.0, -0.000584307716895742, 0.0, -6.917894069199151e-07, 20.62372225084427, 0.0, 0.0, 0.00018357508041095047, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000584307716895742, 0.00018357508041095047, 0.0, 0.0, 20.000410657496108, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 277 + stamp: + secs: 1624875214 + nsecs: 840786934 + frame_id: "/world" +pose: + pose: + position: + x: 0.162601508849 + y: 0.0527052841034 + z: 0.0 + orientation: + x: -0.0 + y: -2.25210597149e-05 + z: -0.0 + w: 0.999999999746 + covariance: [20.615548534022757, -6.762402945985347e-07, 0.0, 0.0, -0.0005472522575175168, 0.0, -6.762402945985349e-07, 20.615546781855187, 0.0, 0.0, 0.0001657896527496541, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005472522575175169, 0.0001657896527496541, 0.0, 0.0, 20.00029193741773, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 278 + stamp: + secs: 1624875214 + nsecs: 920783043 + frame_id: "/world" +pose: + pose: + position: + x: 0.162576133978 + y: 0.0524507262866 + z: 0.0 + orientation: + x: 0.0 + y: 9.61466366046e-05 + z: 0.0 + w: 0.999999995378 + covariance: [20.599461886779764, -6.520194312784196e-07, 0.0, 0.0, -0.0004908327417855123, 0.0, -6.520194312784199e-07, 20.599460192470453, 0.0, 0.0, 0.00014646592893456417, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004908327417855126, 0.00014646592893456417, 0.0, 0.0, 20.0003362226886, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 279 + stamp: + secs: 1624875215 + nsecs: 812054 + frame_id: "/world" +pose: + pose: + position: + x: 0.162550582216 + y: 0.0522045884132 + z: 0.0 + orientation: + x: 0.0 + y: 0.000351035794111 + z: 0.0 + w: 0.999999938387 + covariance: [20.5793314737984, -6.240221326598273e-07, 0.0, 0.0, -0.0004287150676580984, 0.0, -6.240221326598276e-07, 20.579329849462052, 0.0, 0.0, 0.00014169052043131863, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00042871506765809856, 0.00014169052043131863, 0.0, 0.0, 20.00062436178246, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 280 + stamp: + secs: 1624875215 + nsecs: 80796003 + frame_id: "/world" +pose: + pose: + position: + x: 0.162513180984 + y: 0.0518581847377 + z: 0.0 + orientation: + x: 0.0 + y: 0.000147991373756 + z: 0.0 + w: 0.999999989049 + covariance: [20.655624784043354, -6.978142184183057e-07, 0.0, 0.0, -0.00040365880205901034, 0.0, -6.978142184183057e-07, 20.655622966017948, 0.0, 0.0, 0.00016305726861329606, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00040365880205901045, 0.000163057268613296, 0.0, 0.0, 20.000008362107646, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 281 + stamp: + secs: 1624875215 + nsecs: 160804033 + frame_id: "/world" +pose: + pose: + position: + x: 0.1624857888 + y: 0.0516043506176 + z: 0.0 + orientation: + x: -0.0 + y: -0.000191124759285 + z: -0.0 + w: 0.999999981736 + covariance: [20.630381194029106, -6.632394048144913e-07, 0.0, 0.0, -0.0003478979304880035, 0.0, -6.632394048144913e-07, 20.63037946473694, 0.0, 0.0, 0.00014746077366116747, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003478979304880036, 0.0001474607736611674, 0.0, 0.0, 20.000002915448064, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 282 + stamp: + secs: 1624875215 + nsecs: 240817070 + frame_id: "/world" +pose: + pose: + position: + x: 0.162452710047 + y: 0.0512971708706 + z: 0.0 + orientation: + x: -0.0 + y: -0.00020099560729 + z: -0.0 + w: 0.9999999798 + covariance: [20.659282152686703, -6.846602664046609e-07, 0.0, 0.0, -0.0003319449118813775, 0.0, -6.846602664046608e-07, 20.659280365805124, 0.0, 0.0, 0.00013718299564862162, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003319449118813776, 0.00013718299564862156, 0.0, 0.0, 20.000008738253356, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 283 + stamp: + secs: 1624875215 + nsecs: 320810080 + frame_id: "/world" +pose: + pose: + position: + x: 0.16239979279 + y: 0.0508003724103 + z: 0.0 + orientation: + x: -0.0 + y: -0.000208300109191 + z: -0.0 + w: 0.999999978306 + covariance: [20.8722155057111, -8.894922911066705e-07, 0.0, 0.0, -0.0004473297827182661, 0.0, -8.8949229110667e-07, 20.87221317923689, 0.0, 0.0, 0.000143531600829799, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00044732978271826634, 0.00014353160082979896, 0.0, 0.0, 20.000004723422208, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 284 + stamp: + secs: 1624875215 + nsecs: 400810003 + frame_id: "/world" +pose: + pose: + position: + x: 0.162371002879 + y: 0.0505432545842 + z: 0.0 + orientation: + x: 0.0 + y: 9.50800776774e-05 + z: 0.0 + w: 0.99999999548 + covariance: [20.792092011421158, -7.977816121172167e-07, 0.0, 0.0, -0.00044453227629551363, 0.0, -7.977816121172166e-07, 20.792089923220797, 0.0, 0.0, 0.00014406784163026995, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00044453227629551374, 0.00014406784163026993, 0.0, 0.0, 20.0000946405043, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 285 + stamp: + secs: 1624875215 + nsecs: 480870008 + frame_id: "/world" +pose: + pose: + position: + x: 0.162333972411 + y: 0.0502276930857 + z: 0.0 + orientation: + x: 0.0 + y: 0.000376395377119 + z: 0.0 + w: 0.999999929163 + covariance: [20.78263852954081, -7.80121643488602e-07, 0.0, 0.0, -0.0005341650736032816, 0.0, -7.801216434886016e-07, 20.782636486188736, 0.0, 0.0, 0.00017410089177941776, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005341650736032816, 0.00017410089177941778, 0.0, 0.0, 20.000114702344504, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 286 + stamp: + secs: 1624875215 + nsecs: 560842037 + frame_id: "/world" +pose: + pose: + position: + x: 0.16229926048 + y: 0.0499245335949 + z: 0.0 + orientation: + x: -0.0 + y: -0.000201333062044 + z: -0.0 + w: 0.999999979732 + covariance: [20.760572316875358, -7.501817804371899e-07, 0.0, 0.0, -0.0005647225844258641, 0.0, -7.501817804371896e-07, 20.760570347769306, 0.0, 0.0, 0.00016438312126154992, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005647225844258642, 0.00016438312126154997, 0.0, 0.0, 20.000266221317, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 287 + stamp: + secs: 1624875215 + nsecs: 640876055 + frame_id: "/world" +pose: + pose: + position: + x: 0.162264563691 + y: 0.0496225891964 + z: 0.0 + orientation: + x: -0.0 + y: -0.000422537504587 + z: -0.0 + w: 0.999999910731 + covariance: [20.74346059778606, -7.248706576023869e-07, 0.0, 0.0, -0.0005262227655620526, 0.0, -7.248706576023867e-07, 20.743458691970147, 0.0, 0.0, 0.00014954454785856813, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005262227655620526, 0.00014954454785856815, 0.0, 0.0, 20.00001468917229, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 288 + stamp: + secs: 1624875215 + nsecs: 720865965 + frame_id: "/world" +pose: + pose: + position: + x: 0.162227663496 + y: 0.0493254422883 + z: 0.0 + orientation: + x: -0.0 + y: -0.000255068823089 + z: -0.0 + w: 0.99999996747 + covariance: [20.723959026822413, -6.991669181966224e-07, 0.0, 0.0, -0.0004975397508841813, 0.0, -6.99166918196622e-07, 20.723957189679407, 0.0, 0.0, 0.00017575181672932046, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004975397508841812, 0.00017575181672932046, 0.0, 0.0, 20.000148639271465, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 289 + stamp: + secs: 1624875215 + nsecs: 800864935 + frame_id: "/world" +pose: + pose: + position: + x: 0.162185250681 + y: 0.0490246623393 + z: 0.0 + orientation: + x: -0.0 + y: -0.000297087705719 + z: -0.0 + w: 0.999999955869 + covariance: [20.70226500030047, -6.773216235269457e-07, 0.0, 0.0, -0.0006134545051771166, 0.0, -6.773216235269452e-07, 20.702263228411702, 0.0, 0.0, 0.0002701453432097663, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006134545051771164, 0.0002701453432097662, 0.0, 0.0, 20.000067559696028, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 290 + stamp: + secs: 1624875215 + nsecs: 880872965 + frame_id: "/world" +pose: + pose: + position: + x: 0.16213937813 + y: 0.0487195545205 + z: 0.0 + orientation: + x: -0.0 + y: -0.000211426890352 + z: -0.0 + w: 0.999999977649 + covariance: [20.68158973466781, -6.625242207778437e-07, 0.0, 0.0, -0.0007393502092005424, 0.0, -6.625242207778433e-07, 20.681588015964515, 0.0, 0.0, 0.00034889597252729177, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007393502092005421, 0.00034889597252729177, 0.0, 0.0, 20.00009607502927, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 291 + stamp: + secs: 1624875215 + nsecs: 960884094 + frame_id: "/world" +pose: + pose: + position: + x: 0.162092396441 + y: 0.0484138830175 + z: 0.0 + orientation: + x: 0.0 + y: 1.7755637071e-05 + z: 0.0 + w: 0.999999999842 + covariance: [20.66609459885883, -6.529462677754265e-07, 0.0, 0.0, -0.0007537188739933198, 0.0, -6.529462677754262e-07, 20.666092922459335, 0.0, 0.0, 0.00038046894739539365, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007537188739933196, 0.0003804689473953937, 0.0, 0.0, 20.00000926579327, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 292 + stamp: + secs: 1624875216 + nsecs: 40893078 + frame_id: "/world" +pose: + pose: + position: + x: 0.162041346744 + y: 0.0481082903742 + z: 0.0 + orientation: + x: 0.0 + y: 3.11490417932e-05 + z: 0.0 + w: 0.999999999515 + covariance: [20.654175868466083, -6.48318466614775e-07, 0.0, 0.0, -0.000707748557197155, 0.0, -6.483184666147745e-07, 20.654174231137212, 0.0, 0.0, 0.00043073265178365615, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007077485571971547, 0.0004307326517836562, 0.0, 0.0, 20.000054480089748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 293 + stamp: + secs: 1624875216 + nsecs: 120889902 + frame_id: "/world" +pose: + pose: + position: + x: 0.161977161381 + y: 0.0476562131705 + z: 0.0 + orientation: + x: -0.0 + y: -0.000262217718639 + z: -0.0 + w: 0.999999965621 + covariance: [20.6934352844889, -7.435558852719146e-07, 0.0, 0.0, -0.002102208331530969, 0.0, -7.435558852719143e-07, 20.69343330479393, 0.0, 0.0, 0.0004991909971219595, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0021022083315309684, 0.0004991909971219595, 0.0, 0.0, 20.000080297019384, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 294 + stamp: + secs: 1624875216 + nsecs: 200895071 + frame_id: "/world" +pose: + pose: + position: + x: 0.161925177121 + y: 0.0472178149548 + z: 0.0 + orientation: + x: -0.0 + y: -0.000238012407611 + z: -0.0 + w: 0.999999971675 + covariance: [20.666066993906185, -7.874965017448961e-07, 0.0, 0.0, -0.0033349000268777206, 0.0, -7.874965017448959e-07, 20.66606438759467, 0.0, 0.0, 0.00046299027781704247, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0033349000268777197, 0.0004629902778170424, 0.0, 0.0, 20.000078787139667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 295 + stamp: + secs: 1624875216 + nsecs: 280922890 + frame_id: "/world" +pose: + pose: + position: + x: 0.161873170743 + y: 0.0467792551784 + z: 0.0 + orientation: + x: -0.0 + y: -0.000337887815924 + z: -0.0 + w: 0.999999942916 + covariance: [20.64507051227236, -8.228421215798722e-07, 0.0, 0.0, -0.003720176479059717, 0.0, -8.228421215798717e-07, 20.645067204375298, 0.0, 0.0, 0.00041043437820396765, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0037201764790597155, 0.0004104343782039677, 0.0, 0.0, 20.000241921216286, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 296 + stamp: + secs: 1624875216 + nsecs: 360913038 + frame_id: "/world" +pose: + pose: + position: + x: 0.161823093775 + y: 0.0463582959346 + z: 0.0 + orientation: + x: -0.0 + y: -0.000400454526335 + z: -0.0 + w: 0.999999919818 + covariance: [20.620131805254047, -8.364727619926463e-07, 0.0, 0.0, -0.0035728698044949596, 0.0, -8.364727619926459e-07, 20.620127934260882, 0.0, 0.0, 0.00034947764450218643, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.003572869804494958, 0.00034947764450218654, 0.0, 0.0, 20.00021327421349, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 297 + stamp: + secs: 1624875216 + nsecs: 440912962 + frame_id: "/world" +pose: + pose: + position: + x: 0.161765041745 + y: 0.0458723005337 + z: 0.0 + orientation: + x: -0.0 + y: -0.000327457180896 + z: -0.0 + w: 0.999999946386 + covariance: [20.637981274357475, -9.058528939420069e-07, 0.0, 0.0, -0.003363758283619958, 0.0, -9.058528939420065e-07, 20.637976548019864, 0.0, 0.0, 0.0003041672665353959, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0033637582836199566, 0.00030416726653539596, 0.0, 0.0, 20.000188228015507, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 298 + stamp: + secs: 1624875216 + nsecs: 520924091 + frame_id: "/world" +pose: + pose: + position: + x: 0.161709907846 + y: 0.045420067695 + z: 0.0 + orientation: + x: -0.0 + y: -0.000378738599043 + z: -0.0 + w: 0.999999928279 + covariance: [20.63360499228382, -9.34583057204464e-07, 0.0, 0.0, -0.0029558905992569146, 0.0, -9.345830572044635e-07, 20.633599722156738, 0.0, 0.0, 0.00027250772382459554, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.002955890599256914, 0.00027250772382459554, 0.0, 0.0, 20.00034661057524, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 299 + stamp: + secs: 1624875216 + nsecs: 600929976 + frame_id: "/world" +pose: + pose: + position: + x: 0.161655703577 + y: 0.0449853287192 + z: 0.0 + orientation: + x: -0.0 + y: -0.000516233099285 + z: -0.0 + w: 0.999999866752 + covariance: [20.620579205158332, -9.438094389586085e-07, 0.0, 0.0, -0.0025119759251275584, 0.0, -9.438094389586082e-07, 20.620573586169243, 0.0, 0.0, 0.00025039803438461786, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0025119759251275575, 0.0002503980343846178, 0.0, 0.0, 20.000515084583263, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 300 + stamp: + secs: 1624875216 + nsecs: 680928946 + frame_id: "/world" +pose: + pose: + position: + x: 0.161589226998 + y: 0.0444567437742 + z: 0.0 + orientation: + x: -0.0 + y: -0.000500314847216 + z: -0.0 + w: 0.999999874843 + covariance: [20.661410675227515, -1.0369918610605247e-06, 0.0, 0.0, -0.002267233587483667, 0.0, -1.0369918610605243e-06, 20.661404185762716, 0.0, 0.0, 0.0002495313300150435, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0022672335874836655, 0.0002495313300150435, 0.0, 0.0, 20.000350348584234, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 301 + stamp: + secs: 1624875216 + nsecs: 760956049 + frame_id: "/world" +pose: + pose: + position: + x: 0.161518165693 + y: 0.0438972404468 + z: 0.0 + orientation: + x: -0.0 + y: -0.000122674273703 + z: -0.0 + w: 0.999999992476 + covariance: [20.710720801826188, -1.1401841317730176e-06, 0.0, 0.0, -0.002068194267513167, 0.0, -1.140184131773017e-06, 20.710713398288004, 0.0, 0.0, 0.0002527027299433043, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0020681942675131668, 0.00025270272994330425, 0.0, 0.0, 20.000026213405423, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 302 + stamp: + secs: 1624875216 + nsecs: 840961933 + frame_id: "/world" +pose: + pose: + position: + x: 0.161437967833 + y: 0.043265723339 + z: 0.0 + orientation: + x: 0.0 + y: 0.00039691636233 + z: 0.0 + w: 0.999999921229 + covariance: [20.793620475391354, -1.2930620102859225e-06, 0.0, 0.0, -0.001931225026227353, 0.0, -1.2930620102859221e-06, 20.793611828158905, 0.0, 0.0, 0.0002511965972886196, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0019312250262273523, 0.00025119659728861947, 0.0, 0.0, 20.000094105984928, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 303 + stamp: + secs: 1624875216 + nsecs: 920965910 + frame_id: "/world" +pose: + pose: + position: + x: 0.161372785408 + y: 0.0427491747583 + z: 0.0 + orientation: + x: 0.0 + y: 0.000551844271027 + z: 0.0 + w: 0.999999847734 + covariance: [20.786760524980693, -1.288560115057128e-06, 0.0, 0.0, -0.0017186086481936918, 0.0, -1.2885601150571278e-06, 20.786751748117744, 0.0, 0.0, 0.00022473908636468895, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0017186086481936907, 0.0002247390863646888, 0.0, 0.0, 20.000176253924103, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 304 + stamp: + secs: 1624875217 + nsecs: 967979 + frame_id: "/world" +pose: + pose: + position: + x: 0.161299969479 + y: 0.0421703256166 + z: 0.0 + orientation: + x: 0.0 + y: 0.000859000883043 + z: 0.0 + w: 0.999999631059 + covariance: [20.81587356909352, -1.3407616106388121e-06, 0.0, 0.0, -0.0016152316504781529, 0.0, -1.3407616106388117e-06, 20.815864287753975, 0.0, 0.0, 0.00020870192199873167, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001615231650478152, 0.0002087019219987315, 0.0, 0.0, 20.001048563978756, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 305 + stamp: + secs: 1624875217 + nsecs: 80965996 + frame_id: "/world" +pose: + pose: + position: + x: 0.161233757134 + y: 0.0416312213863 + z: 0.0 + orientation: + x: 0.0 + y: 0.0012177961037 + z: 0.0 + w: 0.999999258486 + covariance: [20.812647879248143, -1.3318993175990537e-06, 0.0, 0.0, -0.0014725188558029835, 0.0, -1.3318993175990533e-06, 20.81263853407752, 0.0, 0.0, 0.00016408005378911069, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014725188558029827, 0.00016408005378911058, 0.0, 0.0, 20.00184225836238, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 306 + stamp: + secs: 1624875217 + nsecs: 160985947 + frame_id: "/world" +pose: + pose: + position: + x: 0.161171433254 + y: 0.0411132939917 + z: 0.0 + orientation: + x: 0.0 + y: 0.00144615439909 + z: 0.0 + w: 0.999998954318 + covariance: [20.796180899505075, -1.2965083363311475e-06, 0.0, 0.0, -0.0013066262791414994, 0.0, -1.2965083363311475e-06, 20.796171694562247, 0.0, 0.0, 0.00011652521216749084, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013066262791414983, 0.00011652521216749076, 0.0, 0.0, 20.00313164065976, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 307 + stamp: + secs: 1624875217 + nsecs: 240998983 + frame_id: "/world" +pose: + pose: + position: + x: 0.161102842217 + y: 0.0405445804932 + z: 0.0 + orientation: + x: 0.0 + y: 0.00163795534348 + z: 0.0 + w: 0.99999865855 + covariance: [20.814121860000874, -1.314475792890502e-06, 0.0, 0.0, -0.0011550389132877637, 0.0, -1.314475792890502e-06, 20.81411243261102, 0.0, 0.0, 8.703164490934267e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011550389132877628, 8.703164490934262e-05, 0.0, 0.0, 20.00382570758238, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 308 + stamp: + secs: 1624875217 + nsecs: 320986986 + frame_id: "/world" +pose: + pose: + position: + x: 0.161015218096 + y: 0.0398179500227 + z: 0.0 + orientation: + x: 0.0 + y: 0.00152125108186 + z: 0.0 + w: 0.999998842897 + covariance: [20.928738482579853, -1.4811213077917854e-06, 0.0, 0.0, -0.0010654064058135924, 0.0, -1.4811213077917859e-06, 20.928727756312142, 0.0, 0.0, 6.846741154178876e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010654064058135918, 6.84674115417887e-05, 0.0, 0.0, 20.00315778935062, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 309 + stamp: + secs: 1624875217 + nsecs: 400985956 + frame_id: "/world" +pose: + pose: + position: + x: 0.160952506582 + y: 0.0393025526889 + z: 0.0 + orientation: + x: 0.0 + y: 0.00134697179697 + z: 0.0 + w: 0.999999092833 + covariance: [20.878942735889567, -1.3863600351930417e-06, 0.0, 0.0, -0.001044172636771669, 0.0, -1.3863600351930421e-06, 20.87893264231307, 0.0, 0.0, 8.259748097017452e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010441726367716683, 8.259748097017445e-05, 0.0, 0.0, 20.004019419190875, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 310 + stamp: + secs: 1624875217 + nsecs: 481002092 + frame_id: "/world" +pose: + pose: + position: + x: 0.160889291288 + y: 0.0387905955488 + z: 0.0 + orientation: + x: 0.0 + y: 0.00143228165645 + z: 0.0 + w: 0.999998974284 + covariance: [20.829164327163323, -1.2977278149738822e-06, 0.0, 0.0, -0.0011597888142339298, 0.0, -1.2977278149738827e-06, 20.829154839433034, 0.0, 0.0, 0.00012230066891430838, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001159788814233929, 0.00012230066891430832, 0.0, 0.0, 20.004377955714432, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 311 + stamp: + secs: 1624875217 + nsecs: 561059952 + frame_id: "/world" +pose: + pose: + position: + x: 0.160826219118 + y: 0.0382825908911 + z: 0.0 + orientation: + x: 0.0 + y: 0.00126155970792 + z: 0.0 + w: 0.999999204233 + covariance: [20.786246231546325, -1.222328763312125e-06, 0.0, 0.0, -0.0011833492220971584, 0.0, -1.2223287633121261e-06, 20.786237269317127, 0.0, 0.0, 0.00014448251588197953, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011833492220971577, 0.00014448251588197947, 0.0, 0.0, 20.003500334665404, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 312 + stamp: + secs: 1624875217 + nsecs: 641020060 + frame_id: "/world" +pose: + pose: + position: + x: 0.160755045723 + y: 0.0377087345275 + z: 0.0 + orientation: + x: 0.0 + y: 0.000961312508988 + z: 0.0 + w: 0.999999537939 + covariance: [20.789560801586738, -1.2183241571085118e-06, 0.0, 0.0, -0.0011698884435242748, 0.0, -1.218324157108513e-06, 20.78955184683916, 0.0, 0.0, 0.00015288235856695167, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011698884435242744, 0.00015288235856695162, 0.0, 0.0, 20.002622095090707, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 313 + stamp: + secs: 1624875217 + nsecs: 721040964 + frame_id: "/world" +pose: + pose: + position: + x: 0.160698298375 + y: 0.037256135634 + z: 0.0 + orientation: + x: 0.0 + y: 0.00127854651875 + z: 0.0 + w: 0.999999182659 + covariance: [20.725026976007904, -1.1101260990733781e-06, 0.0, 0.0, -0.0010214820436043933, 0.0, -1.1101260990733794e-06, 20.72501880969629, 0.0, 0.0, 0.00014784471484805684, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010214820436043927, 0.00014784471484805678, 0.0, 0.0, 20.00569077829132, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 314 + stamp: + secs: 1624875217 + nsecs: 801040888 + frame_id: "/world" +pose: + pose: + position: + x: 0.160628446104 + y: 0.0367142899433 + z: 0.0 + orientation: + x: 0.0 + y: 0.00175280179163 + z: 0.0 + w: 0.999998463842 + covariance: [20.722888487070183, -1.0994511145891653e-06, 0.0, 0.0, -0.0009294177232807143, 0.0, -1.0994511145891662e-06, 20.72288040853553, 0.0, 0.0, 0.00017255174770127764, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009294177232807137, 0.0001725517477012776, 0.0, 0.0, 20.005160255881986, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 315 + stamp: + secs: 1624875217 + nsecs: 881045103 + frame_id: "/world" +pose: + pose: + position: + x: 0.16055476771 + y: 0.0361753532931 + z: 0.0 + orientation: + x: 0.0 + y: 0.00140503407086 + z: 0.0 + w: 0.999999012939 + covariance: [20.719250348195565, -1.0887785396343112e-06, 0.0, 0.0, -0.0008383745574200791, 0.0, -1.0887785396343123e-06, 20.719242386309645, 0.0, 0.0, 0.00023923119252675996, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008383745574200783, 0.00023923119252675985, 0.0, 0.0, 20.00422707191085, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 316 + stamp: + secs: 1624875217 + nsecs: 961061954 + frame_id: "/world" +pose: + pose: + position: + x: 0.160480684924 + y: 0.0356554774549 + z: 0.0 + orientation: + x: 0.0 + y: 0.00104844152137 + z: 0.0 + w: 0.999999450385 + covariance: [20.706138008902055, -1.064253746663794e-06, 0.0, 0.0, -0.0007369635858607742, 0.0, -1.0642537466637948e-06, 20.706130276801186, 0.0, 0.0, 0.0002921303672285344, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007369635858607736, 0.0002921303672285344, 0.0, 0.0, 20.003110090469296, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 317 + stamp: + secs: 1624875218 + nsecs: 41070938 + frame_id: "/world" +pose: + pose: + position: + x: 0.160408003766 + y: 0.0351304617076 + z: 0.0 + orientation: + x: 0.0 + y: 0.00086260705066 + z: 0.0 + w: 0.999999627954 + covariance: [20.686380157907305, -1.0364064310717715e-06, 0.0, 0.0, -0.0010095401347704198, 0.0, -1.0364064310717717e-06, 20.6863726902438, 0.0, 0.0, 0.00031425700244328345, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010095401347704196, 0.0003142570024432834, 0.0, 0.0, 20.00286613104907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 318 + stamp: + secs: 1624875218 + nsecs: 121073961 + frame_id: "/world" +pose: + pose: + position: + x: 0.160323333661 + y: 0.0345002281587 + z: 0.0 + orientation: + x: 0.0 + y: 0.00107251951093 + z: 0.0 + w: 0.999999424851 + covariance: [20.709612935362706, -1.0851443724768963e-06, 0.0, 0.0, -0.001515309733415238, 0.0, -1.085144372476897e-06, 20.709605186088748, 0.0, 0.0, 0.0003369727563395624, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0015153097334152379, 0.00033697275633956243, 0.0, 0.0, 20.00389857262301, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 319 + stamp: + secs: 1624875218 + nsecs: 201076984 + frame_id: "/world" +pose: + pose: + position: + x: 0.160233121887 + y: 0.0338274455151 + z: 0.0 + orientation: + x: 0.0 + y: 0.00122854427494 + z: 0.0 + w: 0.999999245339 + covariance: [20.74932202236467, -1.156809149690564e-06, 0.0, 0.0, -0.001726490298478896, 0.0, -1.156809149690565e-06, 20.749313806530374, 0.0, 0.0, 0.00033232839472280084, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0017264902984788957, 0.0003323283947228009, 0.0, 0.0, 20.003662970173433, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 320 + stamp: + secs: 1624875218 + nsecs: 281084061 + frame_id: "/world" +pose: + pose: + position: + x: 0.16015779676 + y: 0.0332643992136 + z: 0.0 + orientation: + x: 0.0 + y: 0.00121013824097 + z: 0.0 + w: 0.999999267782 + covariance: [20.724854539061365, -1.1236651724048098e-06, 0.0, 0.0, -0.001676407226939717, 0.0, -1.1236651724048104e-06, 20.72484658127838, 0.0, 0.0, 0.0002972081286882326, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0016764072269397172, 0.0002972081286882327, 0.0, 0.0, 20.00424515216363, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 321 + stamp: + secs: 1624875218 + nsecs: 361097097 + frame_id: "/world" +pose: + pose: + position: + x: 0.160070496157 + y: 0.032614570537 + z: 0.0 + orientation: + x: 0.0 + y: 0.00118782664209 + z: 0.0 + w: 0.999999294534 + covariance: [20.74602677039448, -1.1628204421565693e-06, 0.0, 0.0, -0.0016606882137924009, 0.0, -1.1628204421565697e-06, 20.746018561162302, 0.0, 0.0, 0.0002831862007554831, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0016606882137924009, 0.0002831862007554832, 0.0, 0.0, 20.00334848641612, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 322 + stamp: + secs: 1624875218 + nsecs: 441102982 + frame_id: "/world" +pose: + pose: + position: + x: 0.159985390084 + y: 0.0319862596812 + z: 0.0 + orientation: + x: 0.0 + y: 0.000694074789324 + z: 0.0 + w: 0.99999975913 + covariance: [20.75296201327784, -1.1754256694349716e-06, 0.0, 0.0, -0.0015132420769921678, 0.0, -1.1754256694349722e-06, 20.752953741035746, 0.0, 0.0, 0.00026191887775933167, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001513242076992168, 0.0002619188777593318, 0.0, 0.0, 20.00157930857842, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 323 + stamp: + secs: 1624875218 + nsecs: 521126986 + frame_id: "/world" +pose: + pose: + position: + x: 0.159903253868 + y: 0.0313806543171 + z: 0.0 + orientation: + x: 0.0 + y: 0.000692116963689 + z: 0.0 + w: 0.999999760487 + covariance: [20.747697272223963, -1.164848388212891e-06, 0.0, 0.0, -0.0013136531363351057, 0.0, -1.1648483882128916e-06, 20.747689093954993, 0.0, 0.0, 0.00023071110545953957, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001313653136335106, 0.00023071110545953968, 0.0, 0.0, 20.002814842554187, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 324 + stamp: + secs: 1624875218 + nsecs: 601133108 + frame_id: "/world" +pose: + pose: + position: + x: 0.159822568621 + y: 0.0307811165327 + z: 0.0 + orientation: + x: 0.0 + y: 0.00084407470487 + z: 0.0 + w: 0.999999643769 + covariance: [20.73883052633613, -1.1465929771651227e-06, 0.0, 0.0, -0.0011809587447069927, 0.0, -1.146592977165123e-06, 20.738822486511594, 0.0, 0.0, 0.0001966970300161578, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011809587447069929, 0.00019669703001615784, 0.0, 0.0, 20.002000277593957, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 325 + stamp: + secs: 1624875218 + nsecs: 681112051 + frame_id: "/world" +pose: + pose: + position: + x: 0.159729504564 + y: 0.030083818306 + z: 0.0 + orientation: + x: 0.0 + y: 0.00104192787977 + z: 0.0 + w: 0.999999457193 + covariance: [20.777851371440995, -1.2006527253794518e-06, 0.0, 0.0, -0.0011433726216507388, 0.0, -1.200652725379452e-06, 20.777842956220812, 0.0, 0.0, 0.00017113989880327836, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011433726216507386, 0.0001711398988032784, 0.0, 0.0, 20.003873520074052, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 326 + stamp: + secs: 1624875218 + nsecs: 761149883 + frame_id: "/world" +pose: + pose: + position: + x: 0.159642760115 + y: 0.029432128097 + z: 0.0 + orientation: + x: 0.0 + y: 0.00146039736415 + z: 0.0 + w: 0.999998933619 + covariance: [20.78683588222587, -1.204151443054739e-06, 0.0, 0.0, -0.00102504227980001, 0.0, -1.2041514430547397e-06, 20.78682744299188, 0.0, 0.0, 0.00014088033482266283, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010250422798000096, 0.00014088033482266291, 0.0, 0.0, 20.00496455729258, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 327 + stamp: + secs: 1624875218 + nsecs: 841130018 + frame_id: "/world" +pose: + pose: + position: + x: 0.159568576009 + y: 0.028880526286 + z: 0.0 + orientation: + x: 0.0 + y: 0.00159445018171 + z: 0.0 + w: 0.999998728864 + covariance: [20.74394501373731, -1.1284570298426436e-06, 0.0, 0.0, -0.0008795736629207193, 0.0, -1.1284570298426442e-06, 20.74393711040587, 0.0, 0.0, 0.00012779959821879555, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008795736629207192, 0.0001277995982187956, 0.0, 0.0, 20.00609191488779, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 328 + stamp: + secs: 1624875218 + nsecs: 921133995 + frame_id: "/world" +pose: + pose: + position: + x: 0.159486047397 + y: 0.0282879709869 + z: 0.0 + orientation: + x: 0.0 + y: 0.0011070091597 + z: 0.0 + w: 0.999999387265 + covariance: [20.72877798027312, -1.0976991615576195e-06, 0.0, 0.0, -0.0008128279126514634, 0.0, -1.09769916155762e-06, 20.72877031479769, 0.0, 0.0, 0.0001641843354055838, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008128279126514633, 0.00016418433540558387, 0.0, 0.0, 20.00225626089652, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 329 + stamp: + secs: 1624875219 + nsecs: 1161098 + frame_id: "/world" +pose: + pose: + position: + x: 0.159407480512 + y: 0.0277387301592 + z: 0.0 + orientation: + x: 0.0 + y: 0.000201645548053 + z: 0.0 + w: 0.99999997967 + covariance: [20.697013971063765, -1.0423259501065553e-06, 0.0, 0.0, -0.0006928016480857839, 0.0, -1.0423259501065558e-06, 20.697006720310092, 0.0, 0.0, 0.00019157545748115605, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006928016480857837, 0.00019157545748115607, 0.0, 0.0, 20.000714278853486, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 330 + stamp: + secs: 1624875219 + nsecs: 81146002 + frame_id: "/world" +pose: + pose: + position: + x: 0.15931751802 + y: 0.0271081921155 + z: 0.0 + orientation: + x: -0.0 + y: -3.19963751269e-05 + z: -0.0 + w: 0.999999999488 + covariance: [20.70912994276209, -1.0516850804438625e-06, 0.0, 0.0, -0.0006663974001348203, 0.0, -1.0516850804438627e-06, 20.709122655279046, 0.0, 0.0, 0.00020539324838565078, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00066639740013482, 0.0002053932483856508, 0.0, 0.0, 20.000674253949907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 331 + stamp: + secs: 1624875219 + nsecs: 161160946 + frame_id: "/world" +pose: + pose: + position: + x: 0.159238043091 + y: 0.0265468518856 + z: 0.0 + orientation: + x: 0.0 + y: 4.19835723275e-05 + z: 0.0 + w: 0.999999999119 + covariance: [20.684306085510467, -1.0062857211648324e-06, 0.0, 0.0, -0.0006534008204134142, 0.0, -1.0062857211648326e-06, 20.684299133686405, 0.0, 0.0, 0.00019137826332782208, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006534008204134141, 0.00019137826332782214, 0.0, 0.0, 20.000401618304906, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 332 + stamp: + secs: 1624875219 + nsecs: 241172075 + frame_id: "/world" +pose: + pose: + position: + x: 0.159158799218 + y: 0.0259870437568 + z: 0.0 + orientation: + x: -0.0 + y: -1.2688915543e-05 + z: -0.0 + w: 0.999999999919 + covariance: [20.664823548040584, -9.68114362951079e-07, 0.0, 0.0, -0.0005931138572846118, 0.0, -9.681143629510792e-07, 20.664816877178165, 0.0, 0.0, 0.000169276288089482, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005931138572846116, 0.0001692762880894821, 0.0, 0.0, 20.00021636370725, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 333 + stamp: + secs: 1624875219 + nsecs: 321162939 + frame_id: "/world" +pose: + pose: + position: + x: 0.159081850202 + y: 0.0254437814398 + z: 0.0 + orientation: + x: -0.0 + y: -3.41676105308e-05 + z: -0.0 + w: 0.999999999416 + covariance: [20.642767897781685, -9.259964080798568e-07, 0.0, 0.0, -0.0005106631486115327, 0.0, -9.259964080798567e-07, 20.642761531097815, 0.0, 0.0, 0.00014449891291986377, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005106631486115325, 0.00014449891291986385, 0.0, 0.0, 20.000134886318946, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 334 + stamp: + secs: 1624875219 + nsecs: 401169062 + frame_id: "/world" +pose: + pose: + position: + x: 0.158998408177 + y: 0.02485492262 + z: 0.0 + orientation: + x: -0.0 + y: -0.000140220022997 + z: -0.0 + w: 0.999999990169 + covariance: [20.64646094314798, -9.200886431600163e-07, 0.0, 0.0, -0.0004360912451261229, 0.0, -9.200886431600163e-07, 20.64645463023765, 0.0, 0.0, 0.0001236654566403315, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00043609124512612285, 0.00012366545664033155, 0.0, 0.0, 20.000010354793005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 335 + stamp: + secs: 1624875219 + nsecs: 481182098 + frame_id: "/world" +pose: + pose: + position: + x: 0.158914037921 + y: 0.0242696028623 + z: 0.0 + orientation: + x: -0.0 + y: -0.000431802356032 + z: -0.0 + w: 0.999999906773 + covariance: [20.64557144850664, -9.087735112175001e-07, 0.0, 0.0, -0.000465759311318279, 0.0, -9.087735112175002e-07, 20.645565231523555, 0.0, 0.0, 0.00014053659256826718, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00046575931131827904, 0.00014053659256826724, 0.0, 0.0, 20.000116442231338, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 336 + stamp: + secs: 1624875219 + nsecs: 561197996 + frame_id: "/world" +pose: + pose: + position: + x: 0.158829077004 + y: 0.023688989642 + z: 0.0 + orientation: + x: -0.0 + y: -0.000492899518145 + z: -0.0 + w: 0.999999878525 + covariance: [20.640195865548186, -8.929546264666004e-07, 0.0, 0.0, -0.0005354138648876461, 0.0, -8.929546264666006e-07, 20.640189781516693, 0.0, 0.0, 0.00016683189389081086, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005354138648876464, 0.0001668318938908109, 0.0, 0.0, 20.00007964601121, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 337 + stamp: + secs: 1624875219 + nsecs: 641206980 + frame_id: "/world" +pose: + pose: + position: + x: 0.158730609156 + y: 0.0230136647507 + z: 0.0 + orientation: + x: -0.0 + y: -0.00011817234789 + z: -0.0 + w: 0.999999993018 + covariance: [20.677061074724083, -9.350102620462788e-07, 0.0, 0.0, -0.0006190010889725351, 0.0, -9.350102620462788e-07, 20.677054731978007, 0.0, 0.0, 0.00018072870218925174, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006190010889725354, 0.00018072870218925182, 0.0, 0.0, 20.000010709539012, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 338 + stamp: + secs: 1624875219 + nsecs: 721213102 + frame_id: "/world" +pose: + pose: + position: + x: 0.158642473572 + y: 0.0224088032312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0002304152562 + z: 0.0 + w: 0.999999973454 + covariance: [20.674389784633966, -9.218828630758284e-07, 0.0, 0.0, -0.0006263674049085682, 0.0, -9.21882863075828e-07, 20.67438355255239, 0.0, 0.0, 0.00017288419011543259, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006263674049085685, 0.00017288419011543264, 0.0, 0.0, 20.000102331367543, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 339 + stamp: + secs: 1624875219 + nsecs: 801218987 + frame_id: "/world" +pose: + pose: + position: + x: 0.158540106021 + y: 0.0216979472807 + z: 0.0 + orientation: + x: 0.0 + y: 0.000392890325398 + z: 0.0 + w: 0.999999922819 + covariance: [20.716452877552996, -9.695754592957687e-07, 0.0, 0.0, -0.0007771007728684881, 0.0, -9.69575459295768e-07, 20.716446343791574, 0.0, 0.0, 0.00016746476728885431, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007771007728684886, 0.00016746476728885437, 0.0, 0.0, 20.000178817286134, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 340 + stamp: + secs: 1624875219 + nsecs: 881227016 + frame_id: "/world" +pose: + pose: + position: + x: 0.158457402751 + y: 0.0211106401788 + z: 0.0 + orientation: + x: 0.0 + y: 0.000371266465157 + z: 0.0 + w: 0.999999931081 + covariance: [20.690889105557766, -9.255276468604348e-07, 0.0, 0.0, -0.0008466977631648873, 0.0, -9.255276468604343e-07, 20.69088287020955, 0.0, 0.0, 0.00013298156817582508, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008466977631648876, 0.00013298156817582513, 0.0, 0.0, 20.000193085397452, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 341 + stamp: + secs: 1624875219 + nsecs: 961224079 + frame_id: "/world" +pose: + pose: + position: + x: 0.158377015199 + y: 0.0205344221653 + z: 0.0 + orientation: + x: 0.0 + y: 0.000354582113743 + z: 0.0 + w: 0.999999937136 + covariance: [20.666245349481443, -8.820944368038619e-07, 0.0, 0.0, -0.0008229220059457846, 0.0, -8.820944368038614e-07, 20.6662393999664, 0.0, 0.0, 9.68176936895264e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008229220059457849, 9.681769368952647e-05, 0.0, 0.0, 20.000233821687193, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 342 + stamp: + secs: 1624875220 + nsecs: 41249990 + frame_id: "/world" +pose: + pose: + position: + x: 0.158286563037 + y: 0.0198836489785 + z: 0.0 + orientation: + x: 0.0 + y: 9.34231867796e-05 + z: 0.0 + w: 0.999999995636 + covariance: [20.679590298468078, -8.876512830117536e-07, 0.0, 0.0, -0.0007872103670395861, 0.0, -8.876512830117529e-07, 20.679584300390953, 0.0, 0.0, 6.82818234724861e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007872103670395863, 6.828182347248616e-05, 0.0, 0.0, 20.000001517768425, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 343 + stamp: + secs: 1624875220 + nsecs: 121259928 + frame_id: "/world" +pose: + pose: + position: + x: 0.158201735523 + y: 0.0192698417793 + z: 0.0 + orientation: + x: -0.0 + y: -8.67815036117e-05 + z: -0.0 + w: 0.999999996234 + covariance: [20.674123096225347, -8.680226478955426e-07, 0.0, 0.0, -0.0007141991627531076, 0.0, -8.680226478955418e-07, 20.67411721809753, 0.0, 0.0, 4.3674189327397966e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0007141991627531078, 4.3674189327398007e-05, 0.0, 0.0, 20.000012345277252, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 344 + stamp: + secs: 1624875220 + nsecs: 201256037 + frame_id: "/world" +pose: + pose: + position: + x: 0.158118568123 + y: 0.0186712738534 + z: 0.0 + orientation: + x: 0.0 + y: 6.92966244807e-06 + z: 0.0 + w: 0.999999999976 + covariance: [20.66356270492448, -8.423439901455779e-07, 0.0, 0.0, -0.0006201714037730119, 0.0, -8.423439901455771e-07, 20.663556991705562, 0.0, 0.0, 3.56762868802865e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000620171403773012, 3.567628688028653e-05, 0.0, 0.0, 20.00001137548716, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 345 + stamp: + secs: 1624875220 + nsecs: 281263113 + frame_id: "/world" +pose: + pose: + position: + x: 0.158037940994 + y: 0.0180974096796 + z: 0.0 + orientation: + x: 0.0 + y: 0.000120282078338 + z: 0.0 + w: 0.999999992766 + covariance: [20.64537008431764, -8.079539051450016e-07, 0.0, 0.0, -0.0005122109993957025, 0.0, -8.079539051450008e-07, 20.6453646003625, 0.0, 0.0, 3.982857476695675e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005122109993957024, 3.9828574766956766e-05, 0.0, 0.0, 20.0000740828428, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 346 + stamp: + secs: 1624875220 + nsecs: 361269951 + frame_id: "/world" +pose: + pose: + position: + x: 0.157956875925 + y: 0.0175237187798 + z: 0.0 + orientation: + x: 0.0 + y: 0.000332305762711 + z: 0.0 + w: 0.999999944786 + covariance: [20.631492301500664, -7.794773770391211e-07, 0.0, 0.0, -0.0004198152170008819, 0.0, -7.794773770391204e-07, 20.63148700798186, 0.0, 0.0, 4.560322238032232e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004198152170008819, 4.560322238032233e-05, 0.0, 0.0, 20.00023976751301, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 347 + stamp: + secs: 1624875220 + nsecs: 441265106 + frame_id: "/world" +pose: + pose: + position: + x: 0.157864967584 + y: 0.016875128191 + z: 0.0 + orientation: + x: 0.0 + y: 0.000288295607662 + z: 0.0 + w: 0.999999958443 + covariance: [20.65327016064443, -7.937620558138228e-07, 0.0, 0.0, -0.00035155587946716476, 0.0, -7.937620558138223e-07, 20.65326476713428, 0.0, 0.0, 5.076951270163633e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00035155587946716465, 5.076951270163632e-05, 0.0, 0.0, 20.000058106548472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 348 + stamp: + secs: 1624875220 + nsecs: 521287918 + frame_id: "/world" +pose: + pose: + position: + x: 0.157780752348 + y: 0.0162883899357 + z: 0.0 + orientation: + x: 0.0 + y: 0.000130664251933 + z: 0.0 + w: 0.999999991463 + covariance: [20.64400431197302, -7.71012002913126e-07, 0.0, 0.0, -0.00029931642497672795, 0.0, -7.710120029131257e-07, 20.643999072839097, 0.0, 0.0, 6.708122907235103e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00029931642497672795, 6.708122907235103e-05, 0.0, 0.0, 20.000064697848902, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 349 + stamp: + secs: 1624875220 + nsecs: 601290941 + frame_id: "/world" +pose: + pose: + position: + x: 0.157695472699 + y: 0.0157026727967 + z: 0.0 + orientation: + x: 0.0 + y: 0.000124284748211 + z: 0.0 + w: 0.999999992277 + covariance: [20.635832915480975, -7.503781095543408e-07, 0.0, 0.0, -0.0002812484944818067, 0.0, -7.503781095543405e-07, 20.63582781926224, 0.0, 0.0, 9.08308712418437e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002812484944818067, 9.083087124184368e-05, 0.0, 0.0, 20.000049870158083, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 350 + stamp: + secs: 1624875220 + nsecs: 681297064 + frame_id: "/world" +pose: + pose: + position: + x: 0.157610897575 + y: 0.0151183075518 + z: 0.0 + orientation: + x: -0.0 + y: -0.000103574171362 + z: -0.0 + w: 0.999999994636 + covariance: [20.62880361145428, -7.312416297365344e-07, 0.0, 0.0, -0.00026611504156141813, 0.0, -7.312416297365341e-07, 20.628798646999705, 0.0, 0.0, 8.94791483784892e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000266115041561418, 8.947914837848918e-05, 0.0, 0.0, 20.00002566680076, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 351 + stamp: + secs: 1624875220 + nsecs: 761307001 + frame_id: "/world" +pose: + pose: + position: + x: 0.157510952166 + y: 0.0144218091725 + z: 0.0 + orientation: + x: -0.0 + y: -0.000315629593933 + z: -0.0 + w: 0.999999950189 + covariance: [20.67234090758031, -7.685699078611064e-07, 0.0, 0.0, -0.00025511968045125545, 0.0, -7.685699078611063e-07, 20.672335690213384, 0.0, 0.0, 7.63556024276154e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002551196804512553, 7.63556024276154e-05, 0.0, 0.0, 20.000031242321462, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 352 + stamp: + secs: 1624875220 + nsecs: 841311932 + frame_id: "/world" +pose: + pose: + position: + x: 0.157412786419 + y: 0.0137497398706 + z: 0.0 + orientation: + x: -0.0 + y: -0.000366922107257 + z: -0.0 + w: 0.999999932684 + covariance: [20.697586348372432, -7.842454372681197e-07, 0.0, 0.0, -0.00022865384878145904, 0.0, -7.842454372681198e-07, 20.69758102830709, 0.0, 0.0, 9.117765038805897e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00022865384878145893, 9.117765038805896e-05, 0.0, 0.0, 20.000168375393216, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 353 + stamp: + secs: 1624875220 + nsecs: 921345949 + frame_id: "/world" +pose: + pose: + position: + x: 0.157312734259 + y: 0.0130794183741 + z: 0.0 + orientation: + x: -0.0 + y: -0.000222509740346 + z: -0.0 + w: 0.999999975245 + covariance: [20.717696574682265, -7.936712137673721e-07, 0.0, 0.0, -0.00020000306043691516, 0.0, -7.93671213767372e-07, 20.71769119735294, 0.0, 0.0, 0.00012082631834484578, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002000030604369151, 0.00012082631834484579, 0.0, 0.0, 20.000004275616025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 354 + stamp: + secs: 1624875221 + nsecs: 1331091 + frame_id: "/world" +pose: + pose: + position: + x: 0.157220975465 + y: 0.0124608833195 + z: 0.0 + orientation: + x: -0.0 + y: -0.000514565632236 + z: -0.0 + w: 0.999999867611 + covariance: [20.70406906063207, -7.687399930678391e-07, 0.0, 0.0, -0.00039294076471866593, 0.0, -7.68739993067839e-07, 20.70406386537806, 0.0, 0.0, 0.0001455178844543886, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0003929407647186658, 0.0001455178844543886, 0.0, 0.0, 20.000943620837123, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 355 + stamp: + secs: 1624875221 + nsecs: 81346989 + frame_id: "/world" +pose: + pose: + position: + x: 0.157130524389 + y: 0.0118453082363 + z: 0.0 + orientation: + x: -0.0 + y: -0.000781661750156 + z: -0.0 + w: 0.999999694502 + covariance: [20.686444727464927, -7.431218438516786e-07, 0.0, 0.0, -0.0006312840377254331, 0.0, -7.431218438516784e-07, 20.68643972014311, 0.0, 0.0, 0.0001604217934042721, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000631284037725433, 0.0001604217934042721, 0.0, 0.0, 20.000525259080327, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 356 + stamp: + secs: 1624875221 + nsecs: 161340952 + frame_id: "/world" +pose: + pose: + position: + x: 0.157033686213 + y: 0.011189347781 + z: 0.0 + orientation: + x: -0.0 + y: -0.000587503199851 + z: -0.0 + w: 0.99999982742 + covariance: [20.69199330212741, -7.417664892758093e-07, 0.0, 0.0, -0.0006676475630537228, 0.0, -7.417664892758092e-07, 20.691988315483982, 0.0, 0.0, 0.00015822059699860993, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006676475630537227, 0.00015822059699860993, 0.0, 0.0, 20.00071915841384, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 357 + stamp: + secs: 1624875221 + nsecs: 241344929 + frame_id: "/world" +pose: + pose: + position: + x: 0.156941784066 + y: 0.0105691583828 + z: 0.0 + orientation: + x: -0.0 + y: -0.000702243842733 + z: -0.0 + w: 0.999999753427 + covariance: [20.6827691290608, -7.236245844501952e-07, 0.0, 0.0, -0.0005875357544164717, 0.0, -7.236245844501952e-07, 20.682764272904127, 0.0, 0.0, 0.00014154215511640118, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005875357544164717, 0.00014154215511640118, 0.0, 0.0, 20.001101270887286, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 358 + stamp: + secs: 1624875221 + nsecs: 321346045 + frame_id: "/world" +pose: + pose: + position: + x: 0.156812913657 + y: 0.00970012018088 + z: 0.0 + orientation: + x: -0.0 + y: -0.000870504836538 + z: -0.0 + w: 0.999999621111 + covariance: [20.785949593207626, -8.223856530849749e-07, 0.0, 0.0, -0.0006298286678019106, 0.0, -8.22385653084975e-07, 20.7859440875989, 0.0, 0.0, 0.00014833723622981017, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006298286678019104, 0.00014833723622981017, 0.0, 0.0, 20.00126823950756, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 359 + stamp: + secs: 1624875221 + nsecs: 401350975 + frame_id: "/world" +pose: + pose: + position: + x: 0.156708247943 + y: 0.00899615966393 + z: 0.0 + orientation: + x: -0.0 + y: -0.000639067240015 + z: -0.0 + w: 0.999999795797 + covariance: [20.79308304143612, -8.197435263718387e-07, 0.0, 0.0, -0.0006071034505387081, 0.0, -8.197435263718386e-07, 20.793077563577214, 0.0, 0.0, 0.00014164210732928563, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.000607103450538708, 0.0001416421073292856, 0.0, 0.0, 20.00077181167342, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 360 + stamp: + secs: 1624875221 + nsecs: 481375933 + frame_id: "/world" +pose: + pose: + position: + x: 0.156606852116 + y: 0.00831365695085 + z: 0.0 + orientation: + x: -0.0 + y: -0.000368392165123 + z: -0.0 + w: 0.999999932144 + covariance: [20.788648196892574, -8.047259991277277e-07, 0.0, 0.0, -0.0005628467487807399, 0.0, -8.047259991277278e-07, 20.78864282714855, 0.0, 0.0, 0.00012756676180324968, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005628467487807399, 0.00012756676180324965, 0.0, 0.0, 20.00048633719131, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 361 + stamp: + secs: 1624875221 + nsecs: 561382055 + frame_id: "/world" +pose: + pose: + position: + x: 0.156505631211 + y: 0.00763090237581 + z: 0.0 + orientation: + x: 0.0 + y: 3.76663577197e-07 + z: 0.0 + w: 1.0 + covariance: [20.785014845639214, -7.902449258913504e-07, 0.0, 0.0, -0.0005168679646392098, 0.0, -7.902449258913505e-07, 20.785009578384564, 0.0, 0.0, 0.00011090488301874536, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005168679646392099, 0.00011090488301874536, 0.0, 0.0, 20.000029109967233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 362 + stamp: + secs: 1624875221 + nsecs: 641385078 + frame_id: "/world" +pose: + pose: + position: + x: 0.156404221332 + y: 0.00694810796535 + z: 0.0 + orientation: + x: 0.0 + y: 0.000567989151188 + z: 0.0 + w: 0.999999838694 + covariance: [20.782496876425242, -7.765920750304485e-07, 0.0, 0.0, -0.0004576699532627549, 0.0, -7.765920750304486e-07, 20.782491705646073, 0.0, 0.0, 9.78034417669808e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004576699532627548, 9.78034417669808e-05, 0.0, 0.0, 20.0002537684525, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 363 + stamp: + secs: 1624875221 + nsecs: 721393108 + frame_id: "/world" +pose: + pose: + position: + x: 0.156304272691 + y: 0.00627493628491 + z: 0.0 + orientation: + x: 0.0 + y: 0.00070712908035 + z: 0.0 + w: 0.999999749984 + covariance: [20.77541196180996, -7.585847053693611e-07, 0.0, 0.0, -0.0004300515632179827, 0.0, -7.585847053693612e-07, 20.775406915674132, 0.0, 0.0, 8.808388281717999e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00043005156321798254, 8.808388281717997e-05, 0.0, 0.0, 20.000197288925296, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 364 + stamp: + secs: 1624875221 + nsecs: 801403046 + frame_id: "/world" +pose: + pose: + position: + x: 0.156204693007 + y: 0.00560042917158 + z: 0.0 + orientation: + x: 0.0 + y: 0.000562982231156 + z: 0.0 + w: 0.999999841525 + covariance: [20.768760253936545, -7.412842191639886e-07, 0.0, 0.0, -0.00045002268087878847, 0.0, -7.412842191639886e-07, 20.768755325488893, 0.0, 0.0, 7.819898181687303e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00045002268087878836, 7.819898181687303e-05, 0.0, 0.0, 20.000318684065782, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 365 + stamp: + secs: 1624875221 + nsecs: 881417036 + frame_id: "/world" +pose: + pose: + position: + x: 0.156105209026 + y: 0.00492461612516 + z: 0.0 + orientation: + x: 0.0 + y: 0.00101973452154 + z: 0.0 + w: 0.999999480071 + covariance: [20.763317620897624, -7.252099907867666e-07, 0.0, 0.0, -0.0004634602304694881, 0.0, -7.252099907867667e-07, 20.76331280019966, 0.0, 0.0, 6.941029509982594e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00046346023046948796, 6.941029509982594e-05, 0.0, 0.0, 20.002181026498853, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 366 + stamp: + secs: 1624875221 + nsecs: 961416006 + frame_id: "/world" +pose: + pose: + position: + x: 0.155997996004 + y: 0.00420380853379 + z: 0.0 + orientation: + x: 0.0 + y: 0.00170453904585 + z: 0.0 + w: 0.999998547272 + covariance: [20.779136691253917, -7.292335680977055e-07, 0.0, 0.0, -0.0004722969962248904, 0.0, -7.292335680977056e-07, 20.779131847958865, 0.0, 0.0, 8.274038836261275e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00047229699622489016, 8.274038836261276e-05, 0.0, 0.0, 20.004280668222925, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 367 + stamp: + secs: 1624875222 + nsecs: 41435957 + frame_id: "/world" +pose: + pose: + position: + x: 0.155887572021 + y: 0.00352671991045 + z: 0.0 + orientation: + x: 0.0 + y: 0.0019597039688 + z: 0.0 + w: 0.999998079778 + covariance: [20.736983859227166, -7.239596510597972e-07, 0.0, 0.0, -0.0016707642807338512, 0.0, -7.239596510597971e-07, 20.73697918482915, 0.0, 0.0, 0.0004194086583789156, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0016707642807338517, 0.00041940865837891564, 0.0, 0.0, 20.00685488900616, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 368 + stamp: + secs: 1624875222 + nsecs: 121417046 + frame_id: "/world" +pose: + pose: + position: + x: 0.155763676333 + y: 0.00281863913944 + z: 0.0 + orientation: + x: 0.0 + y: 0.00218378517839 + z: 0.0 + w: 0.999997615538 + covariance: [20.688364639298523, -8.441800392786103e-07, 0.0, 0.0, -0.003034292232710272, 0.0, -8.441800392786103e-07, 20.68835972669695, 0.0, 0.0, 0.000813249797039473, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0030342922327102734, 0.0008132497970394732, 0.0, 0.0, 20.00914191313448, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 369 + stamp: + secs: 1624875222 + nsecs: 201446056 + frame_id: "/world" +pose: + pose: + position: + x: 0.155613734554 + y: 0.00197001207375 + z: 0.0 + orientation: + x: 0.0 + y: 0.00216156332752 + z: 0.0 + w: 0.999997663819 + covariance: [20.698377065120418, -1.1342650064572843e-06, 0.0, 0.0, -0.0038289259583123548, 0.0, -1.1342650064572845e-06, 20.698371202024788, 0.0, 0.0, 0.001064511673807411, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0038289259583123556, 0.001064511673807411, 0.0, 0.0, 20.008146861143434, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 370 + stamp: + secs: 1624875222 + nsecs: 281445026 + frame_id: "/world" +pose: + pose: + position: + x: 0.155440192759 + y: 0.000984488096916 + z: 0.0 + orientation: + x: 0.0 + y: 0.00174722317012 + z: 0.0 + w: 0.999998473604 + covariance: [20.753693628543743, -1.569775158850328e-06, 0.0, 0.0, -0.004252856668205558, 0.0, -1.5697751588503284e-06, 20.753686199116597, 0.0, 0.0, 0.0011868378194081766, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.004252856668205558, 0.0011868378194081763, 0.0, 0.0, 20.006757449419663, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 371 + stamp: + secs: 1624875222 + nsecs: 361438990 + frame_id: "/world" +pose: + pose: + position: + x: 0.155300276838 + y: 0.000188356934893 + z: 0.0 + orientation: + x: 0.0 + y: 0.00152779247057 + z: 0.0 + w: 0.999998832924 + covariance: [20.730724829164387, -1.7521223152363443e-06, 0.0, 0.0, -0.003905157600226667, 0.0, -1.7521223152363445e-06, 20.73071689425514, 0.0, 0.0, 0.0010866113633104283, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.003905157600226666, 0.001086611363310428, 0.0, 0.0, 20.007284529003837, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 372 + stamp: + secs: 1624875222 + nsecs: 441457987 + frame_id: "/world" +pose: + pose: + position: + x: 0.155163260014 + y: -0.000583192818771 + z: 0.0 + orientation: + x: 0.0 + y: 0.00144302168022 + z: 0.0 + w: 0.999998958844 + covariance: [20.703836373286276, -1.8846267363285303e-06, 0.0, 0.0, -0.0034478349842875467, 0.0, -1.8846267363285312e-06, 20.703828125208137, 0.0, 0.0, 0.0009763368589298458, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.003447834984287545, 0.0009763368589298456, 0.0, 0.0, 20.00635021967609, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 373 + stamp: + secs: 1624875222 + nsecs: 521461964 + frame_id: "/world" +pose: + pose: + position: + x: 0.155002969717 + y: -0.00147734267232 + z: 0.0 + orientation: + x: 0.0 + y: 0.00155320427646 + z: 0.0 + w: 0.999998793778 + covariance: [20.72416642890659, -2.1507640032388537e-06, 0.0, 0.0, -0.0031240336753270465, 0.0, -2.1507640032388546e-06, 20.724157308148136, 0.0, 0.0, 0.0009109686118442868, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0031240336753270456, 0.0009109686118442865, 0.0, 0.0, 20.008145600785948, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 374 + stamp: + secs: 1624875222 + nsecs: 601474047 + frame_id: "/world" +pose: + pose: + position: + x: 0.154839632657 + y: -0.00237193887772 + z: 0.0 + orientation: + x: 0.0 + y: 0.00152630679532 + z: 0.0 + w: 0.999998835193 + covariance: [20.739363559640463, -2.3794364126888178e-06, 0.0, 0.0, -0.0028101630481330033, 0.0, -2.379436412688818e-06, 20.739353729696745, 0.0, 0.0, 0.0008725466071699706, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.002810163048133003, 0.0008725466071699703, 0.0, 0.0, 20.006423962470638, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 375 + stamp: + secs: 1624875222 + nsecs: 681476116 + frame_id: "/world" +pose: + pose: + position: + x: 0.154679810868 + y: -0.0032367063325 + z: 0.0 + orientation: + x: 0.0 + y: 0.00130105721498 + z: 0.0 + w: 0.999999153625 + covariance: [20.739784275824327, -2.527244513442199e-06, 0.0, 0.0, -0.0024833526301344076, 0.0, -2.5272445134421988e-06, 20.73977404837287, 0.0, 0.0, 0.0008137714588820873, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.002483352630134407, 0.0008137714588820869, 0.0, 0.0, 20.00620600020181, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 376 + stamp: + secs: 1624875222 + nsecs: 761483908 + frame_id: "/world" +pose: + pose: + position: + x: 0.154524940906 + y: -0.00407902516349 + z: 0.0 + orientation: + x: 0.0 + y: 0.00119126099572 + z: 0.0 + w: 0.999999290448 + covariance: [20.73262163637809, -2.6080108171703307e-06, 0.0, 0.0, -0.0021269622541680014, 0.0, -2.6080108171703303e-06, 20.732611229331436, 0.0, 0.0, 0.0007096931401174388, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0021269622541680014, 0.0007096931401174385, 0.0, 0.0, 20.005258290308806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 377 + stamp: + secs: 1624875222 + nsecs: 841490030 + frame_id: "/world" +pose: + pose: + position: + x: 0.154344350134 + y: -0.00506422615549 + z: 0.0 + orientation: + x: 0.0 + y: 0.000982632887378 + z: 0.0 + w: 0.999999517216 + covariance: [20.776012246754817, -2.862625961395717e-06, 0.0, 0.0, -0.001933146654430527, 0.0, -2.862625961395716e-06, 20.776000961631112, 0.0, 0.0, 0.0006336209915504408, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0019331466544305269, 0.0006336209915504406, 0.0, 0.0, 20.003902720961637, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 378 + stamp: + secs: 1624875222 + nsecs: 921515942 + frame_id: "/world" +pose: + pose: + position: + x: 0.154197798958 + y: -0.00586728860624 + z: 0.0 + orientation: + x: 0.0 + y: 0.00082216926806 + z: 0.0 + w: 0.999999662019 + covariance: [20.745032084354172, -2.800267378946476e-06, 0.0, 0.0, -0.0016821618961842637, 0.0, -2.800267378946475e-06, 20.745021122186433, 0.0, 0.0, 0.000526198331170989, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001682161896184264, 0.000526198331170989, 0.0, 0.0, 20.003583182145043, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 379 + stamp: + secs: 1624875223 + nsecs: 1523018 + frame_id: "/world" +pose: + pose: + position: + x: 0.154056804194 + y: -0.00663969488511 + z: 0.0 + orientation: + x: 0.0 + y: 0.000931445411726 + z: 0.0 + w: 0.999999566205 + covariance: [20.71011980233006, -2.7032655402496434e-06, 0.0, 0.0, -0.0014434900028164486, 0.0, -2.7032655402496426e-06, 20.71010927918576, 0.0, 0.0, 0.0004350204928219468, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014434900028164489, 0.00043502049282194703, 0.0, 0.0, 20.004075744336216, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 380 + stamp: + secs: 1624875223 + nsecs: 81501961 + frame_id: "/world" +pose: + pose: + position: + x: 0.15385235361 + y: -0.00769546114219 + z: 0.0 + orientation: + x: 0.0 + y: 0.00142722346536 + z: 0.0 + w: 0.999998981516 + covariance: [20.779226356652085, -3.0209850771228206e-06, 0.0, 0.0, -0.0013887050877574845, 0.0, -3.0209850771228197e-06, 20.779214745881536, 0.0, 0.0, 0.0005847180282572756, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013887050877574845, 0.0005847180282572757, 0.0, 0.0, 20.00636992651554, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 381 + stamp: + secs: 1624875223 + nsecs: 161525011 + frame_id: "/world" +pose: + pose: + position: + x: 0.153661488455 + y: -0.00862267531756 + z: 0.0 + orientation: + x: 0.0 + y: 0.00158223382097 + z: 0.0 + w: 0.999998748267 + covariance: [20.7893730804891, -3.098140615712087e-06, 0.0, 0.0, -0.0012562824143973522, 0.0, -3.098140615712086e-06, 20.78936134971797, 0.0, 0.0, 0.0007450225593033366, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001256282414397352, 0.0007450225593033366, 0.0, 0.0, 20.006552935639156, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 382 + stamp: + secs: 1624875223 + nsecs: 241575956 + frame_id: "/world" +pose: + pose: + position: + x: 0.153466881244 + y: -0.00955159327139 + z: 0.0 + orientation: + x: 0.0 + y: 0.000994000493481 + z: 0.0 + w: 0.999999505981 + covariance: [20.796936575927223, -3.1537433129069546e-06, 0.0, 0.0, -0.001167353715093117, 0.0, -3.1537433129069533e-06, 20.796924804800828, 0.0, 0.0, 0.0008353408247890341, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011673537150931168, 0.0008353408247890344, 0.0, 0.0, 20.002531682198615, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 383 + stamp: + secs: 1624875223 + nsecs: 321557045 + frame_id: "/world" +pose: + pose: + position: + x: 0.153244554574 + y: -0.01060146539 + z: 0.0 + orientation: + x: 0.0 + y: 0.000429220452188 + z: 0.0 + w: 0.999999907885 + covariance: [20.84537263168969, -3.366378196426343e-06, 0.0, 0.0, -0.0011364202655829042, 0.0, -3.3663781964263415e-06, 20.84536025791343, 0.0, 0.0, 0.000898921245358838, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011364202655829037, 0.0008989212453588382, 0.0, 0.0, 20.002192251302752, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 384 + stamp: + secs: 1624875223 + nsecs: 401560068 + frame_id: "/world" +pose: + pose: + position: + x: 0.153045045955 + y: -0.0115426094386 + z: 0.0 + orientation: + x: 0.0 + y: 0.000759869279186 + z: 0.0 + w: 0.999999711299 + covariance: [20.84521757506134, -3.363445346340856e-06, 0.0, 0.0, -0.0010026632063493167, 0.0, -3.3634453463408548e-06, 20.845205346060286, 0.0, 0.0, 0.0008369379906091838, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001002663206349316, 0.0008369379906091839, 0.0, 0.0, 20.003707088859546, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 385 + stamp: + secs: 1624875223 + nsecs: 481564999 + frame_id: "/world" +pose: + pose: + position: + x: 0.152876800273 + y: -0.0123343444321 + z: 0.0 + orientation: + x: 0.0 + y: 0.000900926282092 + z: 0.0 + w: 0.999999594166 + covariance: [20.79174215704275, -3.1369674440547995e-06, 0.0, 0.0, -0.0008262440288601117, 0.0, -3.136967444054799e-06, 20.79173084015123, 0.0, 0.0, 0.0007162848117418477, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0008262440288601113, 0.0007162848117418475, 0.0, 0.0, 20.003023045993363, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 386 + stamp: + secs: 1624875223 + nsecs: 561572075 + frame_id: "/world" +pose: + pose: + position: + x: 0.152720839657 + y: -0.0130669061806 + z: 0.0 + orientation: + x: 0.0 + y: 0.000811088025145 + z: 0.0 + w: 0.999999671068 + covariance: [20.730617361616478, -2.8782910020400628e-06, 0.0, 0.0, -0.0006653619158400996, 0.0, -2.8782910020400628e-06, 20.730607046172135, 0.0, 0.0, 0.0005953047159363716, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006653619158400991, 0.0005953047159363716, 0.0, 0.0, 20.002918505131987, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 387 + stamp: + secs: 1624875223 + nsecs: 641552925 + frame_id: "/world" +pose: + pose: + position: + x: 0.152571091953 + y: -0.0137698559383 + z: 0.0 + orientation: + x: 0.0 + y: 0.00115723448701 + z: 0.0 + w: 0.999999330404 + covariance: [20.67458777799205, -2.639733464961201e-06, 0.0, 0.0, -0.000525845472248932, 0.0, -2.639733464961201e-06, 20.67457837194092, 0.0, 0.0, 0.0004860957533325603, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005258454722489316, 0.0004860957533325603, 0.0, 0.0, 20.005316805896605, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 388 + stamp: + secs: 1624875223 + nsecs: 721590996 + frame_id: "/world" +pose: + pose: + position: + x: 0.152414613991 + y: -0.014501958341 + z: 0.0 + orientation: + x: 0.0 + y: 0.00127662208794 + z: 0.0 + w: 0.999999185118 + covariance: [20.641129140740368, -2.4899960373282e-06, 0.0, 0.0, -0.00043291852539481865, 0.0, -2.4899960373281997e-06, 20.641120318623248, 0.0, 0.0, 0.00040934200254220645, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00043291852539481833, 0.0004093420025422063, 0.0, 0.0, 20.003793542131135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 389 + stamp: + secs: 1624875223 + nsecs: 801577091 + frame_id: "/world" +pose: + pose: + position: + x: 0.152225849097 + y: -0.0153847281274 + z: 0.0 + orientation: + x: 0.0 + y: 0.000699966882943 + z: 0.0 + w: 0.999999755023 + covariance: [20.662884956685954, -2.551108387086257e-06, 0.0, 0.0, -0.00041026626387792635, 0.0, -2.5511083870862564e-06, 20.662875973936604, 0.0, 0.0, 0.0003643759347519196, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004102662638779263, 0.0003643759347519195, 0.0, 0.0, 20.001721949614655, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 390 + stamp: + secs: 1624875223 + nsecs: 881587982 + frame_id: "/world" +pose: + pose: + position: + x: 0.15203658321 + y: -0.0162700399151 + z: 0.0 + orientation: + x: 0.0 + y: 0.000264938754518 + z: 0.0 + w: 0.999999964904 + covariance: [20.680479218806838, -2.5915198863813355e-06, 0.0, 0.0, -0.00043726090761271564, 0.0, -2.591519886381335e-06, 20.680470142302234, 0.0, 0.0, 0.00032993565724223503, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00043726090761271553, 0.0003299356572422349, 0.0, 0.0, 20.001016455099546, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 391 + stamp: + secs: 1624875223 + nsecs: 961596966 + frame_id: "/world" +pose: + pose: + position: + x: 0.15184659155 + y: -0.017159801306 + z: 0.0 + orientation: + x: 0.0 + y: 7.73210094253e-05 + z: 0.0 + w: 0.999999997011 + covariance: [20.69461235762909, -2.6158292541651037e-06, 0.0, 0.0, -0.0005113672840142378, 0.0, -2.615829254165103e-06, 20.694603237996485, 0.0, 0.0, 0.00030264186531965423, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0005113672840142377, 0.0003026418653196541, 0.0, 0.0, 20.000485813399003, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 392 + stamp: + secs: 1624875224 + nsecs: 41613102 + frame_id: "/world" +pose: + pose: + position: + x: 0.14823540778 + y: -0.0176994536117 + z: 0.0 + orientation: + x: -0.0 + y: -0.000261178654598 + z: -0.0 + w: 0.999999965893 + covariance: [17.167748024353084, -1.8514036913747595e-06, 0.0, 0.0, -0.0004025987348446096, 0.0, -1.8514036913747595e-06, 17.16774159353181, 0.0, 0.0, 0.00019830512880603525, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004025987348446096, 0.00019830512880603517, 0.0, 0.0, 16.6666722730505, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 393 + stamp: + secs: 1624875224 + nsecs: 121651888 + frame_id: "/world" +pose: + pose: + position: + x: 0.143400551438 + y: -0.0104782241647 + z: 0.0 + orientation: + x: -0.0 + y: -0.000315195141974 + z: -0.0 + w: 0.999999950326 + covariance: [15.57613929794372, -1.6674450773762659e-06, 0.0, 0.0, -0.0003970093994048909, 0.0, -1.6674450773762656e-06, 15.576133529921588, 0.0, 0.0, 0.00016649769668547216, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00039700939940489087, 0.00016649769668547208, 0.0, 0.0, 15.116288862870006, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 394 + stamp: + secs: 1624875224 + nsecs: 201616049 + frame_id: "/world" +pose: + pose: + position: + x: 0.143291725506 + y: -0.0124227345379 + z: 0.0 + orientation: + x: -0.0 + y: -1.50600963187e-05 + z: -0.0 + w: 0.999999999887 + covariance: [17.313067539701194, -2.0271406971512193e-06, 0.0, 0.0, -0.00047974576413471775, 0.0, -2.0271406971512193e-06, 17.313060549500648, 0.0, 0.0, 0.00019335694842028206, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00047974576413471775, 0.00019335694842028195, 0.0, 0.0, 16.747483459950566, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 395 + stamp: + secs: 1624875224 + nsecs: 281644106 + frame_id: "/world" +pose: + pose: + position: + x: 0.143101007651 + y: -0.0130549844885 + z: 0.0 + orientation: + x: 0.0 + y: 0.000231997899013 + z: 0.0 + w: 0.999999973088 + covariance: [18.42288994407507, -1.986235650247959e-06, 0.0, 0.0, -0.0004386793062851585, 0.0, -1.9862356502479592e-06, 18.422883109086747, 0.0, 0.0, 0.00019300409558779295, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004386793062851585, 0.00019300409558779287, 0.0, 0.0, 17.862882703704756, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 396 + stamp: + secs: 1624875224 + nsecs: 361641884 + frame_id: "/world" +pose: + pose: + position: + x: 0.142867944663 + y: -0.0138366166287 + z: 0.0 + orientation: + x: -0.0 + y: -0.000274053113815 + z: -0.0 + w: 0.999999962447 + covariance: [19.198435900240614, -2.070782559585404e-06, 0.0, 0.0, -0.0006301138508029878, 0.0, -2.070782559585405e-06, 19.198428788104987, 0.0, 0.0, 0.0002148373887966314, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0006301138508029878, 0.00021483738879663134, 0.0, 0.0, 18.60866947420577, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 397 + stamp: + secs: 1624875224 + nsecs: 441647053 + frame_id: "/world" +pose: + pose: + position: + x: 0.142592135677 + y: -0.0147756541189 + z: 0.0 + orientation: + x: -0.0 + y: -0.000770800923003 + z: -0.0 + w: 0.999999702933 + covariance: [19.754470833565673, -2.2762168007767494e-06, 0.0, 0.0, -0.0009489810235539507, 0.0, -2.27621680077675e-06, 19.7544630135696, 0.0, 0.0, 0.00023775989410530706, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009489810235539503, 0.000237759894105307, 0.0, 0.0, 19.099527766791667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 398 + stamp: + secs: 1624875224 + nsecs: 521675110 + frame_id: "/world" +pose: + pose: + position: + x: 0.142321202568 + y: -0.0157009596944 + z: 0.0 + orientation: + x: -0.0 + y: -0.000689882589474 + z: -0.0 + w: 0.999999762031 + covariance: [20.119135122637534, -2.4050819961705166e-06, 0.0, 0.0, -0.0011198810567986837, 0.0, -2.4050819961705174e-06, 20.119126845384898, 0.0, 0.0, 0.0002412817448613939, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011198810567986832, 0.00024128174486139387, 0.0, 0.0, 19.41999900893056, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 399 + stamp: + secs: 1624875224 + nsecs: 601653099 + frame_id: "/world" +pose: + pose: + position: + x: 0.142055607619 + y: -0.016611567258 + z: 0.0 + orientation: + x: -0.0 + y: -0.000466046410249 + z: -0.0 + w: 0.9999998914 + covariance: [20.353693583441622, -2.4707107654903346e-06, 0.0, 0.0, -0.001189483758681675, 0.0, -2.470710765490335e-06, 20.353685057692896, 0.0, 0.0, 0.00023164811596797482, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011894837586816749, 0.0002316481159679748, 0.0, 0.0, 19.626767466524903, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 400 + stamp: + secs: 1624875224 + nsecs: 681679964 + frame_id: "/world" +pose: + pose: + position: + x: 0.141791041485 + y: -0.017519993298 + z: 0.0 + orientation: + x: -0.0 + y: -6.83174185916e-05 + z: -0.0 + w: 0.999999997666 + covariance: [20.50703788154367, -2.5055256253580426e-06, 0.0, 0.0, -0.0011453338762353296, 0.0, -2.505525625358043e-06, 20.50702921210649, 0.0, 0.0, 0.00020648005860973565, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011453338762353292, 0.00020648005860973565, 0.0, 0.0, 19.760376976373024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 401 + stamp: + secs: 1624875224 + nsecs: 761667967 + frame_id: "/world" +pose: + pose: + position: + x: 0.141533420598 + y: -0.0184121425351 + z: 0.0 + orientation: + x: 0.0 + y: 0.000136520231437 + z: 0.0 + w: 0.999999990681 + covariance: [20.59944590600934, -2.494751938406599e-06, 0.0, 0.0, -0.0011603654315307113, 0.0, -2.4947519384066e-06, 20.59943724092644, 0.0, 0.0, 0.00018119305694589002, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011603654315307111, 0.00018119305694589002, 0.0, 0.0, 19.846340424618052, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 402 + stamp: + secs: 1624875224 + nsecs: 841675043 + frame_id: "/world" +pose: + pose: + position: + x: 0.14125479126 + y: -0.0193844865333 + z: 0.0 + orientation: + x: 0.0 + y: 0.000261675800212 + z: 0.0 + w: 0.999999965763 + covariance: [20.68439898799884, -2.558083497317597e-06, 0.0, 0.0, -0.0012227693295450413, 0.0, -2.5580834973175983e-06, 20.68439005554895, 0.0, 0.0, 0.00016352026208963304, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001222769329545041, 0.00016352026208963304, 0.0, 0.0, 19.90155445386313, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 403 + stamp: + secs: 1624875224 + nsecs: 921693087 + frame_id: "/world" +pose: + pose: + position: + x: 0.140985159467 + y: -0.0203255092704 + z: 0.0 + orientation: + x: 0.0 + y: 0.000153637374031 + z: 0.0 + w: 0.999999988198 + covariance: [20.729039616344, -2.5563105852862905e-06, 0.0, 0.0, -0.0013129905782866737, 0.0, -2.5563105852862926e-06, 20.729030645119142, 0.0, 0.0, 0.00018812885352037558, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013129905782866737, 0.00018812885352037555, 0.0, 0.0, 19.93693160790719, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 404 + stamp: + secs: 1624875225 + nsecs: 1673937 + frame_id: "/world" +pose: + pose: + position: + x: 0.140759849607 + y: -0.0211116035137 + z: 0.0 + orientation: + x: 0.0 + y: 6.7737933964e-05 + z: 0.0 + w: 0.999999997706 + covariance: [20.70256177037326, -2.3740719335378905e-06, 0.0, 0.0, -0.001311833425661631, 0.0, -2.374071933537892e-06, 20.70255340503714, 0.0, 0.0, 0.00021186176878348095, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013118334256616306, 0.00021186176878348087, 0.0, 0.0, 19.959617382512672, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 405 + stamp: + secs: 1624875225 + nsecs: 81682920 + frame_id: "/world" +pose: + pose: + position: + x: 0.140494408842 + y: -0.0220405370409 + z: 0.0 + orientation: + x: 0.0 + y: 0.000295312327576 + z: 0.0 + w: 0.999999956395 + covariance: [20.725056234427438, -2.375117597162301e-06, 0.0, 0.0, -0.0013518641318834073, 0.0, -2.3751175971623027e-06, 20.725047824058, 0.0, 0.0, 0.00022849362725802126, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013518641318834067, 0.0002284936272580212, 0.0, 0.0, 19.974405829761373, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 406 + stamp: + secs: 1624875225 + nsecs: 161689043 + frame_id: "/world" +pose: + pose: + position: + x: 0.14019528269 + y: -0.0230945962837 + z: 0.0 + orientation: + x: 0.0 + y: 0.000803194793036 + z: 0.0 + w: 0.999999677439 + covariance: [20.78112227029476, -2.49471559696094e-06, 0.0, 0.0, -0.0014390338083664943, 0.0, -2.4947155969609418e-06, 20.781113380748874, 0.0, 0.0, 0.00023295765094764537, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0014390338083664935, 0.00023295765094764531, 0.0, 0.0, 19.98459902056286, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 407 + stamp: + secs: 1624875225 + nsecs: 241738081 + frame_id: "/world" +pose: + pose: + position: + x: 0.139936055444 + y: -0.0240116801523 + z: 0.0 + orientation: + x: 0.0 + y: 0.000948363256439 + z: 0.0 + w: 0.999999550303 + covariance: [20.777049956887538, -2.4339632568213365e-06, 0.0, 0.0, -0.0013597817036656455, 0.0, -2.4339632568213374e-06, 20.777041238522735, 0.0, 0.0, 0.00020857525438102648, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0013597817036656449, 0.0002085752543810264, 0.0, 0.0, 19.99076931683874, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 408 + stamp: + secs: 1624875225 + nsecs: 321708918 + frame_id: "/world" +pose: + pose: + position: + x: 0.139701235881 + y: -0.0248422969976 + z: 0.0 + orientation: + x: 0.0 + y: 0.000727577900055 + z: 0.0 + w: 0.999999735315 + covariance: [20.7442798437478, -2.2924664477838426e-06, 0.0, 0.0, -0.0011866214680192784, 0.0, -2.2924664477838447e-06, 20.74427159956747, 0.0, 0.0, 0.0001775157748534391, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011866214680192782, 0.00017751577485343906, 0.0, 0.0, 19.99413060162622, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 409 + stamp: + secs: 1624875225 + nsecs: 401716948 + frame_id: "/world" +pose: + pose: + position: + x: 0.139424020089 + y: -0.0258140374286 + z: 0.0 + orientation: + x: 0.0 + y: 0.000520154297943 + z: 0.0 + w: 0.99999986472 + covariance: [20.765139046354534, -2.319475512072365e-06, 0.0, 0.0, -0.0010714971909872675, 0.0, -2.319475512072367e-06, 20.765130681812753, 0.0, 0.0, 0.00019898742562932443, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.001071497190987267, 0.0001989874256293244, 0.0, 0.0, 19.996468029899514, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 410 + stamp: + secs: 1624875225 + nsecs: 481729984 + frame_id: "/world" +pose: + pose: + position: + x: 0.139190164083 + y: -0.0266254220142 + z: 0.0 + orientation: + x: 0.0 + y: 0.00019430977002 + z: 0.0 + w: 0.999999981122 + covariance: [20.726178313763892, -2.175512900234643e-06, 0.0, 0.0, -0.0009955114324961913, 0.0, -2.1755129002346446e-06, 20.726170462601516, 0.0, 0.0, 0.0002452786228243398, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0009955114324961911, 0.0002452786228243398, 0.0, 0.0, 19.99736978904107, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 411 + stamp: + secs: 1624875225 + nsecs: 561794043 + frame_id: "/world" +pose: + pose: + position: + x: 0.138927876682 + y: -0.0275386752265 + z: 0.0 + orientation: + x: -0.0 + y: -0.00020206902177 + z: -0.0 + w: 0.999999979584 + covariance: [20.7267582407134, -2.153077442402032e-06, 0.0, 0.0, -0.0010041984789175423, 0.0, -2.1530774424020335e-06, 20.726750463549273, 0.0, 0.0, 0.0002716133883308646, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0010041984789175421, 0.0002716133883308646, 0.0, 0.0, 19.998229744903114, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 412 + stamp: + secs: 1624875225 + nsecs: 641766071 + frame_id: "/world" +pose: + pose: + position: + x: 0.138660614292 + y: -0.0284652869326 + z: 0.0 + orientation: + x: -0.0 + y: -0.000181319775596 + z: -0.0 + w: 0.999999983562 + covariance: [20.726835054098615, -2.1390702255421217e-06, 0.0, 0.0, -0.0011849649586509407, 0.0, -2.1390702255421242e-06, 20.726827333295116, 0.0, 0.0, 0.0003593981253804352, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0011849649586509405, 0.0003593981253804352, 0.0, 0.0, 19.998893981982253, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 0 + stamp: + secs: 1624875191 + nsecs: 558718920 + frame_id: "/world" +pose: + pose: + position: + x: 0.0572746301754 + y: 0.0203722993784 + z: 0.0 + orientation: + x: 0.0 + y: 0.0403072300115 + z: 0.0 + w: 0.999187333391 + covariance: [0.09798037477464908, -1.76815747288255e-18, 0.0, 0.0, 6.13465559113359e-14, 0.0, -1.768157472882468e-18, 0.09798037477464923, 0.0, 0.0, -3.650408832264524e-12, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 6.134655591133087e-14, -3.65040883226233e-12, 0.0, 0.0, 0.09798030471140112, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 1 + stamp: + secs: 1624875193 + nsecs: 318856001 + frame_id: "/world" +pose: + pose: + position: + x: -0.027583960469 + y: -0.0109764176242 + z: 0.0 + orientation: + x: 0.0 + y: 0.036213534968 + z: 0.0 + w: 0.999344074824 + covariance: [1.554217329643122, -4.566444801867897e-07, 0.0, 0.0, -0.000281048174497082, 0.0, -4.5664448018679055e-07, 1.5542167317263673, 0.0, 0.0, 0.00015192348150098203, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00028104817449708355, 0.0001519234815009988, 0.0, 0.0, 1.299112780009319, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 2 + stamp: + secs: 1624875196 + nsecs: 679157019 + frame_id: "/world" +pose: + pose: + position: + x: -0.0350439582251 + y: -0.0101363733353 + z: 0.0 + orientation: + x: -0.0 + y: -0.0227576230445 + z: -0.0 + w: 0.999741011759 + covariance: [1.722061717912726, -6.992448547714762e-06, 0.0, 0.0, -0.00023144360307907232, 0.0, -6.992448547714182e-06, 1.722053572694819, 0.0, 0.0, 0.0002266268869105261, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00023144360307907107, 0.00022662688691052536, 0.0, 0.0, 1.42831978995475, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 3 + stamp: + secs: 1624875196 + nsecs: 839164019 + frame_id: "/world" +pose: + pose: + position: + x: -0.0351885377998 + y: -0.0124582058831 + z: 0.0 + orientation: + x: 0.0 + y: 0.0148910534036 + z: 0.0 + w: 0.999889122117 + covariance: [1.3432536805914232, -5.052998560607446e-07, 0.0, 0.0, -1.9376407947275175e-05, 0.0, -5.052998560607228e-07, 1.3432531985575902, 0.0, 0.0, 1.8173610583605937e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9376407947275118e-05, 1.817361058360595e-05, 0.0, 0.0, 1.3239173697098163, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 4 + stamp: + secs: 1624875196 + nsecs: 999176979 + frame_id: "/world" +pose: + pose: + position: + x: -0.0687726925752 + y: 0.0159339240222 + z: 0.0 + orientation: + x: -0.0 + y: -0.00743833312454 + z: -0.0 + w: 0.999972335217 + covariance: [1.459788005551589, -1.5138467014668445e-07, 0.0, 0.0, -9.935670829771625e-06, 0.0, -1.5138467014668239e-07, 1.459787924821341, 0.0, 0.0, 7.19242763474458e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.93567082977164e-06, 7.1924276347445615e-06, 0.0, 0.0, 1.4533478938696847, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 5 + stamp: + secs: 1624875197 + nsecs: 239206076 + frame_id: "/world" +pose: + pose: + position: + x: -0.0794258114336 + y: 0.0124998106343 + z: 0.0 + orientation: + x: 0.0 + y: 0.00515393207601 + z: 0.0 + w: 0.999986718404 + covariance: [1.3664617598037045, -9.00688179183941e-08, 0.0, 0.0, -4.6822433635555305e-06, 0.0, -9.006881791839332e-08, 1.3664617352008077, 0.0, 0.0, 3.4124895544140678e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.682243363555532e-06, 3.4124895544140767e-06, 0.0, 0.0, 1.3607924744740298, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 6 + stamp: + secs: 1624875197 + nsecs: 399205923 + frame_id: "/world" +pose: + pose: + position: + x: -0.105267058789 + y: -0.0210065311936 + z: 0.0 + orientation: + x: -0.0 + y: -0.00205014890847 + z: -0.0 + w: 0.999997898443 + covariance: [1.343523072135344, -5.974507518064478e-08, 0.0, 0.0, -1.6303003996095524e-05, 0.0, -5.974507518064463e-08, 1.3435230568344847, 0.0, 0.0, 9.137910967919138e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6303003996095568e-05, 9.137910967919101e-07, 0.0, 0.0, 1.3384011455814553, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 7 + stamp: + secs: 1624875197 + nsecs: 559230089 + frame_id: "/world" +pose: + pose: + position: + x: -0.123176392691 + y: -0.0901488591105 + z: 0.0 + orientation: + x: 0.0 + y: 0.00139627928502 + z: 0.0 + w: 0.999999025202 + covariance: [1.5759159354006635, -7.165159174616461e-08, 0.0, 0.0, -1.4836899276197031e-05, 0.0, -7.165159174616425e-08, 1.5759159003246415, 0.0, 0.0, 5.797075679901155e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4836899276197077e-05, 5.797075679901158e-07, 0.0, 0.0, 1.5679609766107694, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 8 + stamp: + secs: 1624875197 + nsecs: 719242096 + frame_id: "/world" +pose: + pose: + position: + x: -0.0938375235075 + y: -0.0281688531413 + z: 0.0 + orientation: + x: 0.0 + y: 0.000143521087332 + z: 0.0 + w: 0.999999989701 + covariance: [1.318610225144991, -4.9961067161111296e-08, 0.0, 0.0, -8.965443186230044e-06, 0.0, -4.996106716111135e-08, 1.3186101946252342, 0.0, 0.0, 1.51696291673374e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.965443186230073e-06, 1.5169629167337433e-06, 0.0, 0.0, 1.311898612995714, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 9 + stamp: + secs: 1624875197 + nsecs: 879255056 + frame_id: "/world" +pose: + pose: + position: + x: -0.104400119775 + y: -0.0767517060568 + z: 0.0 + orientation: + x: 0.0 + y: 0.0004786283699 + z: 0.0 + w: 0.999999885457 + covariance: [1.8041462923916858, -8.597628848894249e-08, 0.0, 0.0, -5.971868052093918e-05, 0.0, -8.597628848894208e-08, 1.8041462070145848, 0.0, 0.0, 2.888597443576239e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.9718680520939154e-05, 2.888597443576241e-06, 0.0, 0.0, 1.790831425360643, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 10 + stamp: + secs: 1624875198 + nsecs: 39307117 + frame_id: "/world" +pose: + pose: + position: + x: -0.101660674375 + y: -0.0895387989054 + z: 0.0 + orientation: + x: -0.0 + y: -0.000587201332259 + z: -0.0 + w: 0.999999827597 + covariance: [1.6076669373105446, -7.805846152768439e-08, 0.0, 0.0, -3.0220995309225865e-05, 0.0, -7.805846152768423e-08, 1.6076667812982182, 0.0, 0.0, 4.244376485208481e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.022099530922585e-05, 4.244376485208467e-06, 0.0, 0.0, 1.594421280653708, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 11 + stamp: + secs: 1624875198 + nsecs: 199289083 + frame_id: "/world" +pose: + pose: + position: + x: -0.104252589519 + y: -0.0977639275462 + z: 0.0 + orientation: + x: -0.0 + y: -0.000818003976646 + z: -0.0 + w: 0.999999665435 + covariance: [1.5682842721173635, -5.146944726517667e-08, 0.0, 0.0, -7.0439550568543135e-06, 0.0, -5.146944726517659e-08, 1.568284153081746, 0.0, 0.0, 2.332136666347177e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.043955056854302e-06, 2.332136666347172e-06, 0.0, 0.0, 1.5587449238388582, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 12 + stamp: + secs: 1624875198 + nsecs: 279292107 + frame_id: "/world" +pose: + pose: + position: + x: -0.106576209297 + y: -0.1072020102 + z: 0.0 + orientation: + x: -0.0 + y: -9.44184571137e-05 + z: -0.0 + w: 0.999999995543 + covariance: [1.4722214359795125, -3.253618874495884e-08, 0.0, 0.0, -1.2857914016508653e-06, 0.0, -3.25361887449588e-08, 1.4722213576724794, 0.0, 0.0, 9.452914954863612e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2857914016508655e-06, 9.452914954863592e-07, 0.0, 0.0, 1.4653424008324532, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 13 + stamp: + secs: 1624875198 + nsecs: 439341068 + frame_id: "/world" +pose: + pose: + position: + x: -0.107898699183 + y: -0.109235068271 + z: 0.0 + orientation: + x: -0.0 + y: -0.000929036659281 + z: -0.0 + w: 0.999999568445 + covariance: [2.3902266630971885, -1.428659820653964e-07, 0.0, 0.0, -5.5036115926805454e-05, 0.0, -1.4286598206539583e-07, 2.390226302146701, 0.0, 0.0, 1.2663447703806026e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.503611592680549e-05, 1.2663447703806022e-06, 0.0, 0.0, 2.354843063836702, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 14 + stamp: + secs: 1624875198 + nsecs: 599320889 + frame_id: "/world" +pose: + pose: + position: + x: -0.109688711654 + y: -0.112164679356 + z: 0.0 + orientation: + x: -0.0 + y: -0.00139212369844 + z: -0.0 + w: 0.999999030995 + covariance: [3.7240419555971487, -3.6524582650955037e-07, 0.0, 0.0, -0.00012971802247171676, 0.0, -3.652458265095487e-07, 3.7240409191770683, 0.0, 0.0, 2.2699925950166143e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00012971802247171667, 2.2699925950166148e-06, 0.0, 0.0, 3.6243087103717544, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 15 + stamp: + secs: 1624875198 + nsecs: 679327011 + frame_id: "/world" +pose: + pose: + position: + x: -0.109982796944 + y: -0.112646062937 + z: 0.0 + orientation: + x: 0.0 + y: 0.000583886842749 + z: 0.0 + w: 0.999999829538 + covariance: [1.6067021647461341, -3.1508383780291965e-08, 0.0, 0.0, -9.502866302532009e-06, 0.0, -3.150838378029184e-08, 1.6067020726180257, 0.0, 0.0, 2.8771228545940963e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.50286630253204e-06, 2.877122854594091e-06, 0.0, 0.0, 1.597602062429705, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 16 + stamp: + secs: 1624875198 + nsecs: 759337902 + frame_id: "/world" +pose: + pose: + position: + x: -0.11030939931 + y: -0.109867873443 + z: 0.0 + orientation: + x: -0.0 + y: -0.00056800326171 + z: -0.0 + w: 0.999999838686 + covariance: [1.470492692951918, -1.0414819489178212e-08, 0.0, 0.0, -2.1584098778848975e-06, 0.0, -1.0414819489178126e-08, 1.4704926618802079, 0.0, 0.0, 1.4565919806390564e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1584098778849e-06, 1.4565919806390526e-06, 0.0, 0.0, 1.4672171777229677, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 17 + stamp: + secs: 1624875198 + nsecs: 839340925 + frame_id: "/world" +pose: + pose: + position: + x: -0.107418013476 + y: -0.11281734616 + z: 0.0 + orientation: + x: -0.0 + y: -0.000105360068711 + z: -0.0 + w: 0.99999999445 + covariance: [1.4465265349526004, -1.9178952671966603e-08, 0.0, 0.0, -1.845720339978819e-06, 0.0, -1.9178952671966547e-08, 1.4465264786031014, 0.0, 0.0, 8.279643902947588e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8457203399788139e-06, 8.279643902947542e-06, 0.0, 0.0, 1.4396473475691502, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 18 + stamp: + secs: 1624875198 + nsecs: 999368906 + frame_id: "/world" +pose: + pose: + position: + x: -0.108163345181 + y: -0.11396041587 + z: 0.0 + orientation: + x: -0.0 + y: -0.000809614630396 + z: -0.0 + w: 0.999999672262 + covariance: [1.3208730540685167, -3.540696839242339e-08, 0.0, 0.0, -7.015670127024012e-07, 0.0, -3.5406968392423215e-08, 1.3208729567053457, 0.0, 0.0, 5.068376908860847e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.01567012702399e-07, 5.06837690886081e-06, 0.0, 0.0, 1.306906703716319, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 19 + stamp: + secs: 1624875199 + nsecs: 159377098 + frame_id: "/world" +pose: + pose: + position: + x: -0.105278889866 + y: -0.11968272569 + z: 0.0 + orientation: + x: -0.0 + y: -0.000432800093556 + z: -0.0 + w: 0.999999906342 + covariance: [1.567968676251421, -6.047791763077736e-08, 0.0, 0.0, -1.00318383466217e-05, 0.0, -6.047791763077704e-08, 1.567968523565198, 0.0, 0.0, 2.3064748136968444e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0031838346621688e-05, 2.306474813696848e-05, 0.0, 0.0, 1.5418977925759254, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 20 + stamp: + secs: 1624875199 + nsecs: 399395943 + frame_id: "/world" +pose: + pose: + position: + x: -0.0513979646357 + y: -0.156564701314 + z: 0.0 + orientation: + x: -0.0 + y: -0.00353550822883 + z: -0.0 + w: 0.999993750071 + covariance: [1.3932763992326205, -6.832814419209259e-08, 0.0, 0.0, -7.30086599652405e-06, 0.0, -6.832814419209255e-08, 1.3932762804451482, 0.0, 0.0, 3.220375251271922e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.300865996524021e-06, 3.2203752512719194e-05, 0.0, 0.0, 1.364776586862331, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 21 + stamp: + secs: 1624875199 + nsecs: 719436884 + frame_id: "/world" +pose: + pose: + position: + x: -0.0517245621772 + y: -0.160071365224 + z: 0.0 + orientation: + x: -0.0 + y: -6.93841430129e-05 + z: -0.0 + w: 0.999999997593 + covariance: [2.0190692161454273, -1.9761432989369794e-07, 0.0, 0.0, -4.0303283774099e-05, 0.0, -1.9761432989369752e-07, 2.019069005060484, 0.0, 0.0, 2.9765630653967722e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.030328377409904e-05, 2.976563065396772e-05, 0.0, 0.0, 1.9338876016401743, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 22 + stamp: + secs: 1624875199 + nsecs: 879475117 + frame_id: "/world" +pose: + pose: + position: + x: -0.0492116747622 + y: -0.138319436 + z: 0.0 + orientation: + x: -0.0 + y: -0.000523572397685 + z: -0.0 + w: 0.999999862936 + covariance: [1.438705368833236, -4.20167519040597e-08, 0.0, 0.0, -1.5593925962249142e-05, 0.0, -4.201675190405974e-08, 1.438705323480808, 0.0, 0.0, 4.724767179730621e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.559392596224915e-05, 4.724767179730615e-06, 0.0, 0.0, 1.4201483332844367, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 23 + stamp: + secs: 1624875199 + nsecs: 959450006 + frame_id: "/world" +pose: + pose: + position: + x: -0.0493194105363 + y: -0.139298472176 + z: 0.0 + orientation: + x: -0.0 + y: -0.000374752076434 + z: -0.0 + w: 0.99999992978 + covariance: [1.6633870417494834, -2.8444543774765755e-08, 0.0, 0.0, -1.3776611681980623e-05, 0.0, -2.844454377476574e-08, 1.6633870075852641, 0.0, 0.0, 9.013707553576074e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3776611681980608e-05, 9.01370755357607e-06, 0.0, 0.0, 1.6508751591751365, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 24 + stamp: + secs: 1624875200 + nsecs: 119455099 + frame_id: "/world" +pose: + pose: + position: + x: -0.0472362724043 + y: -0.137158179135 + z: 0.0 + orientation: + x: -0.0 + y: -0.000306998577178 + z: -0.0 + w: 0.999999952876 + covariance: [1.6085090924737866, -5.073547127661546e-08, 0.0, 0.0, -7.840658855426918e-06, 0.0, -5.0735471276615244e-08, 1.6085090333087346, 0.0, 0.0, 7.007291774981734e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.840658855426916e-06, 7.007291774981729e-06, 0.0, 0.0, 1.5864332822965217, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 25 + stamp: + secs: 1624875200 + nsecs: 279485941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0472532280232 + y: -0.137749633261 + z: 0.0 + orientation: + x: -0.0 + y: -0.000501277324597 + z: -0.0 + w: 0.999999874361 + covariance: [1.4837381026437435, -3.6673719400244536e-08, 0.0, 0.0, -4.589911367116733e-06, 0.0, -3.6673719400244436e-08, 1.4837380604119785, 0.0, 0.0, 2.8513588735705137e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.589911367116728e-06, 2.8513588735705107e-06, 0.0, 0.0, 1.4668378673352518, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 26 + stamp: + secs: 1624875200 + nsecs: 359488964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0468450474523 + y: -0.138380131322 + z: 0.0 + orientation: + x: -0.0 + y: -0.000706618657084 + z: -0.0 + w: 0.999999750345 + covariance: [1.6416401012456643, -2.9414096951515502e-08, 0.0, 0.0, -9.94178763979709e-06, 0.0, -2.9414096951515472e-08, 1.64164006675844, 0.0, 0.0, 2.3111302625391226e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.941787639797104e-06, 2.311130262539118e-06, 0.0, 0.0, 1.6271838514337313, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 27 + stamp: + secs: 1624875200 + nsecs: 519515038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0468552189113 + y: -0.139319620841 + z: 0.0 + orientation: + x: -0.0 + y: -0.000493065474951 + z: -0.0 + w: 0.999999878443 + covariance: [1.6066841038172364, -3.957626643853144e-08, 0.0, 0.0, -5.988683230057577e-06, 0.0, -3.9576266438531445e-08, 1.606684054066615, 0.0, 0.0, 1.801261424182683e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.988683230057586e-06, 1.8012614241826826e-06, 0.0, 0.0, 1.585121771915552, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 28 + stamp: + secs: 1624875200 + nsecs: 679512978 + frame_id: "/world" +pose: + pose: + position: + x: -0.0468243322211 + y: -0.140227671283 + z: 0.0 + orientation: + x: -0.0 + y: -0.000344883747262 + z: -0.0 + w: 0.999999940528 + covariance: [1.5730833980219525, -2.5535706764035964e-08, 0.0, 0.0, -1.2022807876467717e-06, 0.0, -2.553570676403593e-08, 1.5730833653910565, 0.0, 0.0, 6.196402147088079e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2022807876467725e-06, 6.196402147088072e-07, 0.0, 0.0, 1.55812809226502, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 29 + stamp: + secs: 1624875200 + nsecs: 759515047 + frame_id: "/world" +pose: + pose: + position: + x: -0.0468923925038 + y: -0.140693920783 + z: 0.0 + orientation: + x: -0.0 + y: -0.000210898355484 + z: -0.0 + w: 0.999999977761 + covariance: [1.6434524132645036, -1.7747626309011437e-08, 0.0, 0.0, -1.3454574130668092e-05, 0.0, -1.7747626309011355e-08, 1.6434523905361105, 0.0, 0.0, 7.583728999230624e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3454574130668112e-05, 7.583728999230639e-06, 0.0, 0.0, 1.6327049300914147, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 30 + stamp: + secs: 1624875200 + nsecs: 919538975 + frame_id: "/world" +pose: + pose: + position: + x: -0.0468903383731 + y: -0.133899783958 + z: 0.0 + orientation: + x: -0.0 + y: -0.000310222432561 + z: -0.0 + w: 0.999999951881 + covariance: [1.5756743547064231, -2.80348296807963e-08, 0.0, 0.0, -9.83403731540205e-06, 0.0, -2.803482968079625e-08, 1.5756743190165419, 0.0, 0.0, 5.0612104549832426e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.834037315402061e-06, 5.061210454983251e-06, 0.0, 0.0, 1.5607276596484776, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 31 + stamp: + secs: 1624875200 + nsecs: 999536991 + frame_id: "/world" +pose: + pose: + position: + x: -0.0421047406635 + y: -0.120549912312 + z: 0.0 + orientation: + x: -0.0 + y: -0.000246916226538 + z: -0.0 + w: 0.999999969516 + covariance: [1.5428992137455255, -1.861659349823939e-08, 0.0, 0.0, -1.3892393849501633e-06, 0.0, -1.861659349823939e-08, 1.5428991900048434, 0.0, 0.0, 8.52312902308112e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.389239384950167e-06, 8.523129023081148e-07, 0.0, 0.0, 1.5327523331542856, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 32 + stamp: + secs: 1624875201 + nsecs: 159560919 + frame_id: "/world" +pose: + pose: + position: + x: -0.0383762313085 + y: -0.101973229468 + z: 0.0 + orientation: + x: 0.0 + y: 0.000613639855793 + z: 0.0 + w: 0.999999811723 + covariance: [1.7100211879947058, -4.007664156396667e-08, 0.0, 0.0, -2.214689745108399e-06, 0.0, -4.007664156396663e-08, 1.7100211368572462, 0.0, 0.0, 1.2413572021190935e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2146897451083987e-06, 1.2413572021190942e-06, 0.0, 0.0, 1.6863085905594057, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 33 + stamp: + secs: 1624875201 + nsecs: 319547892 + frame_id: "/world" +pose: + pose: + position: + x: -0.0380724996761 + y: -0.097823313537 + z: 0.0 + orientation: + x: 0.0 + y: 0.000331691739789 + z: 0.0 + w: 0.99999994499 + covariance: [1.517891669470173, -3.684757008899174e-08, 0.0, 0.0, -1.1477478111215661e-06, 0.0, -3.684757008899154e-08, 1.5178916224991381, 0.0, 0.0, 1.7535664512162068e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1477478111215627e-06, 1.753566451216211e-06, 0.0, 0.0, 1.49354467297991, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 34 + stamp: + secs: 1624875201 + nsecs: 399564028 + frame_id: "/world" +pose: + pose: + position: + x: -0.0379899186705 + y: -0.0983111849511 + z: 0.0 + orientation: + x: 0.0 + y: 4.5981322794e-05 + z: 0.0 + w: 0.999999998943 + covariance: [1.5211293015764231, -2.2155946075013456e-08, 0.0, 0.0, -3.217381314585383e-05, 0.0, -2.2155946075013383e-08, 1.5211292642014083, 0.0, 0.0, 3.1093019179549904e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.217381314585377e-05, 3.109301917954992e-06, 0.0, 0.0, 1.506047962988126, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 35 + stamp: + secs: 1624875201 + nsecs: 559628963 + frame_id: "/world" +pose: + pose: + position: + x: -0.0377455884532 + y: -0.100097054232 + z: 0.0 + orientation: + x: 0.0 + y: 0.000276709580231 + z: 0.0 + w: 0.999999961716 + covariance: [1.631787347209926, -4.97804631382229e-08, 0.0, 0.0, -4.0635989330069155e-05, 0.0, -4.9780463138222885e-08, 1.63178718211217, 0.0, 0.0, 1.3508630087062141e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.063598933006912e-05, 1.3508630087062088e-05, 0.0, 0.0, 1.603334669100237, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 36 + stamp: + secs: 1624875201 + nsecs: 719597101 + frame_id: "/world" +pose: + pose: + position: + x: -0.0379183135091 + y: -0.101250645803 + z: 0.0 + orientation: + x: 0.0 + y: 0.000276810230556 + z: 0.0 + w: 0.999999961688 + covariance: [1.5819868748089125, -4.469491904099483e-08, 0.0, 0.0, -1.2146243082898805e-05, 0.0, -4.469491904099484e-08, 1.581986727469817, 0.0, 0.0, 5.6487880397793325e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.214624308289881e-05, 5.648788039779324e-06, 0.0, 0.0, 1.5590937646749092, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 37 + stamp: + secs: 1624875201 + nsecs: 799607038 + frame_id: "/world" +pose: + pose: + position: + x: -0.0378180036625 + y: -0.0973063325344 + z: 0.0 + orientation: + x: 0.0 + y: 0.000538997905486 + z: 0.0 + w: 0.999999854741 + covariance: [1.6493483195305634, -3.16524518346531e-08, 0.0, 0.0, -3.7270859876198807e-06, 0.0, -3.165245183465308e-08, 1.6493482161883941, 0.0, 0.0, 1.3501422126349808e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7270859876198853e-06, 1.350142212634981e-06, 0.0, 0.0, 1.632898521101565, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 38 + stamp: + secs: 1624875201 + nsecs: 959651947 + frame_id: "/world" +pose: + pose: + position: + x: -0.0386223184876 + y: -0.094963530874 + z: 0.0 + orientation: + x: -0.0 + y: -5.29806272338e-05 + z: -0.0 + w: 0.999999998597 + covariance: [1.5594112760270131, -3.972684389783873e-08, 0.0, 0.0, -3.8987657587604996e-06, 0.0, -3.972684389783888e-08, 1.559411146968128, 0.0, 0.0, 2.473841810074445e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.898765758760506e-06, 2.473841810074444e-06, 0.0, 0.0, 1.536665146874787, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 39 + stamp: + secs: 1624875202 + nsecs: 119643927 + frame_id: "/world" +pose: + pose: + position: + x: -0.044346291302 + y: -0.0933186909233 + z: 0.0 + orientation: + x: -0.0 + y: -0.000373168258589 + z: -0.0 + w: 0.999999930373 + covariance: [1.59733330992441, -2.8434376709611482e-08, 0.0, 0.0, -2.2178845611380467e-06, 0.0, -2.8434376709611456e-08, 1.5973332179962834, 0.0, 0.0, 1.0193648744618593e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.217884561138047e-06, 1.019364874461857e-06, 0.0, 0.0, 1.5798532162847543, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 40 + stamp: + secs: 1624875202 + nsecs: 199664116 + frame_id: "/world" +pose: + pose: + position: + x: -0.0445028775416 + y: -0.0938304405913 + z: 0.0 + orientation: + x: 0.0 + y: 0.000104595164647 + z: 0.0 + w: 0.99999999453 + covariance: [1.6729228014340431, -1.7882971641900337e-08, 0.0, 0.0, -4.071934919431013e-06, 0.0, -1.7882971641900344e-08, 1.6729227435996568, 0.0, 0.0, 2.231797465248661e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.07193491943101e-06, 2.2317974652486568e-07, 0.0, 0.0, 1.6611852562898852, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 41 + stamp: + secs: 1624875202 + nsecs: 279659033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0433307676976 + y: -0.0889342424361 + z: 0.0 + orientation: + x: -0.0 + y: -2.46155965115e-07 + z: -0.0 + w: 1.0 + covariance: [1.6826334386354467, -2.2775695845288156e-08, 0.0, 0.0, -2.6297079694963795e-06, 0.0, -2.2775695845288162e-08, 1.6826333643747082, 0.0, 0.0, 4.912657459554499e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.629707969496376e-06, 4.912657459554494e-07, 0.0, 0.0, 1.6663245843401226, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 42 + stamp: + secs: 1624875202 + nsecs: 439666033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0417404557898 + y: -0.0893794431557 + z: 0.0 + orientation: + x: 0.0 + y: 0.000218185127129 + z: 0.0 + w: 0.999999976198 + covariance: [1.4892241765801857, -2.3651267510989245e-08, 0.0, 0.0, -4.245572901785336e-07, 0.0, -2.3651267510989206e-08, 1.4892240996654744, 0.0, 0.0, 5.502601014376109e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.245572901785317e-07, 5.50260101437611e-06, 0.0, 0.0, 1.4706130681818455, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 43 + stamp: + secs: 1624875202 + nsecs: 519676924 + frame_id: "/world" +pose: + pose: + position: + x: -0.0419304861079 + y: -0.0896571871044 + z: 0.0 + orientation: + x: 0.0 + y: 9.73196905106e-05 + z: 0.0 + w: 0.999999995264 + covariance: [1.4730829357344306, -1.4541577325793268e-08, 0.0, 0.0, -2.539495919868144e-07, 0.0, -1.4541577325793215e-08, 1.4730828973085255, 0.0, 0.0, 2.7096512855637874e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.539495919868148e-07, 2.709651285563784e-05, 0.0, 0.0, 1.4608277523223228, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 44 + stamp: + secs: 1624875202 + nsecs: 679709911 + frame_id: "/world" +pose: + pose: + position: + x: -0.0424010066117 + y: -0.0902143205601 + z: 0.0 + orientation: + x: -0.0 + y: -0.000141061703064 + z: -0.0 + w: 0.999999990051 + covariance: [1.6271246133635615, -2.9143833428754392e-08, 0.0, 0.0, -1.4073961326760227e-06, 0.0, -2.9143833428754382e-08, 1.6271246149955068, 0.0, 0.0, 2.377015182229369e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.407396132676022e-06, 2.3770151822293572e-05, 0.0, 0.0, 1.6005764355703802, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 45 + stamp: + secs: 1624875202 + nsecs: 839711905 + frame_id: "/world" +pose: + pose: + position: + x: -0.042694308045 + y: -0.0906429916197 + z: 0.0 + orientation: + x: 0.0 + y: 0.0003668598338 + z: 0.0 + w: 0.999999932707 + covariance: [1.6340766941865663, -2.579060047927871e-08, 0.0, 0.0, -8.117379910471021e-07, 0.0, -2.57906004792787e-08, 1.6340767256622244, 0.0, 0.0, 4.09291472884541e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.117379910471021e-07, 4.092914728845403e-05, 0.0, 0.0, 1.6091128075682755, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 46 + stamp: + secs: 1624875202 + nsecs: 919713974 + frame_id: "/world" +pose: + pose: + position: + x: -0.0429777559936 + y: -0.0908470232876 + z: 0.0 + orientation: + x: 0.0 + y: 0.0001306382147 + z: 0.0 + w: 0.999999991467 + covariance: [1.5261738481943805, -1.432679118813629e-08, 0.0, 0.0, -4.74085795850265e-06, 0.0, -1.4326791188136276e-08, 1.526173889675054, 0.0, 0.0, 1.3890508040215112e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.740857958502658e-06, 1.3890508040215102e-05, 0.0, 0.0, 1.5121643988355644, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 47 + stamp: + secs: 1624875203 + nsecs: 79710960 + frame_id: "/world" +pose: + pose: + position: + x: -0.0436169848191 + y: -0.0982436720432 + z: 0.0 + orientation: + x: -0.0 + y: -0.000290367961587 + z: -0.0 + w: 0.999999957843 + covariance: [1.6017885372987877, -2.3841341147627994e-08, 0.0, 0.0, -4.015575540279747e-06, 0.0, -2.3841341147628014e-08, 1.6017886189047628, 0.0, 0.0, 1.0103678273456817e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.0155755402797385e-06, 1.0103678273456828e-05, 0.0, 0.0, 1.5784293744844788, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 48 + stamp: + secs: 1624875203 + nsecs: 159800053 + frame_id: "/world" +pose: + pose: + position: + x: -0.0438559583061 + y: -0.0985644157248 + z: 0.0 + orientation: + x: -0.0 + y: -0.000392072414172 + z: -0.0 + w: 0.99999992314 + covariance: [1.4825796374770797, -1.5857829219250565e-08, 0.0, 0.0, -2.69687084061956e-06, 0.0, -1.585782921925057e-08, 1.4825796929604984, 0.0, 0.0, 2.4026375110076312e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.696870840619561e-06, 2.4026375110076257e-06, 0.0, 0.0, 1.4663614268241507, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 49 + stamp: + secs: 1624875203 + nsecs: 319755077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0493230240099 + y: -0.10223199107 + z: 0.0 + orientation: + x: -0.0 + y: -0.000300343739556 + z: -0.0 + w: 0.999999954897 + covariance: [1.60653473250912, -2.7124891909091244e-08, 0.0, 0.0, -3.7550553716098153e-06, 0.0, -2.7124891909091337e-08, 1.6065348270938427, 0.0, 0.0, 4.29409686192339e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7550553716098098e-06, 4.294096861923379e-06, 0.0, 0.0, 1.5756771965882284, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 50 + stamp: + secs: 1624875203 + nsecs: 479762077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0505490783351 + y: -0.111605085793 + z: 0.0 + orientation: + x: -0.0 + y: -0.000241428121693 + z: -0.0 + w: 0.999999970856 + covariance: [1.84737049079415, -2.922627256976527e-08, 0.0, 0.0, -1.7025309494107868e-06, 0.0, -2.922627256976527e-08, 1.8473705918198282, 0.0, 0.0, 6.238056914969386e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7025309494107874e-06, 6.238056914969388e-06, 0.0, 0.0, 1.811477749686665, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 51 + stamp: + secs: 1624875203 + nsecs: 559773922 + frame_id: "/world" +pose: + pose: + position: + x: -0.051119566352 + y: -0.112466968817 + z: 0.0 + orientation: + x: -0.0 + y: -0.000455535058267 + z: -0.0 + w: 0.999999896244 + covariance: [1.7568837631224563, -1.7931930620798718e-08, 0.0, 0.0, -3.1785120140042935e-05, 0.0, -1.7931930620798695e-08, 1.7568838167031033, 0.0, 0.0, 2.094560887493035e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.178512014004293e-05, 2.0945608874930366e-06, 0.0, 0.0, 1.733772696177587, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 52 + stamp: + secs: 1624875203 + nsecs: 719793081 + frame_id: "/world" +pose: + pose: + position: + x: -0.0485611986579 + y: -0.10531277436 + z: 0.0 + orientation: + x: -0.0 + y: -0.000966981687428 + z: -0.0 + w: 0.999999532473 + covariance: [1.7584649746109604, -2.2260416656094905e-08, 0.0, 0.0, -2.5224541905817038e-05, 0.0, -2.2260416656094846e-08, 1.7584649933436465, 0.0, 0.0, 3.989353299752479e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5224541905817103e-05, 3.989353299752492e-06, 0.0, 0.0, 1.7291637430559073, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 53 + stamp: + secs: 1624875203 + nsecs: 799793005 + frame_id: "/world" +pose: + pose: + position: + x: -0.0519732983567 + y: -0.104805326161 + z: 0.0 + orientation: + x: -0.0 + y: -0.000632691528 + z: -0.0 + w: 0.999999799851 + covariance: [1.6899575875831965, -1.4315400442820027e-08, 0.0, 0.0, -6.062236427912193e-06, 0.0, -1.4315400442820041e-08, 1.689957590037377, 0.0, 0.0, 5.858032734096925e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.0622364279122005e-06, 5.858032734096915e-06, 0.0, 0.0, 1.6709784268601051, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 54 + stamp: + secs: 1624875203 + nsecs: 959813118 + frame_id: "/world" +pose: + pose: + position: + x: -0.0532496421776 + y: -0.104018366042 + z: 0.0 + orientation: + x: -0.0 + y: -0.000417159303538 + z: -0.0 + w: 0.999999912989 + covariance: [1.690189314550645, -1.6692355513810168e-08, 0.0, 0.0, -1.4308254529764918e-05, 0.0, -1.669235551381015e-08, 1.6901893128790795, 0.0, 0.0, 4.419280349902722e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4308254529764884e-05, 4.4192803499027135e-06, 0.0, 0.0, 1.6669537372843841, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 55 + stamp: + secs: 1624875204 + nsecs: 39814949 + frame_id: "/world" +pose: + pose: + position: + x: -0.0494467384867 + y: -0.0908628917065 + z: 0.0 + orientation: + x: 0.0 + y: 0.000142091509724 + z: 0.0 + w: 0.999999989905 + covariance: [1.7415896588455306, -1.2347610114612996e-08, 0.0, 0.0, -6.445133200591962e-06, 0.0, -1.2347610114613013e-08, 1.741589653970016, 0.0, 0.0, 1.3130970555017154e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.4451332005919656e-06, 1.3130970555017186e-06, 0.0, 0.0, 1.724154045334715, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 56 + stamp: + secs: 1624875204 + nsecs: 199822903 + frame_id: "/world" +pose: + pose: + position: + x: -0.0502074031025 + y: -0.0913759483059 + z: 0.0 + orientation: + x: -0.0 + y: -0.000524794202554 + z: -0.0 + w: 0.999999862296 + covariance: [1.721128162305667, -1.4969301643558706e-08, 0.0, 0.0, -2.912118240908404e-06, 0.0, -1.4969301643558722e-08, 1.7211281544922326, 0.0, 0.0, 3.8143878236581737e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.912118240908407e-06, 3.8143878236581745e-06, 0.0, 0.0, 1.6988297756645927, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 57 + stamp: + secs: 1624875204 + nsecs: 279830933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0458441319714 + y: -0.0914166477506 + z: 0.0 + orientation: + x: -0.0 + y: -0.0010054083722 + z: -0.0 + w: 0.999999494577 + covariance: [1.8753385782665344, -1.3390146805105362e-08, 0.0, 0.0, 1.954588384354656e-05, 0.0, -1.3390146805105324e-08, 1.8753385688903415, 0.0, 0.0, -1.609882161697127e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 1.9545883843546548e-05, -1.6098821616971285e-06, 0.0, 0.0, 1.8549799691652982, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 58 + stamp: + secs: 1624875204 + nsecs: 439868927 + frame_id: "/world" +pose: + pose: + position: + x: -0.0406754034563 + y: -0.103255378783 + z: 0.0 + orientation: + x: -0.0 + y: -0.00115889885933 + z: -0.0 + w: 0.999999328476 + covariance: [2.2356836525157413, -2.6356396038873715e-08, 0.0, 0.0, 2.1208240636445297e-05, 0.0, -2.6356396038873685e-08, 2.2356836138162635, 0.0, 0.0, 1.4524347245559193e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 2.1208240636445294e-05, 1.4524347245559191e-05, 0.0, 0.0, 2.193818127584565, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 59 + stamp: + secs: 1624875204 + nsecs: 519855022 + frame_id: "/world" +pose: + pose: + position: + x: -0.0401683794027 + y: -0.100363908088 + z: 0.0 + orientation: + x: 0.0 + y: 0.000246163536015 + z: 0.0 + w: 0.999999969702 + covariance: [1.9441370828689204, -1.003092440794691e-08, 0.0, 0.0, 1.6521734397098366e-06, 0.0, -1.003092440794688e-08, 1.9441370666945148, 0.0, 0.0, 5.161410872489511e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 1.6521734397098368e-06, 5.161410872489506e-06, 0.0, 0.0, 1.9267718728121748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 60 + stamp: + secs: 1624875204 + nsecs: 599853039 + frame_id: "/world" +pose: + pose: + position: + x: -0.0320538877694 + y: -0.101508990045 + z: 0.0 + orientation: + x: -0.0 + y: -0.000486444474804 + z: -0.0 + w: 0.999999881686 + covariance: [2.224023441653499, -1.232025025036988e-08, 0.0, 0.0, -1.262694507292239e-06, 0.0, -1.2320250250369877e-08, 2.2240234212495165, 0.0, 0.0, 2.8562568752089127e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2626945072922387e-06, 2.856256875208906e-06, 0.0, 0.0, 2.200976882258232, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 61 + stamp: + secs: 1624875204 + nsecs: 679869890 + frame_id: "/world" +pose: + pose: + position: + x: -0.0316030396462 + y: -0.0983281188403 + z: 0.0 + orientation: + x: -0.0 + y: -0.000180620695683 + z: -0.0 + w: 0.999999983688 + covariance: [1.9130502209114313, -1.0900965218086603e-08, 0.0, 0.0, -5.649689118215256e-07, 0.0, -1.09009652180866e-08, 1.9130502027593808, 0.0, 0.0, 9.436160387824917e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.649689118215253e-07, 9.436160387824895e-07, 0.0, 0.0, 1.8908498236309916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 62 + stamp: + secs: 1624875204 + nsecs: 839879036 + frame_id: "/world" +pose: + pose: + position: + x: -0.0200144671656 + y: -0.0400120765718 + z: 0.0 + orientation: + x: -0.0 + y: -0.000316972996109 + z: -0.0 + w: 0.999999949764 + covariance: [1.7028245532933362, -1.0082556174570601e-08, 0.0, 0.0, -1.9425534995711304e-07, 0.0, -1.0082556174570624e-08, 1.7028245364817804, 0.0, 0.0, 3.818537243014908e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9425534995711333e-07, 3.818537243014911e-07, 0.0, 0.0, 1.680368114463213, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 63 + stamp: + secs: 1624875204 + nsecs: 919888020 + frame_id: "/world" +pose: + pose: + position: + x: -0.0220065618636 + y: -0.0436619838995 + z: 0.0 + orientation: + x: -0.0 + y: -4.0558182804e-05 + z: -0.0 + w: 0.999999999178 + covariance: [1.7098937804014613, -5.9414100768566e-09, 0.0, 0.0, -8.284742576197036e-07, 0.0, -5.941410076856598e-09, 1.7098937704965722, 0.0, 0.0, 6.224309520825801e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.284742576197045e-07, 6.224309520825804e-07, 0.0, 0.0, 1.695813601176916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 64 + stamp: + secs: 1624875205 + nsecs: 79900026 + frame_id: "/world" +pose: + pose: + position: + x: -0.0230240562091 + y: -0.03689692522 + z: 0.0 + orientation: + x: -0.0 + y: -0.000147503297175 + z: -0.0 + w: 0.999999989121 + covariance: [1.6626264391578567, -8.276977671200087e-09, 0.0, 0.0, 2.039765183880795e-06, 0.0, -8.276977671200054e-09, 1.662626425335902, 0.0, 0.0, 4.504318233839471e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 2.039765183880795e-06, 4.504318233839477e-07, 0.0, 0.0, 1.6411049130688857, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 65 + stamp: + secs: 1624875205 + nsecs: 159907103 + frame_id: "/world" +pose: + pose: + position: + x: -0.0266496882937 + y: -0.0465259059609 + z: 0.0 + orientation: + x: -0.0 + y: -0.000111931322106 + z: -0.0 + w: 0.999999993736 + covariance: [1.9737145997554886, -5.544216645388783e-09, 0.0, 0.0, -2.5363725571601462e-06, 0.0, -5.544216645388777e-09, 1.9737145903896072, 0.0, 0.0, 7.500137520407622e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5363725571601467e-06, 7.500137520407632e-07, 0.0, 0.0, 1.958590089675371, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 66 + stamp: + secs: 1624875205 + nsecs: 319930077 + frame_id: "/world" +pose: + pose: + position: + x: -0.0269142603989 + y: -0.0175280208285 + z: 0.0 + orientation: + x: -0.0 + y: -0.000500599664265 + z: -0.0 + w: 0.9999998747 + covariance: [1.7589721179624682, -5.4717752090678645e-09, 0.0, 0.0, -1.9249645974366213e-06, 0.0, -5.471775209067857e-09, 1.758972108458479, 0.0, 0.0, 4.4263254141069117e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9249645974366226e-06, 4.42632541410691e-07, 0.0, 0.0, 1.743096493958481, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 67 + stamp: + secs: 1624875205 + nsecs: 399924994 + frame_id: "/world" +pose: + pose: + position: + x: -0.00502593884927 + y: -0.0332294060332 + z: 0.0 + orientation: + x: -0.0 + y: -0.00172873658121 + z: -0.0 + w: 0.999998505734 + covariance: [2.36773689835362, -1.060180967951381e-08, 0.0, 0.0, -3.65062682618636e-06, 0.0, -1.0601809679513795e-08, 2.3677368797062437, 0.0, 0.0, 5.592820416019655e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.650626826186359e-06, 5.592820416019654e-07, 0.0, 0.0, 2.335083660380648, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 68 + stamp: + secs: 1624875205 + nsecs: 639954090 + frame_id: "/world" +pose: + pose: + position: + x: 0.000915048102236 + y: -0.0352859342117 + z: 0.0 + orientation: + x: -0.0 + y: -0.000558478734636 + z: -0.0 + w: 0.999999844051 + covariance: [1.814886534292705, -1.4620742680822371e-08, 0.0, 0.0, -5.528590868892705e-05, 0.0, -1.4620742680822368e-08, 1.814886492645989, 0.0, 0.0, 2.6639326119657106e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.5285908688926943e-05, 2.6639326119657063e-06, 0.0, 0.0, 1.7665315613990007, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 69 + stamp: + secs: 1624875205 + nsecs: 719960928 + frame_id: "/world" +pose: + pose: + position: + x: 0.000487930395772 + y: -0.0348964023964 + z: 0.0 + orientation: + x: -0.0 + y: -6.36711653451e-06 + z: -0.0 + w: 0.99999999998 + covariance: [1.8384861658215785, -4.115242282418451e-09, 0.0, 0.0, -1.3183649634973198e-05, 0.0, -4.115242282418457e-09, 1.8384861465237596, 0.0, 0.0, 6.289441550349059e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3183649634973224e-05, 6.289441550349059e-07, 0.0, 0.0, 1.8255404847461798, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 70 + stamp: + secs: 1624875205 + nsecs: 799964905 + frame_id: "/world" +pose: + pose: + position: + x: 0.000654237467015 + y: -0.0248910982783 + z: 0.0 + orientation: + x: -0.0 + y: -0.000487911584604 + z: -0.0 + w: 0.999999880971 + covariance: [2.076062220120913, -6.772168283757481e-09, 0.0, 0.0, -1.6836750649049097e-05, 0.0, -6.7721682837574865e-09, 2.0760621743196483, 0.0, 0.0, 3.2736171135276477e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6836750649049097e-05, 3.2736171135276466e-07, 0.0, 0.0, 2.0557069752080324, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 71 + stamp: + secs: 1624875205 + nsecs: 879976034 + frame_id: "/world" +pose: + pose: + position: + x: 0.00358072680381 + y: -0.0204914290119 + z: 0.0 + orientation: + x: -0.0 + y: -0.000794501597799 + z: -0.0 + w: 0.999999684384 + covariance: [2.061209702114319, -8.701699160538504e-09, 0.0, 0.0, -9.99234822052326e-06, 0.0, -8.701699160538493e-09, 2.0612096357892655, 0.0, 0.0, 9.67013952701286e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.992348220523255e-06, 9.670139527012846e-07, 0.0, 0.0, 2.0339946047395476, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 72 + stamp: + secs: 1624875206 + nsecs: 40004015 + frame_id: "/world" +pose: + pose: + position: + x: 0.00374962264163 + y: -0.0201459596281 + z: 0.0 + orientation: + x: -0.0 + y: -3.67823224402e-05 + z: -0.0 + w: 0.999999999324 + covariance: [1.8374565777051153, -7.842096864430714e-09, 0.0, 0.0, -1.2023104232680047e-05, 0.0, -7.842096864430737e-09, 1.8374565159381022, 0.0, 0.0, 9.191710945817088e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.202310423268008e-05, 9.191710945817081e-07, 0.0, 0.0, 1.8110341062945683, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 73 + stamp: + secs: 1624875206 + nsecs: 119993925 + frame_id: "/world" +pose: + pose: + position: + x: 0.0104055911001 + y: -0.028960935542 + z: 0.0 + orientation: + x: 0.0 + y: 0.000171465079911 + z: 0.0 + w: 0.9999999853 + covariance: [2.2595335900073517, -6.35184944667205e-09, 0.0, 0.0, -6.172743038743401e-06, 0.0, -6.351849446672053e-09, 2.2595335405020833, 0.0, 0.0, 7.3546025999592865e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.172743038743407e-06, 7.354602599959281e-06, 0.0, 0.0, 2.238039543414941, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 74 + stamp: + secs: 1624875206 + nsecs: 199990988 + frame_id: "/world" +pose: + pose: + position: + x: 0.010431663715 + y: -0.0289205949462 + z: 0.0 + orientation: + x: 0.0 + y: 0.000229238872598 + z: 0.0 + w: 0.999999973725 + covariance: [1.9908053439204292, -7.168988118452248e-09, 0.0, 0.0, -6.6204886620212e-06, 0.0, -7.168988118452242e-09, 1.9908052963083813, 0.0, 0.0, 1.7811831647884757e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.620488662021201e-06, 1.7811831647884804e-05, 0.0, 0.0, 1.9679211687927058, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 75 + stamp: + secs: 1624875206 + nsecs: 360021114 + frame_id: "/world" +pose: + pose: + position: + x: 0.00785914318748 + y: -0.0350768428395 + z: 0.0 + orientation: + x: 0.0 + y: 0.000248634792076 + z: 0.0 + w: 0.99999996909 + covariance: [2.0703869372238475, -2.693776379122903e-08, 0.0, 0.0, -7.60039416416451e-05, 0.0, -2.693776379122903e-08, 2.070386838074582, 0.0, 0.0, 3.192255168623166e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.60039416416451e-05, 3.192255168623164e-05, 0.0, 0.0, 2.0262986303594546, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 76 + stamp: + secs: 1624875206 + nsecs: 440040112 + frame_id: "/world" +pose: + pose: + position: + x: 0.00729663358885 + y: -0.0299380414191 + z: 0.0 + orientation: + x: -0.0 + y: -0.000175934079993 + z: -0.0 + w: 0.999999984524 + covariance: [2.0185997122747614, -3.129135822852729e-08, 0.0, 0.0, -3.268252870199444e-05, 0.0, -3.1291358228527326e-08, 2.018599613155992, 0.0, 0.0, 1.2672590987433358e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.268252870199439e-05, 1.267259098743336e-05, 0.0, 0.0, 1.990758746164286, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 77 + stamp: + secs: 1624875206 + nsecs: 520046949 + frame_id: "/world" +pose: + pose: + position: + x: 0.00733621385628 + y: -0.0303820392214 + z: 0.0 + orientation: + x: 0.0 + y: 4.16339982765e-05 + z: 0.0 + w: 0.999999999133 + covariance: [2.058872321102207, -4.202269411675908e-08, 0.0, 0.0, -3.892845415776081e-05, 0.0, -4.2022694116759e-08, 2.058872188840962, 0.0, 0.0, 6.674223576324628e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.892845415776076e-05, 6.674223576324626e-06, 0.0, 0.0, 2.0282161283893023, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 78 + stamp: + secs: 1624875206 + nsecs: 680032969 + frame_id: "/world" +pose: + pose: + position: + x: -0.00326216756132 + y: -0.0382560828494 + z: 0.0 + orientation: + x: 0.0 + y: 0.000654857864109 + z: 0.0 + w: 0.999999785581 + covariance: [2.6833053800628184, -2.2453229870170714e-07, 0.0, 0.0, -0.00042736016868733217, 0.0, -2.2453229870170733e-07, 2.6833045599186542, 0.0, 0.0, 0.00010426928004800407, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00042736016868733255, 0.00010426928004800414, 0.0, 0.0, 2.616112683178166, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 79 + stamp: + secs: 1624875206 + nsecs: 760040998 + frame_id: "/world" +pose: + pose: + position: + x: -0.00339454594581 + y: -0.0395768513578 + z: 0.0 + orientation: + x: 0.0 + y: 3.83969782731e-05 + z: 0.0 + w: 0.999999999263 + covariance: [1.7985984875954946, -1.290144350572323e-07, 0.0, 0.0, -9.160050612739759e-05, 0.0, -1.2901443505723215e-07, 1.7985980113558706, 0.0, 0.0, 2.377432212042449e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.16005061273979e-05, 2.3774322120424525e-05, 0.0, 0.0, 1.7816697781723911, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 80 + stamp: + secs: 1624875206 + nsecs: 840075016 + frame_id: "/world" +pose: + pose: + position: + x: -0.00343968751584 + y: -0.0402519430325 + z: 0.0 + orientation: + x: 0.0 + y: 0.00044407023632 + z: 0.0 + w: 0.999999901401 + covariance: [1.7757537798636551, -1.4933294896593207e-07, 0.0, 0.0, -2.743492850514558e-05, 0.0, -1.4933294896593226e-07, 1.775753231050367, 0.0, 0.0, 7.451675207806738e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.743492850514565e-05, 7.451675207806744e-06, 0.0, 0.0, 1.7623314766118436, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 81 + stamp: + secs: 1624875206 + nsecs: 920078039 + frame_id: "/world" +pose: + pose: + position: + x: -0.00246173577697 + y: -0.0295442996873 + z: 0.0 + orientation: + x: 0.0 + y: 1.18165935754e-05 + z: 0.0 + w: 0.99999999993 + covariance: [1.6902744992829395, -2.0580428685988454e-07, 0.0, 0.0, -8.605845414963455e-06, 0.0, -2.058042868598842e-07, 1.6902737440121003, 0.0, 0.0, 2.422437042719097e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.60584541496347e-06, 2.4224370427190994e-06, 0.0, 0.0, 1.6725927172120376, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 82 + stamp: + secs: 1624875207 + nsecs: 80077887 + frame_id: "/world" +pose: + pose: + position: + x: -0.00821615852125 + y: -0.0317940629996 + z: 0.0 + orientation: + x: 0.0 + y: 0.000620694243467 + z: 0.0 + w: 0.999999807369 + covariance: [1.8636980566411203, -4.839730953360694e-07, 0.0, 0.0, -8.338566502042762e-06, 0.0, -4.839730953360695e-07, 1.8636962942470128, 0.0, 0.0, 3.226998618054302e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.338566502042764e-06, 3.226998618054303e-05, 0.0, 0.0, 1.8189392706097263, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 83 + stamp: + secs: 1624875207 + nsecs: 240103006 + frame_id: "/world" +pose: + pose: + position: + x: -0.00669040752248 + y: -0.0880337867714 + z: 0.0 + orientation: + x: -0.0 + y: -0.000789906092025 + z: -0.0 + w: 0.999999688024 + covariance: [1.8711996169300524, -4.135006044691124e-07, 0.0, 0.0, -4.356049060256984e-06, 0.0, -4.1350060446911306e-07, 1.8711981418115107, 0.0, 0.0, 1.6347039593020524e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.356049060256984e-06, 1.6347039593020544e-05, 0.0, 0.0, 1.8294016044511539, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 84 + stamp: + secs: 1624875207 + nsecs: 320096970 + frame_id: "/world" +pose: + pose: + position: + x: -0.00592696577547 + y: -0.0283491020271 + z: 0.0 + orientation: + x: -0.0 + y: -0.00400013906984 + z: -0.0 + w: 0.999991999412 + covariance: [1.8254277226526838, -2.5848726571057886e-07, 0.0, 0.0, -1.2038847706237911e-06, 0.0, -2.5848726571057897e-07, 1.8254268080557343, 0.0, 0.0, 4.040646512021606e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2038847706237905e-06, 4.040646512021594e-06, 0.0, 0.0, 1.7972944760545944, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 85 + stamp: + secs: 1624875207 + nsecs: 480118990 + frame_id: "/world" +pose: + pose: + position: + x: -0.00427694282486 + y: -0.0345147050858 + z: 0.0 + orientation: + x: 0.0 + y: 0.0023167359751 + z: 0.0 + w: 0.999997316364 + covariance: [1.6297797477971856, -2.8477501588407455e-07, 0.0, 0.0, 5.112830004550244e-07, 0.0, -2.8477501588407397e-07, 1.6297787436769104, 0.0, 0.0, 9.339361107790823e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 5.112830004550251e-07, 9.339361107790807e-07, 0.0, 0.0, 1.5945939262185393, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 86 + stamp: + secs: 1624875207 + nsecs: 640136957 + frame_id: "/world" +pose: + pose: + position: + x: -0.00757101970728 + y: -0.0320286322555 + z: 0.0 + orientation: + x: -0.0 + y: -0.00155990848621 + z: -0.0 + w: 0.999998783342 + covariance: [1.6614432577736096, -1.9714609870693578e-07, 0.0, 0.0, -2.649406162343664e-05, 0.0, -1.9714609870693555e-07, 1.6614425605895764, 0.0, 0.0, 4.118822135091302e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6494061623436728e-05, 4.118822135091312e-06, 0.0, 0.0, 1.6350442185544718, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 87 + stamp: + secs: 1624875207 + nsecs: 720158100 + frame_id: "/world" +pose: + pose: + position: + x: -0.00329196440245 + y: -0.032853458787 + z: 0.0 + orientation: + x: 0.0 + y: 0.000326546679687 + z: 0.0 + w: 0.999999946684 + covariance: [1.6040678153099837, -1.3011900341901313e-07, 0.0, 0.0, -1.042096607231989e-05, 0.0, -1.3011900341901287e-07, 1.6040673478257008, 0.0, 0.0, 1.858924208829491e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0420966072319891e-05, 1.8589242088294905e-06, 0.0, 0.0, 1.5856007730756487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 88 + stamp: + secs: 1624875207 + nsecs: 880158901 + frame_id: "/world" +pose: + pose: + position: + x: -0.00356324457845 + y: -0.0337990906803 + z: 0.0 + orientation: + x: -0.0 + y: -0.000634146938411 + z: -0.0 + w: 0.999999798929 + covariance: [1.7176335416929889, -1.7937846867736778e-07, 0.0, 0.0, -3.875965590353022e-06, 0.0, -1.7937846867736816e-07, 1.7176328924872304, 0.0, 0.0, 4.1852318415957595e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.875965590353015e-06, 4.1852318415957526e-07, 0.0, 0.0, 1.689673957413502, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 89 + stamp: + secs: 1624875207 + nsecs: 960160017 + frame_id: "/world" +pose: + pose: + position: + x: -0.00361146300675 + y: -0.0345568392217 + z: 0.0 + orientation: + x: 0.0 + y: 0.000132370079954 + z: 0.0 + w: 0.999999991239 + covariance: [1.6339774928720778, -1.1738435026580455e-07, 0.0, 0.0, -1.0584383440271161e-05, 0.0, -1.1738435026580415e-07, 1.6339770671879437, 0.0, 0.0, 2.3721108084883797e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0584383440271158e-05, 2.3721108084883797e-06, 0.0, 0.0, 1.6142633223224827, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 90 + stamp: + secs: 1624875208 + nsecs: 120165110 + frame_id: "/world" +pose: + pose: + position: + x: -0.00382407113079 + y: -0.0355370020367 + z: 0.0 + orientation: + x: -0.0 + y: -0.000296782066312 + z: -0.0 + w: 0.99999995596 + covariance: [1.7212915937665996, -1.6234818554945164e-07, 0.0, 0.0, -1.3964181568622037e-05, 0.0, -1.623481855494517e-07, 1.7212910069379699, 0.0, 0.0, 9.16140082263144e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.396418156862204e-05, 9.161400822631454e-06, 0.0, 0.0, 1.6915310290709678, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 91 + stamp: + secs: 1624875208 + nsecs: 200181961 + frame_id: "/world" +pose: + pose: + position: + x: -0.0050723855732 + y: -0.0440310058195 + z: 0.0 + orientation: + x: -0.0 + y: -0.000412507104631 + z: -0.0 + w: 0.999999914919 + covariance: [1.7478717226096319, -1.1574713769071338e-07, 0.0, 0.0, -1.926546373056468e-05, 0.0, -1.1574713769071309e-07, 1.7478713173790048, 0.0, 0.0, 1.8519057019452253e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.926546373056465e-05, 1.8519057019452287e-05, 0.0, 0.0, 1.7260674659991069, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 92 + stamp: + secs: 1624875208 + nsecs: 360193968 + frame_id: "/world" +pose: + pose: + position: + x: -0.00552432225663 + y: -0.0473789401518 + z: 0.0 + orientation: + x: -0.0 + y: -0.000407679556129 + z: -0.0 + w: 0.999999916899 + covariance: [1.7606384814445657, -1.6920718050462404e-07, 0.0, 0.0, -1.71207824203988e-05, 0.0, -1.692071805046245e-07, 1.7606379444433968, 0.0, 0.0, 2.251255923945774e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7120782420398767e-05, 2.2512559239457718e-05, 0.0, 0.0, 1.7280773139665202, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 93 + stamp: + secs: 1624875208 + nsecs: 440201998 + frame_id: "/world" +pose: + pose: + position: + x: -0.00573814580659 + y: -0.048284491919 + z: 0.0 + orientation: + x: -0.0 + y: -0.000145639975462 + z: -0.0 + w: 0.999999989394 + covariance: [1.7494457784046296, -1.0737174290387708e-07, 0.0, 0.0, -1.8221926922154206e-05, 0.0, -1.073717429038775e-07, 1.7494454526333596, 0.0, 0.0, 7.69432780200795e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.822192692215421e-05, 7.694327802007916e-06, 0.0, 0.0, 1.7283804743562345, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 94 + stamp: + secs: 1624875208 + nsecs: 600233078 + frame_id: "/world" +pose: + pose: + position: + x: -0.00441720612399 + y: -0.0143104799209 + z: 0.0 + orientation: + x: -0.0 + y: -0.000898818249404 + z: -0.0 + w: 0.999999596063 + covariance: [1.755540367965466, -1.3012863943496627e-07, 0.0, 0.0, -4.6314329238828526e-05, 0.0, -1.3012863943496592e-07, 1.755539964388035, 0.0, 0.0, 3.0504352014404467e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.6314329238828506e-05, 3.0504352014404425e-06, 0.0, 0.0, 1.728443756959659, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 95 + stamp: + secs: 1624875208 + nsecs: 760227919 + frame_id: "/world" +pose: + pose: + position: + x: -0.00875847401444 + y: -0.0154019784809 + z: 0.0 + orientation: + x: -0.0 + y: -0.000464915948349 + z: -0.0 + w: 0.999999891927 + covariance: [1.665502210506273, -1.0291300213564642e-07, 0.0, 0.0, -5.0621628960778616e-05, 0.0, -1.0291300213564628e-07, 1.6655018551936212, 0.0, 0.0, 7.78742200340651e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.06216289607787e-05, 7.787422003406519e-06, 0.0, 0.0, 1.6436355051644356, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 96 + stamp: + secs: 1624875208 + nsecs: 840251923 + frame_id: "/world" +pose: + pose: + position: + x: -0.0048650978665 + y: -0.0149031529164 + z: 0.0 + orientation: + x: -0.0 + y: -0.000675393829776 + z: -0.0 + w: 0.999999771922 + covariance: [1.7076282226493074, -6.661029969352896e-08, 0.0, 0.0, -1.9086760063287902e-05, 0.0, -6.661029969352887e-08, 1.7076279783538304, 0.0, 0.0, 3.2527120769580688e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.90867600632879e-05, 3.2527120769580713e-06, 0.0, 0.0, 1.6937754658007105, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 97 + stamp: + secs: 1624875209 + nsecs: 256062 + frame_id: "/world" +pose: + pose: + position: + x: -0.00836897567144 + y: -0.000198529961433 + z: 0.0 + orientation: + x: -0.0 + y: -0.000708145875186 + z: -0.0 + w: 0.999999749265 + covariance: [2.2639347615328345, -1.7410817352037372e-07, 0.0, 0.0, -2.2682286731668303e-05, 0.0, -1.7410817352037345e-07, 2.2639341084734594, 0.0, 0.0, 8.802747091435968e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.268228673166828e-05, 8.80274709143597e-06, 0.0, 0.0, 2.225319097221873, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 98 + stamp: + secs: 1624875209 + nsecs: 80250978 + frame_id: "/world" +pose: + pose: + position: + x: -0.0123811811887 + y: 0.00261610939291 + z: 0.0 + orientation: + x: 0.0 + y: 1.8918256449e-05 + z: 0.0 + w: 0.999999999821 + covariance: [1.950211143806021, -9.115537249034647e-08, 0.0, 0.0, -1.6457441496081313e-05, 0.0, -9.115537249034638e-08, 1.950210801844978, 0.0, 0.0, 3.7173008262198036e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.645744149608132e-05, 3.7173008262198074e-06, 0.0, 0.0, 1.9291039310692284, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 99 + stamp: + secs: 1624875209 + nsecs: 160267115 + frame_id: "/world" +pose: + pose: + position: + x: -0.015854664647 + y: -0.00411641411716 + z: 0.0 + orientation: + x: -0.0 + y: -0.000758654279928 + z: -0.0 + w: 0.999999712222 + covariance: [1.9302244361453849, -9.582727880531267e-08, 0.0, 0.0, -4.820024722222409e-05, 0.0, -9.582727880531233e-08, 1.930224076113128, 0.0, 0.0, 1.2809551950644853e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.820024722222406e-05, 1.2809551950644846e-05, 0.0, 0.0, 1.907600409513591, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 100 + stamp: + secs: 1624875209 + nsecs: 320278883 + frame_id: "/world" +pose: + pose: + position: + x: -0.0176537944286 + y: -0.0107489380159 + z: 0.0 + orientation: + x: -0.0 + y: -0.000815600163292 + z: -0.0 + w: 0.999999667398 + covariance: [1.8297123838285967, -1.1714023635125232e-07, 0.0, 0.0, -2.7823861107502735e-05, 0.0, -1.1714023635125227e-07, 1.8297119492162224, 0.0, 0.0, 8.19461831004986e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.7823861107502653e-05, 8.194618310049838e-06, 0.0, 0.0, 1.8032142978814398, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 101 + stamp: + secs: 1624875209 + nsecs: 400302887 + frame_id: "/world" +pose: + pose: + position: + x: -0.0188562982378 + y: 0.0191978026038 + z: 0.0 + orientation: + x: -0.0 + y: -0.00251721351973 + z: -0.0 + w: 0.999996831813 + covariance: [2.2634169073766186, -1.1079395375356554e-07, 0.0, 0.0, -9.7822021406345e-06, 0.0, -1.1079395375356541e-07, 2.2634164981982803, 0.0, 0.0, 3.0137814559272017e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.782202140634493e-06, 3.0137814559272e-06, 0.0, 0.0, 2.237273617176817, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 102 + stamp: + secs: 1624875209 + nsecs: 640314102 + frame_id: "/world" +pose: + pose: + position: + x: -0.0226500453243 + y: 0.00868031054124 + z: 0.0 + orientation: + x: -0.0 + y: -0.000558053224148 + z: -0.0 + w: 0.999999844288 + covariance: [2.7264445493091114, -5.445060667631012e-07, 0.0, 0.0, -3.757157897646139e-05, 0.0, -5.445060667631013e-07, 2.7264425406228905, 0.0, 0.0, 5.036235011324074e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.757157897646136e-05, 5.036235011324065e-06, 0.0, 0.0, 2.5772697954842028, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 103 + stamp: + secs: 1624875209 + nsecs: 800335884 + frame_id: "/world" +pose: + pose: + position: + x: -0.0230214917772 + y: 0.00816598389624 + z: 0.0 + orientation: + x: -0.0 + y: -0.000359351812885 + z: -0.0 + w: 0.999999935433 + covariance: [1.9192466769154861, -1.3194379498795747e-07, 0.0, 0.0, -1.0172049618106268e-05, 0.0, -1.3194379498795713e-07, 1.9192461906799372, 0.0, 0.0, 7.81342821394139e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0172049618106267e-05, 7.81342821394141e-06, 0.0, 0.0, 1.880160013081389, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 104 + stamp: + secs: 1624875209 + nsecs: 880331039 + frame_id: "/world" +pose: + pose: + position: + x: -0.0189030370624 + y: 0.0201314226004 + z: 0.0 + orientation: + x: -0.0 + y: -0.00068298572671 + z: -0.0 + w: 0.999999766765 + covariance: [1.9499457089296692, -3.0919708529097934e-08, 0.0, 0.0, -2.872388861410236e-06, 0.0, -3.0919708529097835e-08, 1.94994559566827, 0.0, 0.0, 2.2896289334546105e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.8723888614102303e-06, 2.289628933454613e-06, 0.0, 0.0, 1.9406232179513276, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 105 + stamp: + secs: 1624875209 + nsecs: 960352898 + frame_id: "/world" +pose: + pose: + position: + x: -0.0172645085389 + y: 0.0121864065367 + z: 0.0 + orientation: + x: -0.0 + y: -0.000248222073615 + z: -0.0 + w: 0.999999969193 + covariance: [1.9202764219969553, -3.6699361807453496e-08, 0.0, 0.0, -1.7644648159514204e-06, 0.0, -3.669936180745341e-08, 1.9202762891533507, 0.0, 0.0, 3.853491314920656e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7644648159514183e-06, 3.853491314920655e-06, 0.0, 0.0, 1.9081279979090322, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 106 + stamp: + secs: 1624875210 + nsecs: 40358067 + frame_id: "/world" +pose: + pose: + position: + x: -0.015800821562 + y: 0.0205928897181 + z: 0.0 + orientation: + x: -0.0 + y: -0.000715114604136 + z: -0.0 + w: 0.999999744306 + covariance: [1.7868420685733515, -4.520472855443249e-08, 0.0, 0.0, -6.413384191381361e-06, 0.0, -4.5204728554432324e-08, 1.7868419059365102, 0.0, 0.0, 1.835420625583562e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.413384191381355e-06, 1.8354206255835638e-06, 0.0, 0.0, 1.7711601903415117, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 107 + stamp: + secs: 1624875210 + nsecs: 200356007 + frame_id: "/world" +pose: + pose: + position: + x: -0.0215801325025 + y: 0.00944961005098 + z: 0.0 + orientation: + x: -0.0 + y: -0.00102349444487 + z: -0.0 + w: 0.999999476229 + covariance: [2.3786207934169528, -1.0497014591544331e-07, 0.0, 0.0, -1.2608155957759731e-05, 0.0, -1.0497014591544322e-07, 2.3786204144421035, 0.0, 0.0, 1.2625407141937574e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.260815595775972e-05, 1.2625407141937578e-06, 0.0, 0.0, 2.338427103592896, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 108 + stamp: + secs: 1624875210 + nsecs: 280395031 + frame_id: "/world" +pose: + pose: + position: + x: -0.0177650671655 + y: 0.014427704836 + z: 0.0 + orientation: + x: -0.0 + y: -0.000133968059037 + z: -0.0 + w: 0.999999991026 + covariance: [1.915970495894569, -3.899513611452234e-08, 0.0, 0.0, -3.020423791447676e-06, 0.0, -3.899513611452232e-08, 1.9159703548455238, 0.0, 0.0, 8.32435535259478e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.0204237914476806e-06, 8.324355352594779e-07, 0.0, 0.0, 1.9003392566737118, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 109 + stamp: + secs: 1624875210 + nsecs: 360379934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0180528364044 + y: 0.0138177616402 + z: 0.0 + orientation: + x: -0.0 + y: -0.000740356107303 + z: -0.0 + w: 0.999999725936 + covariance: [1.8191632501593693, -3.936595825779304e-08, 0.0, 0.0, -1.4507111235675544e-05, 0.0, -3.9365958257793084e-08, 1.8191631073257044, 0.0, 0.0, 2.8908994490007875e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.4507111235675522e-05, 2.890899449000781e-06, 0.0, 0.0, 1.8024192315405911, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 110 + stamp: + secs: 1624875210 + nsecs: 520392895 + frame_id: "/world" +pose: + pose: + position: + x: -0.0156765062243 + y: 0.0197297137429 + z: 0.0 + orientation: + x: -0.0 + y: -0.000921353950671 + z: -0.0 + w: 0.999999575553 + covariance: [1.7558929986139256, -4.986990041967492e-08, 0.0, 0.0, -1.3692852084904276e-05, 0.0, -4.98699004196749e-08, 1.7558928170165167, 0.0, 0.0, 6.2560682738503005e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3692852084904309e-05, 6.256068273850301e-06, 0.0, 0.0, 1.7334926097557812, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 111 + stamp: + secs: 1624875210 + nsecs: 600393057 + frame_id: "/world" +pose: + pose: + position: + x: -0.0167348255494 + y: 0.022608323214 + z: 0.0 + orientation: + x: 0.0 + y: 0.000269944431353 + z: 0.0 + w: 0.999999963565 + covariance: [1.741809148582395, -2.8330819696685416e-08, 0.0, 0.0, -5.271531131532721e-06, 0.0, -2.833081969668545e-08, 1.7418090477861665, 0.0, 0.0, 8.105575283580987e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.271531131532728e-06, 8.10557528358099e-06, 0.0, 0.0, 1.7285462305191341, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 112 + stamp: + secs: 1624875210 + nsecs: 680402040 + frame_id: "/world" +pose: + pose: + position: + x: -0.0151815409614 + y: 0.0268601168938 + z: 0.0 + orientation: + x: 0.0 + y: 0.000181074163638 + z: 0.0 + w: 0.999999983606 + covariance: [1.7175549472333327, -3.813921933680286e-08, 0.0, 0.0, -2.002005146551767e-06, 0.0, -3.8139219336802933e-08, 1.71755481772857, 0.0, 0.0, 5.289110247841489e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.002005146551767e-06, 5.289110247841467e-06, 0.0, 0.0, 1.698919815966571, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 113 + stamp: + secs: 1624875210 + nsecs: 840425968 + frame_id: "/world" +pose: + pose: + position: + x: -0.0161310134795 + y: 0.0270475415063 + z: 0.0 + orientation: + x: 0.0 + y: 0.000102547760067 + z: 0.0 + w: 0.999999994742 + covariance: [1.9268786555899176, -7.170560326846011e-08, 0.0, 0.0, -1.959339302409181e-05, 0.0, -7.17056032684601e-08, 1.9268784183472327, 0.0, 0.0, 1.129281591489704e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.959339302409178e-05, 1.1292815914897023e-05, 0.0, 0.0, 1.8893645464156361, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 114 + stamp: + secs: 1624875210 + nsecs: 920433044 + frame_id: "/world" +pose: + pose: + position: + x: -0.0144401127726 + y: 0.0163655523434 + z: 0.0 + orientation: + x: -0.0 + y: -0.000413082990648 + z: -0.0 + w: 0.999999914681 + covariance: [1.8600431441394358, -4.832389505927113e-08, 0.0, 0.0, -1.279381318524166e-05, 0.0, -4.832389505927124e-08, 1.8600429890061072, 0.0, 0.0, 5.253915309008619e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.279381318524166e-05, 5.253915309008612e-06, 0.0, 0.0, 1.8343950181993693, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 115 + stamp: + secs: 1624875211 + nsecs: 80427885 + frame_id: "/world" +pose: + pose: + position: + x: -0.0198610711195 + y: 0.0256738120055 + z: 0.0 + orientation: + x: -0.0 + y: -0.00038599638508 + z: -0.0 + w: 0.999999925503 + covariance: [1.7964962856169158, -5.4336771093988836e-08, 0.0, 0.0, -5.660641251633593e-06, 0.0, -5.4336771093988915e-08, 1.7964961136084119, 0.0, 0.0, 2.0190906453571833e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.660641251633592e-06, 2.019090645357179e-06, 0.0, 0.0, 1.7658696462941879, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 116 + stamp: + secs: 1624875211 + nsecs: 160442114 + frame_id: "/world" +pose: + pose: + position: + x: -0.0153228042934 + y: 0.0270988993171 + z: 0.0 + orientation: + x: 0.0 + y: 0.0001606858847 + z: 0.0 + w: 0.99999998709 + covariance: [2.028278757410154, -3.3278841386987134e-08, 0.0, 0.0, -9.180811537239745e-05, 0.0, -3.327884138698714e-08, 2.0282786094966827, 0.0, 0.0, 8.669867631696277e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.180811537239748e-05, 8.669867631696276e-07, 0.0, 0.0, 2.008833895673048, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 117 + stamp: + secs: 1624875211 + nsecs: 240469933 + frame_id: "/world" +pose: + pose: + position: + x: -0.0278697410627 + y: 0.0441777289446 + z: 0.0 + orientation: + x: -0.0 + y: -0.000502936148865 + z: -0.0 + w: 0.999999873528 + covariance: [1.8281871309889548, -3.153766810008135e-08, 0.0, 0.0, -5.805639353296562e-05, 0.0, -3.153766810008143e-08, 1.82818677002589, 0.0, 0.0, 1.2864844505347815e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.8056393532965785e-05, 1.2864844505347819e-06, 0.0, 0.0, 1.8093899057609084, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 118 + stamp: + secs: 1624875211 + nsecs: 320468903 + frame_id: "/world" +pose: + pose: + position: + x: -0.0209562233462 + y: 0.0459004277255 + z: 0.0 + orientation: + x: -0.0 + y: -0.00199480456087 + z: -0.0 + w: 0.999998010375 + covariance: [1.7547167198499007, -3.500954541907041e-08, 0.0, 0.0, -3.7081918348769676e-05, 0.0, -3.500954541907046e-08, 1.7547162334232373, 0.0, 0.0, 4.793621031509397e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.7081918348769696e-05, 4.7936210315094e-06, 0.0, 0.0, 1.7336410041953976, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 119 + stamp: + secs: 1624875211 + nsecs: 480456114 + frame_id: "/world" +pose: + pose: + position: + x: -0.0347148789225 + y: 0.0319532120273 + z: 0.0 + orientation: + x: 0.0 + y: 0.000218589213485 + z: 0.0 + w: 0.999999976109 + covariance: [1.8304492449014345, -6.603087813110398e-08, 0.0, 0.0, -3.1749523646959775e-05, 0.0, -6.603087813110394e-08, 1.8304483358625017, 0.0, 0.0, 5.076344509695924e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.174952364695976e-05, 5.0763445096959235e-06, 0.0, 0.0, 1.7907237563209897, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 120 + stamp: + secs: 1624875211 + nsecs: 640486956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0351585550581 + y: 0.0307477117648 + z: 0.0 + orientation: + x: 0.0 + y: 0.000249420675031 + z: 0.0 + w: 0.999999968895 + covariance: [1.7676537484061823, -5.5087761952654015e-08, 0.0, 0.0, -1.583371319393279e-05, 0.0, -5.508776195265402e-08, 1.7676529994813701, 0.0, 0.0, 2.1644922186936384e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.583371319393279e-05, 2.1644922186936363e-06, 0.0, 0.0, 1.7323073682258, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 121 + stamp: + secs: 1624875211 + nsecs: 720500946 + frame_id: "/world" +pose: + pose: + position: + x: -0.0355765290453 + y: 0.0301628610216 + z: 0.0 + orientation: + x: -0.0 + y: -9.06694762601e-05 + z: -0.0 + w: 0.99999999589 + covariance: [1.777231743902157, -2.7228484439367553e-08, 0.0, 0.0, -6.601429633196624e-06, 0.0, -2.7228484439367537e-08, 1.777231375392685, 0.0, 0.0, 5.284172979764981e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.60142963319664e-06, 5.284172979764985e-07, 0.0, 0.0, 1.758909384332062, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 122 + stamp: + secs: 1624875211 + nsecs: 800513029 + frame_id: "/world" +pose: + pose: + position: + x: -0.0300332962856 + y: 0.0242580191195 + z: 0.0 + orientation: + x: 0.0 + y: 0.000274336377285 + z: 0.0 + w: 0.99999996237 + covariance: [1.9958154267746533, -3.737769522343045e-08, 0.0, 0.0, -8.260644785140953e-06, 0.0, -3.737769522343051e-08, 1.9958149216239023, 0.0, 0.0, 5.16626571355384e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.260644785140964e-06, 5.166265713553839e-07, 0.0, 0.0, 1.9687116480162554, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 123 + stamp: + secs: 1624875211 + nsecs: 960508108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0306737985319 + y: 0.0229282026058 + z: 0.0 + orientation: + x: 0.0 + y: 2.27042507861e-05 + z: 0.0 + w: 0.999999999742 + covariance: [2.1989459317633226, -5.27817951334415e-08, 0.0, 0.0, -9.065201075282274e-06, 0.0, -5.278179513344153e-08, 2.198945221630889, 0.0, 0.0, 6.23935876348776e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.065201075282268e-06, 6.239358763487763e-06, 0.0, 0.0, 2.1575027261563324, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 124 + stamp: + secs: 1624875212 + nsecs: 200536013 + frame_id: "/world" +pose: + pose: + position: + x: -0.0126580108318 + y: -0.0151249009866 + z: 0.0 + orientation: + x: -0.0 + y: -0.000666069468156 + z: -0.0 + w: 0.999999778176 + covariance: [1.8199122309059839, -7.14701003043723e-08, 0.0, 0.0, -4.4546002503291785e-06, 0.0, -7.147010030437258e-08, 1.8199113006807888, 0.0, 0.0, 3.6254735317293405e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.454600250329176e-06, 3.625473531729332e-05, 0.0, 0.0, 1.7553242812714172, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 125 + stamp: + secs: 1624875212 + nsecs: 280575037 + frame_id: "/world" +pose: + pose: + position: + x: -0.0125649076302 + y: -0.0165661076265 + z: 0.0 + orientation: + x: 0.0 + y: 0.000414913811558 + z: 0.0 + w: 0.999999913923 + covariance: [1.817834826313506, -2.7232208346303692e-08, 0.0, 0.0, -2.441821857697161e-06, 0.0, -2.723220834630375e-08, 1.817834485991075, 0.0, 0.0, 1.0961316579068044e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4418218576971594e-06, 1.0961316579068047e-05, 0.0, 0.0, 1.7920381133497134, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 126 + stamp: + secs: 1624875212 + nsecs: 440562963 + frame_id: "/world" +pose: + pose: + position: + x: -0.0127478132455 + y: -0.0181545302618 + z: 0.0 + orientation: + x: -0.0 + y: -0.000389798937385 + z: -0.0 + w: 0.999999924028 + covariance: [1.6440719564889155, -2.3974049976273022e-08, 0.0, 0.0, -2.969353714947975e-05, 0.0, -2.397404997627293e-08, 1.6440716692284039, 0.0, 0.0, 3.024493519713834e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9693537149479706e-05, 3.0244935197138356e-06, 0.0, 0.0, 1.6203362794871619, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 127 + stamp: + secs: 1624875212 + nsecs: 520561934 + frame_id: "/world" +pose: + pose: + position: + x: -0.012777427079 + y: -0.0190119077224 + z: 0.0 + orientation: + x: -0.0 + y: -0.000351418861302 + z: -0.0 + w: 0.999999938252 + covariance: [1.7060298322335328, -1.3926411273134203e-08, 0.0, 0.0, -1.3996164461524566e-05, 0.0, -1.3926411273134228e-08, 1.7060296603095202, 0.0, 0.0, 1.5443891247214911e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3996164461524556e-05, 1.5443891247214899e-06, 0.0, 0.0, 1.6920592116878344, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 128 + stamp: + secs: 1624875212 + nsecs: 680576086 + frame_id: "/world" +pose: + pose: + position: + x: -0.012990063607 + y: -0.0207035111067 + z: 0.0 + orientation: + x: -0.0 + y: -0.000848261374327 + z: -0.0 + w: 0.999999640226 + covariance: [1.6893448886589562, -2.0313716703536002e-08, 0.0, 0.0, -6.315143989311915e-06, 0.0, -2.0313716703536035e-08, 1.68934464459119, 0.0, 0.0, 1.4454981630327028e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.315143989311915e-06, 1.4454981630327021e-05, 0.0, 0.0, 1.6689841198517645, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 129 + stamp: + secs: 1624875212 + nsecs: 760581970 + frame_id: "/world" +pose: + pose: + position: + x: -0.0163981854269 + y: -0.0107370428213 + z: 0.0 + orientation: + x: -0.0 + y: -0.000300191002887 + z: -0.0 + w: 0.999999954943 + covariance: [1.7087616492210447, -1.345410801219459e-08, 0.0, 0.0, -6.964730210419931e-06, 0.0, -1.3454108012194585e-08, 1.7087615008879269, 0.0, 0.0, 7.970168215677332e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.964730210419942e-06, 7.970168215677327e-06, 0.0, 0.0, 1.6952589287418098, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 130 + stamp: + secs: 1624875212 + nsecs: 920597076 + frame_id: "/world" +pose: + pose: + position: + x: -0.00394081515953 + y: -0.0105029290837 + z: 0.0 + orientation: + x: -0.0 + y: -0.00189355578176 + z: -0.0 + w: 0.999998207222 + covariance: [1.8123882068979635, -2.4227729432102258e-08, 0.0, 0.0, -5.984297053563993e-06, 0.0, -2.4227729432102215e-08, 1.8123879639457239, 0.0, 0.0, 4.274749227316811e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.9842970535639865e-06, 4.274749227316814e-06, 0.0, 0.0, 1.7879608462334327, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 131 + stamp: + secs: 1624875213 + nsecs: 160621881 + frame_id: "/world" +pose: + pose: + position: + x: -0.00419653377753 + y: -0.0126589751912 + z: 0.0 + orientation: + x: -0.0 + y: -0.000145064431591 + z: -0.0 + w: 0.999999989478 + covariance: [1.7577397912783934, -4.799700146928299e-08, 0.0, 0.0, -7.136252937790986e-06, 0.0, -4.799700146928312e-08, 1.7577393229809886, 0.0, 0.0, 3.292681180080038e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.136252937790955e-06, 3.292681180080037e-06, 0.0, 0.0, 1.703356177818743, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 132 + stamp: + secs: 1624875213 + nsecs: 240650892 + frame_id: "/world" +pose: + pose: + position: + x: -0.00443373726017 + y: -0.0136002813928 + z: 0.0 + orientation: + x: -0.0 + y: -0.000400502512205 + z: -0.0 + w: 0.999999919799 + covariance: [1.810941085345762, -1.8836335253451973e-08, 0.0, 0.0, -1.8991463162802732e-05, 0.0, -1.8836335253451933e-08, 1.8109409016127287, 0.0, 0.0, 1.038634429250637e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8991463162802712e-05, 1.0386344292506374e-06, 0.0, 0.0, 1.7884909351984446, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 133 + stamp: + secs: 1624875213 + nsecs: 400671005 + frame_id: "/world" +pose: + pose: + position: + x: -0.00690523722517 + y: -0.0115603637182 + z: 0.0 + orientation: + x: -0.0 + y: -0.000253709407933 + z: -0.0 + w: 0.999999967816 + covariance: [1.7269889510693466, -1.8966168513855578e-08, 0.0, 0.0, -1.370766082662116e-05, 0.0, -1.89661685138556e-08, 1.7269887567650364, 0.0, 0.0, 1.328800424836996e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3707660826621137e-05, 1.3288004248369962e-06, 0.0, 0.0, 1.702782391291518, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 134 + stamp: + secs: 1624875213 + nsecs: 480642080 + frame_id: "/world" +pose: + pose: + position: + x: -0.00699910200236 + y: -0.0122619658331 + z: 0.0 + orientation: + x: -0.0 + y: -9.48849666012e-05 + z: -0.0 + w: 0.999999995498 + covariance: [1.6814662160216771, -9.704480916695382e-09, 0.0, 0.0, -2.9097341401353432e-05, 0.0, -9.704480916695398e-09, 1.6814661125972725, 0.0, 0.0, 1.6012792379698798e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.9097341401353408e-05, 1.601279237969879e-06, 0.0, 0.0, 1.6689514953203453, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 135 + stamp: + secs: 1624875213 + nsecs: 560708046 + frame_id: "/world" +pose: + pose: + position: + x: -0.00704852338253 + y: -0.013286972516 + z: 0.0 + orientation: + x: -0.0 + y: -0.000268331167432 + z: -0.0 + w: 0.999999963999 + covariance: [1.6574732388107336, -1.500420885912657e-08, 0.0, 0.0, -2.066258949807088e-05, 0.0, -1.5004208859126616e-08, 1.6574730570341403, 0.0, 0.0, 1.047047609612736e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.06625894980709e-05, 1.0470476096127355e-06, 0.0, 0.0, 1.6395384005182905, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 136 + stamp: + secs: 1624875213 + nsecs: 720674992 + frame_id: "/world" +pose: + pose: + position: + x: -0.00708965380905 + y: -0.0151748239816 + z: 0.0 + orientation: + x: -0.0 + y: -0.00107304963146 + z: -0.0 + w: 0.999999424282 + covariance: [1.8544511249583058, -3.0318930441036966e-08, 0.0, 0.0, -1.2814365196650198e-05, 0.0, -3.03189304410369e-08, 1.8544507468929952, 0.0, 0.0, 2.0343933278549468e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2814365196650188e-05, 2.0343933278549497e-06, 0.0, 0.0, 1.8175831778265379, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 137 + stamp: + secs: 1624875213 + nsecs: 880716085 + frame_id: "/world" +pose: + pose: + position: + x: -0.0247137326465 + y: -0.00544502108451 + z: 0.0 + orientation: + x: -0.0 + y: -0.000632701845676 + z: -0.0 + w: 0.999999799844 + covariance: [1.73004047642552, -1.9621380452843024e-08, 0.0, 0.0, -1.0728602425091702e-05, 0.0, -1.9621380452843037e-08, 1.7300402391305918, 0.0, 0.0, 8.158622804101262e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.072860242509171e-05, 8.158622804101267e-06, 0.0, 0.0, 1.7048240198901057, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 138 + stamp: + secs: 1624875213 + nsecs: 960705042 + frame_id: "/world" +pose: + pose: + position: + x: -0.00589747873095 + y: -0.0511820441512 + z: 0.0 + orientation: + x: -0.0 + y: -0.0022870506106 + z: -0.0 + w: 0.999997384696 + covariance: [2.476728209911555, -2.7189894787959586e-08, 0.0, 0.0, -3.8943407014738214e-05, 0.0, -2.718989478795958e-08, 2.4767279186898965, 0.0, 0.0, 9.760559604343356e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.894340701473822e-05, 9.760559604343356e-06, 0.0, 0.0, 2.444166244980211, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 139 + stamp: + secs: 1624875214 + nsecs: 120723009 + frame_id: "/world" +pose: + pose: + position: + x: -0.0159555094821 + y: -0.0423671685151 + z: 0.0 + orientation: + x: -0.0 + y: -0.000263977577843 + z: -0.0 + w: 0.999999965158 + covariance: [1.8817610974819143, -4.0377701822125527e-08, 0.0, 0.0, -0.00010793625619293747, 0.0, -4.037770182212546e-08, 1.881760664164917, 0.0, 0.0, 4.442859218565017e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0001079362561929374, 4.442859218565011e-06, 0.0, 0.0, 1.8373166666647096, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 140 + stamp: + secs: 1624875214 + nsecs: 280742884 + frame_id: "/world" +pose: + pose: + position: + x: -0.0164479106857 + y: -0.0266694880648 + z: 0.0 + orientation: + x: -0.0 + y: -0.00126591214118 + z: -0.0 + w: 0.999999198733 + covariance: [1.9832325565756552, -4.191070345222918e-08, 0.0, 0.0, -5.983787315777082e-05, 0.0, -4.1910703452229195e-08, 1.9832319929473496, 0.0, 0.0, 1.866131034076362e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.9837873157770814e-05, 1.8661310340763602e-06, 0.0, 0.0, 1.9389327890311985, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 141 + stamp: + secs: 1624875214 + nsecs: 360743999 + frame_id: "/world" +pose: + pose: + position: + x: -0.0179661941506 + y: -0.0270122022306 + z: 0.0 + orientation: + x: -0.0 + y: -0.000254917516947 + z: -0.0 + w: 0.999999967509 + covariance: [1.674801097568249, -1.811492561463064e-08, 0.0, 0.0, -1.3189674742983305e-05, 0.0, -1.8114925614630605e-08, 1.6748008534438652, 0.0, 0.0, 1.172562778415646e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.3189674742983275e-05, 1.172562778415646e-05, 0.0, 0.0, 1.655730425923764, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 142 + stamp: + secs: 1624875214 + nsecs: 520785093 + frame_id: "/world" +pose: + pose: + position: + x: -0.0177621977282 + y: -0.027390538112 + z: 0.0 + orientation: + x: 0.0 + y: 7.02743391511e-05 + z: 0.0 + w: 0.999999997531 + covariance: [1.7160833032247964, -2.224161124586853e-08, 0.0, 0.0, -9.21931862777409e-06, 0.0, -2.2241611245868505e-08, 1.7160830477614726, 0.0, 0.0, 1.1484637366955054e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.219318627774097e-06, 1.1484637366955059e-05, 0.0, 0.0, 1.6942463374656325, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 143 + stamp: + secs: 1624875214 + nsecs: 600769043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181091753222 + y: -0.0285220466819 + z: 0.0 + orientation: + x: 0.0 + y: 0.000479977640177 + z: 0.0 + w: 0.999999884811 + covariance: [1.682529460783641, -1.5017497423673953e-08, 0.0, 0.0, -1.549448471382952e-05, 0.0, -1.5017497423673953e-08, 1.6825293042362472, 0.0, 0.0, 4.042389726519246e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5494484713829544e-05, 4.042389726519249e-06, 0.0, 0.0, 1.66850988968082, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 144 + stamp: + secs: 1624875214 + nsecs: 680773020 + frame_id: "/world" +pose: + pose: + position: + x: -0.0186190140113 + y: -0.0200777770547 + z: 0.0 + orientation: + x: 0.0 + y: 6.7314019886e-05 + z: 0.0 + w: 0.999999997734 + covariance: [1.775531613143067, -2.263906562386046e-08, 0.0, 0.0, -1.2499276318203065e-05, 0.0, -2.2639065623860494e-08, 1.7755313891341333, 0.0, 0.0, 2.565860386645869e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.249927631820307e-05, 2.565860386645864e-06, 0.0, 0.0, 1.754516691258627, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 145 + stamp: + secs: 1624875214 + nsecs: 840786934 + frame_id: "/world" +pose: + pose: + position: + x: -0.0191579941375 + y: -0.0215878702341 + z: 0.0 + orientation: + x: -0.0 + y: -0.000710052934631 + z: -0.0 + w: 0.999999747912 + covariance: [1.8967194325879266, -4.241625215166131e-08, 0.0, 0.0, -1.8896153278672175e-05, 0.0, -4.241625215166134e-08, 1.8967190654155976, 0.0, 0.0, 3.622038746655128e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.88961532786722e-05, 3.622038746655136e-05, 0.0, 0.0, 1.8592280960317893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 146 + stamp: + secs: 1624875214 + nsecs: 920783043 + frame_id: "/world" +pose: + pose: + position: + x: -0.0277137290155 + y: -0.00293535405265 + z: 0.0 + orientation: + x: 0.0 + y: 3.40797880468e-05 + z: 0.0 + w: 0.999999999419 + covariance: [2.230430153299887, -4.756721458125785e-08, 0.0, 0.0, -1.064354874328306e-05, 0.0, -4.756721458125787e-08, 2.2304298427652665, 0.0, 0.0, 2.2353773209621798e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0643548743283073e-05, 2.235377320962179e-05, 0.0, 0.0, 2.1939577298984845, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 147 + stamp: + secs: 1624875215 + nsecs: 80796003 + frame_id: "/world" +pose: + pose: + position: + x: -0.0239585873436 + y: -0.0062342909353 + z: 0.0 + orientation: + x: 0.0 + y: 0.000712132191441 + z: 0.0 + w: 0.999999746434 + covariance: [1.786948240739618, -3.9358238609088995e-08, 0.0, 0.0, -2.5054394537498006e-05, 0.0, -3.935823860908905e-08, 1.7869480166930096, 0.0, 0.0, 6.033720516010694e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.5054394537498034e-05, 6.0337205160106885e-06, 0.0, 0.0, 1.7577777624864048, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 148 + stamp: + secs: 1624875215 + nsecs: 160804033 + frame_id: "/world" +pose: + pose: + position: + x: -0.0263929021945 + y: -0.00684020648068 + z: 0.0 + orientation: + x: -0.0 + y: -0.000330875261076 + z: -0.0 + w: 0.999999945261 + covariance: [1.8091440389710087, -2.2508537454904115e-08, 0.0, 0.0, -1.1184096364113381e-05, 0.0, -2.250853745490419e-08, 1.809143912659984, 0.0, 0.0, 2.3928832940313463e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.118409636411339e-05, 2.392883294031336e-06, 0.0, 0.0, 1.7921785540785415, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 149 + stamp: + secs: 1624875215 + nsecs: 320810080 + frame_id: "/world" +pose: + pose: + position: + x: -0.0262556310407 + y: -0.00257554184772 + z: 0.0 + orientation: + x: -0.0 + y: -0.000553320052148 + z: -0.0 + w: 0.999999846918 + covariance: [1.7257169983750016, -2.887215893959103e-08, 0.0, 0.0, -2.0839792791054042e-05, 0.0, -2.887215893959098e-08, 1.7257168365096953, 0.0, 0.0, 3.277273409671279e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.0839792791054032e-05, 3.277273409671277e-06, 0.0, 0.0, 1.7030458295424296, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 150 + stamp: + secs: 1624875215 + nsecs: 400810003 + frame_id: "/world" +pose: + pose: + position: + x: -0.0296610786575 + y: 0.00168027198319 + z: 0.0 + orientation: + x: -0.0 + y: -0.000133604754941 + z: -0.0 + w: 0.999999991075 + covariance: [1.7760199041739, -2.367491376008142e-08, 0.0, 0.0, -1.2049783621271146e-05, 0.0, -2.367491376008137e-08, 1.7760197700925517, 0.0, 0.0, 1.640503390238761e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.204978362127114e-05, 1.6405033902387638e-06, 0.0, 0.0, 1.7569022699144987, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 151 + stamp: + secs: 1624875215 + nsecs: 560842037 + frame_id: "/world" +pose: + pose: + position: + x: -0.030088594316 + y: 0.000695336183922 + z: 0.0 + orientation: + x: 0.0 + y: 0.000376638912957 + z: 0.0 + w: 0.999999929072 + covariance: [1.6709392937874474, -3.0554808814418046e-08, 0.0, 0.0, -7.51160805491788e-06, 0.0, -3.0554808814417966e-08, 1.6709391230205617, 0.0, 0.0, 9.406945221654571e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.511608054917877e-06, 9.406945221654561e-06, 0.0, 0.0, 1.6448962532836724, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 152 + stamp: + secs: 1624875215 + nsecs: 640876055 + frame_id: "/world" +pose: + pose: + position: + x: -0.0189968732394 + y: -0.00588618382175 + z: 0.0 + orientation: + x: -0.0 + y: -0.000153993311551 + z: -0.0 + w: 0.999999988143 + covariance: [1.9458403826887298, -2.815408347038933e-08, 0.0, 0.0, -7.99692423660816e-06, 0.0, -2.8154083470389363e-08, 1.9458402335642075, 0.0, 0.0, 8.750129212436589e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.996924236608163e-06, 8.750129212436604e-06, 0.0, 0.0, 1.9212561828607555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 153 + stamp: + secs: 1624875215 + nsecs: 800864935 + frame_id: "/world" +pose: + pose: + position: + x: -0.00993021979217 + y: -0.0226336156956 + z: 0.0 + orientation: + x: -0.0 + y: -0.00248970957058 + z: -0.0 + w: 0.999996900668 + covariance: [1.9846308489516553, -4.465353311579839e-08, 0.0, 0.0, -2.1701735585398107e-06, 0.0, -4.465353311579835e-08, 1.9846306333925328, 0.0, 0.0, 3.8609679116356746e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1701735585398175e-06, 3.860967911635674e-05, 0.0, 0.0, 1.943700401610839, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 154 + stamp: + secs: 1624875215 + nsecs: 960884094 + frame_id: "/world" +pose: + pose: + position: + x: -0.0135344938785 + y: -0.0190627382726 + z: 0.0 + orientation: + x: 0.0 + y: 0.0012422125781 + z: 0.0 + w: 0.999999228454 + covariance: [1.890802308862325, -1.1294541302390839e-07, 0.0, 0.0, -0.0004989383884347277, 0.0, -1.1294541302390858e-07, 1.890800643038723, 0.0, 0.0, 2.1601230958583295e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0004989383884347279, 2.1601230958583264e-05, 0.0, 0.0, 1.8381975901145113, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 155 + stamp: + secs: 1624875216 + nsecs: 120889902 + frame_id: "/world" +pose: + pose: + position: + x: -0.0140338834248 + y: -0.0212345246735 + z: 0.0 + orientation: + x: -0.0 + y: -0.000598612726465 + z: -0.0 + w: 0.999999820831 + covariance: [1.9362057630151812, -1.1952045563597273e-07, 0.0, 0.0, -0.00025669139106513874, 0.0, -1.1952045563597273e-07, 1.9362014342811933, 0.0, 0.0, 5.352238995584263e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.0002566913910651389, 5.352238995584258e-06, 0.0, 0.0, 1.9002942268670815, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 156 + stamp: + secs: 1624875216 + nsecs: 200895071 + frame_id: "/world" +pose: + pose: + position: + x: -0.0141336973117 + y: -0.0229941989455 + z: 0.0 + orientation: + x: 0.0 + y: 0.000297532869607 + z: 0.0 + w: 0.999999955737 + covariance: [2.0379603362087533, -6.270301393790661e-08, 0.0, 0.0, -6.385941059764795e-05, 0.0, -6.270301393790661e-08, 2.0379576380679, 0.0, 0.0, 1.0658927689375592e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.385941059764787e-05, 1.065892768937558e-06, 0.0, 0.0, 2.020535902160479, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 157 + stamp: + secs: 1624875216 + nsecs: 280922890 + frame_id: "/world" +pose: + pose: + position: + x: -0.0181141481185 + y: -0.00714884038211 + z: 0.0 + orientation: + x: 0.0 + y: 0.000760002020438 + z: 0.0 + w: 0.999999711198 + covariance: [2.2336925604878783, -8.576990136636337e-08, 0.0, 0.0, -3.072687749286127e-05, 0.0, -8.57699013663634e-08, 2.233688666536996, 0.0, 0.0, 1.9708381412087154e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.072687749286122e-05, 1.9708381412087113e-05, 0.0, 0.0, 2.211075083457304, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 158 + stamp: + secs: 1624875216 + nsecs: 360913038 + frame_id: "/world" +pose: + pose: + position: + x: -0.00623181004681 + y: 0.00329876527136 + z: 0.0 + orientation: + x: -0.0 + y: -0.0010060813585 + z: -0.0 + w: 0.9999994939 + covariance: [1.7760505305802554, -6.372936151679336e-08, 0.0, 0.0, -8.125297298334594e-06, 0.0, -6.372936151679318e-08, 1.7760477176965102, 0.0, 0.0, 9.08370732928812e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.125297298334615e-06, 9.083707329288124e-06, 0.0, 0.0, 1.7587306568009633, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 159 + stamp: + secs: 1624875216 + nsecs: 600929976 + frame_id: "/world" +pose: + pose: + position: + x: -0.00863278428775 + y: -0.00662414472651 + z: 0.0 + orientation: + x: 0.0 + y: 0.000486766547536 + z: 0.0 + w: 0.999999881529 + covariance: [2.019899522224218, -2.8840132946824433e-07, 0.0, 0.0, -8.07223753890459e-05, 0.0, -2.884013294682437e-07, 2.019887471733767, 0.0, 0.0, 2.728816990831616e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.07223753890459e-05, 2.728816990831616e-05, 0.0, 0.0, 1.9311985011979682, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 160 + stamp: + secs: 1624875216 + nsecs: 760956049 + frame_id: "/world" +pose: + pose: + position: + x: -0.00891683724342 + y: -0.00869444442759 + z: 0.0 + orientation: + x: -0.0 + y: -0.00082515791413 + z: -0.0 + w: 0.999999659557 + covariance: [1.7444861875084943, -1.06789932933038e-07, 0.0, 0.0, -2.491525158341813e-05, 0.0, -1.0678993293303793e-07, 1.7444820708334643, 0.0, 0.0, 8.136925590560365e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.491525158341814e-05, 8.13692559056037e-06, 0.0, 0.0, 1.7117681942122305, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 161 + stamp: + secs: 1624875216 + nsecs: 840961933 + frame_id: "/world" +pose: + pose: + position: + x: -0.00904886237411 + y: -0.00992791982665 + z: 0.0 + orientation: + x: 0.0 + y: 2.66785031672e-06 + z: 0.0 + w: 0.999999999996 + covariance: [1.870180454005927, -4.7974480573056856e-08, 0.0, 0.0, -6.9923948651325484e-06, 0.0, -4.7974480573056703e-08, 1.8701786929083137, 0.0, 0.0, 2.441366611356094e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.992394865132542e-06, 2.441366611356092e-06, 0.0, 0.0, 1.8554399725853994, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 162 + stamp: + secs: 1624875216 + nsecs: 920965910 + frame_id: "/world" +pose: + pose: + position: + x: -0.0139583728875 + y: -0.00744317769063 + z: 0.0 + orientation: + x: -0.0 + y: -0.00095108253289 + z: -0.0 + w: 0.999999547721 + covariance: [1.9207163223599037, -5.8365637507756615e-08, 0.0, 0.0, -2.906136228531491e-06, 0.0, -5.8365637507756674e-08, 1.9207142440805265, 0.0, 0.0, 1.4624076512237565e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.90613622853148e-06, 1.4624076512237523e-06, 0.0, 0.0, 1.9025410782912338, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 163 + stamp: + secs: 1624875217 + nsecs: 80965996 + frame_id: "/world" +pose: + pose: + position: + x: -0.0144283166657 + y: -0.00938026722802 + z: 0.0 + orientation: + x: -0.0 + y: -0.000816974520533 + z: -0.0 + w: 0.999999666276 + covariance: [1.7612928010550937, -6.157133780834283e-08, 0.0, 0.0, -5.597707793377422e-06, 0.0, -6.157133780834285e-08, 1.7612906274225448, 0.0, 0.0, 5.191994051419185e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.59770779337741e-06, 5.191994051419173e-07, 0.0, 0.0, 1.7403514683951793, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 164 + stamp: + secs: 1624875217 + nsecs: 160985947 + frame_id: "/world" +pose: + pose: + position: + x: -0.011198531709 + y: -0.00805949230635 + z: 0.0 + orientation: + x: -0.0 + y: -0.000156333920919 + z: -0.0 + w: 0.99999998778 + covariance: [1.7706940674247764, -3.0283291064472704e-08, 0.0, 0.0, -1.9896639129704306e-06, 0.0, -3.02832910644727e-08, 1.77069300027923, 0.0, 0.0, 1.0529763509315963e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.989663912970425e-06, 1.0529763509315972e-07, 0.0, 0.0, 1.7596711200372572, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 165 + stamp: + secs: 1624875217 + nsecs: 240998983 + frame_id: "/world" +pose: + pose: + position: + x: -0.00750152131664 + y: -0.0036410270864 + z: 0.0 + orientation: + x: -0.0 + y: -0.000508811198224 + z: -0.0 + w: 0.999999870556 + covariance: [1.7761945565724073, -3.8265220311603956e-08, 0.0, 0.0, -6.357517432109237e-07, 0.0, -3.826522031160385e-08, 1.7761932092179338, 0.0, 0.0, 1.3208691765571122e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.357517432109236e-07, 1.3208691765571104e-08, 0.0, 0.0, 1.7609733874802644, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 166 + stamp: + secs: 1624875217 + nsecs: 400985956 + frame_id: "/world" +pose: + pose: + position: + x: -0.00696761023803 + y: -0.0051198525222 + z: 0.0 + orientation: + x: -0.0 + y: -0.000742803581219 + z: -0.0 + w: 0.999999724121 + covariance: [1.8180570188735319, -5.818035153391153e-08, 0.0, 0.0, -4.878021979650565e-06, 0.0, -5.818035153391143e-08, 1.818054970498542, 0.0, 0.0, -2.3357274196385267e-08, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.878021979650561e-06, -2.3357274196385343e-08, 0.0, 0.0, 1.793211409937832, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 167 + stamp: + secs: 1624875217 + nsecs: 481002092 + frame_id: "/world" +pose: + pose: + position: + x: -0.00865423581956 + y: -0.00498649163186 + z: 0.0 + orientation: + x: -0.0 + y: -0.000402445730884 + z: -0.0 + w: 0.999999919019 + covariance: [2.001475569036382, -6.291976679635332e-08, 0.0, 0.0, -6.5518724848095285e-06, 0.0, -6.291976679635327e-08, 2.0014733533329814, 0.0, 0.0, 1.852736422359716e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.551872484809532e-06, 1.852736422359717e-07, 0.0, 0.0, 1.9716807102202987, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 168 + stamp: + secs: 1624875217 + nsecs: 641020060 + frame_id: "/world" +pose: + pose: + position: + x: -0.015520449065 + y: -0.000718194972907 + z: 0.0 + orientation: + x: -0.0 + y: -0.00135389919787 + z: -0.0 + w: 0.999999083478 + covariance: [2.420131841911906, -1.1196391133912191e-07, 0.0, 0.0, -5.7507321086209995e-06, 0.0, -1.1196391133912205e-07, 2.4201278992254576, 0.0, 0.0, 1.585479957450454e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.750732108621005e-06, 1.5854799574504537e-07, 0.0, 0.0, 2.3649747689187275, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 169 + stamp: + secs: 1624875217 + nsecs: 721040964 + frame_id: "/world" +pose: + pose: + position: + x: -0.0176545031441 + y: -0.00297295608791 + z: 0.0 + orientation: + x: 0.0 + y: 0.000170364589184 + z: 0.0 + w: 0.999999985488 + covariance: [1.7542934679971265, -3.1912306461547856e-08, 0.0, 0.0, -7.797428102711512e-07, 0.0, -3.191230646154794e-08, 1.754292348974104, 0.0, 0.0, 1.807645337043654e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.797428102711521e-07, 1.8076453370436575e-05, 0.0, 0.0, 1.7367822910071253, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 170 + stamp: + secs: 1624875217 + nsecs: 801040888 + frame_id: "/world" +pose: + pose: + position: + x: -0.00410236126918 + y: 0.0140200276578 + z: 0.0 + orientation: + x: -0.0 + y: -0.00140206773077 + z: -0.0 + w: 0.999999017103 + covariance: [2.03459013078347, -4.696413724706143e-08, 0.0, 0.0, -2.2603874112814893e-07, 0.0, -4.696413724706141e-08, 2.0345885118129643, 0.0, 0.0, 1.9392348747922244e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.2603874112814885e-07, 1.9392348747922278e-05, 0.0, 0.0, 2.0067121233885685, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 171 + stamp: + secs: 1624875217 + nsecs: 961061954 + frame_id: "/world" +pose: + pose: + position: + x: -0.0090901628641 + y: -0.00333069958549 + z: 0.0 + orientation: + x: 0.0 + y: 0.000245697553199 + z: 0.0 + w: 0.999999969816 + covariance: [1.821482382362624, -6.494295307547523e-08, 0.0, 0.0, -1.6290825636853137e-08, 0.0, -6.494295307547538e-08, 1.8214801578734259, 0.0, 0.0, 6.7273992866800306e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6290825636853147e-08, 6.7273992866800196e-06, 0.0, 0.0, 1.7777276549094412, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 172 + stamp: + secs: 1624875218 + nsecs: 121073961 + frame_id: "/world" +pose: + pose: + position: + x: -0.00220648142904 + y: 0.00699029183312 + z: 0.0 + orientation: + x: -0.0 + y: -0.00105551461204 + z: -0.0 + w: 0.999999442944 + covariance: [1.9892900722934472, -7.425040758016299e-08, 0.0, 0.0, -4.70103166156141e-06, 0.0, -7.425040758016304e-08, 1.9892875342845573, 0.0, 0.0, 2.1239272694446582e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.70103166156141e-06, 2.1239272694446582e-06, 0.0, 0.0, 1.9324829385278268, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 173 + stamp: + secs: 1624875218 + nsecs: 281084061 + frame_id: "/world" +pose: + pose: + position: + x: -0.0125016009645 + y: 0.00893361224124 + z: 0.0 + orientation: + x: -0.0 + y: -0.000176548453512 + z: -0.0 + w: 0.999999984415 + covariance: [1.842340861928419, -4.506086555483202e-08, 0.0, 0.0, -7.296077421818681e-06, 0.0, -4.506086555483205e-08, 1.8423393306847407, 0.0, 0.0, 5.200340890237755e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.296077421818674e-06, 5.200340890237751e-06, 0.0, 0.0, 1.8046114116260852, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 174 + stamp: + secs: 1624875218 + nsecs: 361097097 + frame_id: "/world" +pose: + pose: + position: + x: -0.00661349187337 + y: 0.0147183409108 + z: 0.0 + orientation: + x: -0.0 + y: -0.000892858321568 + z: -0.0 + w: 0.999999601402 + covariance: [2.17015377709267, -3.308452160788557e-08, 0.0, 0.0, -7.310813064323434e-05, 0.0, -3.308452160788556e-08, 2.1701526772356687, 0.0, 0.0, 2.902312144247622e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.310813064323437e-05, 2.9023121442476196e-06, 0.0, 0.0, 2.141558073570872, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 175 + stamp: + secs: 1624875218 + nsecs: 521126986 + frame_id: "/world" +pose: + pose: + position: + x: -0.00960792878894 + y: 0.00234989396802 + z: 0.0 + orientation: + x: -0.0 + y: -0.000227857449092 + z: -0.0 + w: 0.99999997404 + covariance: [1.9220771648511952, -3.91128655194514e-08, 0.0, 0.0, -5.301272603977029e-05, 0.0, -3.9112865519451386e-08, 1.9220757964788517, 0.0, 0.0, 1.0523728661211618e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.3012726039770276e-05, 1.0523728661211612e-06, 0.0, 0.0, 1.8872942206137542, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 176 + stamp: + secs: 1624875218 + nsecs: 601133108 + frame_id: "/world" +pose: + pose: + position: + x: -0.0114511372161 + y: 0.00473942656332 + z: 0.0 + orientation: + x: -0.0 + y: -0.000641578273327 + z: -0.0 + w: 0.999999794189 + covariance: [1.7539186696197737, -1.6260886819244515e-08, 0.0, 0.0, -9.444924618903406e-06, 0.0, -1.6260886819244505e-08, 1.7539180900066778, 0.0, 0.0, 2.5238383546960664e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.444924618903409e-06, 2.5238383546960595e-07, 0.0, 0.0, 1.7391612637708633, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 177 + stamp: + secs: 1624875218 + nsecs: 681112051 + frame_id: "/world" +pose: + pose: + position: + x: -0.00321234374268 + y: 0.00108809835907 + z: 0.0 + orientation: + x: -0.0 + y: -0.00120390429085 + z: -0.0 + w: 0.999999275307 + covariance: [1.8443245292567965, -2.0331084695336856e-08, 0.0, 0.0, -6.677308321683935e-06, 0.0, -2.0331084695336846e-08, 1.8443237983751994, 0.0, 0.0, 9.311744835961264e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.677308321683938e-06, 9.311744835961258e-07, 0.0, 0.0, 1.8252993298718596, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 178 + stamp: + secs: 1624875218 + nsecs: 841130018 + frame_id: "/world" +pose: + pose: + position: + x: -0.00360279417834 + y: -0.00101304208978 + z: 0.0 + orientation: + x: -0.0 + y: -0.000507844430792 + z: -0.0 + w: 0.999999871047 + covariance: [1.8072511137023821, -3.665424510130845e-08, 0.0, 0.0, -1.772689308839707e-05, 0.0, -3.665424510130855e-08, 1.807249798118531, 0.0, 0.0, 6.116834228113157e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.772689308839706e-05, 6.116834228113135e-07, 0.0, 0.0, 1.7650653305577524, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 179 + stamp: + secs: 1624875219 + nsecs: 1161098 + frame_id: "/world" +pose: + pose: + position: + x: -0.0247193267308 + y: 0.0241437234491 + z: 0.0 + orientation: + x: -0.0 + y: -0.000236151985194 + z: -0.0 + w: 0.999999972116 + covariance: [1.7591145070165783, -2.2796333745991692e-08, 0.0, 0.0, -6.62053613269735e-06, 0.0, -2.2796333745991675e-08, 1.7591136879805906, 0.0, 0.0, 3.4022954142557725e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.620536132697361e-06, 3.4022954142557836e-07, 0.0, 0.0, 1.73076432535645, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 180 + stamp: + secs: 1624875219 + nsecs: 81146002 + frame_id: "/world" +pose: + pose: + position: + x: -0.0249665430873 + y: 0.023178707076 + z: 0.0 + orientation: + x: -0.0 + y: -0.000180825473005 + z: -0.0 + w: 0.999999983651 + covariance: [1.7753536209401872, -1.2487073998883996e-08, 0.0, 0.0, -2.1177216676354725e-06, 0.0, -1.2487073998884011e-08, 1.775353174118469, 0.0, 0.0, 3.6508249105706666e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.117721667635472e-06, 3.6508249105706645e-06, 0.0, 0.0, 1.7588415053923856, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 181 + stamp: + secs: 1624875219 + nsecs: 161160946 + frame_id: "/world" +pose: + pose: + position: + x: 0.00627649167009 + y: 0.025831140694 + z: 0.0 + orientation: + x: -0.0 + y: -0.00423538501684 + z: -0.0 + w: 0.999991030717 + covariance: [1.915675738767697, -1.5136360053834664e-08, 0.0, 0.0, -9.57031186644239e-07, 0.0, -1.5136360053834677e-08, 1.915675203087062, 0.0, 0.0, -1.1181527613166993e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.570311866442411e-07, -1.1181527613166978e-06, 0.0, 0.0, 1.8941023635676388, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 182 + stamp: + secs: 1624875219 + nsecs: 401169062 + frame_id: "/world" +pose: + pose: + position: + x: -0.00241391806304 + y: 0.0232959518887 + z: 0.0 + orientation: + x: 0.0 + y: 0.0026198656672 + z: 0.0 + w: 0.999996568146 + covariance: [1.8979764580468057, -7.718719610028475e-08, 0.0, 0.0, -0.00017698301314779578, 0.0, -7.7187196100285e-08, 1.8979749369940044, 0.0, 0.0, 3.703840151375312e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -0.00017698301314779557, 3.703840151375313e-05, 0.0, 0.0, 1.834942985143629, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 183 + stamp: + secs: 1624875219 + nsecs: 481182098 + frame_id: "/world" +pose: + pose: + position: + x: 0.000488929172891 + y: 0.0202035533422 + z: 0.0 + orientation: + x: -0.0 + y: -0.0010575132483 + z: -0.0 + w: 0.999999440833 + covariance: [1.967268717640398, -8.124880105687398e-08, 0.0, 0.0, -6.410270234248261e-05, 0.0, -8.12488010568742e-08, 1.9672678328868163, 0.0, 0.0, 1.4811381274155901e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.410270234248262e-05, 1.4811381274155864e-05, 0.0, 0.0, 1.9368120245280191, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 184 + stamp: + secs: 1624875219 + nsecs: 641206980 + frame_id: "/world" +pose: + pose: + position: + x: 0.000150580897616 + y: 0.0191635069808 + z: 0.0 + orientation: + x: 0.0 + y: 0.000352402986802 + z: 0.0 + w: 0.999999937906 + covariance: [1.8330957624158029, -1.0011890317262454e-07, 0.0, 0.0, -1.781860068437479e-05, 0.0, -1.0011890317262446e-07, 1.8330948796389053, 0.0, 0.0, 4.840250627668411e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7818600684374797e-05, 4.840250627668415e-06, 0.0, 0.0, 1.8045402179955512, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 185 + stamp: + secs: 1624875219 + nsecs: 721213102 + frame_id: "/world" +pose: + pose: + position: + x: -0.0101143909028 + y: 0.013152693522 + z: 0.0 + orientation: + x: -0.0 + y: -0.000704737528125 + z: -0.0 + w: 0.999999751672 + covariance: [1.8780078749318798, -5.6125090318239745e-08, 0.0, 0.0, -1.2061174266450217e-05, 0.0, -5.612509031823968e-08, 1.8780074002053222, 0.0, 0.0, 1.0688581815074524e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2061174266450207e-05, 1.0688581815074518e-06, 0.0, 0.0, 1.862727273943907, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 186 + stamp: + secs: 1624875219 + nsecs: 801218987 + frame_id: "/world" +pose: + pose: + position: + x: -0.00790245581082 + y: 0.0162568178309 + z: 0.0 + orientation: + x: -0.0 + y: -0.000330782149686 + z: -0.0 + w: 0.999999945292 + covariance: [1.6665279450948722, -5.3242424275311136e-08, 0.0, 0.0, -6.1380357561798565e-06, 0.0, -5.324242427531104e-08, 1.6665275007005185, 0.0, 0.0, 2.712203691428772e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.1380357561798616e-06, 2.712203691428774e-07, 0.0, 0.0, 1.65103863619447, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 187 + stamp: + secs: 1624875219 + nsecs: 961224079 + frame_id: "/world" +pose: + pose: + position: + x: -0.00836176023115 + y: 0.0147978469525 + z: 0.0 + orientation: + x: -0.0 + y: -0.00039331077613 + z: -0.0 + w: 0.999999922653 + covariance: [2.1617291370105503, -1.1313644736176536e-07, 0.0, 0.0, -4.610758893442236e-05, 0.0, -1.1313644736176536e-07, 2.161728189583937, 0.0, 0.0, 2.580959591891213e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.6107588934422433e-05, 2.580959591891216e-06, 0.0, 0.0, 2.126295270250253, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 188 + stamp: + secs: 1624875220 + nsecs: 41249990 + frame_id: "/world" +pose: + pose: + position: + x: -0.00872158834957 + y: 0.0142043909338 + z: 0.0 + orientation: + x: 0.0 + y: 5.43625831497e-05 + z: 0.0 + w: 0.999999998522 + covariance: [1.940988648567923, -5.805310080438981e-08, 0.0, 0.0, -1.7179605348563708e-05, 0.0, -5.805310080438991e-08, 1.9409881553507258, 0.0, 0.0, 1.2320975491890255e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.7179605348563732e-05, 1.2320975491890272e-06, 0.0, 0.0, 1.9218628610396518, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 189 + stamp: + secs: 1624875220 + nsecs: 121259928 + frame_id: "/world" +pose: + pose: + position: + x: -0.0106671808718 + y: -0.0073106335756 + z: 0.0 + orientation: + x: -0.0 + y: -0.000130724310983 + z: -0.0 + w: 0.999999991456 + covariance: [1.7254283464709286, -4.1238502545744535e-08, 0.0, 0.0, -4.386917218653393e-06, 0.0, -4.1238502545744476e-08, 1.7254279932020467, 0.0, 0.0, 3.117885886104504e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.386917218653389e-06, 3.117885886104505e-07, 0.0, 0.0, 1.7109786957927335, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 190 + stamp: + secs: 1624875220 + nsecs: 281263113 + frame_id: "/world" +pose: + pose: + position: + x: -0.0111911309172 + y: -0.00903861026461 + z: 0.0 + orientation: + x: 0.0 + y: 0.000429046989817 + z: 0.0 + w: 0.999999907959 + covariance: [1.8257192119880727, -9.389381799297072e-08, 0.0, 0.0, -1.941417420174091e-06, 0.0, -9.389381799297076e-08, 1.825718405070981, 0.0, 0.0, 1.7814727168622604e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9414174201740914e-06, 1.7814727168622604e-07, 0.0, 0.0, 1.7889140335846472, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 191 + stamp: + secs: 1624875220 + nsecs: 441265106 + frame_id: "/world" +pose: + pose: + position: + x: -0.00930873845963 + y: -0.00218760148319 + z: 0.0 + orientation: + x: -0.0 + y: -0.000235260216926 + z: -0.0 + w: 0.999999972326 + covariance: [1.8929002561189059, -7.499047261805555e-08, 0.0, 0.0, -6.556241205160164e-06, 0.0, -7.499047261805557e-08, 1.8928996151479351, 0.0, 0.0, 1.0204168431368456e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.5562412051601454e-06, 1.0204168431368429e-05, 0.0, 0.0, 1.8611737937040271, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 192 + stamp: + secs: 1624875220 + nsecs: 521287918 + frame_id: "/world" +pose: + pose: + position: + x: -0.0134994622676 + y: -0.00434107942587 + z: 0.0 + orientation: + x: -0.0 + y: -6.21331316797e-05 + z: -0.0 + w: 0.99999999807 + covariance: [1.996011363979168, -4.2792338804747156e-08, 0.0, 0.0, -1.0961135507353738e-05, 0.0, -4.27923388047471e-08, 1.9960110068997, 0.0, 0.0, 4.923457624710882e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.096113550735375e-05, 4.923457624710877e-06, 0.0, 0.0, 1.977287367486983, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 193 + stamp: + secs: 1624875220 + nsecs: 601290941 + frame_id: "/world" +pose: + pose: + position: + x: -0.0138647064633 + y: -0.00506375187263 + z: 0.0 + orientation: + x: -0.0 + y: -4.59505859656e-06 + z: -0.0 + w: 0.999999999989 + covariance: [1.7023079580221285, -3.636340947934349e-08, 0.0, 0.0, -7.892323264565757e-06, 0.0, -3.636340947934346e-08, 1.7023076601375446, 0.0, 0.0, 1.3813970642136962e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.89232326456576e-06, 1.381397064213698e-06, 0.0, 0.0, 1.6854248089103239, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 194 + stamp: + secs: 1624875220 + nsecs: 761307001 + frame_id: "/world" +pose: + pose: + position: + x: -0.0169791044043 + y: -0.00596658554036 + z: 0.0 + orientation: + x: -0.0 + y: -6.60669137371e-05 + z: -0.0 + w: 0.999999997818 + covariance: [1.8464777452866636, -5.3437419888738654e-08, 0.0, 0.0, -4.3851281302913945e-06, 0.0, -5.3437419888738674e-08, 1.8464773095966396, 0.0, 0.0, 5.293491722186717e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.38512813029139e-06, 5.293491722186716e-07, 0.0, 0.0, 1.8196021614866136, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 195 + stamp: + secs: 1624875220 + nsecs: 841311932 + frame_id: "/world" +pose: + pose: + position: + x: -0.0152549844083 + y: -0.00507478461002 + z: 0.0 + orientation: + x: 0.0 + y: 0.000462758100158 + z: 0.0 + w: 0.999999892927 + covariance: [1.754665400136773, -3.968194428115132e-08, 0.0, 0.0, -2.3631176001098618e-05, 0.0, -3.968194428115139e-08, 1.754665088662969, 0.0, 0.0, 1.1152744605477394e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.3631176001098604e-05, 1.1152744605477372e-05, 0.0, 0.0, 1.7346522193876082, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 196 + stamp: + secs: 1624875221 + nsecs: 1331091 + frame_id: "/world" +pose: + pose: + position: + x: -0.0163864327726 + y: -0.00580270666773 + z: 0.0 + orientation: + x: 0.0 + y: 0.000368950052603 + z: 0.0 + w: 0.999999931938 + covariance: [1.8156315507935108, -5.420700712112123e-08, 0.0, 0.0, -1.5932321895309828e-05, 0.0, -5.420700712112121e-08, 1.8156311847692008, 0.0, 0.0, 7.631162140586666e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5932321895309828e-05, 7.631162140586658e-06, 0.0, 0.0, 1.790536648146295, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 197 + stamp: + secs: 1624875221 + nsecs: 81346989 + frame_id: "/world" +pose: + pose: + position: + x: -0.016614315665 + y: -0.00660014124438 + z: 0.0 + orientation: + x: 0.0 + y: 6.53206699113e-05 + z: 0.0 + w: 0.999999997867 + covariance: [1.98894600277261, -3.792099522913069e-08, 0.0, 0.0, -4.632526033931528e-06, 0.0, -3.792099522913071e-08, 1.9889457571871914, 0.0, 0.0, 1.9998074194998513e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.632526033931527e-06, 1.9998074194998467e-06, 0.0, 0.0, 1.971503208817878, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 198 + stamp: + secs: 1624875221 + nsecs: 161340952 + frame_id: "/world" +pose: + pose: + position: + x: -0.0169929262025 + y: -0.00758666330406 + z: 0.0 + orientation: + x: 0.0 + y: 0.000452424461374 + z: 0.0 + w: 0.999999897656 + covariance: [1.8237681173721898, -3.4880148618443987e-08, 0.0, 0.0, -1.2075300067625661e-05, 0.0, -3.4880148618444e-08, 1.823767895913601, 0.0, 0.0, 2.9719904122339188e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2075300067625653e-05, 2.971990412233914e-06, 0.0, 0.0, 1.8070095537634245, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 199 + stamp: + secs: 1624875221 + nsecs: 241344929 + frame_id: "/world" +pose: + pose: + position: + x: -0.0174719986672 + y: -0.00857649530523 + z: 0.0 + orientation: + x: 0.0 + y: 2.0126500014e-05 + z: 0.0 + w: 0.999999999797 + covariance: [1.922304026583644, -4.799602574853959e-08, 0.0, 0.0, -8.686688253487691e-06, 0.0, -4.79960257485396e-08, 1.9223037260336646, 0.0, 0.0, 2.7539170269504376e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -8.686688253487686e-06, 2.753917026950443e-06, 0.0, 0.0, 1.897858601183947, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 200 + stamp: + secs: 1624875221 + nsecs: 401350975 + frame_id: "/world" +pose: + pose: + position: + x: -0.0153108627627 + y: -0.00795743731862 + z: 0.0 + orientation: + x: -0.0 + y: -4.34551842683e-06 + z: -0.0 + w: 0.999999999991 + covariance: [1.8343686905516035, -5.863026761107662e-08, 0.0, 0.0, -7.204421859490565e-06, 0.0, -5.863026761107648e-08, 1.8343683252694316, 0.0, 0.0, 1.298408785070291e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.204421859490562e-06, 1.2984087850702912e-06, 0.0, 0.0, 1.8018970234149791, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 201 + stamp: + secs: 1624875221 + nsecs: 481375933 + frame_id: "/world" +pose: + pose: + position: + x: -0.00515460156512 + y: -0.00680932342745 + z: 0.0 + orientation: + x: 0.0 + y: 0.00110638357679 + z: 0.0 + w: 0.999999387958 + covariance: [1.8145095679097962, -3.356562613766591e-08, 0.0, 0.0, -4.592002155510226e-05, 0.0, -3.356562613766591e-08, 1.8145093507507661, 0.0, 0.0, 2.9238167286360603e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.592002155510225e-05, 2.923816728636054e-06, 0.0, 0.0, 1.7955429013986095, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 202 + stamp: + secs: 1624875221 + nsecs: 721393108 + frame_id: "/world" +pose: + pose: + position: + x: -0.00436507957377 + y: 0.00818059900262 + z: 0.0 + orientation: + x: -0.0 + y: -0.00313104988872 + z: -0.0 + w: 0.999995098251 + covariance: [1.949449007460753, -1.4742196765553655e-07, 0.0, 0.0, -7.458582724576659e-05, 0.0, -1.4742196765553653e-07, 1.949447852464053, 0.0, 0.0, 5.836810021374224e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -7.45858272457665e-05, 5.836810021374216e-06, 0.0, 0.0, 1.8656575695842958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 203 + stamp: + secs: 1624875221 + nsecs: 881417036 + frame_id: "/world" +pose: + pose: + position: + x: -0.0105088435457 + y: 0.010500298877 + z: 0.0 + orientation: + x: 0.0 + y: 8.25726346131e-06 + z: 0.0 + w: 0.999999999966 + covariance: [2.0311497008761306, -8.428522190033619e-08, 0.0, 0.0, -1.987984771970762e-05, 0.0, -8.428522190033629e-08, 2.031149025808958, 0.0, 0.0, 1.6888094878777858e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.9879847719707623e-05, 1.6888094878777854e-06, 0.0, 0.0, 1.9788984248993076, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 204 + stamp: + secs: 1624875222 + nsecs: 41435957 + frame_id: "/world" +pose: + pose: + position: + x: -0.0116713220419 + y: 0.0134815823038 + z: 0.0 + orientation: + x: -0.0 + y: -0.00148512595015 + z: -0.0 + w: 0.9999988972 + covariance: [2.1538917461679805, -6.044865588774103e-08, 0.0, 0.0, -1.2580963906825414e-05, 0.0, -6.044865588774115e-08, 2.153891262797581, 0.0, 0.0, 1.1524313444299223e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2580963906825415e-05, 1.1524313444299233e-05, 0.0, 0.0, 2.1161446688745262, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 205 + stamp: + secs: 1624875222 + nsecs: 121417046 + frame_id: "/world" +pose: + pose: + position: + x: -0.0186365076339 + y: 0.010010012416 + z: 0.0 + orientation: + x: -0.0 + y: -0.000112193034115 + z: -0.0 + w: 0.999999993706 + covariance: [1.7342465915672414, -1.6971172271969652e-08, 0.0, 0.0, -2.6220239734469574e-06, 0.0, -1.697117227196967e-08, 1.734246459155423, 0.0, 0.0, 3.0971258855759275e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6220239734469637e-06, 3.0971258855759262e-06, 0.0, 0.0, 1.7230253177048247, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 206 + stamp: + secs: 1624875222 + nsecs: 201446056 + frame_id: "/world" +pose: + pose: + position: + x: -0.0165699860315 + y: 0.0114662788232 + z: 0.0 + orientation: + x: -0.0 + y: -0.000464308939087 + z: -0.0 + w: 0.999999892209 + covariance: [1.8672224777122646, -1.511924796926902e-08, 0.0, 0.0, -1.6389942120255714e-06, 0.0, -1.511924796926908e-08, 1.8672223623865782, 0.0, 0.0, 1.0569111033105775e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6389942120255705e-06, 1.056911103310577e-06, 0.0, 0.0, 1.8563750574955418, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 207 + stamp: + secs: 1624875222 + nsecs: 281445026 + frame_id: "/world" +pose: + pose: + position: + x: -0.0139670054382 + y: -0.00825586066529 + z: 0.0 + orientation: + x: -0.0 + y: -0.000488999001178 + z: -0.0 + w: 0.99999988044 + covariance: [1.9225436843602657, -2.634062034097246e-08, 0.0, 0.0, -2.4176541091827893e-06, 0.0, -2.6340620340972427e-08, 1.9225434852139456, 0.0, 0.0, 4.3295076262250424e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4176541091827893e-06, 4.329507626225045e-07, 0.0, 0.0, 1.9020987186543783, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 208 + stamp: + secs: 1624875222 + nsecs: 441457987 + frame_id: "/world" +pose: + pose: + position: + x: 0.00974671291554 + y: 0.0230666008456 + z: 0.0 + orientation: + x: -0.0 + y: -0.00371003514794 + z: -0.0 + w: 0.999993117796 + covariance: [2.150987509510738, -5.244741805963288e-08, 0.0, 0.0, -1.5237091272488477e-05, 0.0, -5.244741805963278e-08, 2.1509871164763754, 0.0, 0.0, 6.945301002405438e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.5237091272488474e-05, 6.94530100240543e-06, 0.0, 0.0, 2.106977482829993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 209 + stamp: + secs: 1624875222 + nsecs: 681476116 + frame_id: "/world" +pose: + pose: + position: + x: -0.0265509633026 + y: 0.0385884009918 + z: 0.0 + orientation: + x: 0.0 + y: 0.00037670259011 + z: 0.0 + w: 0.999999929048 + covariance: [1.8136210558334427, -6.305301541286962e-08, 0.0, 0.0, -1.213868519329487e-05, 0.0, -6.305301541286962e-08, 1.8136205958157, 0.0, 0.0, 4.127674870520173e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.2138685193294867e-05, 4.127674870520166e-06, 0.0, 0.0, 1.7528465848919756, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 210 + stamp: + secs: 1624875222 + nsecs: 761483908 + frame_id: "/world" +pose: + pose: + position: + x: 0.00757589766217 + y: 0.0332913709641 + z: 0.0 + orientation: + x: -0.0 + y: -0.00128547261191 + z: -0.0 + w: 0.99999917378 + covariance: [2.3761564510930375, -3.9282660858314565e-08, 0.0, 0.0, -4.685335292613336e-06, 0.0, -3.9282660858314585e-08, 2.376156166857276, 0.0, 0.0, 4.872614235967291e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.685335292613333e-06, 4.872614235967289e-06, 0.0, 0.0, 2.3360818904329155, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 211 + stamp: + secs: 1624875222 + nsecs: 921515942 + frame_id: "/world" +pose: + pose: + position: + x: 0.00747665075005 + y: 0.0327088642318 + z: 0.0 + orientation: + x: -0.0 + y: -4.72317197007e-05 + z: -0.0 + w: 0.999999998885 + covariance: [1.7967348833668766, -2.7788558335206744e-08, 0.0, 0.0, -2.417462112255062e-05, 0.0, -2.778855833520681e-08, 1.7967346842745933, 0.0, 0.0, 3.597873963892489e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.4174621122550642e-05, 3.5978739638924805e-06, 0.0, 0.0, 1.7666730647305602, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 212 + stamp: + secs: 1624875223 + nsecs: 1523018 + frame_id: "/world" +pose: + pose: + position: + x: 0.000453744158822 + y: 0.0248119972448 + z: 0.0 + orientation: + x: -0.0 + y: -0.000651615984115 + z: -0.0 + w: 0.999999787698 + covariance: [1.7721615873599765, -9.709682202380962e-09, 0.0, 0.0, -1.0904918936634507e-05, 0.0, -9.70968220238099e-09, 1.7721615165755162, 0.0, 0.0, 7.866881969222498e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0904918936634504e-05, 7.866881969222484e-07, 0.0, 0.0, 1.7613833402343388, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 213 + stamp: + secs: 1624875223 + nsecs: 81501961 + frame_id: "/world" +pose: + pose: + position: + x: -0.00113691557289 + y: 0.0220271643003 + z: 0.0 + orientation: + x: 0.0 + y: 0.000513293633802 + z: 0.0 + w: 0.999999868265 + covariance: [1.6887865945444143, -1.4560993339927585e-08, 0.0, 0.0, -1.0868115129112175e-05, 0.0, -1.4560993339927608e-08, 1.6887864931903005, 0.0, 0.0, 1.302832193387308e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0868115129112192e-05, 1.3028321933873072e-05, 0.0, 0.0, 1.6727430821347509, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 214 + stamp: + secs: 1624875223 + nsecs: 241575956 + frame_id: "/world" +pose: + pose: + position: + x: -0.0011313536776 + y: 0.022825955926 + z: 0.0 + orientation: + x: 0.0 + y: 0.000244499132092 + z: 0.0 + w: 0.99999997011 + covariance: [1.812963321921567, -2.7370767258969255e-08, 0.0, 0.0, -2.608972380741921e-05, 0.0, -2.73707672589692e-08, 1.812963167727304, 0.0, 0.0, 9.503619317269996e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.6089723807419203e-05, 9.503619317269992e-06, 0.0, 0.0, 1.7861410690563264, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 215 + stamp: + secs: 1624875223 + nsecs: 321557045 + frame_id: "/world" +pose: + pose: + position: + x: -0.00138059031145 + y: 0.022359590705 + z: 0.0 + orientation: + x: -0.0 + y: -0.000382253911441 + z: -0.0 + w: 0.999999926941 + covariance: [1.6634599015799008, -1.8162209208659365e-08, 0.0, 0.0, -1.6242467951275973e-05, 0.0, -1.816220920865939e-08, 1.663459803077689, 0.0, 0.0, 4.087541869577971e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.6242467951275956e-05, 4.087541869577973e-06, 0.0, 0.0, 1.6465664768987758, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 216 + stamp: + secs: 1624875223 + nsecs: 481564999 + frame_id: "/world" +pose: + pose: + position: + x: -0.000611757965217 + y: 0.0201900833438 + z: 0.0 + orientation: + x: -0.0 + y: -0.00100394770678 + z: -0.0 + w: 0.999999496044 + covariance: [1.8115882288710639, -2.9200881163422354e-08, 0.0, 0.0, -1.5320422896194035e-05, 0.0, -2.920088116342238e-08, 1.811588073372357, 0.0, 0.0, 2.4413406474438e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.532042289619402e-05, 2.441340647443794e-06, 0.0, 0.0, 1.7856040010922698, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 217 + stamp: + secs: 1624875223 + nsecs: 561572075 + frame_id: "/world" +pose: + pose: + position: + x: -0.000953172742303 + y: 0.0199452386238 + z: 0.0 + orientation: + x: -0.0 + y: -0.000655506227161 + z: -0.0 + w: 0.999999785156 + covariance: [1.7514653156245148, -1.9970061930216264e-08, 0.0, 0.0, -5.742161172756162e-06, 0.0, -1.9970061930216267e-08, 1.7514652089789067, 0.0, 0.0, 1.0934690155708493e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.742161172756158e-06, 1.0934690155708514e-06, 0.0, 0.0, 1.7331392891022368, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 218 + stamp: + secs: 1624875223 + nsecs: 721590996 + frame_id: "/world" +pose: + pose: + position: + x: 0.00282834168011 + y: 0.00957745710809 + z: 0.0 + orientation: + x: 0.0 + y: 8.47434489077e-05 + z: 0.0 + w: 0.999999996409 + covariance: [1.8532923124992646, -3.014335850062694e-08, 0.0, 0.0, -1.0910866363502732e-05, 0.0, -3.014335850062699e-08, 1.8532921638441768, 0.0, 0.0, 2.4906123114842124e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.0910866363502742e-05, 2.4906123114842077e-05, 0.0, 0.0, 1.8232555878052323, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 219 + stamp: + secs: 1624875223 + nsecs: 801577091 + frame_id: "/world" +pose: + pose: + position: + x: 0.00192829840345 + y: -0.00221379494976 + z: 0.0 + orientation: + x: 0.0 + y: 0.00047571603585 + z: 0.0 + w: 0.999999886847 + covariance: [1.922019627817414, -2.482893666113636e-08, 0.0, 0.0, -5.63470002593294e-06, 0.0, -2.4828936661136313e-08, 1.9220195304661287, 0.0, 0.0, 1.2133412663807352e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.634700025932942e-06, 1.2133412663807358e-05, 0.0, 0.0, 1.8991720879523406, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 220 + stamp: + secs: 1624875223 + nsecs: 961596966 + frame_id: "/world" +pose: + pose: + position: + x: 0.00715419898661 + y: 0.00802851820143 + z: 0.0 + orientation: + x: -0.0 + y: -0.000106420012518 + z: -0.0 + w: 0.999999994337 + covariance: [1.763403015544808, -2.657478219487305e-08, 0.0, 0.0, -6.819861121088835e-06, 0.0, -2.6574782194872974e-08, 1.7634029279658727, 0.0, 0.0, 1.9044411423635882e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.819861121088809e-06, 1.9044411423635895e-05, 0.0, 0.0, 1.7395269723086135, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 221 + stamp: + secs: 1624875224 + nsecs: 41613102 + frame_id: "/world" +pose: + pose: + position: + x: -0.00965684466772 + y: 0.0051812522801 + z: 0.0 + orientation: + x: 0.0 + y: 0.000523985266483 + z: 0.0 + w: 0.99999986272 + covariance: [1.9501399713735685, -2.4838466021580715e-08, 0.0, 0.0, -1.803510953420811e-05, 0.0, -2.4838466021580725e-08, 1.9501399039344196, 0.0, 0.0, 1.0276630966633254e-05, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.8035109534208112e-05, 1.0276630966633245e-05, 0.0, 0.0, 1.9290822114705835, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 222 + stamp: + secs: 1624875224 + nsecs: 201616049 + frame_id: "/world" +pose: + pose: + position: + x: -0.0100933563779 + y: 0.00443334536622 + z: 0.0 + orientation: + x: -0.0 + y: -0.000474759929823 + z: -0.0 + w: 0.999999887301 + covariance: [1.8323483297791563, -3.465430872018018e-08, 0.0, 0.0, -1.8379907795263756e-05, 0.0, -3.465430872018019e-08, 1.8323482410250742, 0.0, 0.0, 9.168516578011825e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.837990779526374e-05, 9.168516578011828e-06, 0.0, 0.0, 1.8040680180079156, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 223 + stamp: + secs: 1624875224 + nsecs: 281644106 + frame_id: "/world" +pose: + pose: + position: + x: -0.00712449017371 + y: -0.00136042664287 + z: 0.0 + orientation: + x: 0.0 + y: 6.04221504641e-06 + z: 0.0 + w: 0.999999999982 + covariance: [1.812971504234954, -2.182059823662339e-08, 0.0, 0.0, -9.62349027952663e-06, 0.0, -2.182059823662338e-08, 1.8129714506894317, 0.0, 0.0, 7.69934019815006e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -9.623490279526627e-06, 7.69934019815006e-06, 0.0, 0.0, 1.795440513373712, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 224 + stamp: + secs: 1624875224 + nsecs: 441647053 + frame_id: "/world" +pose: + pose: + position: + x: -0.00715121372408 + y: -0.00655713985432 + z: 0.0 + orientation: + x: 0.0 + y: 0.000256779091594 + z: 0.0 + w: 0.999999967032 + covariance: [1.669726039542415, -2.783442498062321e-08, 0.0, 0.0, -4.568043839645863e-06, 0.0, -2.7834424980623158e-08, 1.6697259754458755, 0.0, 0.0, 4.006733276253261e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -4.568043839645857e-06, 4.00673327625326e-06, 0.0, 0.0, 1.6472110925798713, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 225 + stamp: + secs: 1624875224 + nsecs: 601653099 + frame_id: "/world" +pose: + pose: + position: + x: -0.00244139280261 + y: 0.0082851913033 + z: 0.0 + orientation: + x: -0.0 + y: -0.000294330998062 + z: -0.0 + w: 0.999999956685 + covariance: [1.6605719193664692, -2.5797544294792705e-08, 0.0, 0.0, -1.1304137126256088e-06, 0.0, -2.5797544294792675e-08, 1.660571861022126, 0.0, 0.0, 2.1536117528796487e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -1.1304137126256115e-06, 2.1536117528796465e-06, 0.0, 0.0, 1.6382216182937268, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 226 + stamp: + secs: 1624875224 + nsecs: 681679964 + frame_id: "/world" +pose: + pose: + position: + x: -0.00177194098159 + y: 0.00754145774385 + z: 0.0 + orientation: + x: -0.0 + y: -0.000211409942188 + z: -0.0 + w: 0.999999977653 + covariance: [1.8068506465452607, -2.486258712143874e-08, 0.0, 0.0, -5.381295440989572e-07, 0.0, -2.4862587121438723e-08, 1.8068505906573447, 0.0, 0.0, 9.863035534601412e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.38129544098958e-07, 9.863035534601433e-07, 0.0, 0.0, 1.7837011561016793, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 227 + stamp: + secs: 1624875224 + nsecs: 841675043 + frame_id: "/world" +pose: + pose: + position: + x: 0.0128689538688 + y: 0.0237631333425 + z: 0.0 + orientation: + x: -0.0 + y: -0.0020266239252 + z: -0.0 + w: 0.999997946396 + covariance: [2.0585713408294177, -4.6637922075132873e-08, 0.0, 0.0, -5.584699428968881e-06, 0.0, -4.6637922075132966e-08, 2.0585712362850557, 0.0, 0.0, 3.015664016049707e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -5.58469942896888e-06, 3.015664016049701e-06, 0.0, 0.0, 2.010406153140263, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 228 + stamp: + secs: 1624875225 + nsecs: 81682920 + frame_id: "/world" +pose: + pose: + position: + x: 0.00989291394214 + y: 0.0223202190851 + z: 0.0 + orientation: + x: -0.0 + y: -0.00016398719774 + z: -0.0 + w: 0.999999986554 + covariance: [1.8919777259409702, -6.686286029120172e-08, 0.0, 0.0, -3.3772544202411305e-06, 0.0, -6.686286029120162e-08, 1.891977576742007, 0.0, 0.0, 5.470024994617234e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -3.377254420241132e-06, 5.470024994617241e-06, 0.0, 0.0, 1.8128432298468902, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 229 + stamp: + secs: 1624875225 + nsecs: 241738081 + frame_id: "/world" +pose: + pose: + position: + x: 0.0074147595362 + y: 0.0269235046738 + z: 0.0 + orientation: + x: -0.0 + y: -0.00012726063262 + z: -0.0 + w: 0.999999991902 + covariance: [1.7426276809575674, -2.8325007788772667e-08, 0.0, 0.0, -6.655033974176582e-07, 0.0, -2.832500778877274e-08, 1.742627617970086, 0.0, 0.0, 1.2689829736664133e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -6.655033974176592e-07, 1.2689829736664106e-06, 0.0, 0.0, 1.704041127676915, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 230 + stamp: + secs: 1624875225 + nsecs: 321708918 + frame_id: "/world" +pose: + pose: + position: + x: -0.0016721715533 + y: 0.019384089063 + z: 0.0 + orientation: + x: 0.0 + y: 0.000746943622505 + z: 0.0 + w: 0.999999721038 + covariance: [1.783335878995612, -1.892268542293102e-08, 0.0, 0.0, -2.19117473837793e-05, 0.0, -1.8922685422931007e-08, 1.78333583683227, 0.0, 0.0, 7.981046086627504e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1911747383779276e-05, 7.981046086627506e-06, 0.0, 0.0, 1.757185218810035, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 231 + stamp: + secs: 1624875225 + nsecs: 481729984 + frame_id: "/world" +pose: + pose: + position: + x: -0.00693751493239 + y: 0.0290699850166 + z: 0.0 + orientation: + x: -0.0 + y: -3.34338565574e-05 + z: -0.0 + w: 0.999999999441 + covariance: [1.9031595989972274, -3.8334893098726945e-08, 0.0, 0.0, -2.181579749486606e-05, 0.0, -3.833489309872697e-08, 1.9031595119379818, 0.0, 0.0, 8.342397597122603e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.181579749486612e-05, 8.342397597122608e-06, 0.0, 0.0, 1.8590198940253084, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- +header: + seq: 232 + stamp: + secs: 1624875225 + nsecs: 641766071 + frame_id: "/world" +pose: + pose: + position: + x: -0.0172194439491 + y: 0.0194378851393 + z: 0.0 + orientation: + x: 0.0 + y: 0.000568660222151 + z: 0.0 + w: 0.999999838313 + covariance: [1.8676158879487474, -3.691464656214016e-08, 0.0, 0.0, -2.1017640081323175e-05, 0.0, -3.691464656214016e-08, 1.867615804114841, 0.0, 0.0, 8.037180861960741e-06, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, -2.1017640081323186e-05, 8.037180861960741e-06, 0.0, 0.0, 1.8250910876171649, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-07] +--- diff --git a/GPU_version/vio/package.xml b/GPU_version/vio/package.xml new file mode 100755 index 0000000..262fff1 --- /dev/null +++ b/GPU_version/vio/package.xml @@ -0,0 +1,34 @@ + + + vio + 0.1.0 + + Monocular Visual Odometry + + Majid Geravand + Gfuse + GPLv3 + + + catkin + + + cmake_modules + roscpp + roslib + sensor_msgs + image_transport + cv_bridge + tf + geometry_msgs + message_generation + + + roscpp + roslib + image_transport + cv_bridge + tf + geometry_msgs + message_runtime + \ No newline at end of file diff --git a/GPU_version/vio/param/px30.yaml b/GPU_version/vio/param/px30.yaml new file mode 100644 index 0000000..4b69c4f --- /dev/null +++ b/GPU_version/vio/param/px30.yaml @@ -0,0 +1,9 @@ +vio: + cam_model: ATAN + cam_width: 640 + cam_height: 480 + cam_fx: 0.463632 + cam_fy: 0.823661 + cam_cx: 0.497465 + cam_cy: 0.498443 + cam_d0: 1.01687 diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml new file mode 100644 index 0000000..1ecb630 --- /dev/null +++ b/GPU_version/vio/param/vo_fast.yaml @@ -0,0 +1,15 @@ +svo: + grid_size: 30 #Feature grid size of a cell in [px]. + max_n_kfs: 20 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. + loba_num_iter: 0 #Number of iterations in the local bundle adjustment. + quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. + quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad. + init_min_tracked: 100 + init_min_disparity: 30 + structureoptim_max_pts: 50 + kfselect_mindist: 0.01 + poseoptim_thresh: 4.0 + ACC_noise: 0.5 #acc white noise in continuous in EKF + GYO_noise: 0.001 #gyro white noise in continuous in EKF + svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF + Cmd_Cov: 1.0 #control vector noise in EKF diff --git a/GPU_version/vio/src/atan_camera.cpp b/GPU_version/vio/src/atan_camera.cpp new file mode 100644 index 0000000..c86f027 --- /dev/null +++ b/GPU_version/vio/src/atan_camera.cpp @@ -0,0 +1,82 @@ +/* + * atan_camera.cpp + * + * Created on: Aug 21, 2012 + * Author: cforster + */ + + +#include +#include +#include + +namespace vk { + +ATANCamera:: +ATANCamera(double width, double height, + double fx, double fy, + double cx, double cy, + double s) : + AbstractCamera(width, height), + fx_(width*fx), fy_(height*fy), + fx_inv_(1.0/fx_), fy_inv_(1.0/fy_), + cx_(cx*width - 0.5), cy_(cy*height - 0.5), + s_(s), s_inv_(1.0/s_) +{ + if(s_ != 0.0) + { + tans_ = 2.0 * tan(s_ / 2.0); + tans_inv_ = 1.0 / tans_; + s_inv_ = 1.0 / s_; + distortion_ = true; + } + else + { + s_inv_ = 0.0; + tans_ = 0.0; + distortion_ = false; + } +} + +ATANCamera:: +~ATANCamera() +{} + +Vector3d ATANCamera:: +cam2world(const double& x, const double& y) const +{ + Vector2d dist_cam((x - cx_) * fx_inv_, + (y - cy_) * fy_inv_); + double dist_r = dist_cam.norm(); + double r = invrtrans(dist_r); + double d_factor; + if(dist_r > 0.01) + d_factor = r / dist_r; + else + d_factor = 1.0; + return unproject2d(d_factor * dist_cam).normalized(); +} + +Vector3d ATANCamera:: +cam2world (const Vector2d& px) const +{ + return cam2world(px[0], px[1]); +} + +Vector2d ATANCamera:: +world2cam(const Vector3d& xyz_c) const +{ + return world2cam(project2d(xyz_c)); +} + +Vector2d ATANCamera:: +world2cam(const Vector2d& uv) const +{ + double r = uv.norm(); + double factor = rtrans_factor(r); + Vector2d dist_cam = factor * uv; + return Vector2d(cx_ + fx_ * dist_cam[0], + cy_ + fy_ * dist_cam[1]); +} + +} /* end vk */ diff --git a/GPU_version/vio/src/cl_class.cpp b/GPU_version/vio/src/cl_class.cpp new file mode 100644 index 0000000..a26d455 --- /dev/null +++ b/GPU_version/vio/src/cl_class.cpp @@ -0,0 +1,48 @@ +// +// Created by root on 4/27/21. +// +#include "gpu_svo/cl_class.h" +opencl::opencl() { + std::vector all_platforms; + cl::Platform::get(&all_platforms); + if (all_platforms.size() == 0){ + std::cerr << " No platforms found. Check OpenCL installation!\n"; + exit(1); + } + //get default device of the default platform + std::vector all_devices; + for(auto i:all_platforms){ + std::cout << "Find platform number:"<< i.getInfo() << "\n"; + i.getDevices(CL_DEVICE_TYPE_GPU, &all_devices); + } + + if (all_devices.size() == 0){ + std::cout << " No devices found. Check OpenCL installation!\n"; + exit(1); + } + device = all_devices[0]; + std::cout << "CL_DEVICE_NAME: " << device.getInfo() <<'\n' + << "CL_DEVICE_OPENCL_C_VERSION: " <()<<'\n' + << "CL_DEVICE_BUILT_IN_KERNELS: " <()<<'\n' + << "CL_DEVICE_COMPILER_AVAILABLE: " <()<<'\n' + << "CL_DEVICE_LOCAL_MEM_SIZE: " <()<<'\n' + << "CL_DEVICE_GLOBAL_MEM_SIZE: " <()<<'\n'; + + context=new cl::Context({ device }); + cl::Program::Sources sources; + read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl"); + sources.push_back({ fast.src_str, fast.size }); + program=new cl::Program(*context, sources); + if(program->build({ device },"-DFAST_THRESH=40") !=0) + std::cout << " Error building: " << program->getBuildInfo(device)<<'\n' + <getBuildInfo(device) << '\n'; + queue=new cl::CommandQueue(*context,device,0,NULL); +} +opencl::~opencl() { +} +void opencl::clear_buf() { + queue->finish(); + queue->flush(); + _kernels.clear(); +} + diff --git a/GPU_version/vio/src/config.cpp b/GPU_version/vio/src/config.cpp new file mode 100644 index 0000000..5c3cab7 --- /dev/null +++ b/GPU_version/vio/src/config.cpp @@ -0,0 +1,73 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include + +namespace svo { + +Config::Config() : + trace_name(vk::getParam("svo/trace_name", "svo")), + trace_dir(vk::getParam("svo/trace_dir", "/tmp")), + n_pyr_levels(vk::getParam("svo/n_pyr_levels", 3)), + use_imu(vk::getParam("svo/use_imu", false)), + core_n_kfs(vk::getParam("svo/core_n_kfs", 3)), + map_scale(vk::getParam("svo/map_scale", 1.0)), + grid_size(vk::getParam("svo/grid_size", 30)), + init_min_disparity(vk::getParam("svo/init_min_disparity", 50.0)), + init_min_tracked(vk::getParam("svo/init_min_tracked", 50)), + init_min_inliers(vk::getParam("svo/init_min_inliers", 40)), + klt_max_level(vk::getParam("svo/klt_max_level", 4)), + klt_min_level(vk::getParam("svo/klt_min_level", 2)), + reproj_thresh(vk::getParam("svo/reproj_thresh", 2.0)), + poseoptim_thresh(vk::getParam("svo/poseoptim_thresh", 2.0)), + poseoptim_num_iter(vk::getParam("svo/poseoptim_num_iter", 10)), + structureoptim_max_pts(vk::getParam("svo/structureoptim_max_pts", 20)), + structureoptim_num_iter(vk::getParam("svo/structureoptim_num_iter", 5)), + loba_thresh(vk::getParam("svo/loba_thresh", 2.0)), + loba_robust_huber_width(vk::getParam("svo/loba_robust_huber_width", 1.0)), + loba_num_iter(vk::getParam("svo/loba_num_iter", 0)), + kfselect_mindist(vk::getParam("svo/kfselect_mindist", 0.12)), + triang_min_corner_score(vk::getParam("svo/triang_min_corner_score", 20.0)), + triang_half_patch_size(vk::getParam("svo/triang_half_patch_size", 4)), + subpix_n_iter(vk::getParam("svo/subpix_n_iter", 10)), + max_n_kfs(vk::getParam("svo/max_n_kfs", 10)), + img_imu_delay(vk::getParam("svo/img_imu_delay", 0.0)), + max_fts(vk::getParam("svo/max_fts", 120)), + quality_min_fts(vk::getParam("svo/quality_min_fts", 50)), + quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)), + ACC_noise(vk::getParam("svo/ACC_noise", 1e-4)), + GYO_noise(vk::getParam("svo/GYO_noise", 1e-4)), + iuc(vk::getParam("svo/IUC", 1e-1)), + abc(vk::getParam("svo/ABC", 1e-1)), + gbc(vk::getParam("svo/GBC", 1e-1)), + ebp(vk::getParam("svo/EBP", 1e-2)), + nrp(vk::getParam("svo/NRP", 1e-5)), + svo_ekf(vk::getParam("svo/svo_ekf", 30)), + acc_x_off(vk::getParam("svo/Acceleration_offset_x", 0.0)), + acc_y_off(vk::getParam("svo/Acceleration_offset_y", 0.0)), + gyr_z_off(vk::getParam("svo/Gyroscope_offset_z", 0.0)), + cmd_cov(vk::getParam("svo/Cmd_Cov", 0.0)) +{} + +Config& Config::getInstance() +{ + static Config instance; // Instantiated on first use and guaranteed to be destroyed + return instance; +} + +} // namespace svo + diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp new file mode 100644 index 0000000..c7ebf2c --- /dev/null +++ b/GPU_version/vio/src/depth_filter.cpp @@ -0,0 +1,357 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Seed::batch_counter = 0; +int Seed::seed_counter = 0; + +Seed::Seed(Feature* ftr, float depth_mean, float depth_min) : + batch_id(batch_counter), + id(seed_counter++), + ftr(ftr), + a(10), + b(10), + mu(1.0/depth_mean), + z_range(1.0/depth_min), + sigma2(z_range*z_range/36) +{} + +DepthFilter::DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb) : + feature_detector_(feature_detector), + seed_converged_cb_(seed_converged_cb), + seeds_updating_halt_(false), + thread_(NULL), + new_keyframe_set_(false), + new_keyframe_min_depth_(0.0), + new_keyframe_mean_depth_(0.0) +{} + +DepthFilter::~DepthFilter() +{ + stopThread(); + SVO_INFO_STREAM("DepthFilter destructed."); +} + +void DepthFilter::startThread() +{ + thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this); +} + +void DepthFilter::stopThread() +{ + SVO_INFO_STREAM("DepthFilter stop thread invoked."); + if(thread_ != NULL) + { + SVO_INFO_STREAM("DepthFilter interrupt and join thread... "); + seeds_updating_halt_ = true; + thread_->interrupt(); + usleep(5000); + thread_->join(); + thread_ = NULL; + } +} + +void DepthFilter::addFrame(FramePtr frame) +{ + if(thread_ != NULL) + { + { + lock_t lock(frame_queue_mut_); + if(frame_queue_.size() > 2) + frame_queue_.pop(); + frame_queue_.push(frame); + } + seeds_updating_halt_ = false; + frame_queue_cond_.notify_one(); + }else{ + updateSeeds(frame); + } + +} + +void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min) +{ + new_keyframe_min_depth_ = depth_min; + new_keyframe_mean_depth_ = depth_mean; + + if(thread_ != NULL) + { + new_keyframe_ = frame; + new_keyframe_set_ = true; + seeds_updating_halt_ = true; + frame_queue_cond_.notify_one(); + }else if(seeds_cont_>3){ + initializeSeeds(frame); + seeds_cont_=0; + }else if(seeds_cont_==0){ + initializeSeeds(frame); + } + ++seeds_cont_; +} + +void DepthFilter::initializeSeeds(FramePtr frame) +{ + Features new_features; + feature_detector_->setExistingFeatures(frame->fts_); + feature_detector_->detect(frame.get(), frame->img_pyr_, + Config::triangMinCornerScore(), new_features); + + // initialize a seed for every new feature + seeds_updating_halt_ = true; + lock_t lock(seeds_mut_); // by locking the updateSeeds function stops + ++Seed::batch_counter; + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_)); + }); + + if(options_.verbose) + SVO_INFO_STREAM("DepthFilter: Initialized "<::iterator it=seeds_.begin(); + size_t n_removed = 0; + while(it!=seeds_.end()) + { + if(it->ftr->frame == frame.get()) + { + it = seeds_.erase(it); + ++n_removed; + } + else + ++it; + } + seeds_updating_halt_ = false; +} + +void DepthFilter::reset() +{ + seeds_updating_halt_ = true; + { + lock_t lock(seeds_mut_); + seeds_.clear(); + } + lock_t lock(); + while(!frame_queue_.empty()) + frame_queue_.pop(); + seeds_updating_halt_ = false; + + if(options_.verbose) + SVO_INFO_STREAM("DepthFilter: RESET."); +} + +void DepthFilter::updateSeedsLoop() +{ + while(!boost::this_thread::interruption_requested()) + { + FramePtr frame; + { + lock_t lock(frame_queue_mut_); + while(frame_queue_.empty() && new_keyframe_set_ == false) + frame_queue_cond_.wait(lock); + if(new_keyframe_set_) + { + new_keyframe_set_ = false; + if(seeds_cont_>3){ + seeds_updating_halt_ = false; + } + clearFrameQueue(); + frame = new_keyframe_; + } + else + { + frame = frame_queue_.front(); + frame_queue_.pop(); + } + } + updateSeeds(frame); + + if((frame->isKeyframe() && seeds_cont_>3) ||(frame->isKeyframe() && seeds_cont_==0)){ + initializeSeeds(frame); + seeds_cont_=0; + } + + } +} + +void DepthFilter::updateSeeds(FramePtr frame) +{ + // update only a limited number of seeds, because we don't have time to do it + // for all the seeds in every frame! + size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size(); + lock_t lock(seeds_mut_); + std::list::iterator it=seeds_.begin(); + + const double focal_length = frame->cam_->errorMultiplier2(); + double px_noise = 1.0; + double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz) + while( it!=seeds_.end()) + { + // set this value true when seeds updating should be interrupted + if(seeds_updating_halt_) + return; + + // check if seed is not already too old + if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) { + it = seeds_.erase(it); + continue; + } + if(it->ftr->frame == NULL){ + it = seeds_.erase(it); + continue; + } + // check if point is visible in the current image + SE2 T(it->ftr->frame->T_f_w_.se2() * frame->T_f_w_.inverse()); + Eigen::Matrix R; + R<mu * it->ftr->f) ); + if(xyz_f.z() < 0.0 && xyz_f.y() < 0.08) { + ++it; // behind the camera + continue; + } + if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast())) { + ++it; // point does not project in image + continue; + } + + float z_min = it->mu + sqrt(it->sigma2); + float z_max = max(it->mu - sqrt(it->sigma2), 0.00000001f); + + if(z_min < 0){ + ++it; + continue; + } + + double z; + if(!matcher_.findEpipolarMatchDirect( + *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_min, 1.0/z_max, z)) + { + it->b++; // increase outlier probability when no match was found + ++it; + ++n_failed_matches; + continue; + } + double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle); + double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau)); + + // update the estimate + updateSeed(1.0/z, tau_inverse*tau_inverse, &*it); + ++n_updates; + + if(frame->isKeyframe()) + { + // The feature detector should not initialize new seeds close to this location + feature_detector_->setGridOccpuancy(matcher_.px_cur_); + } + + // if the seed has converged, we initialize a new candidate point and remove the seed + if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh) + { + assert(it->ftr->point == NULL); // TODO this should not happen anymore + Vector3d xyz_world(it->ftr->frame->getSE3Inv() * (it->ftr->f/it->mu)); + Point* point = new Point(xyz_world, it->ftr); + it->ftr->point = point; + seed_converged_cb_(point, it->sigma2); // put in candidate list + it = seeds_.erase(it); + } + else if(isnan(z_min)) + { + SVO_WARN_STREAM("z_min is NaN"); + it = seeds_.erase(it); + } + else + ++it; + } +} + +void DepthFilter::clearFrameQueue() +{ + while(!frame_queue_.empty()) + frame_queue_.pop(); +} + +void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list& seeds) +{ + lock_t lock(seeds_mut_); + for(std::list::iterator it=seeds_.begin(); it!=seeds_.end(); ++it) + { + if (it->ftr->frame == frame.get()) + seeds.push_back(*it); + } +} + +void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) +{ + float norm_scale = sqrt(seed->sigma2 + tau2); + if(std::isnan(norm_scale)) + return; + boost::math::normal_distribution nd(seed->mu, norm_scale); + float s2 = 1./(1./seed->sigma2 + 1./tau2); + float m = s2*(seed->mu/seed->sigma2 + x/tau2); + float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x); + float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range; + float normalization_constant = C1 + C2; + C1 /= normalization_constant; + C2 /= normalization_constant; + float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.); + float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.)) + + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f)); + + // update parameters + float mu_new = C1*m+C2*seed->mu; + seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new; + seed->mu = mu_new; + seed->a = (e-f)/(f-e/f); + seed->b = seed->a*(1.0f-f)/f; +} + +double DepthFilter::computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle) +{ + Vector3d t(T_ref_cur.translation()); + Vector3d a = f*z-t; + double t_norm = t.norm(); + double beta_plus = acos(a.dot(-t)/(t_norm*a.norm())) + px_error_angle; + return (t_norm*sin(beta_plus)/sin(PI-acos(f.dot(t)/t_norm)-beta_plus) - z); // ( triangle angles sum to PI and law of sines ) tau +} + +} // namespace svo diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp new file mode 100644 index 0000000..918ab25 --- /dev/null +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -0,0 +1,616 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifdef __SSE2__ +#include +#endif +#ifdef __ARM_NEON__ +#include +#endif +#include + +namespace svo { +namespace feature_alignment { + +#define SUBPIX_VERBOSE 0 + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv) +{ + const int halfpatch_size_ = 1; + const int patch_size = 2; + const int patch_area = 4; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dv[patch_area]; + Matrix2f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size+2; + float* it_dv = ref_patch_dv; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){ + break; + } + + if(isnan(u) || isnan(v)){ + // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + std::cout<<"H singularity\n"; + return false; + } + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dv = ref_patch_dv; + float new_chi2 = 0.0; + Vector2f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + + chi2 = new_chi2; + update = Hinv * Jres; + u += update[0]*dir[0]; + v += update[0]*dir[1]; + mean_diff += update[1]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] + << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd) +{ +#ifdef __ARM_NEON__ + if(!no_simd) + return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); +#endif + + const int halfpatch_size_ = 1; + const int patch_size_ = 2; + const int patch_area_ = 4; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_]; + float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_]; + Matrix3f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size_+2; + float* it_dx = ref_patch_dx; + float* it_dy = ref_patch_dy; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){ + break; + } + + if(isnan(u) || isnan(v)) { + // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + } + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dx = ref_patch_dx; + float* it_ref_dy = ref_patch_dy; +// float new_chi2 = 0.0; + Vector3f Jres; Jres.setZero(); + for(int y=0; y> (n)) // rounds to closest integer and descales + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate) +{ + // TODO: This function should not be used as the alignment is not robust to illumination changes! + const int halfpatch_size = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + const int W_BITS = 14; + + // compute derivative of template and prepare inverse compositional + int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area]; + int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area]; + + // compute gradient and hessian + const int ref_step = patch_size+2; + int16_t* it_dx = ref_patch_dx; + int16_t* it_dy = ref_patch_dy; + float A11=0, A12=0, A22=0; + for(int y=0; y(it[1]) - it[-1]; + int16_t dy = static_cast(it[ref_step]) - it[-ref_step]; + *it_dx = dx; + *it_dy = dy; // we are missing a factor 1/2 + A11 += static_cast(dx*dx); // we are missing a factor 1/4 + A12 += static_cast(dx*dy); + A22 += static_cast(dy*dy); + } + } + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.1*0.1; + const int cur_step = cur_img.step.p[0]; + const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 + float chi2 = 0; + float update_u = 0, update_v = 0; + + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + float new_chi2 = 0.0; + +#ifdef __SSE2__ + // compute bilinear interpolation weights + int wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS)); + int wTR = static_cast(subpix_x * (1.0f-subpix_y)*(1 << W_BITS)); + int wBL = static_cast((1.0f-subpix_x)*subpix_y*(1 << W_BITS)); + int wBR = (1 << W_BITS) - wTL - wTR - wBL; + + __m128i qw0 = _mm_set1_epi32(wTL + (wTR << 16)); // Sets the 4 signed 32-bit integer values to [wTL, wTR]. + __m128i qw1 = _mm_set1_epi32(wBL + (wBR << 16)); + __m128i z = _mm_setzero_si128(); + __m128 qb0 = _mm_setzero_ps(); // 4 floats + __m128 qb1 = _mm_setzero_ps(); // 4 floats + __m128i qdelta = _mm_set1_epi32(1 << (W_BITS-1)); + for(int y=0; y(it[1] - it[-1]); + *it_dy = static_cast(it[ref_step] - it[-ref_step]); // divide by 2 missing + Vector3f J(*it_dx, *it_dy, 1.0f); + H += J*J.transpose(); + } + } + Matrix3f Hinv = H.inverse(); + float mean_diff = 0.0; + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.1*0.1; + const int cur_step = cur_img.step.p[0]; + Vector3f update; + Vector3f Jres; + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + +#ifdef __ARM_NEON__ + const int SHIFT_BITS = 7; + const uint16_t wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1<(subpix_x*(1.0f-subpix_y)*(1<((1.0f-subpix_x)*subpix_y*(1<((1 << SHIFT_BITS) - wTL - wTR - wBL); + + // initialize result to zero + int32x4_t vb1 = vdupq_n_s32(0); + int32x4_t vb2 = vdupq_n_s32(0); + int16x8_t vmean_diff = vdupq_n_s16( (int16_t) (mean_diff+0.5) ); + int16x8_t vres_sum = vdupq_n_s16(0); + for(int y=0; y Vr[i] := Va[i] + Vb[i] * Vc[i] + // int32x4_t vmlal_s16(int32x4_t a, int16x4_t b, int16x4_t c); // VMLAL.S16 q0,d0,d0 + int16x8_t grad = vld1q_s16(ref_patch_dx + y*patch_size); + vb1 = vmlal_s16(vb1, vget_low_s16(grad), vget_low_s16(res)); + vb1 = vmlal_s16(vb1, vget_high_s16(grad), vget_high_s16(res)); + + grad = vld1q_s16(ref_patch_dy + y*patch_size); + vb2 = vmlal_s16(vb2, vget_low_s16(grad), vget_low_s16(res)); + vb2 = vmlal_s16(vb2, vget_high_s16(grad), vget_high_s16(res)); + } + + // finally, sum results of vb1, vb2 and vres_sum + int32x2_t tmp; + tmp = vpadd_s32(vget_low_s32(vb1), vget_high_s32(vb1)); + Jres[0] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + tmp = vpadd_s32(vget_low_s32(vb2), vget_high_s32(vb2)); + Jres[1] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + int32x4_t vres_sum1 = vpaddlq_s16(vres_sum); + tmp = vpadd_s32(vget_low_s32(vres_sum1), vget_high_s32(vres_sum1)); + Jres[2] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); +#endif + + update = Hinv * Jres * 2; // * 2 to compensate because above, we did not compute the derivative correctly + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +} // namespace feature_alignment +} // namespace svo diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp new file mode 100644 index 0000000..333e602 --- /dev/null +++ b/GPU_version/vio/src/feature_detection.cpp @@ -0,0 +1,114 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include + +namespace svo { +namespace feature_detection { + +AbstractDetector::AbstractDetector( + const int img_width, + const int img_height, + const int cell_size, + const int n_pyr_levels) : + cell_size_(cell_size), + n_pyr_levels_(n_pyr_levels), + grid_n_cols_(ceil(static_cast(img_width)/cell_size_)), + grid_n_rows_(ceil(static_cast(img_height)/cell_size_)), + grid_occupancy_(grid_n_cols_*grid_n_rows_, false) +{ +} + +void AbstractDetector::resetGrid() +{ + std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false); +} + +void AbstractDetector::setExistingFeatures(const Features& fts) +{ + std::for_each(fts.begin(), fts.end(), [&](Feature* i){ + grid_occupancy_.at( + static_cast(i->px[1]/cell_size_)*grid_n_cols_ + + static_cast(i->px[0]/cell_size_)) = true; + }); +} + +void AbstractDetector::setGridOccpuancy(const Vector2d& px) +{ + grid_occupancy_.at( + static_cast(px[1]/cell_size_)*grid_n_cols_ + + static_cast(px[0]/cell_size_)) = true; +} + +FastDetector::FastDetector( + const int img_width, + const int img_height, + const int cell_size, + opencl* gpu_fast_, + const int n_pyr_levels) : + AbstractDetector(img_width, img_height, cell_size, n_pyr_levels),gpu_fast_(gpu_fast_) +{ +} + +void FastDetector::detect( + Frame* frame, + const ImgPyr& img_pyr, + const double detection_threshold, + Features& fts) +{ + Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f)); + for(int L=0; Lreload_buf(0,0,img); + int icorner[1]={0}; + gpu_fast_->reload_buf(0,2,icorner); + gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows); + cl_int2 fast_corners[350000]; + gpu_fast_->read(0,1,fast_corners); + int count[1]={0}; + gpu_fast_->read(0,2,count); + for(uint i=0;iimg_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0) + continue; + + const int k = static_cast((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_ + + static_cast((fast_corners[i].x*scale)/cell_size_); + + if(grid_occupancy_[k]) + continue; + + const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y); + + if(score > corners.at(k).score){ + corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f); + // Create feature for every corner that has high enough corner score + if(corners.at(k).score > detection_threshold) + fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level)); + } + } + + } + resetGrid(); +} + +} // namespace feature_detection +} // namespace svo + diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp new file mode 100644 index 0000000..30f085f --- /dev/null +++ b/GPU_version/vio/src/frame.cpp @@ -0,0 +1,189 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Frame::frame_counter_ = 0; + +Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) : + id_(frame_counter_++), + timestamp_(timestamp), + cam_(cam), + key_pts_(5), + is_keyframe_(false), + v_kf_(NULL), + T_f_w_(SE2_5(0.0,0.0,0.0)) +{ + initFrame(img); +} + +Frame::~Frame() +{ + std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;}); +} + +void Frame::initFrame(const cv::Mat& img) +{ + // check image + if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height()) + throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale"); + + // Set keypoints to NULL + std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; }); + + // Build Image Pyramid + frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_); +} + +void Frame::setKeyframe() +{ + is_keyframe_ = true; + setKeyPoints(); +} + +void Frame::addFeature(Feature* ftr) +{ + fts_.push_back(ftr); +} + +void Frame::setKeyPoints() +{ + for(size_t i = 0; i < 5; ++i) + if(key_pts_[i] != NULL) + if(key_pts_[i]->point == NULL) + key_pts_[i] = NULL; + + std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); }); +} + +void Frame::checkKeyPoints(Feature* ftr) +{ + const int cu = cam_->width()/2; + const int cv = cam_->height()/2; + + // center pixel + if(key_pts_[0] == NULL) + key_pts_[0] = ftr; + else if(std::max(std::fabs(ftr->px[0]-cu), std::fabs(ftr->px[1]-cv)) + < std::max(std::fabs(key_pts_[0]->px[0]-cu), std::fabs(key_pts_[0]->px[1]-cv))) + key_pts_[0] = ftr; + + if(ftr->px[0] >= cu && ftr->px[1] >= cv) + { + if(key_pts_[1] == NULL) + key_pts_[1] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[1]-cv)) + key_pts_[1] = ftr; + } + if(ftr->px[0] >= cu && ftr->px[1] < cv) + { + if(key_pts_[2] == NULL) + key_pts_[2] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[1]-cv)) + key_pts_[2] = ftr; + } + if(ftr->px[0] < cv && ftr->px[1] < cv) + { + if(key_pts_[3] == NULL) + key_pts_[3] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[1]-cv)) + key_pts_[3] = ftr; + } + if(ftr->px[0] < cv && ftr->px[1] >= cv) + { + if(key_pts_[4] == NULL) + key_pts_[4] = ftr; + else if((ftr->px[0]-cu) * (ftr->px[1]-cv) + > (key_pts_[4]->px[0]-cu) * (key_pts_[4]->px[1]-cv)) + key_pts_[4] = ftr; + } +} + +void Frame::removeKeyPoint(Feature* ftr) +{ + bool found = false; + std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){ + if(i == ftr) { + i = NULL; + found = true; + } + }); + if(found) + setKeyPoints(); +} + +bool Frame::isVisible(const Vector3d& xyz_w) const +{ + Vector3d xyz_f = Vector3d(this->getSE3Inv()*xyz_w); + if(xyz_f.z() < 0.0) + return false; // point is behind the camera + Vector2d px = f2c(xyz_f); + if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height()) + return true; + return false; +} + + +/// Utility functions for the Frame class +namespace frame_utils { + +void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr) +{ + pyr.resize(n_levels); + pyr[0] = img_level_0; + for(int i=1; i depth_vec; + depth_vec.reserve(frame.fts_.size()); + depth_min = std::numeric_limits::max(); + for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it) + { + if((*it)->point != NULL) + { + const double z = frame.w2f((*it)->point->pos_).z(); + depth_vec.push_back(z); + depth_min = fmin(z, depth_min); + } + } + if(depth_vec.empty()) + { + SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!"); + return false; + } + depth_mean = vk::getMedian(depth_vec); + return true; +} + +} // namespace frame_utils +} // namespace svo diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp new file mode 100644 index 0000000..a232ce3 --- /dev/null +++ b/GPU_version/vio/src/frame_handler_base.cpp @@ -0,0 +1,140 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo +{ + +// definition of global and static variables which were declared in the header + +FrameHandlerBase::FrameHandlerBase() : + stage_(STAGE_PAUSED), + set_reset_(false), + set_start_(false), + num_obs_last_(0), + tracking_quality_(TRACKING_INSUFFICIENT) +{ + + SVO_INFO_STREAM("SVO initialized"); +} + +FrameHandlerBase::~FrameHandlerBase() +{ + SVO_INFO_STREAM("SVO destructor invoked"); +} + +bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp) +{ + if(set_start_) + { + resetAll(); + stage_ = STAGE_FIRST_FRAME; + } + + if(stage_ == STAGE_PAUSED) + return false; + + SVO_DEBUG_STREAM("New Frame"); + + // some cleanup from last iteration, can't do before because of visualization + map_.emptyTrash(); + return true; +} + +int FrameHandlerBase::finishFrameProcessingCommon( + const size_t update_id, + const UpdateResult dropout, + const size_t num_observations) +{ + + if(stage_ == STAGE_DEFAULT_FRAME) + num_obs_last_ = num_observations; + + if(dropout == RESULT_FAILURE && + (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING )) + { + stage_ = STAGE_RELOCALIZING; + tracking_quality_ = TRACKING_INSUFFICIENT; + } + if(set_reset_) + resetAll(); + + return 0; +} + +void FrameHandlerBase::resetCommon() +{ + map_.reset(); + stage_ = STAGE_PAUSED; + set_reset_ = false; + set_start_ = false; + tracking_quality_ = TRACKING_INSUFFICIENT; + num_obs_last_ = 0; + SVO_INFO_STREAM("RESET"); +} + +void FrameHandlerBase::setTrackingQuality(const size_t num_observations) +{ + tracking_quality_ = TRACKING_GOOD; + if(num_observations < Config::qualityMinFts()) + { + SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!"); + tracking_quality_ = TRACKING_INSUFFICIENT; + } + const int feature_drop = static_cast(std::min(num_obs_last_, Config::maxFts())) - num_observations; + if(feature_drop > Config::qualityMaxFtsDrop()) + { + SVO_WARN_STREAM("Lost "<< feature_drop <<" features!"); + tracking_quality_ = TRACKING_INSUFFICIENT; + } +} + +bool ptLastOptimComparator(Point* lhs, Point* rhs) +{ + return (lhs->last_structure_optim_ < rhs->last_structure_optim_); +} + +void FrameHandlerBase::optimizeStructure( + FramePtr frame, + size_t max_n_pts, + int max_iter) +{ + deque pts; + for(auto&& it:frame->fts_){ + if(it->point!=NULL)pts.push_back(it->point); + } + max_n_pts = min(max_n_pts, pts.size()); + nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); + for(deque::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it) + { + (*it)->optimize(max_iter); + (*it)->last_structure_optim_ = frame->id_; + } +} + + +} // namespace svo diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp new file mode 100644 index 0000000..6399756 --- /dev/null +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -0,0 +1,302 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix& init) : + FrameHandlerBase(), + cam_(cam), + reprojector_(cam_, map_), + depth_filter_(NULL), + ukfPtr_(init), + time_(ros::Time::now()) +{ + gpu_fast_= new opencl(); + gpu_fast_->make_kernel("fast_gray"); + cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1); + gpu_fast_->write_buf(0,0,img); + cl_int2 corners_[350000]; + gpu_fast_->write_buf(0,1,350000,corners_); + int icorner[1]={0}; + gpu_fast_->write_buf(0,2,1,icorner); + gpu_fast_->write_buf(0,3,(1 << 18)); + klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); + initialize(); +} + +void FrameHandlerMono::initialize() +{ + feature_detection::DetectorPtr feature_detector( + new feature_detection::FastDetector( + cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels())); + DepthFilter::callback_t depth_filter_cb = boost::bind( + &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2); + depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb); + depth_filter_->startThread(); +} + +FrameHandlerMono::~FrameHandlerMono() +{ + delete depth_filter_; +} + +void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time) +{ + if(!startFrameProcessingCommon(timestamp)){ + return; + } + + // some cleanup from last iteration, can't do before because of visualization + core_kfs_.clear(); + overlap_kfs_.clear(); + + // create new frame + new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + time_=time; + // process frame + UpdateResult res = RESULT_FAILURE; + if(stage_ == STAGE_DEFAULT_FRAME) + res = processFrame(); + else if(stage_ == STAGE_SECOND_FRAME) + res = processSecondFrame(); + else if(stage_ == STAGE_FIRST_FRAME) + res = processFirstFrame(); + else if(stage_ == STAGE_RELOCALIZING) + res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()), + map_.getClosestKeyframe(last_frame_)); + + // set last frame + if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_; + new_frame_.reset(); + // finish processing + finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); +} + +FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() +{ + new_frame_->T_f_w_ = SE2_5(0.0,0.0,0.0); + if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE) + return RESULT_NO_KEYFRAME; + new_frame_->setKeyframe(); + map_.addKeyframe(new_frame_); + stage_ = STAGE_SECOND_FRAME; + ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); + SVO_INFO_STREAM("Init: Selected first frame."); + return RESULT_IS_KEYFRAME; +} + +FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() +{ + auto init_f= ukfPtr_.get_location(); + new_frame_->T_f_w_=init_f.second; + new_frame_->Cov_ = init_f.first; + initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); + auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); + new_frame_->T_f_w_ = result.second; + new_frame_->Cov_ = result.first; + if(res == initialization::NO_KEYFRAME) + return RESULT_NO_KEYFRAME; + new_frame_->setKeyframe(); + double depth_mean, depth_min; + frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + + depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + // add frame to map + map_.addKeyframe(new_frame_); + stage_ = STAGE_DEFAULT_FRAME; + klt_homography_init_->reset(); + SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map."); + return RESULT_IS_KEYFRAME; +} + + +FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() +{ + auto init_f= ukfPtr_.get_location(); + new_frame_->T_f_w_=init_f.second; + new_frame_->Cov_ = init_f.first; + // sparse image align + SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), + 30, SparseImgAlign::LevenbergMarquardt, false, false); + if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; + reprojector_.reprojectMap(new_frame_, overlap_kfs_); + size_t sfba_n_edges_final=0; + double sfba_thresh, sfba_error_init, sfba_error_final; + pose_optimizer::optimizeGaussNewton( + Config::poseOptimThresh(), Config::poseOptimNumIter(), false, + new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); + if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || + fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI || + sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ + new_frame_=last_frame_; + return RESULT_NO_KEYFRAME; + } + auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0), + new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_); + new_frame_->T_f_w_ =result.second; + new_frame_->Cov_ = result.first; + std::cout<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; + + optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); + // select keyframe + core_kfs_.insert(new_frame_); + setTrackingQuality(sfba_n_edges_final); + if(tracking_quality_ == TRACKING_INSUFFICIENT) + { + return RESULT_FAILURE; + } + double depth_mean, depth_min; + + frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited + { + depth_filter_->addFrame(new_frame_); + return RESULT_NO_KEYFRAME; + } + new_frame_->setKeyframe(); + + // new keyframe selected + for(auto&& it:new_frame_->fts_){ + if(it->point != NULL)it->point->addFrameRef(it); + } + map_.point_candidates_.addCandidatePointToFrame(new_frame_); + + // init new depth-filters + depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + + // if limited number of keyframes, remove the one furthest apart + if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs()) + { + FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos()); + depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better + map_.safeDeleteFrame(furthest_frame); + } + + // add keyframe to map + map_.addKeyframe(new_frame_); + std::cout<<"Svo pipline was finished\n"; + return RESULT_IS_KEYFRAME; +} + +FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame( + const SE3& T_cur_ref, + FramePtr ref_keyframe) +{ + if(ref_keyframe == nullptr) + { + return RESULT_FAILURE; + } + SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), + 30, SparseImgAlign::GaussNewton, false, false); + size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_); + usleep(2000); + if(img_align_n_tracked > Config::qualityMinFts()) + { + SE2_5 T_f_w_last = last_frame_->T_f_w_; + last_frame_ = ref_keyframe; + FrameHandlerMono::UpdateResult res = processFrame(); + if(res != RESULT_FAILURE) + { + stage_ = STAGE_DEFAULT_FRAME; + } + else + new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose + return res; + } + return RESULT_FAILURE; +} + +bool FrameHandlerMono::relocalizeFrameAtPose( + const int keyframe_id, + const SE3& T_f_kf, + const cv::Mat& img, + const double timestamp) +{ + FramePtr ref_keyframe; + if(!map_.getKeyframeById(keyframe_id, ref_keyframe)) + return false; + new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe); + if(res != RESULT_FAILURE) { + last_frame_ = new_frame_; + return true; + } + return false; +} + +void FrameHandlerMono::resetAll() +{ + resetCommon(); + last_frame_.reset(); + new_frame_.reset(); + core_kfs_.clear(); + overlap_kfs_.clear(); + depth_filter_->reset(); +} + +void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame) +{ + resetAll(); + last_frame_ = first_frame; + last_frame_->setKeyframe(); + map_.addKeyframe(last_frame_); + stage_ = STAGE_DEFAULT_FRAME; +} + +bool FrameHandlerMono::needNewKf(double scene_depth_mean) +{ + if(scene_depth_mean >10.0)return true; + for(auto&& it:overlap_kfs_) + { + if(fabs(it.first->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>0.0523599)return true; + if(fabs((it.first->T_f_w_.translation()-new_frame_->T_f_w_.translation()).norm()) > Config::kfSelectMinDist()) + return true; + } + return false; +} + +void FrameHandlerMono::setCoreKfs(size_t n_closest) +{ + size_t n = min(n_closest, overlap_kfs_.size()-1); + std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(), + boost::bind(&pair::second, _1) > + boost::bind(&pair::second, _2)); + std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair& i){ core_kfs_.insert(i.first); }); +} +void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){ + if(value== nullptr)return; + ukfPtr_.UpdateIMU(value[0],value[1],value[2],time); +} +void FrameHandlerMono::UpdateCmd(double* value,const ros::Time& time){ + if(value== nullptr)return; + ukfPtr_.UpdateCmd(value[0],value[1],value[2],time); +} + +} // namespace svo diff --git a/GPU_version/vio/src/homography.cpp b/GPU_version/vio/src/homography.cpp new file mode 100644 index 0000000..c73617d --- /dev/null +++ b/GPU_version/vio/src/homography.cpp @@ -0,0 +1,285 @@ +// +// Created by root on 5/24/21. +// +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + */ + +#include +#include +#include + +namespace vk { + + Homography:: + Homography(const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px) : + thresh(_thresh_in_px), + error_multiplier2(_error_multiplier2), + fts_c1(_fts1), + fts_c2(_fts2) + { + } + + void Homography:: + calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1) + { + double d = n_c1.dot(xyz_c1); // normal distance from plane to KF + H_c2_from_c1 = T_c2_from_c1.rotation_matrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d; + } + + void Homography:: + calcFromMatches() + { + vector src_pts(fts_c1.size()), dst_pts(fts_c1.size()); + for(size_t i=0; i(0,0); + H_c2_from_c1(0,1) = cvH.at(0,1); + H_c2_from_c1(0,2) = cvH.at(0,2); + H_c2_from_c1(1,0) = cvH.at(1,0); + H_c2_from_c1(1,1) = cvH.at(1,1); + H_c2_from_c1(1,2) = cvH.at(1,2); + H_c2_from_c1(2,0) = cvH.at(2,0); + H_c2_from_c1(2,1) = cvH.at(2,1); + H_c2_from_c1(2,2) = cvH.at(2,2); + } + + size_t Homography:: + computeMatchesInliers() + { + inliers.clear(); inliers.resize(fts_c1.size()); + size_t n_inliers = 0; + for(size_t i=0; i q(decompositions.front().T.rotation_matrix()); + q.vec() *= 1/q.norm(); + q.w() *= 1/q.norm(); + q.x()=0.0; + q.z()=0.0; + T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(),0.0,decompositions.front().T.translation().z())); + return true; + } + + bool Homography:: + decompose() + { + decompositions.clear(); + JacobiSVD svd(H_c2_from_c1, ComputeThinU | ComputeThinV); + + Vector3d singular_values = svd.singularValues(); + + double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT) + double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book. + double d3 = fabs(singular_values[2]); + + Matrix3d U = svd.matrixU(); + Matrix3d V = svd.matrixV(); // VT^T + double s = U.determinant() * V.determinant(); + + if(!(d1 != d2 && d2 != d3)) + { + printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. "); + return false; + } + + double x1_PM; + double x3_PM; + + // All below deals with the case = 1 case. + // Case 1 implies (d1 != d3) + { // Eq. 12 + x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3)); + x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3)); + }; + + double e1[4] = {1.0,-1.0, 1.0,-1.0}; + double e3[4] = {1.0, 1.0,-1.0,-1.0}; + + Vector3d np; + HomographyDecomposition decomp; + + // Case 1, d' > 0: + decomp.d = s * d2; + for(size_t signs=0; signs<4; signs++) + { + // Eq 13 + decomp.R = Matrix3d::Identity(); + double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2; + /* + * correct the rotation matrix by Majid + */ + decomp.R(0,0) = dCosTheta; //1 + decomp.R(0,2) = -dSinTheta; //0 + decomp.R(2,0) = dSinTheta; + decomp.R(2,2) = dCosTheta; + + // Eq 14 + decomp.t[0] = (d1 - d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = 0.0; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Case 1, d' < 0: + decomp.d = s * -d2; + for(size_t signs=0; signs<4; signs++) + { + // Eq 15 + decomp.R = -1 * Matrix3d::Identity(); + double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2; + /* + * correct the rotation matrix by Majid + */ + decomp.R(0,0) = dCosPhi; + decomp.R(0,2) = dSinPhi; + decomp.R(2,0) = dSinPhi; + decomp.R(2,2) = -dCosPhi; + + // Eq 16 + decomp.t[0] = (d1 + d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 + d3) * x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = 0.0; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Save rotation and translation of the decomposition + for(unsigned int i=0; i 0.0) + nPositive++; + } + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(4); + + for(size_t i=0; i 0.0) + nPositive++; + }; + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(2); + + // According to Faugeras and Lustman, ambiguity exists if the two scores are equal + // but in practive, better to look at the ratio! + double dRatio = (double) decompositions[1].score / (double) decompositions[0].score; + + if(dRatio < 0.9) // no ambiguity! + decompositions.erase(decompositions.begin() + 1); + else // two-way ambiguity. Resolve by sampsonus score of all points. + { + double dErrorSquaredLimit = thresh * thresh * 4; + double adSampsonusScores[2]; + for(size_t i=0; i<2; i++) + { + Sophus::SE3 T = decompositions[i].T; + Matrix3d Essential = T.rotation_matrix() * sqew(T.translation()); + double dSumError = 0; + for(size_t m=0; m < fts_c1.size(); m++ ) + { + double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]); + if(d > dErrorSquaredLimit) + d = dErrorSquaredLimit; + dSumError += d; + } + adSampsonusScores[i] = dSumError; + } + + if(adSampsonusScores[0] <= adSampsonusScores[1]) + decompositions.erase(decompositions.begin() + 1); + else + decompositions.erase(decompositions.begin()); + } + } + + +} /* end namespace vk */ + diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp new file mode 100644 index 0000000..379163f --- /dev/null +++ b/GPU_version/vio/src/initialization.cpp @@ -0,0 +1,200 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace initialization { + +InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) +{ + reset(); + detectFeatures(frame_ref, px_ref_, f_ref_,gpu_fast_); + if(px_ref_.size() < 100) + { + SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment."); + return FAILURE; + } + frame_ref_ = frame_ref; + px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end()); + return SUCCESS; +} + +InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) +{ + trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_); + if(disparities_.size() < Config::initMinTracked()) + return NO_KEYFRAME; + double disparity = vk::getMedian(disparities_); + if(disparity < Config::initMinDisparity()){ + SVO_INFO_STREAM("Init: px average disparity is: "<cam_->errorMultiplier2(), Config::poseOptimThresh(), + inliers_, xyz_in_cur_, T_cur_from_ref_); + + if(inliers_.size() < Config::initMinInliers()) + { + SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "< depth_vec; + for(size_t i=0; iT_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2()); + frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(), + -frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())))); + // For each inlier create 3D point and add feature in both frames + SE3 T_world_cur = frame_cur->getSE3Inv(); + for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) + { + Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y); + Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y); + if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) + { + Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); + Point* new_point = new Point(pos); + + Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0)); + frame_cur->addFeature(ftr_cur); + new_point->addFrameRef(ftr_cur); + + Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0)); + frame_ref_->addFeature(ftr_ref); + new_point->addFrameRef(ftr_ref); + } + } + SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<fts_.size()); + return SUCCESS; +} + +void KltHomographyInit::reset() +{ + px_cur_.clear(); + frame_ref_.reset(); +} + +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec, + opencl* gpu_fast) +{ + Features new_features; + feature_detection::FastDetector detector( + frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels()); + detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); + + // now for all maximum corners, initialize a new seed + px_vec.clear(); px_vec.reserve(new_features.size()); + f_vec.clear(); f_vec.reserve(new_features.size()); + for(auto&& ftr:new_features){ + px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1])); + f_vec.push_back(ftr->f); + delete ftr; + } +} + +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities) +{ + const double klt_win_size = 30.0;//30.0 + const int klt_max_iter = 30;//30 + const double klt_eps = 0.001; + vector status; + vector error; + vector min_eig_vec; + cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps); + cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0], + px_ref, px_cur, + status, error, + cv::Size2i(klt_win_size, klt_win_size), + 4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW); + + vector::iterator px_ref_it = px_ref.begin(); + vector::iterator px_cur_it = px_cur.begin(); + vector::iterator f_ref_it = f_ref.begin(); + f_cur.clear(); f_cur.reserve(px_cur.size()); + disparities.clear(); disparities.reserve(px_cur.size()); + for(size_t i=0; px_ref_it != px_ref.end(); ++i) + { + if(!status[i]) + { + px_ref_it = px_ref.erase(px_ref_it); + px_cur_it = px_cur.erase(px_cur_it); + f_ref_it = f_ref.erase(f_ref_it); + continue; + } + f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y)); + disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm()); + ++px_ref_it; + ++px_cur_it; + ++f_ref_it; + } +} + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE2_5& T_cur_from_ref) +{ + vector uv_ref(f_ref.size()); + vector uv_cur(f_cur.size()); + for(size_t i=0, i_max=f_ref.size(); i outliers; + vk::computeInliers(f_cur, f_ref, + Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(), + reprojection_threshold, focal_length, + xyz_in_cur, inliers, outliers); + T_cur_from_ref=SE2_5(Homography.T_c2_from_c1); +} + + +} // namespace initialization +} // namespace svo diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp new file mode 100644 index 0000000..9e4196e --- /dev/null +++ b/GPU_version/vio/src/map.cpp @@ -0,0 +1,379 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include + +namespace svo { + +Map::Map() {} + +Map::~Map() +{ + reset(); + SVO_INFO_STREAM("Map destructed"); +} + +void Map::reset() +{ + keyframes_.clear(); + point_candidates_.reset(); + emptyTrash(); +} + +bool Map::safeDeleteFrame(FramePtr frame) +{ + if(syn_==true)return false; + bool found = false; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + if(*it == frame) + { + std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){ + removePtFrameRef(it->get(), ftr); + }); + keyframes_.erase(it); + found = true; + break; + } + } + + point_candidates_.removeFrameCandidates(frame); + + if(found) + return true; + + //SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there."); + return false; +} + +void Map::removePtFrameRef(Frame* frame, Feature* ftr) +{ + if(ftr->point == NULL) + return; // mappoint may have been deleted in a previous ref. removal + Point* pt = ftr->point; + ftr->point = NULL; + if(pt->obs_.size() <= 2) + { + // If the references list of mappoint has only size=2, delete mappoint + safeDeletePoint(pt); + return; + } + pt->deleteFrameRef(frame); // Remove reference from map_point + frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe +} + +void Map::safeDeletePoint(Point* pt) +{ + // Delete references to mappoints in all keyframes + std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){ + ftr->point=NULL; + ftr->frame->removeKeyPoint(ftr); + }); + pt->obs_.clear(); + + // Delete mappoint + deletePoint(pt); +} + +void Map::deletePoint(Point* pt) +{ + pt->type_ = Point::TYPE_DELETED; + trash_points_.push_back(pt); +} + +void Map::addKeyframe(FramePtr new_keyframe) +{ + keyframes_.push_back(new_keyframe); +} + +void Map::getCloseKeyframes( + const FramePtr& frame, + std::list< std::pair >& close_kfs) const +{ + syn_=true; + for(auto kf : keyframes_) + { + // check if kf has overlaping field of view with frame, use therefore KeyPoints + for(auto keypoint : kf->key_pts_) + { + if(keypoint == nullptr) + continue; + if(frame->isVisible(keypoint->point->pos_)) + { + close_kfs.push_back( + std::make_pair( + kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm())); + break; // this keyframe has an overlapping field of view -> add to close_kfs + } + } + } + syn_=false; +} + +FramePtr Map::getClosestKeyframe(const FramePtr& frame) const +{ + list< pair > close_kfs; + getCloseKeyframes(frame, close_kfs); + if(close_kfs.empty()) + { + return nullptr; + } + + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + if(close_kfs.front().first != frame) + return close_kfs.front().first; + close_kfs.pop_front(); + return close_kfs.front().first; +} + +FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const +{ + FramePtr furthest_kf; + double maxdist = 0.0; + syn_=true; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + double dist = ((*it)->pos()-pos).norm(); + if(dist > maxdist) { + maxdist = dist; + furthest_kf = *it; + } + } + syn_=false; + return furthest_kf; +} + +bool Map::getKeyframeById(const int id, FramePtr& frame) const +{ + bool found = false; + syn_=true; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + if((*it)->id_ == id) { + found = true; + frame = *it; + break; + } + syn_=false; + return found; +} + +void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s) +{ + Eigen::Matrix R3; + R3<T_f_w_.inverse().rotation_matrix(),s*R*(*it)->pos() + t)); + (*it)->T_f_w_ = tem.inverse(); + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if((*ftr)->point->last_published_ts_ == -1000) + continue; + (*ftr)->point->last_published_ts_ = -1000; + (*ftr)->point->pos_ = s*R3*(*ftr)->point->pos_ + Vector3d(t(0),0.0,t(1)); + } + } + syn_=false; +} + +void Map::emptyTrash() +{ + std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){ + delete pt; + pt=NULL; + }); + trash_points_.clear(); + point_candidates_.emptyTrash(); +} + +MapPointCandidates::MapPointCandidates() +{} + +MapPointCandidates::~MapPointCandidates() +{ + reset(); +} + +void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2) +{ + point->type_ = Point::TYPE_CANDIDATE; + boost::unique_lock lock(mut_); + candidates_.push_back(PointCandidate(point, point->obs_.front())); +} + +void MapPointCandidates::addCandidatePointToFrame(FramePtr frame) +{ + boost::unique_lock lock(mut_); + PointCandidateList::iterator it=candidates_.begin(); + while(it != candidates_.end()) + { + if(it->first->obs_.front()->frame == frame.get()) + { + // insert feature in the frame + it->first->type_ = Point::TYPE_UNKNOWN; + it->first->n_failed_reproj_ = 0; + it->second->frame->addFeature(it->second); + it = candidates_.erase(it); + } + else + ++it; + } +} + +bool MapPointCandidates::deleteCandidatePoint(Point* point) +{ + boost::unique_lock lock(mut_); + for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it) + { + if(it->first == point) + { + deleteCandidate(*it); + candidates_.erase(it); + return true; + } + } + return false; +} + +void MapPointCandidates::removeFrameCandidates(FramePtr frame) +{ + boost::unique_lock lock(mut_); + auto it=candidates_.begin(); + while(it!=candidates_.end()) + { + if(it->second->frame == frame.get()) + { + deleteCandidate(*it); + it = candidates_.erase(it); + } + else + ++it; + } +} + +void MapPointCandidates::reset() +{ + boost::unique_lock lock(mut_); + std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){ + delete c.first; + delete c.second; + }); + candidates_.clear(); +} + +void MapPointCandidates::deleteCandidate(PointCandidate& c) +{ + // camera-rig: another frame might still be pointing to the candidate point + // therefore, we can't delete it right now. + delete c.second; c.second=NULL; + c.first->type_ = Point::TYPE_DELETED; + trash_points_.push_back(c.first); +} + +void MapPointCandidates::emptyTrash() +{ + std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){ + delete p; p=NULL; + }); + trash_points_.clear(); +} + +namespace map_debug { + +void mapValidation(Map* map, int id) +{ + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + frameValidation(it->get(), id); +} + +void frameValidation(Frame* frame, int id) +{ + for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point==NULL) + continue; + + if((*it)->point->type_ == Point::TYPE_DELETED) + printf("ERROR DataValidation %i: Referenced point was deleted.\n", id); + + if(!(*it)->point->findFrameRef(frame)) + printf("ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n", id); + + pointValidation((*it)->point, id); + } + for(auto it=frame->key_pts_.begin(); it!=frame->key_pts_.end(); ++it) + if(*it != NULL) + if((*it)->point == NULL) + printf("ERROR DataValidation %i: KeyPoints not correct!\n", id); +} + +void pointValidation(Point* point, int id) +{ + for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it) + { + bool found=false; + for(auto it_ftr=(*it)->frame->fts_.begin(); it_ftr!=(*it)->frame->fts_.end(); ++it_ftr) + if((*it_ftr)->point == point) { + found=true; break; + } + if(!found) + printf("ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n", id, point->id_, (*it)->frame->id_, (int) point->type_); + } +} + +void mapStatistics(Map* map) +{ + // compute average number of features which each frame observes + size_t n_pt_obs(0); + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + n_pt_obs += (*it)->nObs(); + printf("\n\nMap Statistics: Frame avg. point obs = %f\n", (float) n_pt_obs/map->size()); + + // compute average number of observations that each point has + size_t n_frame_obs(0); + size_t n_pts(0); + std::set points; + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + { + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if(points.insert((*ftr)->point).second) { + ++n_pts; + n_frame_obs += (*ftr)->point->nRefs(); + } + } + } + printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts); +} + +} // namespace map_debug +} // namespace svo diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp new file mode 100644 index 0000000..1b025c9 --- /dev/null +++ b/GPU_version/vio/src/matcher.cpp @@ -0,0 +1,327 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +namespace warp { + +void getWarpMatrixAffine( + const vk::AbstractCamera& cam_ref, + const vk::AbstractCamera& cam_cur, + const Vector2d& px_ref, + const Vector3d& f_ref, + const double depth_ref, + const SE3& T_cur_ref, + const int level_ref, + Matrix2d& A_cur_ref) +{ + // Compute affine warp matrix A_ref_cur + const int halfpatch_size = 5; + const Vector3d xyz_ref(f_ref*depth_ref); + Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1< 3.0 && search_level < max_level) + { + search_level += 1; + D *= 0.25; + } + return search_level; +} + +bool warpAffine( + const Matrix2d& A_cur_ref, + const cv::Mat& img_ref, + const Vector2d& px_ref, + const int level_ref, + const int search_level, + const int halfpatch_size, + uint8_t* patch) +{ + if(patch==NULL || img_ref.empty())return false; + const int patch_size = halfpatch_size*2 ; + const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + if(isnan(A_ref_cur(0,0))) + { + printf("Affine warp is NaN, probably camera has no translation\n"); // TODO + return false; + } + // Perform the warp on a larger patch. + uint8_t* patch_ptr = patch; + const Vector2f px_ref_pyr = px_ref.cast() / (1<=img_ref.cols-1 || px[1]>=img_ref.rows-1){ + *patch_ptr = 0; + }else{ + *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]); + } + } + } + return true; +} + +} // namespace warp + +bool depthFromTriangulation( + const SE3& T_search_ref, + const Vector3d& f_ref, + const Vector3d& f_cur, + double& depth) +{ + Matrix A; A << T_search_ref.rotation_matrix() * f_ref, f_cur; + const Matrix2d AtA = A.transpose()*A; + if(AtA.determinant() < 0.000001) + return false; + const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation(); + depth = fabs(depth2[0]/depth2[1]); + return true; +} + +void Matcher::createPatchFromPatchWithBorder() +{ + uint8_t* ref_patch_ptr = patch_; + for(int y=1; yframe->cam_->isInFrame( + ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) + return false; + if(ref_ftr_->frame->img_pyr_.empty())return false; + if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false; + // warp affine + warp::getWarpMatrixAffine( + *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, + (Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(), + cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_); + search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); + if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false; + createPatchFromPatchWithBorder(); + + // px_cur should be set + Vector2d px_scaled(px_cur/(1<type == Feature::EDGELET) + { + Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); + dir_cur.normalize(); + success = feature_alignment::align1D( + cur_frame.img_pyr_[search_level_], dir_cur.cast(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + } + else + { + success = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + } + px_cur = px_scaled * (1<world2cam(A)); + Vector2d px_B(cur_frame.cam_->world2cam(B)); + epi_length_ = (px_A-px_B).norm() / (1<().normalized(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + else + res = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + if(res) + { + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + return true; + } + return false; + } + + size_t n_steps = epi_length_/0.7; // one step per pixel + Vector2d step = epi_dir_/n_steps; + + if(n_steps > options_.max_epi_search_steps) + { + printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n", + n_steps, epi_length_, d_min, d_max); + return false; + } + + // for matching, precompute sum and sum2 of warped reference patch + int pixel_sum = 0; + int pixel_sum_square = 0; + PatchScore patch_score(patch_); + + // now we sample along the epipolar line + Vector2d uv = B-step; + Vector2i last_checked_pxi(0,0); + ++n_steps; + for(size_t i=0; iworld2cam(uv)); + Vector2i pxi(px[0]/(1<isInFrame(pxi, patch_size_, search_level_)) + continue; + + // TODO interpolation would probably be a good idea + uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data + + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols + + (pxi[0]-halfpatch_size_); + int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols); + + if(zmssd < zmssd_best) { + zmssd_best = zmssd; + uv_best = uv; + } + } + + if(zmssd_best < PatchScore::threshold()) + { + if(options_.subpix_refinement) + { + px_cur_ = cur_frame.cam_->world2cam(uv_best); + Vector2d px_scaled(px_cur_/(1<().normalized(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + else + res = feature_alignment::align2D( + cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, + options_.align_max_iter, px_scaled); + if(res) + { + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + return true; + } + return false; + } + px_cur_ = cur_frame.cam_->world2cam(uv_best); + if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) + return true; + } + return false; +} + +} // namespace svo diff --git a/GPU_version/vio/src/math_utils.cpp b/GPU_version/vio/src/math_utils.cpp new file mode 100644 index 0000000..9c265ec --- /dev/null +++ b/GPU_version/vio/src/math_utils.cpp @@ -0,0 +1,202 @@ +/* + * math_utils.cpp + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#include + +namespace vk { + +using namespace Eigen; + +Vector3d +triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, + const Vector3d& feature1, const Vector3d& feature2 ) +{ + Vector3d f2 = R * feature2; + Vector2d b; + b[0] = t.dot(feature1); + b[1] = t.dot(f2); + Matrix2d A; + A(0,0) = feature1.dot(feature1); + A(1,0) = feature1.dot(f2); + A(0,1) = -A(1,0); + A(1,1) = -f2.dot(f2); + Vector2d lambda = A.inverse() * b; + Vector3d xm = lambda[0] * feature1; + Vector3d xn = t + lambda[1] * f2; + return ( xm + xn )/2; +} + +bool +depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c) +{ + // bearing vectors (f_r, f_c) do not need to be unit length + const Vector3d f_c_in_r(R_r_c*f_c); + const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r); + const double b = f_c_in_r.dot(t_r_c); + const double denom = (a*b - f_c_in_r.dot(f_c_in_r)); + + if(abs(denom) < 0.000001) + return false; + + depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom; + depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm(); + return true; +} + +double +reprojError(const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2) +{ + Vector2d e = project2d(f1) - project2d(f2); + return error_multiplier2 * e.norm(); +} + +double +computeInliers(const vector& features1, // c1 + const vector& features2, // c2 + const Matrix3d& R, // R_c1_c2 + const Vector3d& t, // c1_t + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, // in frame c1 + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(features1.size()); + outliers.clear(); outliers.reserve(features1.size()); + xyz_vec.clear(); xyz_vec.reserve(features1.size()); + double tot_error = 0; + //triangulate all features and compute reprojection errors and inliers + for(size_t j=0; j reproj_thresh || e2 > reproj_thresh) + outliers.push_back(j); + else + { + inliers.push_back(j); + tot_error += e1+e2; + } + } + return tot_error; +} + +void +computeInliersOneView(const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(xyz_vec.size()); + outliers.clear(); outliers.reserve(xyz_vec.size()); + for(size_t j = 0; j < xyz_vec.size(); j++ ) + { + double e = reprojError(feature_sphere_vec[j], + R.transpose() * ( xyz_vec[j] - t ), + error_multiplier2); + if(e < reproj_thresh) + inliers.push_back(j); + else + outliers.push_back(j); + } +} + +Vector3d +dcm2rpy(const Matrix3d &R) +{ + Vector3d rpy; + rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) ); + if( fabs( rpy[1] - M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + if( fabs( rpy[1] + M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) ); + rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) ); + } + } + return rpy; +} + +Matrix3d +rpy2dcm(const Vector3d &rpy) +{ + Matrix3d R1; + R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0; + R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]); + R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1); + + Matrix3d R2; + R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]); + R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0; + R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0); + + Matrix3d R3; + R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0; + R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0; + R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0; + + return R3 * R2 * R1; +} + +Quaterniond +angax2quat(const Vector3d& n, const double& angle) +{ + // n must be normalized! + double s(sin(angle/2)); + return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s ); +} + + +Matrix3d +angax2dcm(const Vector3d& n, const double& angle) +{ + // n must be normalized + Matrix3d sqewn(sqew(n)); + return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle))); +} + +double +sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2) +{ + Vector3d v3Dash = unproject2d(v2Dash); + Vector3d v3 = unproject2d(v2); + + double dError = v3Dash.transpose() * Essential * v3; + + Vector3d fv3 = Essential * v3; + Vector3d fTv3Dash = Essential.transpose() * v3Dash; + + Vector2d fv3Slice = fv3.head<2>(); + Vector2d fTv3DashSlice = fTv3Dash.head<2>(); + + return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice))); +} + +} // end namespace vk diff --git a/GPU_version/vio/src/output_helper.cpp b/GPU_version/vio/src/output_helper.cpp new file mode 100644 index 0000000..58aa0bf --- /dev/null +++ b/GPU_version/vio/src/output_helper.cpp @@ -0,0 +1,405 @@ +/* + * output_helper.cpp + * + * Created on: Jan 20, 2013 + * Author: chrigi + */ + +#include +#include + + +void +publishTfTransform(const Sophus::SE3& T, const ros::Time& stamp, + const string& frame_id, const string& child_frame_id, + tf::TransformBroadcaster& br) +{ + tf::Transform transform_msg; + Eigen::Quaterniond q(T.rotation_matrix()); + transform_msg.setOrigin(tf::Vector3(T.translation().x(), T.translation().y(), T.translation().z())); + tf::Quaternion tf_q; tf_q.setX(q.x()); tf_q.setY(q.y()); tf_q.setZ(q.z()); tf_q.setW(q.w()); + transform_msg.setRotation(tf_q); + br.sendTransform(tf::StampedTransform(transform_msg, stamp, frame_id, child_frame_id)); +} + +void +publishPointMarker(ros::Publisher pub, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime) +{ + visualization_msgs::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::Marker::CUBE; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale; + msg.scale.z = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.lifetime = lifetime; + msg.pose.position.x = pos[0]; + msg.pose.position.y = pos[1]; + msg.pose.position.z = pos[2]; + pub.publish(msg); +} + +void +publishLineMarker(ros::Publisher pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime) +{ + visualization_msgs::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::Marker::LINE_STRIP; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.lifetime = lifetime; + msg.points[0].x = start[0]; + msg.points[0].y = start[1]; + msg.points[0].z = start[2]; + msg.points[1].x = end[0]; + msg.points[1].y = end[1]; + msg.points[1].z = end[2]; + pub.publish(msg); +} + + +void +publishArrowMarker(ros::Publisher pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + visualization_msgs::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::Marker::ARROW; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale*0.35; + msg.scale.z = 0.0; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.points[0].x = pos[0]; + msg.points[0].y = pos[1]; + msg.points[0].z = pos[2]; + msg.points[1].x = pos[0] + scale*dir[0]; + msg.points[1].y = pos[1] + scale*dir[1]; + msg.points[1].z = pos[2] + scale*dir[2]; + pub.publish(msg); +} + +void +publishHexacopterMarker(ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + /* + * Function by Markus Achtelik from libsfly_viz. + * Thank you. + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rotors + marker.type = visualization_msgs::Marker::CYLINDER; + marker.scale.x = 0.2*marker_scale; + marker.scale.y = 0.2*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = 0.4; + marker.color.g = 0.4; + marker.color.b = 0.4; + marker.color.a = 0.8; + marker.pose.position.z = 0; + + // front left/right + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub.publish(marker); + + // left/right + marker.pose.position.x = 0; + marker.pose.position.y = 0.22*marker_scale; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = 0; + marker.pose.position.y = -0.22*marker_scale; + marker.id--; + pub.publish(marker); + + // back left/right + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub.publish(marker); + + // make arms + marker.type = visualization_msgs::Marker::CUBE; + marker.scale.x = 0.44*marker_scale; + marker.scale.y = 0.02*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = -0.015*marker_scale; + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub.publish(marker); + + // 30 deg rotation 0.9659 0 0 0.2588 + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = 0.2588; + marker.id--; + pub.publish(marker); + + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = -0.2588; + marker.id--; + pub.publish(marker); +} + +void +publishCameraMarker(ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color) +{ + /* + * draw a pyramid as the camera marker + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rectangles as frame + double r_w = 1.0; + double z_plane = (r_w / 2.0)*marker_scale; + marker.pose.position.x = 0; + marker.pose.position.y = (r_w / 4.0) *marker_scale; + marker.pose.position.z = z_plane; + + marker.type = visualization_msgs::Marker::CUBE; + marker.scale.x = r_w*marker_scale; + marker.scale.y = 0.04*marker_scale; + marker.scale.z = 0.04*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + marker.pose.orientation.z = 0; + marker.pose.orientation.w = 1; + marker.id--; + pub.publish(marker); + marker.pose.position.y = -(r_w/ 4.0)*marker_scale; + marker.id--; + pub.publish(marker); + + marker.scale.x = (r_w/2.0)*marker_scale; + marker.pose.position.x = (r_w / 2.0) *marker_scale; + marker.pose.position.y = 0; + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub.publish(marker); + marker.pose.position.x = -(r_w / 2.0) *marker_scale; + marker.id--; + pub.publish(marker); + + // make pyramid edges + marker.scale.x = (3.0*r_w/4.0)*marker_scale; + marker.pose.position.z = 0.5*z_plane; + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // 0.08198092, -0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // -0.27395078, -0.22863284, 0.9091823 , 0.21462883 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, 0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, -0.34727674, -0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); +} + +void publishFrameMarker(ros::Publisher pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + ros::Duration lifetime) +{ + visualization_msgs::Marker marker; + marker.header.frame_id = "/world"; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.id = id++; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = action; // 0 = add/modify + marker.points.reserve(2); + geometry_msgs::Point point; + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 2)); // Draw arrow in z-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 2)); // Draw arrow in z-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 2)); // Draw arrow in z-direction + marker.points.push_back(point); + marker.scale.x = 0.5*marker_scale; + marker.scale.y = 0.5*marker_scale; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.lifetime = lifetime; + pub.publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 0)); // Draw arrow in x-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 0)); // Draw arrow in x-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 0)); // Draw arrow in x-direction + marker.points.push_back(point); + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub.publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 1)); // Draw arrow in y-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 1)); // Draw arrow in y-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 1)); // Draw arrow in y-direction + marker.points.push_back(point); + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub.publish(marker); +} + + diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp new file mode 100644 index 0000000..cf12a1e --- /dev/null +++ b/GPU_version/vio/src/point.cpp @@ -0,0 +1,178 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include + +namespace svo { + +int Point::point_counter_ = 0; + +Point::Point(const Vector3d& pos) : + id_(point_counter_++), + pos_(pos), + normal_set_(false), + n_obs_(0), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{} + +Point::Point(const Vector3d& pos, Feature* ftr) : + id_(point_counter_++), + pos_(pos), + normal_set_(false), + n_obs_(1), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{ + obs_.push_front(ftr); +} + +Point::~Point() +{} + +void Point::addFrameRef(Feature* ftr) +{ + obs_.push_front(ftr); + ++n_obs_; +} + +Feature* Point::findFrameRef(Frame* frame) +{ + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + if((*it)->frame == frame) + return *it; + return NULL; // no keyframe found +} + +bool Point::deleteFrameRef(Frame* frame) +{ + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + { + if((*it)->frame == frame) + { + obs_.erase(it); + return true; + } + } + return false; +} + +void Point::initNormal() +{ + assert(!obs_.empty()); + const Feature* ftr = obs_.back(); + assert(ftr->frame != NULL); + normal_ = ftr->frame->se3().rotation_matrix().transpose()*(-ftr->f); + double x=pow(20/(pos_-Vector3d(ftr->frame->pos()(0),0.0,ftr->frame->pos()(1))).norm(),2); + normal_information_ = DiagonalMatrix(x, 1.0, 1.0); + normal_set_ = true; +} + +bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const +{ + // TODO: get frame with same point of view AND same pyramid level! + Vector3d obs_dir(Vector3d(framepos(0),0.0,framepos(1)) - pos_); obs_dir.normalize(); + auto max_it=obs_.begin(); + double max_cos_angle = 1; + for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) + { + Vector3d dir(Vector3d((*it)->frame->pos()(0),0.0,(*it)->frame->pos()(1)) - pos_); dir.normalize(); + double cos_angle = obs_dir.dot(dir); + if(cos_angle < max_cos_angle) + { + max_cos_angle = cos_angle; + max_it = it; + } + } + ftr = *max_it; + return true; +} + +void Point::optimize(const size_t n_iter) +{ + Vector3d old_point = pos_; + double chi2 = 0.0; + Matrix3d A; + Vector3d b; + + for(size_t i=0; iframe->se3()*pos_); + Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J); + const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f)); + new_chi2 += e.squaredNorm(); + A.noalias() += J.transpose() * J; + b.noalias() -= J.transpose() * e; + } + + // solve linear system + const Vector3d dp(A.ldlt().solve(b)); + + // check if error increased + if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0])) + { +#ifdef POINT_OPTIMIZER_DEBUG + cout << "it " << i + << "\t FAILURE \t new_chi2 = " << new_chi2 << endl; +#endif + pos_ = old_point; // roll-back + break; + } + + // update the model + Vector3d new_point = pos_ + dp; + old_point = pos_; + pos_ = new_point; + chi2 = new_chi2; +#ifdef POINT_OPTIMIZER_DEBUG + cout << "it " << i + << "\t Success \t new_chi2 = " << new_chi2 + << "\t norm(b) = " << vk::norm_max(b) + << endl; +#endif + + // stop when converged + if(vk::norm_max(dp) <= EPS) + break; + } +#ifdef POINT_OPTIMIZER_DEBUG + cout << endl; +#endif +} + +} // namespace svo diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp new file mode 100644 index 0000000..3d45ab5 --- /dev/null +++ b/GPU_version/vio/src/pose_optimizer.cpp @@ -0,0 +1,178 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs) +{ + // init + double chi2(0.0); + vector chi2_vec_init, chi2_vec_final; + vk::robust_cost::TukeyWeightFunction weight_function; + SE2_5 T_old(frame->T_f_w_); + Matrix3d A; + Vector3d b; + + // compute the scale of the error for robust estimation + std::vector errors; errors.reserve(frame->fts_.size()); + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + //Reprojection error + Vector2d e = vk::project2d((*it)->f) + - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); + + e *= 1.0 / (1<<(*it)->level); + errors.push_back(e.norm()); + } + if(errors.empty()) + return; + vk::robust_cost::MADScaleEstimator scale_estimator; + estimated_scale = 0.5*scale_estimator.compute(errors); + + num_obs = errors.size(); + chi2_vec_init.reserve(num_obs); + chi2_vec_final.reserve(num_obs); + double scale = estimated_scale; + bool direction=true; + size_t n_direction=0; + for(size_t iter=0; itercam_->errorMultiplier2(); + + b.setZero(); + A.setZero(); + double new_chi2(0.0); + + // compute residual + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Matrix23d J; + Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_)); + Frame::jacobian_xyz2uv(xyz_f, J); + Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f); + double sqrt_inv_cov = 1.0 / (1<<(*it)->level); + e *= sqrt_inv_cov; + if(iter == 0) + chi2_vec_init.push_back(e.squaredNorm()); // just for debug + J *= sqrt_inv_cov; + double weight = weight_function.value(e.norm()/scale); + A.noalias() += J.transpose()*J*weight; + b.noalias() -= J.transpose()*e*weight; + new_chi2 += e.squaredNorm()*weight; + } + // solve linear system + const Vector3d dT(A.ldlt().solve(b)); + + // check if error increased + if((iter > 0 && new_chi2 > chi2 && n_direction>3) || (bool) std::isnan((double)dT[0])) + { + if(verbose) + std::cout << "it " << iter + << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl; + frame->T_f_w_ = T_old; // roll-back + break; + }else if((iter > 0 && new_chi2 > chi2 && n_direction<3) ){ + ++n_direction; + scale *=0.5; + if(direction){ + direction=false; + }else{ + direction=true; + } + } + + // update the model + T_old = frame->T_f_w_; + if(direction){ + frame->T_f_w_=SE2_5(SE2::exp(dT)*(T_old.se2())); + }else{ + frame->T_f_w_=SE2_5(SE2::exp(-1.0*dT)*(T_old.se2())); + } + chi2 = new_chi2; + if(verbose) + std::cout << "it " << iter + << "\t Success \t new_chi2 = " << new_chi2 + << "\t norm(dT) = " << vk::norm_max(dT) << std::endl; + + // stop when converged + if(vk::norm_max(dT) <= EPS) + break; + } + + // Set covariance as inverse information matrix. Optimistic estimator! + //const double pixel_variance=1.0; + //frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse(); + + // Remove Measurements with too large reprojection error + double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2(); + size_t n_deleted_refs = 0; + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); + double sqrt_inv_cov = 1.0 / (1<<(*it)->level); + e *= sqrt_inv_cov; + chi2_vec_final.push_back(e.squaredNorm()); + if(e.norm() > reproj_thresh_scaled) + { + // we don't need to delete a reference in the point since it was not created yet + (*it)->point = NULL; + ++n_deleted_refs; + } + } + error_init=0.0; + error_final=0.0; + if(!chi2_vec_init.empty()) + error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2(); + if(!chi2_vec_final.empty()) + error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2(); + + estimated_scale *= frame->cam_->errorMultiplier2(); + if(verbose) + std::cout << "n deleted obs = " << n_deleted_refs + << "\t scale = " << estimated_scale + << "\t error init = " << error_init + << "\t error end = " << error_final << std::endl; + num_obs -= n_deleted_refs; +} + +} // namespace pose_optimizer +} // namespace svo diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp new file mode 100644 index 0000000..897ce59 --- /dev/null +++ b/GPU_version/vio/src/reprojector.cpp @@ -0,0 +1,219 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) : + map_(map) +{ + initializeGrid(cam); +} + +Reprojector::~Reprojector() +{ + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; }); +} + +void Reprojector::initializeGrid(vk::AbstractCamera* cam) +{ + grid_.cell_size = Config::gridSize(); + grid_.grid_n_cols = ceil(static_cast(cam->width())/grid_.cell_size); + grid_.grid_n_rows = ceil(static_cast(cam->height())/grid_.cell_size); + grid_.cells.resize(grid_.grid_n_cols*grid_.grid_n_rows); + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell*& c){ c = new Cell; }); + grid_.cell_order.resize(grid_.cells.size()); + for(size_t i=0; iclear(); }); +} + +void Reprojector::reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs) +{ + resetGrid(); + + // Identify those Keyframes which share a common field of view. +// SVO_START_TIMER("reproject_kfs"); + list< pair > close_kfs; + map_.getCloseKeyframes(frame, close_kfs); + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + // Reproject all mappoints of the closest N kfs with overlap. We only store + // in which grid cell the points fall. + size_t n = 0; + overlap_kfs.reserve(options_.max_n_kfs); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + overlap_kfs.push_back(pair(ref_frame,0)); + + // Try to reproject each mappoint that the other KF observes + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + // check if the feature has a mappoint assigned + if((*it_ftr)->point == NULL) + continue; + + // make sure we project a point only once + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + if(reprojectPoint(frame, (*it_ftr)->point)){ + overlap_kfs.back().second++; + } + + } + } + //SVO_STOP_TIMER("reproject_kfs"); + + // Now project all point candidates + // SVO_START_TIMER("reproject_candidates"); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + auto it=map_.point_candidates_.candidates_.begin(); + while(it!=map_.point_candidates_.candidates_.end()) + { + if(!reprojectPoint(frame, it->first)) + { + it->first->n_failed_reproj_ += 3; + if(it->first->n_failed_reproj_ > 30) + { + map_.point_candidates_.deleteCandidate(*it); + it = map_.point_candidates_.candidates_.erase(it); + continue; + } + } + ++it; + } + } // unlock the mutex when out of scope + //SVO_STOP_TIMER("reproject_candidates"); + // Now we go through each grid cell and select one point to match. + // At the end, we should have at maximum one reprojected point per cell. + //SVO_START_TIMER("feature_align"); + for(size_t i=0; i (size_t) Config::maxFts()) + break; + } +// SVO_STOP_TIMER("feature_align"); +} + +bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) +{ + if(lhs.pt->type_ > rhs.pt->type_) + return true; + return false; +} + +bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) +{ + cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); + Cell::iterator it=cell.begin(); + while(it!=cell.end()) + { + ++n_trials_; + + if(it->pt->type_ == Point::TYPE_DELETED) + { + it = cell.erase(it); + continue; + } + bool found_match = true; + if(options_.find_match_direct) + found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); + if(!found_match) + { + it->pt->n_failed_reproj_++; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) + map_.safeDeletePoint(it->pt); + if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) + map_.point_candidates_.deleteCandidatePoint(it->pt); + it = cell.erase(it); + continue; + } + it->pt->n_succeeded_reproj_++; + if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) + it->pt->type_ = Point::TYPE_GOOD; + + Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); + frame->addFeature(new_feature); + + // Here we add a reference in the feature to the 3D point, the other way + // round is only done if this frame is selected as keyframe. + new_feature->point = it->pt; + + if(matcher_.ref_ftr_->type == Feature::EDGELET) + { + new_feature->type = Feature::EDGELET; + new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad; + new_feature->grad.normalize(); + } + + // If the keyframe is selected and we reproject the rest, we don't have to + // check this point anymore. + it = cell.erase(it); + + // Maximum one point per cell. + return true; + } + return false; +} + +bool Reprojector::reprojectPoint(FramePtr frame, Point* point) +{ + Vector2d px(frame->w2c(point->pos_)); + if(frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher + { + const int k = static_cast(px[1]/grid_.cell_size)*grid_.grid_n_cols + + static_cast(px[0]/grid_.cell_size); + grid_.cells.at(k)->push_back(Candidate(point, px)); + return true; + } + return false; +} + +} // namespace svo diff --git a/GPU_version/vio/src/robust_cost.cpp b/GPU_version/vio/src/robust_cost.cpp new file mode 100644 index 0000000..ec771ae --- /dev/null +++ b/GPU_version/vio/src/robust_cost.cpp @@ -0,0 +1,162 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo 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. +* +* dvo 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 dvo. If not, see . +*/ + +#include +#include +#include +#include + +namespace vk { +namespace robust_cost { + +const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; +const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; + +TDistributionScaleEstimator:: +TDistributionScaleEstimator(const float dof) : + dof_(dof), + initial_sigma_(INITIAL_SIGMA) +{} + +float TDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); + int num = 0; + float lambda = initial_lamda; + int iterations = 0; + do + { + ++iterations; + initial_lamda = lambda; + num = 0; + lambda = 0.0f; + + for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) + { + if(std::isfinite(*it)) + { + ++num; + const float error2 = (*it)*(*it); + lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); + } + } + lambda = float(num) / lambda; + } while(std::abs(lambda - initial_lamda) > 1e-3); + + return std::sqrt(1.0f / lambda); +} + +const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745 + +float MADScaleEstimator:: +compute(std::vector& errors) const +{ + // error must be in absolute values! + return NORMALIZER * vk::getMedian(errors); +} + +float NormalDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); + float var = 0.0; + std::for_each(errors.begin(), errors.end(), [&](const float d) { + var += (d - mean) * (d - mean); + }); + return std::sqrt(var); // return standard deviation +} + +const float TukeyWeightFunction::DEFAULT_B = 4.6851f; + +TukeyWeightFunction::TukeyWeightFunction(const float b) +{ + configure(b); +} + +float TukeyWeightFunction::value(const float& x) const +{ + const float x_square = x * x; + if(x_square <= b_square) + { + const float tmp = 1.0f - x_square / b_square; + return tmp * tmp; + } + else + { + return 0.0f; + } +} + +void TukeyWeightFunction:: +configure(const float& param) +{ + b_square = param * param; +} + +const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; + +TDistributionWeightFunction:: +TDistributionWeightFunction(const float dof) +{ + configure(dof); +} + +float TDistributionWeightFunction:: +value(const float & x) const +{ + return ((dof_ + 1.0f) / (dof_ + (x * x))); +} + +void TDistributionWeightFunction:: +configure(const float& param) +{ + dof_ = param; + normalizer_ = dof_ / (dof_ + 1.0f); +} + +const float HuberWeightFunction::DEFAULT_K = 1.345f; + +HuberWeightFunction:: +HuberWeightFunction(const float k) +{ + configure(k); +} + +void HuberWeightFunction:: +configure(const float& param) +{ + k = param; +} + +float HuberWeightFunction:: +value(const float& t) const +{ + const float t_abs = std::abs(t); + if(t_abs < k) + return 1.0f; + else + return k / t_abs; +} + +} // namespace robust_cost +} // namespace vk + + diff --git a/GPU_version/vio/src/sparse_img_align.cpp b/GPU_version/vio/src/sparse_img_align.cpp new file mode 100644 index 0000000..59c754c --- /dev/null +++ b/GPU_version/vio/src/sparse_img_align.cpp @@ -0,0 +1,269 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +SparseImgAlign::SparseImgAlign( + int max_level, int min_level, int n_iter, + Method method, bool display, bool verbose) : + display_(display), + max_level_(max_level), + min_level_(min_level) +{ + n_iter_ = n_iter; + n_iter_init_ = n_iter_; + method_ = method; + verbose_ = verbose; + eps_ = 0.0001; +} + +size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) +{ + reset(); + + if(ref_frame->fts_.empty()) + { + return 0; + } + + ref_frame_ = ref_frame; + cur_frame_ = cur_frame; + ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); + jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); + SE2 T_cur_from_ref(cur_frame_->T_f_w_.se2() * ref_frame_->T_f_w_.inverse()); + for(level_=max_level_; level_>=min_level_; --level_) + { + visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? + mu_ = 1.0; + jacobian_cache_.setZero(); + have_ref_patch_cache_ = false; + if(verbose_) + printf("\nPYRAMID LEVEL %i\n---------------\n", level_); + optimize(T_cur_from_ref); + } + cur_frame_->T_f_w_ = SE2_5(T_cur_from_ref * ref_frame_->T_f_w_.se2()); + + return n_meas_/patch_area_; +} + +Matrix SparseImgAlign::getFisherInformation() +{ + double sigma_i_sq = 5e-4*255*255; // image noise + Matrix I = H_/sigma_i_sq; + return I; +} + +void SparseImgAlign::precomputeReferencePatches() +{ + const int border = patch_halfsize_+1; + const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_); + const int stride = ref_img.cols; + const float scale = 1.0f/(1<pos()(0),1e-19,ref_frame_->pos()(1)); + const double focal_length = ref_frame_->cam_->errorMultiplier2(); + size_t feature_counter = 0; + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end(); + it!=ite; ++it, ++feature_counter, ++visiblity_it) + { + // check if reference with patch size is within image + const float u_ref = (*it)->px[0]*scale; + const float v_ref = (*it)->px[1]*scale; + const int u_ref_i = floorf(u_ref); + const int v_ref_i = floorf(v_ref); + if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows) + continue; + *visiblity_it = true; + + // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!! + const double depth(((*it)->point->pos_ - ref_pos).norm()); + const Vector3d xyz_ref((*it)->f*depth); + + // evaluate projection jacobian + Matrix frame_jac; + Frame::jacobian_xyz2uv(xyz_ref, frame_jac); + // compute bilateral interpolation weights for reference image + const float subpix_u_ref = u_ref-u_ref_i; + const float subpix_v_ref = v_ref-v_ref_i; + const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref); + const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + size_t pixel_counter = 0; + float* cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + for(int y=0; yimg_pyr_.at(level_); + + if(linearize_system && display_) + resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0)); + + if(have_ref_patch_cache_ == false) + precomputeReferencePatches(); + + // compute the weights on the first iteration + std::vector errors; + if(compute_weight_scale) + errors.reserve(visible_fts_.size()); + const int stride = cur_img.cols; + const int border = patch_halfsize_+1; + const float scale = 1.0f/(1<pos()(0),1e-19,ref_frame_->pos()(1)); + float chi2 = 0.0; + size_t feature_counter = 0; // is used to compute the index of the cached jacobian + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end(); + ++it, ++feature_counter, ++visiblity_it) + { + // check if feature is within image + if(!*visiblity_it) + continue; + + // compute pixel location in cur img + const double depth = ((*it)->point->pos_ - ref_pos).norm(); + const Vector3d xyz_ref((*it)->f*depth); + const Vector3d xyz_cur((T_cur_from_ref.rotation_matrix()(0,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(0,1)*xyz_ref.z())+T_cur_from_ref.translation()(0), + xyz_ref.y(), + -1.0*(T_cur_from_ref.rotation_matrix()(1,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(1,1)*xyz_ref.z())+T_cur_from_ref.translation()(1)); + const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast() * scale); + const float u_cur = uv_cur_pyr[0]; + const float v_cur = uv_cur_pyr[1]; + const int u_cur_i = floorf(u_cur); + const int v_cur_i = floorf(v_cur); + + // check if projection is within the image + if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows) + continue; + + // compute bilateral interpolation weights for the current image + const float subpix_u_cur = u_cur-u_cur_i; + const float subpix_v_cur = v_cur-v_cur_i; + const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur); + const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur); + const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur; + const float w_cur_br = subpix_u_cur * subpix_v_cur; + float* ref_patch_cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + size_t pixel_counter = 0; // is used to compute the index of the cached jacobian + for(int y=0; yvalue(res/scale_); + } + + chi2 += res*res*weight; + n_meas_++; + + if(linearize_system) + { + // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error) + const Vector3d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter)); + H_.noalias() += J*J.transpose()*weight; + Jres_.noalias() -= J*res*weight; + if(display_) + resimg_.at((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0; + } + } + } + } + // compute the weights on the first iteration + if(compute_weight_scale && iter_ == 0) + scale_ = scale_estimator_->compute(errors); + + return chi2/n_meas_; +} + +int SparseImgAlign::solve() +{ + x_ = H_.ldlt().solve(Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void SparseImgAlign::update( + const ModelType& T_curold_from_ref, + ModelType& T_curnew_from_ref) +{ + T_curnew_from_ref = T_curold_from_ref * SE2::exp(-0.25*x_); +} + +void SparseImgAlign::startIteration() +{} + +void SparseImgAlign::finishIteration() +{ + if(display_) + { + cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*10); + cv::waitKey(0); + } +} + +} // namespace svo + diff --git a/GPU_version/vio/src/vision.cpp b/GPU_version/vio/src/vision.cpp new file mode 100644 index 0000000..e1be729 --- /dev/null +++ b/GPU_version/vio/src/vision.cpp @@ -0,0 +1,295 @@ +/* + * vision.cpp + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#include + +#if __SSE2__ +# include +#elif __ARM_NEON__ +# include +#endif + +namespace vk { + +#ifdef __SSE2__ +void halfSampleSSE2(const unsigned char* in, unsigned char* out, int w, int h) +{ + const unsigned long long mask[2] = {0x00FF00FF00FF00FFull, 0x00FF00FF00FF00FFull}; + const unsigned char* nextRow = in + w; + __m128i m = _mm_loadu_si128((const __m128i*)mask); + int sw = w >> 4; + int sh = h >> 1; + for (int i=0; i> 1)*out.cols; + for( int x = in.cols; x > 0 ; x-=16, in_top += 16, in_bottom += 16, out_data += 8) + { + uint8x8x2_t top = vld2_u8( (const uint8_t *)in_top ); + uint8x8x2_t bottom = vld2_u8( (const uint8_t *)in_bottom ); + uint16x8_t sum = vaddl_u8( top.val[0], top.val[1] ); + sum = vaddw_u8( sum, bottom.val[0] ); + sum = vaddw_u8( sum, bottom.val[1] ); + uint8x8_t final_sum = vshrn_n_u16(sum, 2); + vst1_u8(out_data, final_sum); + } + } +} +#endif + + +void +halfSample(const cv::Mat& in, cv::Mat& out) +{ + assert( in.rows/2==out.rows && in.cols/2==out.cols); + assert( in.type()==CV_8U && out.type()==CV_8U); + +#ifdef __SSE2__ + if(aligned_mem::is_aligned16(in.data) && aligned_mem::is_aligned16(out.data) && ((in.cols % 16) == 0)) + { + halfSampleSSE2(in.data, out.data, in.cols, in.rows); + return; + } +#endif +#ifdef __ARM_NEON__ + if( (in.cols % 16) == 0 ) + { + halfSampleNEON(in, out); + return; + } +#endif + + const int stride = in.step.p[0]; + uint8_t* top = (uint8_t*) in.data; + uint8_t* bottom = top + stride; + uint8_t* end = top + stride*in.rows; + const int out_width = out.cols; + uint8_t* p = (uint8_t*) out.data; + while (bottom < end) + { + for (int j=0; j( (uint16_t (top[0]) + top[1] + bottom[0] + bottom[1])/4 ); + p++; + top += 2; + bottom += 2; + } + top += stride; + bottom += stride; + } +} + + +float +shiTomasiScore(const cv::Mat& img, int u, int v) +{ + assert(img.type() == CV_8UC1); + + float dXX = 0.0; + float dYY = 0.0; + float dXY = 0.0; + const int halfbox_size = 4; + const int box_size = 2*halfbox_size; + const int box_area = box_size*box_size; + const int x_min = u-halfbox_size; + const int x_max = u+halfbox_size; + const int y_min = v-halfbox_size; + const int y_max = v+halfbox_size; + + if(x_min < 1 || x_max >= img.cols-1 || y_min < 1 || y_max >= img.rows-1) + return 0.0; // patch is too close to the boundary + + const int stride = img.step.p[0]; + for( int y=y_min; y::depth, cn*2)); + + int x, y, delta = (int)alignSize((cols + 2)*cn, 16); + AutoBuffer _tempBuf(delta*2 + 64); + deriv_type *trow0 = alignPtr(_tempBuf + cn, 16), *trow1 = alignPtr(trow0 + delta, 16); + +#ifdef __SSE2__ + __m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10); +#endif + + for( y = 0; y < rows; y++ ) + { + const uchar* srow0 = src.ptr(y > 0 ? y-1 : rows > 1 ? 1 : 0); + const uchar* srow1 = src.ptr(y); + const uchar* srow2 = src.ptr(y < rows-1 ? y+1 : rows > 1 ? rows-2 : 0); + deriv_type* drow = dst.ptr(y); + + // do vertical convolution + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow0 + x)), z); + __m128i s1 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow1 + x)), z); + __m128i s2 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow2 + x)), z); + __m128i t0 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s0, s2), c3), _mm_mullo_epi16(s1, c10)); + __m128i t1 = _mm_sub_epi16(s2, s0); + _mm_store_si128((__m128i*)(trow0 + x), t0); + _mm_store_si128((__m128i*)(trow1 + x), t1); + } +#endif + for( ; x < colsn; x++ ) + { + int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10; + int t1 = srow2[x] - srow0[x]; + trow0[x] = (deriv_type)t0; + trow1[x] = (deriv_type)t1; + } + + // make border + int x0 = (cols > 1 ? 1 : 0)*cn, x1 = (cols > 1 ? cols-2 : 0)*cn; + for( int k = 0; k < cn; k++ ) + { + trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k]; + trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k]; + } + + // do horizontal convolution, interleave the results and store them to dst + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_loadu_si128((const __m128i*)(trow0 + x - cn)); + __m128i s1 = _mm_loadu_si128((const __m128i*)(trow0 + x + cn)); + __m128i s2 = _mm_loadu_si128((const __m128i*)(trow1 + x - cn)); + __m128i s3 = _mm_load_si128((const __m128i*)(trow1 + x)); + __m128i s4 = _mm_loadu_si128((const __m128i*)(trow1 + x + cn)); + + __m128i t0 = _mm_sub_epi16(s1, s0); + __m128i t1 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s2, s4), c3), _mm_mullo_epi16(s3, c10)); + __m128i t2 = _mm_unpacklo_epi16(t0, t1); + t0 = _mm_unpackhi_epi16(t0, t1); + // this can probably be replaced with aligned stores if we aligned dst properly. + _mm_storeu_si128((__m128i*)(drow + x*2), t2); + _mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0); + } +#endif + for( ; x < colsn; x++ ) + { + deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]); + deriv_type t1 = (deriv_type)((trow1[x+cn] + trow1[x-cn])*3 + trow1[x]*10); + drow[x*2] = t0; drow[x*2+1] = t1; + } + } + +// vector vec_mat; +// cv::split(dst, vec_mat); +// cv::namedWindow("deriv"); +// cv::imshow("deriv", vec_mat[0]); +// cv::namedWindow("derivy"); +// cv::imshow("derivy", vec_mat[1]); +// cv::waitKey(0); +} + +#ifdef __SSE2__ +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale) +{ + depth_32f.create(depth_16u.rows, depth_16u.cols, CV_32FC1); + + const unsigned short* input_ptr = depth_16u.ptr(); + float* output_ptr = depth_32f.ptr(); + + __m128 _scale = _mm_set1_ps(scale); + __m128 _zero = _mm_setzero_ps(); + __m128 _nan = _mm_set1_ps(std::numeric_limits::quiet_NaN()); + + for(int idx = 0; idx < depth_16u.size().area(); idx += 8, input_ptr += 8, output_ptr += 8) + { + __m128 _input, mask; + __m128i _inputi = _mm_load_si128((__m128i*) input_ptr); + + // load low shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpacklo_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 0, _input); + + // load high shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpackhi_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 4, _input); + } +} +#endif + +} + + diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp new file mode 100644 index 0000000..0434f0f --- /dev/null +++ b/GPU_version/vio/src/visualizer.cpp @@ -0,0 +1,80 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Visualizer:: +Visualizer() : + pnh_("~"), + trace_id_(0) +{ + // Init ROS Marker Publishers + //pub_frames_ = pnh_.advertise("keyframes", 10); + pub_points_ = pnh_.advertise("points", 1000); + pub_pose_with_cov_ = pnh_.advertise("pose_cov",10); +} + +void Visualizer::publishMinimal( + UKF& ukf, + const double timestamp) +{ + ++trace_id_; + std_msgs::Header header_msg; + header_msg.frame_id = "/world"; + header_msg.seq = trace_id_; + header_msg.stamp = ros::Time(timestamp); + if(pub_pose_with_cov_.getNumSubscribers() > 0) + { + // publish cam in world frame (Estimated odometry in the worls frame) + auto odom=ukf.get_location(); + Quaterniond q(AngleAxisd(odom.second.pitch()+M_PI_2, Vector3d::UnitZ())); + geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); + msg_pose_with_cov->header = header_msg; + msg_pose_with_cov->pose.pose.position.x = -odom.second.se2().translation()(0); + msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(1); + msg_pose_with_cov->pose.pose.position.z = 0.0; + msg_pose_with_cov->pose.pose.orientation.x = q.x(); + msg_pose_with_cov->pose.pose.orientation.y = q.y(); + msg_pose_with_cov->pose.pose.orientation.z = q.z(); + msg_pose_with_cov->pose.pose.orientation.w = q.w(); + msg_pose_with_cov->pose.covariance = {odom.first(0,0),odom.first(0,1),0.0,0.0,odom.first(0,2),0.0, + odom.first(1,0),odom.first(1,1),0.0,0.0,odom.first(1,2),0.0, + 0.0,0.0,0.0000001,0.0,0.0,0.0, + 0.0,0.0,0.0,0.0000001,0.0,0.0, + odom.first(2,0),odom.first(2,1),0.0,0.0,odom.first(2,2),0.0, + 0.0,0.0,0.0,0.0,0.0,0.0000001}; + pub_pose_with_cov_.publish(msg_pose_with_cov); + if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ + publishPointMarker( + pub_points_, Eigen::Vector3d(-odom.second.se2().translation()(0),-odom.second.se2().translation()(1),0.0), "trajectory", + ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5)); + } + } + +} +} // end namespace gpu_svo diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp new file mode 100644 index 0000000..a182007 --- /dev/null +++ b/GPU_version/vio/src/vo_node.cpp @@ -0,0 +1,210 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . +#define VIO_DEBUG false +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if VIO_DEBUG +#include +#endif + +class VioNode +{ +public: + svo::FrameHandlerMono* vo_; + vk::AbstractCamera* cam_; + VioNode(); + ~VioNode(); + void imgCb(const sensor_msgs::ImageConstPtr& msg); + void imuCb(const sensor_msgs::ImuPtr& imu); + void cmdCb(const geometry_msgs::TwistPtr &imu); + void imu_th(); + bool getOdom(vio::getOdom::Request& req, vio::getOdom::Response& res); + uint trace_id_= 0; + bool start_=false; + bool start(vio::start::Request& req, vio::start::Response& res){ + if(req.on==1){ + start_=true; + if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME) + vo_->start(); + else{ + vo_->depthFilter()->startThread(); + vo_->reset(); + } + + imu_the_=new boost::thread(&VioNode::imu_th,this); + ++trace_id_; + res.ret=0; + }else{ + res.ret=100; + } + return true; + }; + + bool stop(vio::stop::Request& req, vio::stop::Response& res){ + if(req.off==1 && imu_the_!=NULL){ + start_=false; + vo_->depthFilter()->stopThread(); + imu_the_->interrupt(); + if(imu_the_->get_id()==boost::this_thread::get_id()) + imu_the_->detach(); + else + imu_the_->join(); + imu_the_=NULL; + res.ret=0; + }else{ + res.ret=100; + } + return true; + }; +private: + double* imu_; + double* cmd_; + ros::Time imu_time_; + boost::thread* imu_the_= nullptr; +#if VIO_DEBUG + svo::Visualizer visualizer_; +#endif +}; + +VioNode::VioNode() : + vo_(NULL), + cam_(NULL) +{ + imu_=(double *)calloc(static_cast(3), sizeof(double)); + cmd_=(double *)calloc(static_cast(3), sizeof(double)); + if(!vk::camera_loader::loadFromRosNs("vio", cam_)) + throw std::runtime_error("Camera model not correctly specified."); + Eigen::Matrix init; + init<<1e-19,1e-19,1e-19; + vo_ = new svo::FrameHandlerMono(cam_,init); + usleep(500); +} + +VioNode::~VioNode() +{ + start_=false; + usleep(5000); + imu_the_->join(); + delete imu_the_; + delete vo_; + delete cam_; +} + +void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg) +{ + if(!start_)return; + try { + auto start=std::chrono::steady_clock::now(); + cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; + vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp); + } catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + } +} +void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) { + if(!start_)return; + double imu_in[3]; + if(abs(imu->linear_acceleration.x)>5.0){ + return; + }else{ + imu_in[0]=imu->linear_acceleration.x; + }; + if(abs(imu->linear_acceleration.y)>5.0){ + return; + }else{ + imu_in[1]=imu->linear_acceleration.y; + }; + imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x; + imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y; + imu_in[2] = 0.5*imu_[2]+0.5*imu->angular_velocity.z; + memcpy(imu_, imu_in, static_cast(3*sizeof(double))); + vo_->UpdateIMU(imu_in,imu->header.stamp); + imu_time_=imu->header.stamp; +#if VIO_DEBUG + visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec()); +#endif +} +void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) { + if(!start_)return; + double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z}; + memcpy(cmd_, _cmd, static_cast(3*sizeof(double))); + vo_->UpdateCmd(_cmd,imu_time_); +} +void VioNode::imu_th(){ + ros::NodeHandle nh; + ros::Subscriber imu_sub=nh.subscribe(vk::getParam("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this); + ros::Subscriber cmd_sub=nh.subscribe(vk::getParam("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this); + while(start_ && !boost::this_thread::interruption_requested()) + { + ros::spinOnce(); + usleep(1000); + } + +} +bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) { + if(req.get==1){ + res.header.stamp = ros::Time(); + res.header.frame_id = "/world"; + res.header.seq = trace_id_; + auto odom=vo_->ukfPtr_.get_location(); + res.x=odom.second.translation()(0); + res.y=odom.second.translation()(1); + res.yaw=odom.second.pitch(); + res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2), + odom.first(1,0),odom.first(1,1),odom.first(1,2), + odom.first(2,0),odom.first(2,1),odom.first(2,2)}; + } + return true; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "vio"); + ros::NodeHandle nh; + VioNode vio; + ros::ServiceServer start = nh.advertiseService(vk::getParam("vio/start", "start"), + &VioNode::start, &vio); + ros::ServiceServer stop = nh.advertiseService(vk::getParam("vio/stop", "stop"), + &VioNode::stop, &vio); + ros::ServiceServer getOdom = nh.advertiseService(vk::getParam("vio/odom", "get_odom"), + &VioNode::getOdom, &vio); + image_transport::ImageTransport it(nh); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 1, &VioNode::imgCb, &vio); + // start processing callbacks + while(ros::ok()) + { + ros::spinOnce(); + } + printf("SVO terminated.\n"); + return 0; +} diff --git a/GPU_version/vio/srv/getOdom.srv b/GPU_version/vio/srv/getOdom.srv new file mode 100644 index 0000000..9f60ae7 --- /dev/null +++ b/GPU_version/vio/srv/getOdom.srv @@ -0,0 +1,9 @@ +# Odometry information will be return if get is 1 +uint8 get +--- +# Odometry information +Header header +float64 x +float64 y +float64 yaw +float64[9] cov \ No newline at end of file diff --git a/GPU_version/vio/srv/start.srv b/GPU_version/vio/srv/start.srv new file mode 100644 index 0000000..a1c27cc --- /dev/null +++ b/GPU_version/vio/srv/start.srv @@ -0,0 +1,5 @@ +# Turn ON VIO if on is 1 +uint8 on +--- +# Return 0 if VIO is ON +uint8 ret \ No newline at end of file diff --git a/GPU_version/vio/srv/stop.srv b/GPU_version/vio/srv/stop.srv new file mode 100644 index 0000000..7fa22db --- /dev/null +++ b/GPU_version/vio/srv/stop.srv @@ -0,0 +1,5 @@ +# Turn off VIO if off is 1 +uint8 off +--- +# Return 0 if VIO is off +uint8 ret \ No newline at end of file diff --git a/GPU_version/vio/test/README.md b/GPU_version/vio/test/README.md new file mode 100644 index 0000000..c70f832 --- /dev/null +++ b/GPU_version/vio/test/README.md @@ -0,0 +1,24 @@ +#### Download Test Data + +Create a folder where you will store all datasets, e.g.: + + mkdir Datasets + +In your `.bashrc` script, create a new environment variable pointing to the dataset folder: + + export SVO_DATASET_DIR=${HOME}/Datasets + +Source your new `.bashrc` script, go to the new dataset folder and download the test data: + + source ~/.bashrc + cd ${SVO_DATASET_DIR} + wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz + +You can then run the tests, e.g.: + + rosrun svo test_pipeline + +or if you are not using ROS: + + cd svo/bin + ./test_pipeline diff --git a/GPU_version/vio/test/benchmark.csv b/GPU_version/vio/test/benchmark.csv new file mode 100644 index 0000000..a3831e4 --- /dev/null +++ b/GPU_version/vio/test/benchmark.csv @@ -0,0 +1,307 @@ +local_ba,point_optimizer,pose_optimizer,pyramid_creation,reproject,sparse_img_align,tot_time,dropout,img_align_n_tracked,loba_err_fin,loba_err_init,loba_n_erredges_fin,loba_n_erredges_init,n_candidates,repr_n_mps,repr_n_new_references,sfba_error_final,sfba_error_init,sfba_n_edges_final,sfba_thresh +0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.005536000000000,1.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000326000000000,0.000000000000000,0.000000000000000,0.016564000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000189000000000,0.000000000000000,0.000000000000000,0.016754000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000133000000000,0.000000000000000,0.000000000000000,0.017082000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000136000000000,0.000000000000000,0.000000000000000,0.023259000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000120000000000,0.000000000000000,0.000000000000000,0.029114000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.032538000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000123000000000,0.000000000000000,0.000000000000000,0.031375000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000137000000000,0.000000000000000,0.000000000000000,0.029995000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000119000000000,0.000000000000000,0.000000000000000,0.030888000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000461000000000,0.000000000000000,0.000000000000000,0.036835000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000000000000000,0.000000000000000,0.000159000000000,0.000000000000000,0.000000000000000,0.090994000000000,1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000 +0.000000000000000,0.000036000000000,0.000143000000000,0.000117000000000,0.000836000000000,0.001616000000000,0.002862000000000,0.000000000000000,279.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,148.000000000000000,121.000000000000000,0.170175036288445,0.209622100274678,121.000000000000000,0.310240723182540 +0.000000000000000,0.000025000000000,0.000180000000000,0.000123000000000,0.000817000000000,0.000802000000000,0.001981000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,151.000000000000000,121.000000000000000,0.200086249066279,0.226929968835210,119.000000000000000,0.335856340084193 +0.000000000000000,0.000043000000000,0.000247000000000,0.000202000000000,0.000836000000000,0.000882000000000,0.002289000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,150.000000000000000,121.000000000000000,0.220021653137765,0.250579787207819,120.000000000000000,0.370858080741496 +0.000000000000000,0.000027000000000,0.000173000000000,0.000125000000000,0.000796000000000,0.000863000000000,0.002018000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,153.000000000000000,121.000000000000000,0.241933923583207,0.315090761433966,119.000000000000000,0.466334367626374 +0.000000000000000,0.000025000000000,0.000171000000000,0.000123000000000,0.000752000000000,0.000893000000000,0.002012000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,145.000000000000000,121.000000000000000,0.272951476803891,0.278442112643762,118.000000000000000,0.412094340922640 +0.000000000000000,0.000058000000000,0.000180000000000,0.000213000000000,0.001057000000000,0.000834000000000,0.002415000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,176.000000000000000,121.000000000000000,0.295685268713144,0.345204254249458,120.000000000000000,0.510902319377397 +0.000000000000000,0.000029000000000,0.000104000000000,0.000125000000000,0.000868000000000,0.000845000000000,0.002006000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,154.000000000000000,121.000000000000000,0.280726297104857,0.351937246921774,119.000000000000000,0.520867118388044 +0.000000000000000,0.000030000000000,0.000173000000000,0.000117000000000,0.000872000000000,0.000829000000000,0.002054000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,160.000000000000000,121.000000000000000,0.299712702632539,0.325868379793256,119.000000000000000,0.482285213145440 +0.000000000000000,0.000030000000000,0.000135000000000,0.000312000000000,0.001611000000000,0.000955000000000,0.003094000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,157.000000000000000,121.000000000000000,0.266414418920375,0.325723952270968,121.000000000000000,0.482071463677943 +0.000000000000000,0.000019000000000,0.000134000000000,0.000120000000000,0.000807000000000,0.000746000000000,0.001858000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,52.000000000000000,146.000000000000000,121.000000000000000,0.268513470166789,0.275002624777888,120.000000000000000,0.407003878905728 +0.000000000000000,0.000023000000000,0.000116000000000,0.000137000000000,0.000918000000000,0.000845000000000,0.002086000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,76.000000000000000,141.000000000000000,121.000000000000000,0.291779184224341,0.283057831418736,119.000000000000000,0.418925579815900 +0.000000000000000,0.000020000000000,0.000174000000000,0.000139000000000,0.000841000000000,0.000842000000000,0.002050000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,97.000000000000000,146.000000000000000,121.000000000000000,0.284752630550729,0.313033048850307,118.000000000000000,0.463288905244113 +0.000000000000000,0.000018000000000,0.000125000000000,0.000116000000000,0.000918000000000,0.000737000000000,0.001957000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,108.000000000000000,164.000000000000000,121.000000000000000,0.266984542461140,0.310809088449218,117.000000000000000,0.459997446489587 +0.000000000000000,0.000025000000000,0.000187000000000,0.000215000000000,0.001123000000000,0.000838000000000,0.002412000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,172.000000000000000,121.000000000000000,0.281472742741066,0.318106456171594,118.000000000000000,0.470797581856187 +0.000000000000000,0.000032000000000,0.000104000000000,0.000133000000000,0.000982000000000,0.001074000000000,0.002360000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,169.000000000000000,121.000000000000000,0.259578544711984,0.296051216412790,118.000000000000000,0.438155829408007 +0.000000000000000,0.000026000000000,0.000173000000000,0.000114000000000,0.000975000000000,0.000678000000000,0.001991000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,87.000000000000000,169.000000000000000,121.000000000000000,0.260854021690335,0.463271429083291,119.000000000000000,0.685641725628487 +0.000000000000000,0.000020000000000,0.000231000000000,0.000213000000000,0.001255000000000,0.000764000000000,0.002547000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,100.000000000000000,162.000000000000000,121.000000000000000,0.224857460689169,0.241819176684415,120.000000000000000,0.357892398925400 +0.000000000000000,0.000016000000000,0.000174000000000,0.000123000000000,0.000964000000000,0.000761000000000,0.002072000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,177.000000000000000,163.000000000000000,121.000000000000000,0.232802572049831,0.307670939545318,121.000000000000000,0.455352982415734 +0.000000000000000,0.000019000000000,0.000171000000000,0.000117000000000,0.000954000000000,0.000685000000000,0.001989000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,215.000000000000000,163.000000000000000,121.000000000000000,0.223503314093231,0.245376537152124,121.000000000000000,0.363157262109869 +0.000000000000000,0.000022000000000,0.000153000000000,0.000321000000000,0.001789000000000,0.000999000000000,0.003323000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,236.000000000000000,160.000000000000000,121.000000000000000,0.238679756461930,0.260245176933572,121.000000000000000,0.385162868349003 +0.000000000000000,0.000017000000000,0.000103000000000,0.000121000000000,0.000950000000000,0.000690000000000,0.001925000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,253.000000000000000,156.000000000000000,121.000000000000000,0.240028083468930,0.347317483304434,119.000000000000000,0.514029864789977 +0.000000000000000,0.000026000000000,0.000155000000000,0.000141000000000,0.001421000000000,0.000834000000000,0.002605000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,225.000000000000000,203.000000000000000,121.000000000000000,0.237050818956117,0.342187690832661,118.000000000000000,0.506437791010399 +0.000000000000000,0.000031000000000,0.000105000000000,0.000133000000000,0.001151000000000,0.000787000000000,0.002244000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,224.000000000000000,192.000000000000000,121.000000000000000,0.228256287503905,0.255917399607126,118.000000000000000,0.378757763709815 +0.000000000000000,0.000029000000000,0.000173000000000,0.000116000000000,0.001046000000000,0.000670000000000,0.002067000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,237.000000000000000,174.000000000000000,121.000000000000000,0.253024524323249,0.270830988595337,117.000000000000000,0.400829860236131 +0.000000000000000,0.000032000000000,0.000206000000000,0.000201000000000,0.001159000000000,0.000767000000000,0.002415000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,243.000000000000000,168.000000000000000,121.000000000000000,0.255837803512081,0.309767343777557,117.000000000000000,0.458455710853014 +0.000000000000000,0.000029000000000,0.000102000000000,0.000134000000000,0.001078000000000,0.000785000000000,0.002160000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,247.000000000000000,176.000000000000000,121.000000000000000,0.242039161090674,0.295753112981567,120.000000000000000,0.437714633120047 +0.000000000000000,0.000018000000000,0.000174000000000,0.000117000000000,0.001043000000000,0.000946000000000,0.002334000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,284.000000000000000,173.000000000000000,121.000000000000000,0.234035474669157,0.262962395172133,120.000000000000000,0.389184330309739 +0.000000000000000,0.000042000000000,0.000251000000000,0.000203000000000,0.001816000000000,0.000807000000000,0.003189000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,323.000000000000000,168.000000000000000,121.000000000000000,0.243963461663248,0.254676688230514,118.000000000000000,0.376921484440742 +0.000000000000000,0.000022000000000,0.000128000000000,0.000121000000000,0.001084000000000,0.000899000000000,0.002287000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,346.000000000000000,164.000000000000000,121.000000000000000,0.237498671286056,0.242318617577444,120.000000000000000,0.358631550548166 +0.000000000000000,0.000042000000000,0.000198000000000,0.000115000000000,0.001158000000000,0.000720000000000,0.002269000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,174.000000000000000,121.000000000000000,0.231204372046851,0.310626801011824,119.000000000000000,0.459727694560518 +0.000000000000000,0.000020000000000,0.000128000000000,0.000280000000000,0.000998000000000,0.000883000000000,0.002349000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,371.000000000000000,154.000000000000000,121.000000000000000,0.226372262196515,0.240389813013775,118.000000000000000,0.355776916048274 +0.000000000000000,0.000020000000000,0.000125000000000,0.000116000000000,0.001040000000000,0.000676000000000,0.002025000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,383.000000000000000,163.000000000000000,121.000000000000000,0.243430360339485,0.289701992216838,116.000000000000000,0.428758939836188 +0.000000000000000,0.000028000000000,0.000186000000000,0.000201000000000,0.001357000000000,0.000862000000000,0.002663000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,189.000000000000000,121.000000000000000,0.238491256129826,0.289513389727138,119.000000000000000,0.428479837315611 +0.000000000000000,0.000024000000000,0.000104000000000,0.000131000000000,0.001197000000000,0.000689000000000,0.002178000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,187.000000000000000,121.000000000000000,0.253535521704090,0.299150905814168,119.000000000000000,0.442743330689269 +0.000000000000000,0.000013000000000,0.000128000000000,0.000142000000000,0.001157000000000,0.000751000000000,0.002226000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,182.000000000000000,121.000000000000000,0.242747326189240,0.331002657945821,119.000000000000000,0.489883908154680 +0.000000000000000,0.000041000000000,0.000225000000000,0.000214000000000,0.001614000000000,0.000892000000000,0.003073000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,420.000000000000000,166.000000000000000,121.000000000000000,0.214060199250618,0.291833988602564,120.000000000000000,0.431914284304601 +0.000000000000000,0.000016000000000,0.000113000000000,0.000126000000000,0.001256000000000,0.000871000000000,0.002419000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,467.000000000000000,189.000000000000000,121.000000000000000,0.191542639509251,0.219467558857437,120.000000000000000,0.324811977376153 +0.000000000000000,0.000022000000000,0.000110000000000,0.000116000000000,0.001163000000000,0.000737000000000,0.002178000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,491.000000000000000,176.000000000000000,121.000000000000000,0.237287779136815,0.290385733638894,119.000000000000000,0.429770876012296 +0.000000000000000,0.000016000000000,0.000108000000000,0.000532000000000,0.001507000000000,0.001176000000000,0.003390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,503.000000000000000,195.000000000000000,121.000000000000000,0.255418654779892,0.271366103996528,119.000000000000000,0.401621830064983 +0.000000000000000,0.000023000000000,0.000128000000000,0.000118000000000,0.001247000000000,0.000704000000000,0.002255000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,519.000000000000000,192.000000000000000,121.000000000000000,0.243354932726649,0.322133765000893,118.000000000000000,0.476758002689501 +0.000000000000000,0.000028000000000,0.000117000000000,0.000194000000000,0.001487000000000,0.000763000000000,0.002640000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,537.000000000000000,182.000000000000000,121.000000000000000,0.256839812012575,0.290001379744665,120.000000000000000,0.429202006242450 +0.000000000000000,0.000021000000000,0.000192000000000,0.000135000000000,0.001268000000000,0.000737000000000,0.002390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,551.000000000000000,186.000000000000000,121.000000000000000,0.234921062453291,0.278307687859489,121.000000000000000,0.411895400770342 +0.000000000000000,0.000023000000000,0.000110000000000,0.000128000000000,0.001205000000000,0.000853000000000,0.002375000000000,1.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,566.000000000000000,182.000000000000000,121.000000000000000,0.236213436253286,0.243545081705577,120.000000000000000,0.360446727806674 +0.000000000000000,0.000033000000000,0.000148000000000,0.000221000000000,0.001810000000000,0.000752000000000,0.003004000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,194.000000000000000,121.000000000000000,0.256770574347429,0.281629144927868,120.000000000000000,0.416811158357270 +0.000000000000000,0.000018000000000,0.000107000000000,0.000166000000000,0.001283000000000,0.000754000000000,0.002364000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,193.000000000000000,121.000000000000000,0.238569415669078,0.245006057597332,120.000000000000000,0.362608988913209 +0.000000000000000,0.000026000000000,0.000199000000000,0.000126000000000,0.001415000000000,0.000739000000000,0.002538000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,525.000000000000000,207.000000000000000,121.000000000000000,0.272522903927536,0.292694935292295,120.000000000000000,0.433188542480310 +0.000000000000000,0.000024000000000,0.000152000000000,0.000385000000000,0.001329000000000,0.000772000000000,0.002715000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,532.000000000000000,193.000000000000000,121.000000000000000,0.300157389608362,0.352279544787609,121.000000000000000,0.521373718272822 +0.000000000000000,0.000025000000000,0.000103000000000,0.000124000000000,0.001300000000000,0.000765000000000,0.002352000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,542.000000000000000,192.000000000000000,121.000000000000000,0.262925192895899,0.296136504354747,121.000000000000000,0.438282037121495 +0.000000000000000,0.000027000000000,0.000159000000000,0.000227000000000,0.001454000000000,0.000792000000000,0.002702000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,565.000000000000000,171.000000000000000,121.000000000000000,0.281949830225302,0.308467076957178,121.000000000000000,0.456531258033208 +0.000000000000000,0.000027000000000,0.000177000000000,0.000130000000000,0.001440000000000,0.000817000000000,0.002620000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,608.000000000000000,201.000000000000000,121.000000000000000,0.262127192155801,0.344178587739890,120.000000000000000,0.509384339297628 +0.000000000000000,0.000031000000000,0.000173000000000,0.000122000000000,0.001320000000000,0.000744000000000,0.002429000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,636.000000000000000,196.000000000000000,121.000000000000000,0.243118415564597,0.307364154563702,120.000000000000000,0.454898948018352 +0.000000000000000,0.000031000000000,0.000237000000000,0.000391000000000,0.002601000000000,0.000982000000000,0.004294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,663.000000000000000,198.000000000000000,121.000000000000000,0.308234194177187,0.324201216218454,120.000000000000000,0.479817768949582 +0.000000000000000,0.000016000000000,0.000102000000000,0.000118000000000,0.001370000000000,0.000653000000000,0.002310000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,687.000000000000000,198.000000000000000,121.000000000000000,0.262171425081880,0.309537417467615,120.000000000000000,0.458115349322126 +0.000000000000000,0.000024000000000,0.000120000000000,0.000223000000000,0.001806000000000,0.000776000000000,0.002976000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,229.000000000000000,121.000000000000000,0.297772053023002,0.357203726783215,120.000000000000000,0.528661518750357 +0.000000000000000,0.000022000000000,0.000131000000000,0.000225000000000,0.001322000000000,0.000818000000000,0.002549000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,195.000000000000000,121.000000000000000,0.273836774721146,0.292258185663714,119.000000000000000,0.432542138616555 +0.000000000000000,0.000028000000000,0.000189000000000,0.000113000000000,0.001354000000000,0.000656000000000,0.002369000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,668.000000000000000,208.000000000000000,121.000000000000000,0.290291300745150,0.351263582277828,120.000000000000000,0.519870092614610 +0.000000000000000,0.000020000000000,0.000106000000000,0.000362000000000,0.002527000000000,0.000881000000000,0.003941000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,682.000000000000000,211.000000000000000,121.000000000000000,0.258959916330166,0.286061529228379,120.000000000000000,0.423371078465586 +0.000000000000000,0.000023000000000,0.000183000000000,0.000117000000000,0.001452000000000,0.000716000000000,0.002520000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,696.000000000000000,219.000000000000000,121.000000000000000,0.299843168031565,0.306288599781015,118.000000000000000,0.453307123537539 +0.000000000000000,0.000024000000000,0.000190000000000,0.000229000000000,0.001590000000000,0.000704000000000,0.002778000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,704.000000000000000,201.000000000000000,121.000000000000000,0.274895969986079,0.339774469390948,119.000000000000000,0.502866218860539 +0.000000000000000,0.000032000000000,0.000184000000000,0.000127000000000,0.001388000000000,0.000658000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,712.000000000000000,205.000000000000000,121.000000000000000,0.276261011279968,0.281429264300083,120.000000000000000,0.416515325859454 +0.000000000000000,0.000019000000000,0.000103000000000,0.000122000000000,0.001302000000000,0.000759000000000,0.002334000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,743.000000000000000,194.000000000000000,121.000000000000000,0.308426080941849,0.293337004748022,118.000000000000000,0.434138764752467 +0.000000000000000,0.000020000000000,0.000188000000000,0.000336000000000,0.002590000000000,0.000953000000000,0.004130000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,769.000000000000000,216.000000000000000,121.000000000000000,0.273447755201539,0.322902569141660,119.000000000000000,0.477895792772933 +0.000000000000000,0.000019000000000,0.000104000000000,0.000118000000000,0.001466000000000,0.000743000000000,0.002480000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,791.000000000000000,204.000000000000000,121.000000000000000,0.288081750238067,0.251959938318303,120.000000000000000,0.372900704820467 +0.000000000000000,0.000024000000000,0.000118000000000,0.000231000000000,0.001575000000000,0.000986000000000,0.003013000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,190.000000000000000,121.000000000000000,0.282457846343284,0.276454604943847,117.000000000000000,0.409152821737331 +0.000000000000000,0.000016000000000,0.000210000000000,0.000129000000000,0.001436000000000,0.000881000000000,0.002693000000000,0.000000000000000,111.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,208.000000000000000,121.000000000000000,0.294182859046983,0.317841061067694,121.000000000000000,0.470404755925379 +0.000000000000000,0.000015000000000,0.000142000000000,0.000184000000000,0.001424000000000,0.000955000000000,0.002755000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,194.000000000000000,121.000000000000000,0.243748783908456,0.304183797141307,119.000000000000000,0.450192011884479 +0.000000000000000,0.000014000000000,0.000107000000000,0.000318000000000,0.001624000000000,0.000893000000000,0.002992000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,220.000000000000000,121.000000000000000,0.265012454662619,0.270354981784829,119.000000000000000,0.400125381617550 +0.000000000000000,0.000023000000000,0.000213000000000,0.000128000000000,0.001340000000000,0.000949000000000,0.002681000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,811.000000000000000,196.000000000000000,121.000000000000000,0.253755971722852,0.269982133879470,119.000000000000000,0.399573570359238 +0.000000000000000,0.000038000000000,0.000249000000000,0.000219000000000,0.001747000000000,0.000847000000000,0.003214000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,217.000000000000000,121.000000000000000,0.242158110541114,0.277566291448718,118.000000000000000,0.410798121492822 +0.000000000000000,0.000022000000000,0.000200000000000,0.000131000000000,0.001465000000000,0.000761000000000,0.002608000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,826.000000000000000,208.000000000000000,121.000000000000000,0.247025005586580,0.299927107710462,119.000000000000000,0.443892088763821 +0.000000000000000,0.000028000000000,0.000129000000000,0.000115000000000,0.001507000000000,0.000767000000000,0.002582000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,835.000000000000000,206.000000000000000,121.000000000000000,0.251694282278063,0.262688495423133,119.000000000000000,0.388778969531936 +0.000000000000000,0.000018000000000,0.000176000000000,0.000303000000000,0.001569000000000,0.000763000000000,0.002865000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,858.000000000000000,208.000000000000000,121.000000000000000,0.204259521447942,0.239471125876921,121.000000000000000,0.354417264227415 +0.000000000000000,0.000027000000000,0.000197000000000,0.000119000000000,0.001520000000000,0.000717000000000,0.002613000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,182.000000000000000,121.000000000000000,0.288688133937508,0.321107040561571,121.000000000000000,0.475238455753857 +0.000000000000000,0.000074000000000,0.000308000000000,0.000231000000000,0.002302000000000,0.000817000000000,0.004009000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,895.000000000000000,169.000000000000000,121.000000000000000,0.226466756684178,0.283662806855079,120.000000000000000,0.419820962132456 +0.000000000000000,0.000032000000000,0.000120000000000,0.000146000000000,0.001779000000000,0.000719000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,201.000000000000000,121.000000000000000,0.248964133477333,0.301424345755280,121.000000000000000,0.446108027319984 +0.000000000000000,0.000031000000000,0.000209000000000,0.000347000000000,0.002379000000000,0.001268000000000,0.004286000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,881.000000000000000,197.000000000000000,121.000000000000000,0.197089987231914,0.215315450958625,121.000000000000000,0.318666844452755 +0.000000000000000,0.000022000000000,0.000183000000000,0.000138000000000,0.001523000000000,0.000904000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,889.000000000000000,208.000000000000000,121.000000000000000,0.211913610123421,0.217730864556872,121.000000000000000,0.322241701945577 +0.000000000000000,0.000023000000000,0.000188000000000,0.000135000000000,0.001539000000000,0.001010000000000,0.002927000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,191.000000000000000,121.000000000000000,0.223781553150169,0.264534665335954,119.000000000000000,0.391511288186158 +0.000000000000000,0.000024000000000,0.000111000000000,0.000561000000000,0.002319000000000,0.001083000000000,0.004153000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,959.000000000000000,191.000000000000000,121.000000000000000,0.192480342616366,0.242812113218322,120.000000000000000,0.359361932832411 +0.000000000000000,0.000023000000000,0.000113000000000,0.000129000000000,0.001473000000000,0.000780000000000,0.002555000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1010.000000000000000,186.000000000000000,121.000000000000000,0.226239188570666,0.246580462159093,121.000000000000000,0.364939080501376 +0.000000000000000,0.000025000000000,0.000147000000000,0.000235000000000,0.001877000000000,0.001081000000000,0.003415000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1042.000000000000000,191.000000000000000,121.000000000000000,0.213010810373553,0.226959616128893,121.000000000000000,0.335900212048673 +0.000000000000000,0.000019000000000,0.000180000000000,0.000123000000000,0.001581000000000,0.000797000000000,0.002856000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1066.000000000000000,197.000000000000000,121.000000000000000,0.236500218717681,0.308339991149568,118.000000000000000,0.456343184784554 +0.000000000000000,0.000020000000000,0.000176000000000,0.000124000000000,0.001670000000000,0.001020000000000,0.003039000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1045.000000000000000,222.000000000000000,121.000000000000000,0.203403550946325,0.322649020817046,119.000000000000000,0.477520556062907 +0.000000000000000,0.000021000000000,0.000102000000000,0.000120000000000,0.001506000000000,0.000747000000000,0.002537000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,208.000000000000000,121.000000000000000,0.217457541731394,0.242874100604298,119.000000000000000,0.359453694988533 +0.000000000000000,0.000014000000000,0.000102000000000,0.000114000000000,0.001438000000000,0.000775000000000,0.002481000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,202.000000000000000,121.000000000000000,0.237066555963892,0.363698547046353,120.000000000000000,0.538273825733024 +0.000000000000000,0.000028000000000,0.000138000000000,0.000130000000000,0.001867000000000,0.000905000000000,0.003190000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1051.000000000000000,204.000000000000000,121.000000000000000,0.204004037194742,0.217771792656962,121.000000000000000,0.322302253343558 +0.000000000000000,0.000016000000000,0.000108000000000,0.000146000000000,0.001730000000000,0.000807000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1092.000000000000000,216.000000000000000,121.000000000000000,0.189498788663577,0.218460412109291,119.000000000000000,0.323321417274184 +0.000000000000000,0.000021000000000,0.000142000000000,0.000136000000000,0.001571000000000,0.000950000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1150.000000000000000,195.000000000000000,121.000000000000000,0.234911243099688,0.229840840010708,121.000000000000000,0.340164460854319 +0.000000000000000,0.000023000000000,0.000178000000000,0.000327000000000,0.001509000000000,0.001060000000000,0.003143000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1188.000000000000000,188.000000000000000,121.000000000000000,0.238938910296195,0.242239031677996,119.000000000000000,0.358513758367035 +0.000000000000000,0.000026000000000,0.000127000000000,0.000121000000000,0.001413000000000,0.000746000000000,0.002666000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,181.000000000000000,121.000000000000000,0.231069770120829,0.351821144360750,121.000000000000000,0.520695269679250 +0.000000000000000,0.000013000000000,0.000123000000000,0.000156000000000,0.002291000000000,0.000810000000000,0.003493000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,223.000000000000000,121.000000000000000,0.232050436410629,0.243086413240903,121.000000000000000,0.359767900135068 +0.000000000000000,0.000017000000000,0.000183000000000,0.000122000000000,0.001660000000000,0.000828000000000,0.002856000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1126.000000000000000,205.000000000000000,121.000000000000000,0.217672407670591,0.232176089084970,120.000000000000000,0.343620616378729 +0.000000000000000,0.000025000000000,0.000104000000000,0.000126000000000,0.001640000000000,0.000956000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1122.000000000000000,225.000000000000000,121.000000000000000,0.210874531577464,0.231177462797771,120.000000000000000,0.342142641667492 +0.000000000000000,0.000011000000000,0.000104000000000,0.000149000000000,0.001579000000000,0.000928000000000,0.002811000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1124.000000000000000,218.000000000000000,121.000000000000000,0.199068638688048,0.231930228681434,121.000000000000000,0.343256752009726 +0.000000000000000,0.000016000000000,0.000105000000000,0.000114000000000,0.001529000000000,0.000835000000000,0.002636000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,217.000000000000000,121.000000000000000,0.199461859922069,0.201973412653913,121.000000000000000,0.298920644384362 +0.000000000000000,0.000020000000000,0.000107000000000,0.000175000000000,0.002271000000000,0.000832000000000,0.003453000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1170.000000000000000,216.000000000000000,121.000000000000000,0.220686177070489,0.284068818474203,121.000000000000000,0.420421851360250 +0.000000000000000,0.000021000000000,0.000110000000000,0.000115000000000,0.001720000000000,0.000987000000000,0.002996000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1207.000000000000000,220.000000000000000,121.000000000000000,0.211157109658996,0.207022193542394,121.000000000000000,0.306392853689555 +0.000000000000000,0.000019000000000,0.000128000000000,0.000124000000000,0.001660000000000,0.000836000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,230.000000000000000,121.000000000000000,0.227298389602018,0.273505984323976,121.000000000000000,0.404788875408716 +0.000000000000000,0.000019000000000,0.000103000000000,0.000135000000000,0.001696000000000,0.000751000000000,0.002919000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,235.000000000000000,121.000000000000000,0.230641563123264,0.341660758731761,121.000000000000000,0.505657951678639 +0.000000000000000,0.000015000000000,0.000177000000000,0.000130000000000,0.001556000000000,0.000919000000000,0.002864000000000,0.000000000000000,106.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1118.000000000000000,225.000000000000000,121.000000000000000,0.192303276106424,0.284766422639249,121.000000000000000,0.421454308293938 +0.000000000000000,0.000013000000000,0.000138000000000,0.000178000000000,0.002041000000000,0.000992000000000,0.003412000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1112.000000000000000,223.000000000000000,121.000000000000000,0.206634709237684,0.414844586254086,120.000000000000000,0.613969997701391 +0.000000000000000,0.000015000000000,0.000134000000000,0.000127000000000,0.001905000000000,0.000764000000000,0.002995000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1116.000000000000000,248.000000000000000,121.000000000000000,0.217282816286385,0.386388750278064,121.000000000000000,0.571855388443357 +0.000000000000000,0.000016000000000,0.000128000000000,0.000127000000000,0.001825000000000,0.000925000000000,0.003066000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1115.000000000000000,245.000000000000000,121.000000000000000,0.205232081617916,0.260414398266005,121.000000000000000,0.385413312570202 +0.000000000000000,0.000020000000000,0.000107000000000,0.000141000000000,0.001807000000000,0.000923000000000,0.003060000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1141.000000000000000,234.000000000000000,121.000000000000000,0.215575249362573,0.253408200953891,120.000000000000000,0.375044163656510 +0.000000000000000,0.000019000000000,0.000128000000000,0.000120000000000,0.001568000000000,0.000884000000000,0.002762000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1162.000000000000000,189.000000000000000,121.000000000000000,0.263382144346756,0.344167879309156,121.000000000000000,0.509368468563933 +0.000000000000000,0.000023000000000,0.000131000000000,0.000373000000000,0.001757000000000,0.001182000000000,0.003515000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1191.000000000000000,211.000000000000000,121.000000000000000,0.211945557067404,0.302128504397490,121.000000000000000,0.447150188651345 +0.000000000000000,0.000025000000000,0.000154000000000,0.000124000000000,0.001844000000000,0.000803000000000,0.003004000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,227.000000000000000,121.000000000000000,0.202381916559221,0.390013881008811,120.000000000000000,0.577220555675650 +0.000000000000000,0.000015000000000,0.000109000000000,0.000129000000000,0.001708000000000,0.000890000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1231.000000000000000,213.000000000000000,121.000000000000000,0.214162701863959,0.216971594768139,120.000000000000000,0.321117988293146 +0.000000000000000,0.000019000000000,0.000106000000000,0.000141000000000,0.001738000000000,0.000788000000000,0.002834000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,223.000000000000000,121.000000000000000,0.199426583525084,0.257295226754736,121.000000000000000,0.380796925542741 +0.000000000000000,0.000015000000000,0.000104000000000,0.000127000000000,0.001749000000000,0.000817000000000,0.002854000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,237.000000000000000,121.000000000000000,0.197980287517504,0.261753748672962,121.000000000000000,0.387395536882405 +0.000000000000000,0.000022000000000,0.000120000000000,0.000159000000000,0.002300000000000,0.000963000000000,0.003628000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,228.000000000000000,121.000000000000000,0.236498759179881,0.240201532654298,121.000000000000000,0.355498268421338 +0.000000000000000,0.000017000000000,0.000181000000000,0.000128000000000,0.001959000000000,0.000872000000000,0.003382000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,248.000000000000000,121.000000000000000,0.184287569106640,0.201686577196243,121.000000000000000,0.298496127529902 +0.000000000000000,0.000013000000000,0.000178000000000,0.000136000000000,0.001684000000000,0.000852000000000,0.002943000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1105.000000000000000,230.000000000000000,121.000000000000000,0.190585604418642,0.200521952748998,120.000000000000000,0.296772510252565 +0.000000000000000,0.000019000000000,0.000106000000000,0.000143000000000,0.001668000000000,0.000981000000000,0.002955000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1104.000000000000000,218.000000000000000,121.000000000000000,0.200527511479873,0.209064190915326,120.000000000000000,0.309415015408653 +0.000000000000000,0.000014000000000,0.000104000000000,0.000123000000000,0.001517000000000,0.000849000000000,0.002647000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,201.000000000000000,121.000000000000000,0.227018266784565,0.269186250934732,121.000000000000000,0.398395648547930 +0.000000000000000,0.000017000000000,0.000178000000000,0.000348000000000,0.001620000000000,0.001141000000000,0.003354000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,206.000000000000000,121.000000000000000,0.206456021778136,0.213075158345683,121.000000000000000,0.315351251063391 +0.000000000000000,0.000026000000000,0.000108000000000,0.000150000000000,0.001619000000000,0.000821000000000,0.002770000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,197.000000000000000,121.000000000000000,0.195769620795380,0.235699009656539,120.000000000000000,0.348834556747284 +0.000000000000000,0.000018000000000,0.000178000000000,0.000127000000000,0.001593000000000,0.000826000000000,0.002776000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,206.000000000000000,121.000000000000000,0.202443810598108,0.259322628022743,121.000000000000000,0.383797505085764 +0.000000000000000,0.000018000000000,0.000105000000000,0.000140000000000,0.001539000000000,0.001030000000000,0.002870000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1255.000000000000000,195.000000000000000,121.000000000000000,0.234795060265796,0.251596569636394,120.000000000000000,0.372362919449417 +0.000000000000000,0.000017000000000,0.000175000000000,0.000125000000000,0.001584000000000,0.000833000000000,0.002923000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1267.000000000000000,221.000000000000000,121.000000000000000,0.207163879614002,0.272216719799699,121.000000000000000,0.402880748216245 +0.000000000000000,0.000017000000000,0.000133000000000,0.000153000000000,0.002442000000000,0.000831000000000,0.003670000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,216.000000000000000,121.000000000000000,0.170524915168911,0.199849692332816,121.000000000000000,0.295777531500515 +0.000000000000000,0.000013000000000,0.000157000000000,0.000149000000000,0.001748000000000,0.000942000000000,0.003057000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,234.000000000000000,121.000000000000000,0.189221335752284,0.197743002271810,121.000000000000000,0.292659639941872 +0.000000000000000,0.000020000000000,0.000178000000000,0.000125000000000,0.001721000000000,0.000742000000000,0.002827000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1176.000000000000000,225.000000000000000,121.000000000000000,0.198900978356214,0.245389821232497,119.000000000000000,0.363176948895773 +0.000000000000000,0.000013000000000,0.000176000000000,0.000145000000000,0.001697000000000,0.000855000000000,0.002929000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,222.000000000000000,121.000000000000000,0.196051895442564,0.262432687056220,121.000000000000000,0.388400396935210 +0.000000000000000,0.000015000000000,0.000176000000000,0.000128000000000,0.001658000000000,0.000736000000000,0.002754000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1264.000000000000000,222.000000000000000,121.000000000000000,0.188130722176495,0.226906201366918,120.000000000000000,0.335821186914497 +0.000000000000000,0.000015000000000,0.000114000000000,0.000128000000000,0.001732000000000,0.000994000000000,0.003025000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1280.000000000000000,221.000000000000000,121.000000000000000,0.195205519657246,0.208004184809569,119.000000000000000,0.307846188328575 +0.000000000000000,0.000020000000000,0.000195000000000,0.000126000000000,0.002006000000000,0.000791000000000,0.003194000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1295.000000000000000,262.000000000000000,121.000000000000000,0.204929539974912,0.216820480192029,119.000000000000000,0.320894306981155 +0.000000000000000,0.000017000000000,0.000176000000000,0.000139000000000,0.001546000000000,0.000896000000000,0.002812000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,192.000000000000000,121.000000000000000,0.226704780607630,0.257512342665443,120.000000000000000,0.381118257356317 +0.000000000000000,0.000011000000000,0.000105000000000,0.000299000000000,0.001761000000000,0.001102000000000,0.003328000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,234.000000000000000,121.000000000000000,0.206579202251784,0.240623999885611,121.000000000000000,0.356123519730790 +0.000000000000000,0.000020000000000,0.000103000000000,0.000130000000000,0.001498000000000,0.000871000000000,0.002766000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1312.000000000000000,207.000000000000000,121.000000000000000,0.208586912880041,0.214158703806542,121.000000000000000,0.316954877506999 +0.000000000000000,0.000017000000000,0.000176000000000,0.000374000000000,0.001665000000000,0.000976000000000,0.003373000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1139.000000000000000,223.000000000000000,121.000000000000000,0.209543770412094,0.213150938854858,121.000000000000000,0.315463382345879 +0.000000000000000,0.000021000000000,0.000157000000000,0.000124000000000,0.001652000000000,0.000947000000000,0.002947000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1137.000000000000000,212.000000000000000,121.000000000000000,0.214888927283406,0.261239649375707,120.000000000000000,0.386634701996087 +0.000000000000000,0.000018000000000,0.000104000000000,0.000124000000000,0.001479000000000,0.000897000000000,0.002661000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,188.000000000000000,121.000000000000000,0.205526459876717,0.194046988139053,121.000000000000000,0.287189543909738 +0.000000000000000,0.000016000000000,0.000175000000000,0.000142000000000,0.001509000000000,0.000744000000000,0.002626000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,190.000000000000000,121.000000000000000,0.193638964960760,0.209501566643394,121.000000000000000,0.310062329059690 +0.000000000000000,0.000022000000000,0.000103000000000,0.000125000000000,0.001561000000000,0.000725000000000,0.002572000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,218.000000000000000,121.000000000000000,0.192895828207804,0.213686959411965,121.000000000000000,0.316256716855654 +0.000000000000000,0.000023000000000,0.000184000000000,0.000162000000000,0.001899000000000,0.000812000000000,0.003140000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1258.000000000000000,187.000000000000000,121.000000000000000,0.178433372351327,0.196124969530122,120.000000000000000,0.290264953456693 +0.000000000000000,0.000016000000000,0.000134000000000,0.000140000000000,0.001663000000000,0.000876000000000,0.002874000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1274.000000000000000,203.000000000000000,121.000000000000000,0.204379056805537,0.211513754780143,119.000000000000000,0.313040366095618 +0.000000000000000,0.000020000000000,0.000176000000000,0.000130000000000,0.001587000000000,0.000869000000000,0.002823000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1284.000000000000000,206.000000000000000,121.000000000000000,0.194394953473234,0.214318086264921,121.000000000000000,0.317190765131688 +0.000000000000000,0.000017000000000,0.000107000000000,0.000148000000000,0.001575000000000,0.000805000000000,0.002693000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1286.000000000000000,205.000000000000000,121.000000000000000,0.192692252511560,0.217301832678175,121.000000000000000,0.321606720966583 +0.000000000000000,0.000020000000000,0.000173000000000,0.000124000000000,0.001507000000000,0.000785000000000,0.002649000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,210.000000000000000,121.000000000000000,0.204881580167579,0.231890569564824,121.000000000000000,0.343198045458178 +0.000000000000000,0.000041000000000,0.000220000000000,0.000144000000000,0.001900000000000,0.000655000000000,0.003038000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,206.000000000000000,121.000000000000000,0.174115401194611,0.223973720218107,121.000000000000000,0.331481122502030 +0.000000000000000,0.000043000000000,0.000182000000000,0.000145000000000,0.001457000000000,0.000728000000000,0.002734000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,191.000000000000000,121.000000000000000,0.194538477548438,0.187220557909554,121.000000000000000,0.277086431960381 +0.000000000000000,0.000020000000000,0.000182000000000,0.000129000000000,0.001451000000000,0.000813000000000,0.002657000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1148.000000000000000,207.000000000000000,121.000000000000000,0.209547468410092,0.205381330935579,119.000000000000000,0.303964378715603 +0.000000000000000,0.000037000000000,0.000255000000000,0.000211000000000,0.002943000000000,0.001198000000000,0.004740000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1146.000000000000000,192.000000000000000,121.000000000000000,0.194112090334278,0.213834860708738,121.000000000000000,0.316475596512546 +0.000000000000000,0.000021000000000,0.000119000000000,0.000128000000000,0.001482000000000,0.000836000000000,0.002622000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1196.000000000000000,188.000000000000000,121.000000000000000,0.194707145965396,0.200711676435348,120.000000000000000,0.297053280716492 +0.000000000000000,0.000018000000000,0.000128000000000,0.000353000000000,0.001530000000000,0.001123000000000,0.003198000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1222.000000000000000,182.000000000000000,121.000000000000000,0.165297929068422,0.223118899091734,120.000000000000000,0.330215962199139 +0.000000000000000,0.000019000000000,0.000133000000000,0.000124000000000,0.001554000000000,0.001016000000000,0.002890000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,187.000000000000000,121.000000000000000,0.189732092265728,0.231665076279646,121.000000000000000,0.342864305156952 +0.000000000000000,0.000018000000000,0.000195000000000,0.000130000000000,0.001387000000000,0.000871000000000,0.002639000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,167.000000000000000,121.000000000000000,0.173396135722069,0.185374303680201,121.000000000000000,0.274353961674946 +0.000000000000000,0.000015000000000,0.000105000000000,0.000165000000000,0.001566000000000,0.001052000000000,0.002950000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1292.000000000000000,196.000000000000000,121.000000000000000,0.192868888286218,0.205034141945327,120.000000000000000,0.303450551076308 +0.000000000000000,0.000014000000000,0.000102000000000,0.000127000000000,0.001521000000000,0.000791000000000,0.002592000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1302.000000000000000,199.000000000000000,121.000000000000000,0.190528673137881,0.222906754530888,121.000000000000000,0.329902009771293 +0.000000000000000,0.000021000000000,0.000215000000000,0.000167000000000,0.001934000000000,0.000805000000000,0.003204000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.202526348712512,0.259278841345545,120.000000000000000,0.383732682741932 +0.000000000000000,0.000022000000000,0.000156000000000,0.000126000000000,0.001696000000000,0.000788000000000,0.002834000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.196451913404442,0.201069768524068,120.000000000000000,0.297583282351909 +0.000000000000000,0.000025000000000,0.000103000000000,0.000132000000000,0.001543000000000,0.000902000000000,0.002745000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1305.000000000000000,194.000000000000000,121.000000000000000,0.200871633124814,0.183908150343481,120.000000000000000,0.272184068463992 +0.000000000000000,0.000025000000000,0.000102000000000,0.000150000000000,0.001478000000000,0.000854000000000,0.002788000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1335.000000000000000,179.000000000000000,121.000000000000000,0.193823047570574,0.212329442302877,119.000000000000000,0.314247578003029 +0.000000000000000,0.000016000000000,0.000196000000000,0.000126000000000,0.001299000000000,0.000752000000000,0.002455000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1194.000000000000000,176.000000000000000,121.000000000000000,0.207923081779578,0.229508478265270,121.000000000000000,0.339672543925396 +0.000000000000000,0.000018000000000,0.000110000000000,0.000166000000000,0.001844000000000,0.000982000000000,0.003170000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,180.000000000000000,121.000000000000000,0.163342705330096,0.249575746286056,120.000000000000000,0.369372120119674 +0.000000000000000,0.000015000000000,0.000183000000000,0.000124000000000,0.001749000000000,0.000847000000000,0.002963000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,212.000000000000000,121.000000000000000,0.173881865983057,0.194395754595030,121.000000000000000,0.287705721832844 +0.000000000000000,0.000026000000000,0.000176000000000,0.000127000000000,0.001498000000000,0.000877000000000,0.002752000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1259.000000000000000,185.000000000000000,121.000000000000000,0.155523040470466,0.162766723518840,120.000000000000000,0.240894740082153 +0.000000000000000,0.000015000000000,0.000177000000000,0.000144000000000,0.001550000000000,0.000743000000000,0.002669000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,192.000000000000000,121.000000000000000,0.152934150143871,0.272911472202902,121.000000000000000,0.403908984747820 +0.000000000000000,0.000014000000000,0.000174000000000,0.000116000000000,0.001439000000000,0.000731000000000,0.002513000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1291.000000000000000,196.000000000000000,121.000000000000000,0.188113809380047,0.184618919088813,121.000000000000000,0.273236010008633 +0.000000000000000,0.000016000000000,0.000106000000000,0.000145000000000,0.002198000000000,0.000804000000000,0.003318000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1299.000000000000000,184.000000000000000,121.000000000000000,0.189391540723312,0.210953756830538,121.000000000000000,0.312211549881851 +0.000000000000000,0.000019000000000,0.000181000000000,0.000116000000000,0.001662000000000,0.000840000000000,0.002865000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1306.000000000000000,204.000000000000000,121.000000000000000,0.184488742976474,0.215925329354907,121.000000000000000,0.319569479795696 +0.000000000000000,0.000016000000000,0.000183000000000,0.000132000000000,0.001540000000000,0.000737000000000,0.002647000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1313.000000000000000,197.000000000000000,121.000000000000000,0.164667840588859,0.233194073999862,121.000000000000000,0.345127224117476 +0.000000000000000,0.000017000000000,0.000177000000000,0.000144000000000,0.001453000000000,0.000747000000000,0.002725000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1307.000000000000000,179.000000000000000,121.000000000000000,0.190996220324968,0.267097150771334,121.000000000000000,0.395303787014294 +0.000000000000000,0.000017000000000,0.000177000000000,0.000120000000000,0.001414000000000,0.000681000000000,0.002474000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1109.000000000000000,205.000000000000000,121.000000000000000,0.175048358387321,0.227273301247532,121.000000000000000,0.336364481552976 +0.000000000000000,0.000023000000000,0.000186000000000,0.000160000000000,0.001932000000000,0.000738000000000,0.003097000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1100.000000000000000,194.000000000000000,121.000000000000000,0.173557283673669,0.191658131610694,121.000000000000000,0.283654034716855 +0.000000000000000,0.000019000000000,0.000107000000000,0.000116000000000,0.001671000000000,0.000745000000000,0.002708000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,204.000000000000000,121.000000000000000,0.158568907261423,0.243359531069700,121.000000000000000,0.360172098406900 +0.000000000000000,0.000018000000000,0.000103000000000,0.000132000000000,0.001512000000000,0.000732000000000,0.002547000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1187.000000000000000,194.000000000000000,121.000000000000000,0.146749823653506,0.164413086505490,121.000000000000000,0.243331352597904 +0.000000000000000,0.000014000000000,0.000181000000000,0.000144000000000,0.001537000000000,0.000892000000000,0.002808000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,196.000000000000000,121.000000000000000,0.131283592229348,0.155956326816494,121.000000000000000,0.230815358417731 +0.000000000000000,0.000012000000000,0.000176000000000,0.000121000000000,0.001482000000000,0.000877000000000,0.002705000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,196.000000000000000,121.000000000000000,0.159033296053125,0.194395333842714,121.000000000000000,0.287705090036121 +0.000000000000000,0.000020000000000,0.000211000000000,0.000162000000000,0.001881000000000,0.000840000000000,0.003174000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1262.000000000000000,184.000000000000000,121.000000000000000,0.163845159646448,0.177604150932044,121.000000000000000,0.262854149344864 +0.000000000000000,0.000015000000000,0.000115000000000,0.000124000000000,0.001771000000000,0.000924000000000,0.002997000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1269.000000000000000,218.000000000000000,121.000000000000000,0.167741708685618,0.187775885781358,121.000000000000000,0.277908323682058 +0.000000000000000,0.000012000000000,0.000104000000000,0.000127000000000,0.001764000000000,0.000780000000000,0.002830000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,229.000000000000000,121.000000000000000,0.186347950475868,0.251813866360726,121.000000000000000,0.372684503981683 +0.000000000000000,0.000020000000000,0.000102000000000,0.000142000000000,0.001444000000000,0.000872000000000,0.002621000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1275.000000000000000,174.000000000000000,121.000000000000000,0.166303486197133,0.189747084068580,120.000000000000000,0.280825682960661 +0.000000000000000,0.000016000000000,0.000125000000000,0.000117000000000,0.001504000000000,0.000769000000000,0.002568000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,202.000000000000000,121.000000000000000,0.167769122437661,0.195787376215580,121.000000000000000,0.289765303335874 +0.000000000000000,0.000029000000000,0.000158000000000,0.000154000000000,0.001818000000000,0.000771000000000,0.003044000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1304.000000000000000,177.000000000000000,121.000000000000000,0.158463858246833,0.191255881763862,119.000000000000000,0.283058705300228 +0.000000000000000,0.000020000000000,0.000185000000000,0.000118000000000,0.001683000000000,0.000786000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1339.000000000000000,206.000000000000000,121.000000000000000,0.161183566619254,0.178028171732775,121.000000000000000,0.263481700394391 +0.000000000000000,0.000017000000000,0.000177000000000,0.000136000000000,0.001597000000000,0.000782000000000,0.002874000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1383.000000000000000,198.000000000000000,121.000000000000000,0.142916686874711,0.162123928033689,121.000000000000000,0.239943431119633 +0.000000000000000,0.000016000000000,0.000127000000000,0.000234000000000,0.001629000000000,0.000716000000000,0.002906000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,214.000000000000000,121.000000000000000,0.154355717762879,0.218609409653489,121.000000000000000,0.323541914330688 +0.000000000000000,0.000018000000000,0.000175000000000,0.000117000000000,0.001623000000000,0.000874000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,227.000000000000000,121.000000000000000,0.174558038196839,0.199124535422790,121.000000000000000,0.294704311042227 +0.000000000000000,0.000023000000000,0.000101000000000,0.000109000000000,0.001533000000000,0.000703000000000,0.002504000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1228.000000000000000,215.000000000000000,121.000000000000000,0.137995003938898,0.152632389051692,121.000000000000000,0.225895936409808 +0.000000000000000,0.000027000000000,0.000177000000000,0.000139000000000,0.001775000000000,0.000662000000000,0.002820000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,237.000000000000000,121.000000000000000,0.135541904251767,0.167668386817417,121.000000000000000,0.248149206964557 +0.000000000000000,0.000022000000000,0.000181000000000,0.000307000000000,0.001635000000000,0.001033000000000,0.003237000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,200.000000000000000,121.000000000000000,0.163536557849499,0.206044880204936,121.000000000000000,0.304946443542624 +0.000000000000000,0.000013000000000,0.000197000000000,0.000138000000000,0.001693000000000,0.000665000000000,0.002750000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,224.000000000000000,121.000000000000000,0.153373223673703,0.199555553830275,120.000000000000000,0.295342223558018 +0.000000000000000,0.000022000000000,0.000130000000000,0.000127000000000,0.001747000000000,0.000744000000000,0.002816000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,234.000000000000000,121.000000000000000,0.139032943637894,0.186670354479152,121.000000000000000,0.276272121799386 +0.000000000000000,0.000023000000000,0.000189000000000,0.000117000000000,0.001436000000000,0.000905000000000,0.002707000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,191.000000000000000,121.000000000000000,0.139072087006321,0.170137357542569,121.000000000000000,0.251803291766507 +0.000000000000000,0.000019000000000,0.000189000000000,0.000134000000000,0.001636000000000,0.001043000000000,0.003059000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,205.000000000000000,121.000000000000000,0.147221191440148,0.219839242753776,121.000000000000000,0.325362095419246 +0.000000000000000,0.000026000000000,0.000136000000000,0.000141000000000,0.001640000000000,0.000834000000000,0.002814000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,198.000000000000000,121.000000000000000,0.134535729635055,0.177361976863745,120.000000000000000,0.262495743699553 +0.000000000000000,0.000024000000000,0.000188000000000,0.000137000000000,0.001576000000000,0.000999000000000,0.002968000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1224.000000000000000,202.000000000000000,121.000000000000000,0.150440594097383,0.170822892372034,120.000000000000000,0.252817881488855 +0.000000000000000,0.000018000000000,0.000128000000000,0.000123000000000,0.001595000000000,0.000906000000000,0.002923000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,211.000000000000000,121.000000000000000,0.161157776486683,0.176850558454008,121.000000000000000,0.261738825952921 +0.000000000000000,0.000023000000000,0.000171000000000,0.000136000000000,0.001546000000000,0.000836000000000,0.002772000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1094.000000000000000,211.000000000000000,121.000000000000000,0.135044752661059,0.176661372259443,121.000000000000000,0.261458838916931 +0.000000000000000,0.000016000000000,0.000180000000000,0.000129000000000,0.001692000000000,0.000849000000000,0.002919000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1089.000000000000000,203.000000000000000,121.000000000000000,0.143949354754192,0.167668782751111,120.000000000000000,0.248149788217542 +0.000000000000000,0.000017000000000,0.000108000000000,0.000139000000000,0.001568000000000,0.000704000000000,0.002578000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1166.000000000000000,200.000000000000000,121.000000000000000,0.155160670749007,0.231930876186467,120.000000000000000,0.343257712340746 +0.000000000000000,0.000035000000000,0.000227000000000,0.000132000000000,0.001664000000000,0.000790000000000,0.002968000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,195.000000000000000,121.000000000000000,0.133676069237326,0.147626375091364,120.000000000000000,0.218487058408179 +0.000000000000000,0.000015000000000,0.000128000000000,0.000120000000000,0.001551000000000,0.000732000000000,0.002590000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,204.000000000000000,121.000000000000000,0.122400626475427,0.146223124911287,121.000000000000000,0.216410216218663 +0.000000000000000,0.000020000000000,0.000245000000000,0.000118000000000,0.001618000000000,0.000770000000000,0.002815000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,219.000000000000000,121.000000000000000,0.137303784430655,0.150577933818061,121.000000000000000,0.222855351498252 +0.000000000000000,0.000016000000000,0.000214000000000,0.000131000000000,0.001570000000000,0.000888000000000,0.002875000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1239.000000000000000,183.000000000000000,121.000000000000000,0.131464957645487,0.148026075188344,121.000000000000000,0.219078597044465 +0.000000000000000,0.000018000000000,0.000103000000000,0.000139000000000,0.001556000000000,0.000929000000000,0.002796000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,197.000000000000000,121.000000000000000,0.128347170794260,0.131002024287506,121.000000000000000,0.193882998604601 +0.000000000000000,0.000034000000000,0.000281000000000,0.000139000000000,0.001662000000000,0.000725000000000,0.002956000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,194.000000000000000,121.000000000000000,0.162024538919116,0.207094523301695,121.000000000000000,0.306499905326386 +0.000000000000000,0.000019000000000,0.000198000000000,0.000119000000000,0.001621000000000,0.000844000000000,0.002973000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,215.000000000000000,121.000000000000000,0.153235488057770,0.262436341625619,121.000000000000000,0.388405779843294 +0.000000000000000,0.000021000000000,0.000198000000000,0.000131000000000,0.001561000000000,0.000847000000000,0.002828000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1054.000000000000000,201.000000000000000,121.000000000000000,0.157919685289792,0.167838875425546,121.000000000000000,0.248401521304057 +0.000000000000000,0.000015000000000,0.000170000000000,0.000130000000000,0.001648000000000,0.000666000000000,0.002683000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,211.000000000000000,121.000000000000000,0.142424009465490,0.209332762567431,120.000000000000000,0.309812491363346 +0.000000000000000,0.000020000000000,0.000127000000000,0.000139000000000,0.001528000000000,0.000726000000000,0.002588000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,207.000000000000000,121.000000000000000,0.140603779602412,0.159626649619649,121.000000000000000,0.236247445558979 +0.000000000000000,0.000019000000000,0.000194000000000,0.000143000000000,0.001999000000000,0.000624000000000,0.003025000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,205.000000000000000,121.000000000000000,0.147458908627836,0.237281094131071,121.000000000000000,0.351176020676474 +0.000000000000000,0.000022000000000,0.000175000000000,0.000126000000000,0.001454000000000,0.000837000000000,0.002656000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1106.000000000000000,198.000000000000000,121.000000000000000,0.125862132363062,0.146166756283214,120.000000000000000,0.216326793779291 +0.000000000000000,0.000018000000000,0.000233000000000,0.000119000000000,0.001470000000000,0.000923000000000,0.002807000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1145.000000000000000,188.000000000000000,121.000000000000000,0.148866800739627,0.175671536734951,121.000000000000000,0.259993879218198 +0.000000000000000,0.000021000000000,0.000205000000000,0.000128000000000,0.001543000000000,0.000761000000000,0.002708000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1156.000000000000000,186.000000000000000,121.000000000000000,0.127848678819928,0.170790549426913,121.000000000000000,0.252770016569082 +0.000000000000000,0.000016000000000,0.000176000000000,0.000145000000000,0.001515000000000,0.000719000000000,0.002758000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1165.000000000000000,189.000000000000000,121.000000000000000,0.145912191034524,0.246210134411681,121.000000000000000,0.364391034751536 +0.000000000000000,0.000017000000000,0.000137000000000,0.000142000000000,0.001535000000000,0.000625000000000,0.002544000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1007.000000000000000,197.000000000000000,121.000000000000000,0.155415123408675,0.250107101898265,121.000000000000000,0.370158504865490 +0.000000000000000,0.000018000000000,0.000187000000000,0.000129000000000,0.001485000000000,0.000745000000000,0.002609000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,997.000000000000000,196.000000000000000,121.000000000000000,0.156330994449520,0.195977327324711,121.000000000000000,0.290046452877835 +0.000000000000000,0.000027000000000,0.000216000000000,0.000122000000000,0.001463000000000,0.000715000000000,0.002603000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,994.000000000000000,202.000000000000000,121.000000000000000,0.148181823678736,0.163960783045368,121.000000000000000,0.242661951333412 +0.000000000000000,0.000021000000000,0.000148000000000,0.000131000000000,0.001726000000000,0.000788000000000,0.002871000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,990.000000000000000,214.000000000000000,121.000000000000000,0.159041550968813,0.290710356872846,121.000000000000000,0.430251294240852 +0.000000000000000,0.000014000000000,0.000109000000000,0.000146000000000,0.001782000000000,0.000832000000000,0.002935000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,237.000000000000000,121.000000000000000,0.144598980992546,0.166027958406800,121.000000000000000,0.245721363787328 +0.000000000000000,0.000033000000000,0.000225000000000,0.000141000000000,0.001562000000000,0.000724000000000,0.002797000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,200.000000000000000,121.000000000000000,0.138993500529387,0.196990361228138,121.000000000000000,0.291545731774589 +0.000000000000000,0.000018000000000,0.000102000000000,0.000130000000000,0.001577000000000,0.000768000000000,0.002776000000000,1.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1025.000000000000000,218.000000000000000,121.000000000000000,0.159928213604770,0.189984919538914,121.000000000000000,0.281177669551262 +0.000000000000000,0.000012000000000,0.000247000000000,0.000124000000000,0.001584000000000,0.000607000000000,0.002648000000000,0.000000000000000,108.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,232.000000000000000,121.000000000000000,0.169367654114597,0.221725877325533,121.000000000000000,0.328154308402856 +0.000000000000000,0.000029000000000,0.000128000000000,0.000128000000000,0.001642000000000,0.000782000000000,0.002760000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,211.000000000000000,121.000000000000000,0.147968518377667,0.154122927463339,121.000000000000000,0.228101943121478 +0.000000000000000,0.000016000000000,0.000176000000000,0.000142000000000,0.001527000000000,0.000917000000000,0.002821000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,946.000000000000000,207.000000000000000,121.000000000000000,0.137541055415491,0.157761641471241,121.000000000000000,0.233487226761447 +0.000000000000000,0.000032000000000,0.000147000000000,0.000136000000000,0.001548000000000,0.000751000000000,0.002712000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,954.000000000000000,199.000000000000000,121.000000000000000,0.162415187446687,0.197580421079148,121.000000000000000,0.292419026477691 +0.000000000000000,0.000019000000000,0.000178000000000,0.000129000000000,0.001473000000000,0.000728000000000,0.002570000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,952.000000000000000,191.000000000000000,121.000000000000000,0.171041015417160,0.194915117263576,121.000000000000000,0.288474365726664 +0.000000000000000,0.000029000000000,0.000197000000000,0.000115000000000,0.001354000000000,0.000809000000000,0.002551000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,965.000000000000000,175.000000000000000,121.000000000000000,0.154222763807780,0.285544163762687,120.000000000000000,0.422605366108563 +0.000000000000000,0.000019000000000,0.000139000000000,0.000125000000000,0.001460000000000,0.000814000000000,0.002600000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,968.000000000000000,171.000000000000000,121.000000000000000,0.150595603214115,0.221852768512487,121.000000000000000,0.328342103660952 +0.000000000000000,0.000013000000000,0.000104000000000,0.000134000000000,0.001356000000000,0.000752000000000,0.002398000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,996.000000000000000,168.000000000000000,121.000000000000000,0.147430237486138,0.148159285827304,121.000000000000000,0.219275742894069 +0.000000000000000,0.000020000000000,0.000193000000000,0.000142000000000,0.001953000000000,0.000947000000000,0.003301000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1030.000000000000000,177.000000000000000,121.000000000000000,0.145798815037739,0.159663663944367,121.000000000000000,0.236302234970841 +0.000000000000000,0.000015000000000,0.000103000000000,0.000115000000000,0.001290000000000,0.000699000000000,0.002255000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,172.000000000000000,121.000000000000000,0.142196014156506,0.187147109561421,121.000000000000000,0.276977737652069 +0.000000000000000,0.000024000000000,0.000160000000000,0.000150000000000,0.001811000000000,0.000777000000000,0.003271000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1069.000000000000000,202.000000000000000,121.000000000000000,0.139609449101900,0.165271225439012,121.000000000000000,0.244601415643369 +0.000000000000000,0.000018000000000,0.000179000000000,0.000151000000000,0.001532000000000,0.000737000000000,0.002693000000000,0.000000000000000,97.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,203.000000000000000,121.000000000000000,0.147947477270869,0.179555832175793,121.000000000000000,0.265742648149137 +0.000000000000000,0.000019000000000,0.000177000000000,0.000125000000000,0.001512000000000,0.000717000000000,0.002588000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,212.000000000000000,121.000000000000000,0.147575394987053,0.165266294276773,121.000000000000000,0.244594112073245 +0.000000000000000,0.000016000000000,0.000106000000000,0.000137000000000,0.001490000000000,0.000826000000000,0.002618000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,915.000000000000000,203.000000000000000,121.000000000000000,0.154152939014589,0.159181033056470,121.000000000000000,0.235587925595243 +0.000000000000000,0.000013000000000,0.000177000000000,0.000117000000000,0.001355000000000,0.000751000000000,0.002448000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,921.000000000000000,192.000000000000000,121.000000000000000,0.150692609588339,0.183809895950044,121.000000000000000,0.272038654130109 +0.000000000000000,0.000023000000000,0.000221000000000,0.000167000000000,0.001790000000000,0.000948000000000,0.003199000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,932.000000000000000,170.000000000000000,121.000000000000000,0.149963553230709,0.203273297272569,121.000000000000000,0.300844465407444 +0.000000000000000,0.000026000000000,0.000151000000000,0.000122000000000,0.001357000000000,0.000723000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,961.000000000000000,168.000000000000000,121.000000000000000,0.142178288778563,0.151033799800819,121.000000000000000,0.223530034583352 +0.000000000000000,0.000018000000000,0.000155000000000,0.000123000000000,0.001394000000000,0.000766000000000,0.002492000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,986.000000000000000,182.000000000000000,121.000000000000000,0.141207634148801,0.172667793318101,121.000000000000000,0.255548330024774 +0.000000000000000,0.000018000000000,0.000174000000000,0.000143000000000,0.001389000000000,0.000790000000000,0.002550000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1023.000000000000000,171.000000000000000,121.000000000000000,0.140177814193211,0.162740001476768,121.000000000000000,0.240855214879131 +0.000000000000000,0.000021000000000,0.000173000000000,0.000122000000000,0.001280000000000,0.000796000000000,0.002581000000000,1.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1049.000000000000000,171.000000000000000,121.000000000000000,0.138041804635574,0.175894893269503,121.000000000000000,0.260324460535807 +0.000000000000000,0.000040000000000,0.000240000000000,0.000156000000000,0.001610000000000,0.000905000000000,0.003078000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,181.000000000000000,121.000000000000000,0.128640393991057,0.132619727567557,121.000000000000000,0.196277204924269 +0.000000000000000,0.000016000000000,0.000181000000000,0.000117000000000,0.001424000000000,0.000760000000000,0.002543000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,886.000000000000000,172.000000000000000,121.000000000000000,0.138352531751683,0.243902625004102,121.000000000000000,0.360975895470416 +0.000000000000000,0.000020000000000,0.000178000000000,0.000131000000000,0.001344000000000,0.000965000000000,0.002674000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,904.000000000000000,177.000000000000000,121.000000000000000,0.132849298892330,0.165943140041726,121.000000000000000,0.245595863686171 +0.000000000000000,0.000018000000000,0.000129000000000,0.000145000000000,0.001297000000000,0.000878000000000,0.002506000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,916.000000000000000,164.000000000000000,121.000000000000000,0.139100399764650,0.179464095411716,121.000000000000000,0.265606862397320 +0.000000000000000,0.000015000000000,0.000175000000000,0.000119000000000,0.001188000000000,0.000717000000000,0.002249000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,926.000000000000000,158.000000000000000,121.000000000000000,0.124508577433483,0.232688205210390,121.000000000000000,0.344378545000118 +0.000000000000000,0.000041000000000,0.000173000000000,0.000189000000000,0.001590000000000,0.000722000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,950.000000000000000,167.000000000000000,121.000000000000000,0.124364211160562,0.198989442345874,121.000000000000000,0.294504385287040 +0.000000000000000,0.000022000000000,0.000180000000000,0.000121000000000,0.001454000000000,0.000762000000000,0.002586000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,981.000000000000000,174.000000000000000,121.000000000000000,0.121986273502660,0.181985786779231,121.000000000000000,0.269338961458690 +0.000000000000000,0.000022000000000,0.000175000000000,0.000128000000000,0.001283000000000,0.000822000000000,0.002468000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1006.000000000000000,154.000000000000000,121.000000000000000,0.120933033868538,0.208757551950541,121.000000000000000,0.308961183186222 +0.000000000000000,0.000026000000000,0.000175000000000,0.000143000000000,0.001286000000000,0.000813000000000,0.002485000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1029.000000000000000,160.000000000000000,121.000000000000000,0.109269904153490,0.130322654376541,121.000000000000000,0.192877532026941 +0.000000000000000,0.000023000000000,0.000175000000000,0.000120000000000,0.001254000000000,0.000637000000000,0.002411000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,160.000000000000000,121.000000000000000,0.126897062957379,0.186841162125259,121.000000000000000,0.276524916304396 +0.000000000000000,0.000021000000000,0.000140000000000,0.000122000000000,0.001471000000000,0.000691000000000,0.002520000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,891.000000000000000,190.000000000000000,121.000000000000000,0.112694197899670,0.185521960041402,121.000000000000000,0.274572512797542 +0.000000000000000,0.000020000000000,0.000176000000000,0.000199000000000,0.001263000000000,0.000879000000000,0.002581000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,888.000000000000000,161.000000000000000,121.000000000000000,0.099793871260750,0.108940668612828,121.000000000000000,0.161232198827340 +0.000000000000000,0.000018000000000,0.000103000000000,0.000137000000000,0.001309000000000,0.000726000000000,0.002333000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,901.000000000000000,171.000000000000000,121.000000000000000,0.091727983876207,0.161347683449736,121.000000000000000,0.238794571957605 +0.000000000000000,0.000018000000000,0.000175000000000,0.000143000000000,0.001336000000000,0.000860000000000,0.002571000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,909.000000000000000,175.000000000000000,121.000000000000000,0.124196463571817,0.147059251648374,121.000000000000000,0.217647703825064 +0.000000000000000,0.000019000000000,0.000174000000000,0.000125000000000,0.001265000000000,0.000845000000000,0.002464000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,175.000000000000000,121.000000000000000,0.114265472855587,0.148521989180687,121.000000000000000,0.219812542662230 +0.000000000000000,0.000030000000000,0.000158000000000,0.000151000000000,0.001613000000000,0.000756000000000,0.002826000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,943.000000000000000,170.000000000000000,121.000000000000000,0.092242007337942,0.147927216635033,121.000000000000000,0.218932272923301 +0.000000000000000,0.000017000000000,0.000106000000000,0.000120000000000,0.001308000000000,0.001030000000000,0.002622000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,970.000000000000000,159.000000000000000,121.000000000000000,0.106941604326635,0.142972503313410,121.000000000000000,0.211599311615969 +0.000000000000000,0.000021000000000,0.000127000000000,0.000130000000000,0.001305000000000,0.000748000000000,0.002368000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,164.000000000000000,121.000000000000000,0.114683633251297,0.167145956283516,121.000000000000000,0.247376014134327 +0.000000000000000,0.000023000000000,0.000135000000000,0.000297000000000,0.001353000000000,0.000968000000000,0.002824000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1014.000000000000000,170.000000000000000,121.000000000000000,0.103049615014965,0.141054167934770,121.000000000000000,0.208760168771737 +0.000000000000000,0.000020000000000,0.000101000000000,0.000125000000000,0.001114000000000,0.000766000000000,0.002163000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1033.000000000000000,149.000000000000000,121.000000000000000,0.088395574941732,0.099895628586074,121.000000000000000,0.147845538218435 +0.000000000000000,0.000023000000000,0.000190000000000,0.000110000000000,0.001226000000000,0.000774000000000,0.002371000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,162.000000000000000,121.000000000000000,0.098469632616480,0.121321004607324,121.000000000000000,0.179555087237014 +0.000000000000000,0.000022000000000,0.000180000000000,0.000138000000000,0.001374000000000,0.001109000000000,0.002867000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1058.000000000000000,162.000000000000000,121.000000000000000,0.106165831669812,0.125397650867987,120.000000000000000,0.185588518499696 +0.000000000000000,0.000022000000000,0.000177000000000,0.000138000000000,0.001328000000000,0.000617000000000,0.002320000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1063.000000000000000,154.000000000000000,121.000000000000000,0.100439787680905,0.214221605374360,121.000000000000000,0.317047979072175 +0.000000000000000,0.000019000000000,0.000114000000000,0.000131000000000,0.001262000000000,0.000730000000000,0.002294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1073.000000000000000,151.000000000000000,121.000000000000000,0.101286833254115,0.171921771357668,121.000000000000000,0.254444227342640 +0.000000000000000,0.000018000000000,0.000111000000000,0.000120000000000,0.001321000000000,0.000814000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1075.000000000000000,159.000000000000000,121.000000000000000,0.100562747826060,0.189141833234101,121.000000000000000,0.279929921566071 +0.000000000000000,0.000020000000000,0.000137000000000,0.000115000000000,0.001291000000000,0.000697000000000,0.002305000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1102.000000000000000,170.000000000000000,121.000000000000000,0.108513981039663,0.171230581801412,121.000000000000000,0.253421272631674 +0.000000000000000,0.000023000000000,0.000142000000000,0.000126000000000,0.001300000000000,0.000836000000000,0.002476000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1133.000000000000000,146.000000000000000,121.000000000000000,0.093525447667265,0.136448459114485,121.000000000000000,0.201943713921807 +0.000000000000000,0.000018000000000,0.000175000000000,0.000137000000000,0.001338000000000,0.000746000000000,0.002454000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1153.000000000000000,167.000000000000000,121.000000000000000,0.098830315127647,0.135984610426414,121.000000000000000,0.201257228873912 +0.000000000000000,0.000016000000000,0.000153000000000,0.000133000000000,0.001342000000000,0.000846000000000,0.002529000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1169.000000000000000,168.000000000000000,121.000000000000000,0.104911664860530,0.116528427363267,121.000000000000000,0.172462069689336 +0.000000000000000,0.000016000000000,0.000177000000000,0.000153000000000,0.001420000000000,0.001057000000000,0.002862000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,171.000000000000000,121.000000000000000,0.112357270101045,0.155883824635645,121.000000000000000,0.230708054062211 +0.000000000000000,0.000019000000000,0.000183000000000,0.000118000000000,0.001173000000000,0.000692000000000,0.002218000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1205.000000000000000,160.000000000000000,121.000000000000000,0.114134537418471,0.211839504506888,121.000000000000000,0.313522477539393 +0.000000000000000,0.000022000000000,0.000144000000000,0.000340000000000,0.002673000000000,0.001050000000000,0.004300000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1210.000000000000000,169.000000000000000,121.000000000000000,0.118247530316238,0.220707314595291,121.000000000000000,0.326646841420566 +0.000000000000000,0.000021000000000,0.000121000000000,0.000119000000000,0.001312000000000,0.000708000000000,0.002323000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,159.000000000000000,121.000000000000000,0.108077994964789,0.189119078021565,121.000000000000000,0.279896234164773 +0.000000000000000,0.000022000000000,0.000131000000000,0.000170000000000,0.001575000000000,0.000930000000000,0.002873000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,161.000000000000000,121.000000000000000,0.111392372838571,0.157100432720142,121.000000000000000,0.232508649452340 +0.000000000000000,0.000021000000000,0.000129000000000,0.000146000000000,0.001357000000000,0.001069000000000,0.002758000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,169.000000000000000,121.000000000000000,0.115843692072703,0.148606734789863,121.000000000000000,0.219937966947781 +0.000000000000000,0.000018000000000,0.000196000000000,0.000246000000000,0.003021000000000,0.001245000000000,0.004778000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1237.000000000000000,170.000000000000000,121.000000000000000,0.110486981424295,0.140524548316829,121.000000000000000,0.207976323848749 +0.000000000000000,0.000019000000000,0.000176000000000,0.000120000000000,0.001319000000000,0.000781000000000,0.002451000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,166.000000000000000,121.000000000000000,0.116858932300554,0.142202739311708,121.000000000000000,0.210460043128204 +0.000000000000000,0.000023000000000,0.000194000000000,0.000117000000000,0.001352000000000,0.000677000000000,0.002411000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1229.000000000000000,165.000000000000000,121.000000000000000,0.110055898076008,0.160160155663640,121.000000000000000,0.237037039832150 +0.000000000000000,0.000019000000000,0.000128000000000,0.000129000000000,0.001375000000000,0.000783000000000,0.002473000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,170.000000000000000,121.000000000000000,0.126041915882071,0.160651736154074,121.000000000000000,0.237764591667073 +0.000000000000000,0.000024000000000,0.000199000000000,0.000203000000000,0.001799000000000,0.000878000000000,0.003158000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,182.000000000000000,121.000000000000000,0.131395777815393,0.222014190834195,121.000000000000000,0.328581023909914 +0.000000000000000,0.000019000000000,0.000103000000000,0.000125000000000,0.001415000000000,0.000879000000000,0.002580000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,183.000000000000000,121.000000000000000,0.135634727713905,0.187089754575309,121.000000000000000,0.276892849444298 +0.000000000000000,0.000017000000000,0.000101000000000,0.000119000000000,0.001263000000000,0.000774000000000,0.002307000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,171.000000000000000,121.000000000000000,0.140713971719655,0.131521683676958,121.000000000000000,0.194652084756127 +0.000000000000000,0.000016000000000,0.000127000000000,0.000147000000000,0.001449000000000,0.000744000000000,0.002523000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1225.000000000000000,178.000000000000000,121.000000000000000,0.139061294677762,0.203192935815326,121.000000000000000,0.300725561264080 +0.000000000000000,0.000018000000000,0.000191000000000,0.000120000000000,0.001241000000000,0.000897000000000,0.002515000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,165.000000000000000,121.000000000000000,0.137698280788293,0.173674353597993,121.000000000000000,0.257038030883200 +0.000000000000000,0.000018000000000,0.000177000000000,0.000127000000000,0.001350000000000,0.000790000000000,0.002651000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,165.000000000000000,121.000000000000000,0.156706447381789,0.199509549579289,121.000000000000000,0.295274141143093 +0.000000000000000,0.000016000000000,0.000153000000000,0.000147000000000,0.001501000000000,0.000734000000000,0.002643000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1046.000000000000000,206.000000000000000,121.000000000000000,0.145254964747455,0.192623491876607,121.000000000000000,0.285082754555528 +0.000000000000000,0.000017000000000,0.000175000000000,0.000117000000000,0.001356000000000,0.000838000000000,0.002539000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1044.000000000000000,191.000000000000000,121.000000000000000,0.154344725976771,0.284210388757305,121.000000000000000,0.420631380425634 +0.000000000000000,0.000021000000000,0.000118000000000,0.000127000000000,0.001456000000000,0.000725000000000,0.002492000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,193.000000000000000,121.000000000000000,0.154361389589804,0.236960337646421,121.000000000000000,0.350701313890299 +0.000000000000000,0.000018000000000,0.000108000000000,0.000128000000000,0.001548000000000,0.000868000000000,0.002721000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1036.000000000000000,190.000000000000000,121.000000000000000,0.144847037730729,0.155773166896081,121.000000000000000,0.230544292351477 +0.000000000000000,0.000018000000000,0.000129000000000,0.000124000000000,0.001383000000000,0.000975000000000,0.002669000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1027.000000000000000,183.000000000000000,121.000000000000000,0.145841805810321,0.158516249676071,121.000000000000000,0.234604041193658 +0.000000000000000,0.000028000000000,0.000216000000000,0.000131000000000,0.001615000000000,0.000765000000000,0.002878000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1020.000000000000000,192.000000000000000,121.000000000000000,0.144426676201549,0.156437471874113,121.000000000000000,0.231527443868863 +0.000000000000000,0.000021000000000,0.000153000000000,0.000120000000000,0.001406000000000,0.000771000000000,0.002509000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1015.000000000000000,188.000000000000000,121.000000000000000,0.147869059454709,0.168692675134760,120.000000000000000,0.249665165294811 +0.000000000000000,0.000021000000000,0.000170000000000,0.000115000000000,0.001425000000000,0.000608000000000,0.002395000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1012.000000000000000,187.000000000000000,121.000000000000000,0.168987279193652,0.289264692226178,121.000000000000000,0.428111727272626 +0.000000000000000,0.000017000000000,0.000135000000000,0.000127000000000,0.001615000000000,0.000791000000000,0.002734000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1011.000000000000000,192.000000000000000,121.000000000000000,0.170578883496370,0.166353065557405,121.000000000000000,0.246202540171952 +0.000000000000000,0.000019000000000,0.000178000000000,0.000136000000000,0.001520000000000,0.000741000000000,0.002636000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1005.000000000000000,203.000000000000000,121.000000000000000,0.156640038600278,0.207055055847171,121.000000000000000,0.306441476765396 +0.000000000000000,0.000033000000000,0.000177000000000,0.000135000000000,0.001641000000000,0.000861000000000,0.003288000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,999.000000000000000,186.000000000000000,121.000000000000000,0.143274685502394,0.179257874186524,121.000000000000000,0.265301654036128 +0.000000000000000,0.000015000000000,0.000129000000000,0.000142000000000,0.001443000000000,0.000740000000000,0.002555000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,202.000000000000000,121.000000000000000,0.156881076812533,0.218447877056405,121.000000000000000,0.323302867722382 +0.000000000000000,0.000036000000000,0.000177000000000,0.000151000000000,0.001530000000000,0.000725000000000,0.002741000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,820.000000000000000,168.000000000000000,121.000000000000000,0.155951030020751,0.213023655758012,121.000000000000000,0.315275031106667 +0.000000000000000,0.000030000000000,0.000196000000000,0.000128000000000,0.001426000000000,0.000831000000000,0.002660000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,816.000000000000000 \ No newline at end of file diff --git a/GPU_version/vio/test/data/.gitignore b/GPU_version/vio/test/data/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/GPU_version/vio/test/data/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/GPU_version/vio/test/results/.gitignore b/GPU_version/vio/test/results/.gitignore new file mode 100644 index 0000000..cf57c3b --- /dev/null +++ b/GPU_version/vio/test/results/.gitignore @@ -0,0 +1,2 @@ +.gitignore +!.gitignore \ No newline at end of file diff --git a/GPU_version/vio/test/test_depth_filter.cpp b/GPU_version/vio/test/test_depth_filter.cpp new file mode 100644 index 0000000..4d22493 --- /dev/null +++ b/GPU_version/vio/test/test_depth_filter.cpp @@ -0,0 +1,206 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +struct ConvergedSeed { + int x_, y_; + double depth_, error_; + ConvergedSeed(int x, int y, double depth, double error) : + x_(x), y_(y), depth_(depth), error_(error) + {} +}; + +/// DepthFilter Test-Fixture +class DepthFilterTest { + public: + DepthFilterTest(); + virtual ~DepthFilterTest(); + void depthFilterCb(svo::Point* point, double depth_sigma2); + void testReconstruction(std::string dataset_dir, std::string experiment_name); + + std::list results_; + std::vector errors_; + size_t n_converged_seeds_; + vk::PinholeCamera* cam_; + svo::DepthFilter* depth_filter_; + svo::FramePtr frame_ref_; + svo::FramePtr frame_cur_; + cv::Mat depth_ref_; +}; + +DepthFilterTest::DepthFilterTest() : + n_converged_seeds_(0), + cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)), + depth_filter_(NULL) +{ + errors_.reserve(1000); +} + +DepthFilterTest::~DepthFilterTest() +{ + delete cam_; +} + +void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2) +{ + double depth = (frame_ref_->pos() - point->pos_).norm(); + double error = fabs(depth - depth_ref_.at(point->obs_.front()->px[1], + point->obs_.front()->px[0])); + results_.push_back(ConvergedSeed( + point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error)); + errors_.push_back(error); + delete point->obs_.front(); + delete point; + ++n_converged_seeds_; +} + +void DepthFilterTest::testReconstruction( + std::string dataset_dir, + std::string experiment_name) +{ + vk::FileReader sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + std::cout << "RUN EXPERIMENT: read " << sequence.size() << " dataset entries." << std::endl; + std::vector::iterator it = sequence.begin(); + vk::Timer t; + std::list n_converged_per_iteration; + + svo::feature_detection::DetectorPtr feature_detector( + new svo::feature_detection::FastDetector( + cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels())); + svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2); + depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb); + depth_filter_->options_.verbose = true; + + for(int i=0; it != sequence.end() && i<20; ++it, ++i) + { + std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png"); + printf("reading image: '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + Sophus::SE3 T_w_f(it->q_, it->t_); + if(i == 0) + { + // create reference frame and load ground truth depthmap + frame_ref_ = boost::make_shared(cam_, img, 0.0); + frame_ref_->T_f_w_ = T_w_f.inverse(); + depth_filter_->addKeyframe(frame_ref_, 2, 0.5); + vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_); + continue; + } + + n_converged_seeds_ = 0; + frame_cur_ = boost::make_shared(cam_, img, 0.0); + frame_cur_->T_f_w_ = T_w_f.inverse(); + depth_filter_->addFrame(frame_cur_); + n_converged_per_iteration.push_back(n_converged_seeds_); + } + printf("Experiment '%s' took %f ms\n", experiment_name.c_str(), t.stop()*1000); + + // compute mean, median and variance of error in converged area + { + printf("# converged: \t %zu (ref: 287)\n", errors_.size()); + double sum_error = 0; + std::for_each(errors_.begin(), errors_.end(), [&](double& e){sum_error+=e;}); + printf("mean error: \t %f cm (ref: 0.080357)\n", sum_error*100/errors_.size()); + std::vector::iterator it = errors_.begin()+0.5*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("50-percentile: \t %f cm (ref: 0.062042)\n", *it*100); + it = errors_.begin()+0.8*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("80-percentile: \t %f cm (ref: 0.124526)\n", *it*100); + it = errors_.begin()+0.95*errors_.size(); + std::nth_element(errors_.begin(), it, errors_.end()); + printf("95-percentile: \t %f cm (ref: 0.200417)\n", *it*100); + } + + // trace error + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt"); + std::ofstream ofs(trace_name.c_str()); + for(std::list::iterator i=results_.begin(); i!=results_.end(); ++i) + ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl; + ofs.close(); + + // trace convergence rate + trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt"; + ofs.open(trace_name.c_str()); + for(std::list::iterator it=n_converged_per_iteration.begin(); + it!=n_converged_per_iteration.end(); ++it) + ofs << *it << std::endl; + ofs.close(); + + // write ply file for pointcloud visualization in Meshlab + trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply"; + ofs.open(trace_name.c_str()); + ofs << "ply" << std::endl + << "format ascii 1.0" << std::endl + << "element vertex " << results_.size() << std::endl + << "property float x" << std::endl + << "property float y" << std::endl + << "property float z" << std::endl + << "property uchar blue" << std::endl + << "property uchar green" << std::endl + << "property uchar red" << std::endl + << "end_header" << std::endl; + + for(std::list::iterator i=results_.begin(); i!=results_.end(); ++i) + { + cv::Vec3b c = frame_ref_->img_pyr_[0].at(i->y_, i->x_); + Eigen::Vector3d p = cam_->cam2world(i->x_, i->y_)*i->depth_; + ofs << p[0] << " " << p[1] << " " << p[2] << " " + << (int) c[0] << " " << (int) c[1] << " " << (int) c[2] << std::endl; + } +} + +} // namespace + +int main(int argc, char** argv) +{ + DepthFilterTest test; + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string experiment_name("sin2_tex2_h1_v8_d"); + test.testReconstruction(dataset_dir, experiment_name); + return 0; +} diff --git a/GPU_version/vio/test/test_feature_alignment.cpp b/GPU_version/vio/test/test_feature_alignment.cpp new file mode 100644 index 0000000..dac8911 --- /dev/null +++ b/GPU_version/vio/test/test_feature_alignment.cpp @@ -0,0 +1,124 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include "test_utils.h" +#if __ARM_NEON__ +#include +#endif + +using namespace Eigen; + +void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px, + cv::Mat& ref_patch_with_border) +{ + // compute interpolation weights + const int u_r = floor(px[0]); + const int v_r = floor(px[1]); + const float subpix_u = px[0]-u_r; + const float subpix_v = px[1]-v_r; + const float wTL = (1.0-subpix_u)*(1.0-subpix_v); + const float wTR = subpix_u * (1.0-subpix_v); + const float wBL = (1.0-subpix_u)*subpix_v; + const float wBR = subpix_u * subpix_v; + + // loop through search_patch, interpolate + ref_patch_with_border = cv::Mat(10, 10, CV_8UC1); + uint8_t* patch_ptr = ref_patch_with_border.data; + const int stride = img.step.p[0]; + for(int y=0; y<10; ++y) + { + uint8_t* img_ptr = (uint8_t*) img.data + (v_r+y-4-1)*stride + u_r-4-1; + for(int x=0; x<10; ++x, ++patch_ptr, ++img_ptr) + *patch_ptr = wTL*img_ptr[0] + wTR*img_ptr[1] + wBL*img_ptr[stride] + wBR*img_ptr[stride+1]; + } +} + +int main(int argc, char **argv) +{ + + std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(img.type() == CV_8UC1); + + // TODO: test on corner/gradient features! + Vector2d px_true(130.2,120.3); + + // create reference patch with border + cv::Mat ref_patch_with_border; + generateRefPatchNoWarpInterpolate(img, px_true, ref_patch_with_border); + + // create reference patch, aligned + uint8_t* ref_patch = vk::aligned_mem::aligned_alloc(64, 16); + uint8_t* ref_patch_ptr = ref_patch; + for(int y=1; y<9; ++y) + { + uint8_t* ref_patch_border_ptr = ref_patch_with_border.data + y*10 + 1; + for(int x=0; x<8; ++x, ++ref_patch_border_ptr, ++ref_patch_ptr) + *ref_patch_ptr = *ref_patch_border_ptr; + } + + Vector2d px_est, px_error(-1.1, -0.8); + double h_inv; + vk::Timer t; + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + Vector2f dir = px_error.normalized().cast(); + svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv); + } + Vector2d e = px_est-px_true; + printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.982000ms, 0.000033px) \n", t.stop()*1000, e.norm()); + + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D(img, ref_patch_with_border.data, ref_patch, 3, px_est, true); + } + e = px_est-px_true; + printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.306000ms, 0.015102px)\n", t.stop()*1000, e.norm()); + +#ifdef __SSE2__ + // Note, this KLT implementation is not invariant to illuminatino changes! + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D_SSE2(img, ref_patch_with_border.data, ref_patch, 3, px_est); + } + e = px_est-px_true; + printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.460000ms, 0.021881px)\n", t.stop()*1000, e.norm()); +#endif + +#ifdef __ARM_NEON__ + t.start(); + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + svo::feature_alignment::align2D_NEON(img, ref_patch_with_border.data, ref_patch, 3, px_est); + } + e = px_est-px_true; + printf("1000Xalign 2D NEON %fms, error = %fpx \t (ref Odroid-U2: ?, ?)\n", t.stop()*1000, e.norm()); +#endif + + return 0; +} diff --git a/GPU_version/vio/test/test_feature_detection.cpp b/GPU_version/vio/test/test_feature_detection.cpp new file mode 100644 index 0000000..3ba2b2f --- /dev/null +++ b/GPU_version/vio/test/test_feature_detection.cpp @@ -0,0 +1,74 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +using namespace Eigen; +using namespace std; + +void testCornerDetector() +{ + std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(img.type() == CV_8UC1 && !img.empty()); + + vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092); + svo::FramePtr frame(new svo::Frame(cam, img, 0.0)); + + // Corner detection + vk::Timer t; + svo::Features fts; + svo::feature_detection::FastDetector fast_detector( + img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels()); + for(int i=0; i<100; ++i) + { + fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts); + } + printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 7.166360ms, 40000)\n", t.stop()*10, fts.size()); + printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n"); + cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3); + cv::cvtColor(img, img_rgb, CV_GRAY2RGB); + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ + cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1); + }); + cv::imshow("ref_img", img_rgb); + cv::waitKey(0); + + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; }); +} + +} // namespace + + +int main(int argc, char **argv) +{ + testCornerDetector(); + return 0; +} diff --git a/GPU_version/vio/test/test_matcher.cpp b/GPU_version/vio/test/test_matcher.cpp new file mode 100644 index 0000000..0b65cb9 --- /dev/null +++ b/GPU_version/vio/test/test_matcher.cpp @@ -0,0 +1,200 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +/// Matcher Test-Fixture +class MatcherTest { + public: + MatcherTest() + { + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + + // load images + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string img_name(dataset_dir+"/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img_ref(cv::imread(img_name, 0)); + img_name = std::string(dataset_dir+"/img/frame_000006_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img_cur(cv::imread(img_name, 0)); + assert(!img_ref.empty() && !img_cur.empty()); + + // create frame + frame_ref_ = new svo::Frame(cam_, img_ref, 1.0); + frame_cur_ = new svo::Frame(cam_, img_cur, 2.0); + ref_ftr_ = new svo::Feature(frame_ref_, Eigen::Vector2d(300, 260), 0); + + // set poses + Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000); + Eigen::Vector3d t_w_cur(0.5673, 0.5641, 2.0000); + Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0); + Eigen::Quaterniond q_w_cur(0.0, 0.8235, 0.2130, 0.0); + frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse(); + frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur, t_w_cur).inverse(); + + // load ground-truth depth + vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000002_0.depth", *cam_, depth_ref_); + } + + virtual ~MatcherTest() + { + delete cam_; + delete frame_ref_; + delete frame_cur_; + delete ref_ftr_; + } + + // Individual tests + void testEpipolarSearchFullImg(); + void testWarpAffine(); + + // Objects declared here can be used by all tests + vk::PinholeCamera* cam_; + svo::Frame* frame_ref_; + svo::Frame* frame_cur_; + svo::Feature* ref_ftr_; + cv::Mat depth_ref_; +}; + +void MatcherTest::testEpipolarSearchFullImg() +{ + svo::Matcher matcher; + cv::Mat depthmap(frame_ref_->cam_->height(), frame_ref_->cam_->width(), CV_32FC1, cv::Scalar(0)); + cv::Mat error_map(depthmap.rows, depthmap.cols, CV_32FC1, cv::Scalar(0)); + cv::Mat mask(depthmap.rows, depthmap.cols, CV_8UC1, cv::Scalar(1)); + double depth_range = 0.8; + size_t n_converged = 0; + double sum_error = 0; + std::vector errors; errors.reserve(depthmap.cols*depthmap.rows); + for(int y=4; y(y, x); + double depth_estimate; + int res = matcher.findEpipolarMatchDirect( + *frame_ref_, *frame_cur_, ftr, depth_gt, fmax(depth_gt-depth_range,0.0), + depth_gt+depth_range, depth_estimate); + if(res) + { + depthmap.at(y,x) = depth_estimate; + error_map.at(y,x) = fabsf(depth_estimate-depth_gt); + ++n_converged; + sum_error += error_map.at(y,x); + errors.push_back(error_map.at(y,x)); + } + else + mask.at(y,x) = 0; + } + } + + // compute mean, median and variance of error in converged area + printf("n converged: \t %zu mm (ref: 216114)\n", n_converged); + printf("mean error: \t %f mm (ref: 0.410084)\n", sum_error*100/n_converged); + std::vector::iterator it = errors.begin()+0.5*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("50-percentile: \t %f mm (ref: 0.083203)\n", *it*100); + it = errors.begin()+0.8*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("80-percentile: \t %f mm (ref: 0.161824)\n", *it*100); + it = errors.begin()+0.95*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("95-percentile: \t %f mm (ref: 0.263539)\n", *it*100); + + // save results to file + std::ofstream output_stream; + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string output_filename(trace_dir + "/depthmap.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float)); + output_stream.close(); + + output_filename.assign(trace_dir + "/depthmap_error.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float)); + output_stream.close(); + + output_filename.assign(trace_dir + "/depthmap_mask.bin"); + output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); + output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t)); + output_stream.close(); +} + +void MatcherTest::testWarpAffine() +{ + const int halfpatch_size = 15; + const int patch_size=halfpatch_size*2; + svo::Matcher matcher; + double depth = 1.0; + + Sophus::SE3 T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse()); + Eigen::Vector2d px_cur(frame_cur_->cam_->world2cam(T_cur_ref*(ref_ftr_->f*depth))); + + // compute reference patch + cv::Mat ref_patch(matcher.halfpatch_size_+1, matcher.halfpatch_size_+1, + CV_8U, matcher.patch_with_border_); + + Eigen::Matrix2d A_cur_ref; + svo::warp::getWarpMatrixAffine( + *cam_, *cam_, ref_ftr_->px, ref_ftr_->f, 1.0, T_cur_ref, ref_ftr_->level, A_cur_ref); + int level_cur = svo::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1); + svo::warp::warpAffine( + A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, level_cur, matcher.halfpatch_size_+1, matcher.patch_with_border_); + + // copy current patch + Eigen::Vector2i pxi(px_cur[0]/(1<img_pyr_.at(level_cur)(cv::Range(pxi[1]-halfpatch_size, + pxi[1]+halfpatch_size), + cv::Range(pxi[0]-halfpatch_size, + pxi[0]+halfpatch_size)).copyTo(cur_patch); + + // display results + cv::circle(frame_cur_->img_pyr_[level_cur], + cv::Point2f(pxi[0], pxi[1]), + patch_size, cv::Scalar(255), 2); + cv::circle(frame_ref_->img_pyr_[ref_ftr_->level], + cv::Point2f(ref_ftr_->px[0]/(1<level), ref_ftr_->px[1]/(1<level)), + patch_size, cv::Scalar(255), 2); + cv::imshow("cur_patch", cur_patch); + cv::imshow("ref_patch", ref_patch); + cv::imshow("ref_img", frame_ref_->img_pyr_[ref_ftr_->level]); + cv::imshow("cur_img", frame_cur_->img_pyr_[level_cur]); + cv::waitKey(0); +} + +} // namespace + +int main(int argc, char** argv) +{ + MatcherTest test; + test.testEpipolarSearchFullImg(); +// test.testWarpAffine(); + return 0; +} diff --git a/GPU_version/vio/test/test_pipeline.cpp b/GPU_version/vio/test/test_pipeline.cpp new file mode 100644 index 0000000..ede3b25 --- /dev/null +++ b/GPU_version/vio/test/test_pipeline.cpp @@ -0,0 +1,98 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace svo { + +class BenchmarkNode +{ + vk::AbstractCamera* cam_; + svo::FrameHandlerMono* vo_; + +public: + BenchmarkNode(); + ~BenchmarkNode(); + void runFromFolder(); +}; + +BenchmarkNode::BenchmarkNode() +{ + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + vo_ = new svo::FrameHandlerMono(cam_); + vo_->start(); +} + +BenchmarkNode::~BenchmarkNode() +{ + delete vo_; + delete cam_; +} + +void BenchmarkNode::runFromFolder() +{ + for(int img_id = 2; img_id < 188; ++img_id) + { + // load image + std::stringstream ss; + ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_" + << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png"; + if(img_id == 2) + std::cout << "reading image " << ss.str() << std::endl; + cv::Mat img(cv::imread(ss.str().c_str(), 0)); + assert(!img.empty()); + + // process frame + vo_->addImage(img, 0.01*img_id); + + // display tracking quality + if(vo_->lastFrame() != NULL) + { + std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" + << "#Features: " << vo_->lastNumObservations() << " \t" + << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n"; + + // access the pose of the camera via vo_->lastFrame()->T_f_w_. + } + } +} + +} // namespace svo + +int main(int argc, char** argv) +{ + { + svo::BenchmarkNode benchmark; + benchmark.runFromFolder(); + } + printf("BenchmarkNode finished.\n"); + return 0; +} + diff --git a/GPU_version/vio/test/test_pose_optimizer.cpp b/GPU_version/vio/test/test_pose_optimizer.cpp new file mode 100644 index 0000000..59e0b46 --- /dev/null +++ b/GPU_version/vio/test/test_pose_optimizer.cpp @@ -0,0 +1,110 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +using namespace svo; +using namespace Eigen; + +/// Pose Optimizer Test-Fixture +class PoseOptimizerTest { + public: + PoseOptimizerTest() + { + cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); + + // load image + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + std::string img_name(dataset_dir+"/img/frame_000002_0.png"); + printf("Loading image '%s'\n", img_name.c_str()); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + // create frame + frame_.reset(new svo::Frame(cam_, img, 1.0)); + + // set pose + Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000); + Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0); + frame_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse(); + + // load ground-truth depth + vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000002_0.depth", *cam_, depthmap_); + + // detect features + feature_detection::FastDetector detector( + cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()); + detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_); + size_t n_fts = 0; + std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){ + Point* point(new Point(frame_->f2w(i->f*depthmap_.at(i->px[1], i->px[0])), i)); + i->point = point; + ++n_fts; + }); + printf("Added %zu features to frame.\n", n_fts); + } + + virtual ~PoseOptimizerTest() + { + delete cam_; + } + + // Individual tests + void test(const Vector3d& pose_disturbance, double pixel_sigma2); + + // Objects declared here can be used by all tests + vk::PinholeCamera* cam_; + FramePtr frame_; + cv::Mat depthmap_; +}; + +void PoseOptimizerTest::test(const Vector3d& pose_disturbance, double pixel_sigma2) +{ + printf("Add %f px noise to each observation\n", pixel_sigma2); + for(Features::iterator it=frame_->fts_.begin(); it!=frame_->fts_.end(); ++it) + { + (*it)->px += Vector2d(vk::Sample::gaussian(pixel_sigma2), vk::Sample::gaussian(pixel_sigma2)); + (*it)->f = frame_->c2f((*it)->px); + } + frame_->T_f_w_ = frame_->T_f_w_*SE3(Matrix3d::Identity(), pose_disturbance); + double estimated_scale, error_init, error_final; + size_t num_obs; + pose_optimizer::optimizeGaussNewton( + Config::reprojThresh(), 10, true, frame_, + estimated_scale, error_init, error_final, num_obs); +} + +} // namespace + +int main(int argc, char** argv) +{ + PoseOptimizerTest test; + test.test(Eigen::Vector3d(0.2, 0.2, 0.2), 1.0); + return 0; +} diff --git a/GPU_version/vio/test/test_sparse_img_align.cpp b/GPU_version/vio/test/test_sparse_img_align.cpp new file mode 100644 index 0000000..1312da0 --- /dev/null +++ b/GPU_version/vio/test/test_sparse_img_align.cpp @@ -0,0 +1,163 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_utils.h" + +namespace { + +/// SparseImgAlign Test-Fixture +class SparseImgAlignTest { + public: + + SparseImgAlignTest(); + virtual ~SparseImgAlignTest(); + void testSequence( + const std::string& dataset_dir, + const std::string& experiment_name, + svo::feature_detection::AbstractDetector* feature_detector); + + vk::PinholeCamera* cam_; + svo::FramePtr frame_ref_; + svo::FramePtr frame_cur_; +}; + +SparseImgAlignTest::SparseImgAlignTest() : + cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)) +{} + +SparseImgAlignTest::~SparseImgAlignTest() +{} + +void SparseImgAlignTest::testSequence( + const std::string& dataset_dir, + const std::string& experiment_name, + svo::feature_detection::AbstractDetector* feature_detector) +{ + vk::FileReader sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + printf("RUN EXPERIMENT: read %zu dataset entries.\n", sequence.size()); + std::vector::iterator iter = sequence.begin(); + std::list translation_error; + + Sophus::SE3 T_prev_w, T_prevgt_w; + std::string trace_dir(svo::test_utils::getTraceDir()); + std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt"); + std::ofstream ofs(trace_name.c_str()); + for(int i=0; iter != sequence.end() && i<30; ++iter, ++i) + { + // load img + // IMPORTANT: We need to flip the image because the Povray dataset has a + // negative focal length in y direction which we didn't set. + std::string img_name(dataset_dir+"/img/" + iter->image_name_ + "_0.png"); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + // load pose + Sophus::SE3 T_w_gt(iter->q_, iter->t_); + Sophus::SE3 T_gt_w = T_w_gt.inverse(); // ground-truth + + if(i==0) + { + // set reference frame + frame_ref_.reset(new svo::Frame(cam_, img, 0.0)); + frame_ref_->T_f_w_ = T_gt_w; + + // load ground-truth depth + cv::Mat depthmap; + vk::blender_utils::loadBlenderDepthmap( + dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap); + + // extract features, generate features with 3D points + feature_detector->detect( + frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_); + std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) { + Eigen::Vector3d pt_pos_cur = i->f*depthmap.at(i->px[1], i->px[0]); + Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur; + svo::Point* pt = new svo::Point(pt_pos_w, i); + i->point = pt; + }); + + printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs()); + T_prev_w = frame_ref_->T_f_w_; + T_prevgt_w = T_gt_w; + continue; + } + + frame_cur_.reset(new svo::Frame(cam_, img, 0.0)); + //frame_cur_->T_f_w_ = frame_ref_->T_f_w_; // start at reference frame + frame_cur_->T_f_w_ = T_prev_w; // start at last frame + + // run image align + vk::Timer t; + svo::SparseImgAlign img_align(svo::Config::kltMaxLevel(), svo::Config::kltMinLevel(), + 30, svo::SparseImgAlign::GaussNewton, false, false); + img_align.run(frame_ref_, frame_cur_); + + // compute error + Sophus::SE3 T_f_gt = frame_cur_->T_f_w_ * T_gt_w.inverse(); + translation_error.push_back(T_f_gt.translation().norm()); + printf("[%3.i] time = %f ms \t |t| = %f \t translation error = %f\n", + i, t.stop()*1000, (frame_ref_->T_f_w_*T_gt_w.inverse()).translation().norm(), + translation_error.back()); + + // save old pose for next iteration + T_prev_w = frame_cur_->T_f_w_; + T_prevgt_w = T_gt_w; + + ofs << frame_cur_->T_f_w_.inverse().translation().transpose() << " " + << T_gt_w.inverse().translation().transpose() << std::endl; + } + ofs.close(); + + // trace error + trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt"; + ofs.open(trace_name.c_str()); + for(std::list::iterator it=translation_error.begin(); it!=translation_error.end(); ++it) + ofs << *it << std::endl; + ofs.close(); +} + +} // namespace + + +int main(int argc, char** argv) +{ + std::string experiment_name("flying_room_1_rig_1_fast_minlev0"); + std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d"); + svo::Config::triangMinCornerScore() = 20; + svo::Config::kltMinLevel() = 0; + SparseImgAlignTest test; + svo::feature_detection::FastDetector detector( + test.cam_->width(), test.cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()); + test.testSequence(dataset_dir, experiment_name, &detector); + return 0; +} diff --git a/GPU_version/vio/test/test_utils.h b/GPU_version/vio/test/test_utils.h new file mode 100644 index 0000000..2759fe0 --- /dev/null +++ b/GPU_version/vio/test/test_utils.h @@ -0,0 +1,57 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO 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 any later version. +// +// SVO 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 . + +#ifndef TEST_UTILS_H_ +#define TEST_UTILS_H_ + +#include +#include // for getenv +#ifdef SVO_USE_ROS +# include +# include +#endif + +namespace svo { +namespace test_utils { + +std::string getDatasetDir() +{ + const char* env_dir = std::getenv("SVO_DATASET_DIR"); +#ifdef SVO_USE_ROS + std::string dataset_dir(ros::package::getPath("svo")+"/test/data"); + if(env_dir != NULL) + dataset_dir = std::string(env_dir); + return dataset_dir; +#else + return std::string(env_dir); +#endif +} + +std::string getTraceDir() +{ +#ifdef SVO_USE_ROS + std::string default_dir(ros::package::getPath("svo")+"/test/results"); + return vk::getParam("svo/trace_dir", default_dir); +#else + return "/tmp"; +#endif +} + +} // namespace test_utils +} // namespace svo + + +#endif // TEST_UTILS_H_