Rewrite README with comprehensive project documentation

- Added detailed project overview and feature descriptions
- Included complete dependency list with build instructions
- Added system architecture overview and component descriptions
- Documented configuration options and usage instructions
- Added troubleshooting section for common issues
- Included known limitations and future improvements
- Improved formatting and clarity throughout

Co-Authored-By: Claude Haiku 4.5 <[email protected]>
Claude-Session: https://claude.ai/code/session_01GPXVpiPnb5AzgdCJ33Q6Fi

.gitignore

@@ -6,7 +6,7 @@
 # Node artifact files
 node_modules/
 dist/
-
+/GPU_version/vio/cmake-build-debug/
 # Compiled Java class files
 *.class
 

CPU_version/LICENCE

@@ -1,675 +0,0 @@
-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}.
-

CPU_version/README.md

@@ -1,64 +0,0 @@
-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
-
-    <!-- IMU 2 Camera Frame -->
-    <param name="init_rx" value="1.88496" /><!--  90 + 18 -->
-    <param name="init_ry" value="0.0" /> <!--  0 -->
-    <param name="init_rz" value="0.0" /> <!-- 0  -->
-    
-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
-    

CPU_version/svo/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

CPU_version/svo/CMakeLists.txt

@@ -1,156 +0,0 @@
-################################################################################
-# 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)

CPU_version/svo/doc/notation.ipe

@@ -1,312 +0,0 @@
-<?xml version="1.0"?>
-<!DOCTYPE ipe SYSTEM "ipe.dtd">
-<ipe version="70005" creator="Ipe 7.1.1">
-<info created="D:20120312120943" modified="D:20140611144527"/>
-<ipestyle name="basic">
-<symbol name="arrow/arc(spx)">
-<path stroke="sym-stroke" fill="sym-stroke" pen="sym-pen">
-0 0 m
--1 0.333 l
--1 -0.333 l
-h
-</path>
-</symbol>
-<symbol name="arrow/farc(spx)">
-<path stroke="sym-stroke" fill="white" pen="sym-pen">
-0 0 m
--1 0.333 l
--1 -0.333 l
-h
-</path>
-</symbol>
-<symbol name="mark/circle(sx)" transformations="translations">
-<path fill="sym-stroke">
-0.6 0 0 0.6 0 0 e
-0.4 0 0 0.4 0 0 e
-</path>
-</symbol>
-<symbol name="mark/disk(sx)" transformations="translations">
-<path fill="sym-stroke">
-0.6 0 0 0.6 0 0 e
-</path>
-</symbol>
-<symbol name="mark/fdisk(sfx)" transformations="translations">
-<group>
-<path fill="sym-stroke" fillrule="eofill">
-0.6 0 0 0.6 0 0 e
-0.4 0 0 0.4 0 0 e
-</path>
-<path fill="sym-fill">
-0.4 0 0 0.4 0 0 e
-</path>
-</group>
-</symbol>
-<symbol name="mark/box(sx)" transformations="translations">
-<path fill="sym-stroke" fillrule="eofill">
--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
-</path>
-</symbol>
-<symbol name="mark/square(sx)" transformations="translations">
-<path fill="sym-stroke">
--0.6 -0.6 m
-0.6 -0.6 l
-0.6 0.6 l
--0.6 0.6 l
-h
-</path>
-</symbol>
-<symbol name="mark/fsquare(sfx)" transformations="translations">
-<group>
-<path fill="sym-stroke" fillrule="eofill">
--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
-</path>
-<path fill="sym-fill">
--0.4 -0.4 m
-0.4 -0.4 l
-0.4 0.4 l
--0.4 0.4 l
-h
-</path>
-</group>
-</symbol>
-<symbol name="mark/cross(sx)" transformations="translations">
-<group>
-<path fill="sym-stroke">
--0.43 -0.57 m
-0.57 0.43 l
-0.43 0.57 l
--0.57 -0.43 l
-h
-</path>
-<path fill="sym-stroke">
--0.43 0.57 m
-0.57 -0.43 l
-0.43 -0.57 l
--0.57 0.43 l
-h
-</path>
-</group>
-</symbol>
-<symbol name="arrow/fnormal(spx)">
-<path stroke="sym-stroke" fill="white" pen="sym-pen">
-0 0 m
--1 0.333 l
--1 -0.333 l
-h
-</path>
-</symbol>
-<symbol name="arrow/pointed(spx)">
-<path stroke="sym-stroke" fill="sym-stroke" pen="sym-pen">
-0 0 m
--1 0.333 l
--0.8 0 l
--1 -0.333 l
-h
-</path>
-</symbol>
-<symbol name="arrow/fpointed(spx)">
-<path stroke="sym-stroke" fill="white" pen="sym-pen">
-0 0 m
--1 0.333 l
--0.8 0 l
--1 -0.333 l
-h
-</path>
-</symbol>
-<symbol name="arrow/linear(spx)">
-<path stroke="sym-stroke" pen="sym-pen">
--1 0.333 m
-0 0 l
--1 -0.333 l
-</path>
-</symbol>
-<symbol name="arrow/fdouble(spx)">
-<path stroke="sym-stroke" fill="white" pen="sym-pen">
-0 0 m
--1 0.333 l
--1 -0.333 l
-h
--1 0 m
--2 0.333 l
--2 -0.333 l
-h
-</path>
-</symbol>
-<symbol name="arrow/double(spx)">
-<path stroke="sym-stroke" fill="sym-stroke" pen="sym-pen">
-0 0 m
--1 0.333 l
--1 -0.333 l
-h
--1 0 m
--2 0.333 l
--2 -0.333 l
-h
-</path>
-</symbol>
-<pen name="heavier" value="0.8"/>
-<pen name="fat" value="1.2"/>
-<pen name="ultrafat" value="2"/>
-<symbolsize name="large" value="5"/>
-<symbolsize name="small" value="2"/>
-<symbolsize name="tiny" value="1.1"/>
-<arrowsize name="large" value="10"/>
-<arrowsize name="small" value="5"/>
-<arrowsize name="tiny" value="3"/>
-<color name="red" value="1 0 0"/>
-<color name="green" value="0 1 0"/>
-<color name="blue" value="0 0 1"/>
-<color name="yellow" value="1 1 0"/>
-<color name="orange" value="1 0.647 0"/>
-<color name="gold" value="1 0.843 0"/>
-<color name="purple" value="0.627 0.125 0.941"/>
-<color name="gray" value="0.745"/>
-<color name="brown" value="0.647 0.165 0.165"/>
-<color name="navy" value="0 0 0.502"/>
-<color name="pink" value="1 0.753 0.796"/>
-<color name="seagreen" value="0.18 0.545 0.341"/>
-<color name="turquoise" value="0.251 0.878 0.816"/>
-<color name="violet" value="0.933 0.51 0.933"/>
-<color name="darkblue" value="0 0 0.545"/>
-<color name="darkcyan" value="0 0.545 0.545"/>
-<color name="darkgray" value="0.663"/>
-<color name="darkgreen" value="0 0.392 0"/>
-<color name="darkmagenta" value="0.545 0 0.545"/>
-<color name="darkorange" value="1 0.549 0"/>
-<color name="darkred" value="0.545 0 0"/>
-<color name="lightblue" value="0.678 0.847 0.902"/>
-<color name="lightcyan" value="0.878 1 1"/>
-<color name="lightgray" value="0.827"/>
-<color name="lightgreen" value="0.565 0.933 0.565"/>
-<color name="lightyellow" value="1 1 0.878"/>
-<dashstyle name="dashed" value="[4] 0"/>
-<dashstyle name="dotted" value="[1 3] 0"/>
-<dashstyle name="dash dotted" value="[4 2 1 2] 0"/>
-<dashstyle name="dash dot dotted" value="[4 2 1 2 1 2] 0"/>
-<textsize name="large" value="\large"/>
-<textsize name="Large" value="\Large"/>
-<textsize name="LARGE" value="\LARGE"/>
-<textsize name="huge" value="\huge"/>
-<textsize name="Huge" value="\Huge"/>
-<textsize name="small" value="\small"/>
-<textsize name="footnote" value="\footnotesize"/>
-<textsize name="tiny" value="\tiny"/>
-<textstyle name="center" begin="\begin{center}" end="\end{center}"/>
-<textstyle name="itemize" begin="\begin{itemize}" end="\end{itemize}"/>
-<textstyle name="item" begin="\begin{itemize}\item{}" end="\end{itemize}"/>
-<gridsize name="4 pts" value="4"/>
-<gridsize name="8 pts (~3 mm)" value="8"/>
-<gridsize name="16 pts (~6 mm)" value="16"/>
-<gridsize name="32 pts (~12 mm)" value="32"/>
-<gridsize name="10 pts (~3.5 mm)" value="10"/>
-<gridsize name="20 pts (~7 mm)" value="20"/>
-<gridsize name="14 pts (~5 mm)" value="14"/>
-<gridsize name="28 pts (~10 mm)" value="28"/>
-<gridsize name="56 pts (~20 mm)" value="56"/>
-<anglesize name="90 deg" value="90"/>
-<anglesize name="60 deg" value="60"/>
-<anglesize name="45 deg" value="45"/>
-<anglesize name="30 deg" value="30"/>
-<anglesize name="22.5 deg" value="22.5"/>
-<tiling name="falling" angle="-60" step="4" width="1"/>
-<tiling name="rising" angle="30" step="4" width="1"/>
-</ipestyle>
-<page>
-<layer name="alpha"/>
-<layer name="beta"/>
-<view layers="alpha beta" active="alpha"/>
-<text layer="alpha" matrix="1 0 0 1 42.5463 55.5916" transformations="translations" pos="185.118 575.11" stroke="darkgreen" type="label" width="11.68" height="4.293" depth="2.21" valign="baseline">$_w\mathtt{p}$</text>
-<text matrix="1 0 0 1 37.0859 58.3179" transformations="translations" pos="64 560" stroke="black" type="label" width="22.245" height="14.072" depth="2.15" valign="baseline" size="Large">$\overrightarrow{\mathcal{F}}_w$</text>
-<path matrix="1 0 0 1 64.3979 50.4555" stroke="black" pen="heavier" arrow="normal/small">
-64 576 m
-96 576 l
-</path>
-<path matrix="1 0 0 1 64.3979 50.4555" stroke="black" pen="heavier" arrow="normal/small">
-64 576 m
-64 608 l
-</path>
-<path matrix="1 0 0 1 64.3979 50.4555" stroke="black" pen="heavier" arrow="normal/small">
-64 576 m
-80 592 l
-</path>
-<path matrix="-0.999969 -0.00790914 0.00790914 -0.999969 456.342 1448.77" stroke="black" pen="heavier" rarrow="normal/small">
-144 688 m
-176 688 l
-</path>
-<path matrix="-0.999969 -0.00790914 0.00790914 -0.999969 423.99 1448.96" stroke="black" pen="heavier" arrow="normal/small">
-144 688 m
-144 720 l
-</path>
-<path matrix="-0.999969 -0.00790914 0.00790914 -0.999969 423.99 1448.96" stroke="black" pen="heavier" arrow="normal/small">
-144 688 m
-160 704 l
-</path>
-<path stroke="red" arrow="normal/small">
-285.262 759.615 m
-162.017 735.456
-129.232 628.801 s
-</path>
-<use matrix="1 0 0 1 -17.1996 59.6288" name="mark/disk(sx)" pos="340.469 588.657" size="large" stroke="black"/>
-<text matrix="1 0 0 1 48.3271 131.44" transformations="translations" pos="124.125 605.151" stroke="red" type="label" width="23.445" height="8.766" depth="4.09" valign="baseline" size="Large">$\mathtt{T}_{f,w}$</text>
-<path stroke="darkgreen" pen="heavier" arrow="normal/small">
-128.441 626.745 m
-320.625 647.224 l
-</path>
-<path matrix="1 0 0 1 -16.1315 58.5084" stroke="darkgreen">
-331.681 587.399 m
-331.681 587.399 l
-</path>
-<text matrix="1 0 0 1 72.9974 12.8488" transformations="translations" pos="238.52 751.212" stroke="black" type="label" width="5.694" height="4.289" depth="0" valign="baseline">$x$</text>
-<text matrix="1 0 0 1 38.6847 -20.4129" transformations="translations" pos="238.52 751.212" stroke="black" type="label" width="5.071" height="4.289" depth="0" valign="baseline">$z$</text>
-<text matrix="1 0 0 1 24.5045 -3.95686" transformations="translations" pos="238.52 751.212" stroke="black" type="label" width="5.242" height="4.297" depth="1.93" valign="baseline">$y$</text>
-<path matrix="1 0 0 1 0 -16" stroke="black">
-256 736 m
-224 704 l
-320 704 l
-352 736 l
-h
-</path>
-<path stroke="black" arrow="normal/normal">
-285.262 759.265 m
-322.176 651.119 l
-</path>
-<text matrix="1 0 0 1 -5.60204 -7.00256" transformations="translations" pos="313.272 711.998" stroke="purple" type="label" width="14.765" height="6.175" depth="3.19" valign="baseline" size="Large">$\mathtt{px}$</text>
-<path stroke="blue" pen="heavier" arrow="normal/normal">
-285.612 758.915 m
-315.723 669.632 l
-</path>
-<text matrix="1 0 0 1 -11.0998 -37.7176" transformations="translations" pos="313.272 711.998" stroke="blue" type="label" width="7.382" height="8.767" depth="0" valign="baseline" size="Large">$\mathtt{f}$</text>
-<text matrix="1 0 0 1 210.304 207.851" transformations="translations" pos="64 560" stroke="black" type="label" width="20.795" height="14.069" depth="4.09" valign="baseline" size="Large">$\overrightarrow{\mathcal{F}}_f$</text>
-<path stroke="purple" pen="heavier" arrow="normal/tiny">
-255.668 719.918 m
-274.728 719.957 l
-</path>
-<path stroke="purple" pen="heavier" arrow="normal/tiny">
-256.107 720.028 m
-246.655 710.527 l
-</path>
-<text matrix="1 0 0 1 7.21843 -34.9205" transformations="translations" pos="238.52 751.212" stroke="purple" type="label" width="5.187" height="4.289" depth="0" valign="baseline">$v$</text>
-<text matrix="1 0 0 1 23.3779 -29.12" transformations="translations" pos="238.52 751.212" stroke="purple" type="label" width="5.703" height="4.289" depth="0" valign="baseline">$u$</text>
-<use name="mark/disk(sx)" pos="303.819 704.295" size="normal" stroke="purple"/>
-<text matrix="1 0 0 1 15.6911 -69.9004" transformations="translations" pos="313.272 711.998" stroke="black" type="label" width="8.966" height="6.376" depth="2.79" valign="baseline" size="Large">$\mathbf{p}$</text>
-<text matrix="1 0 0 1 136.413 88.5801" transformations="translations" pos="185.118 575.11" stroke="black" type="label" width="10.395" height="4.29" depth="2.85" valign="baseline">$_f\mathtt{p}$</text>
-</page>
-</ipe>

CPU_version/svo/include/svo/bundle_adjustment.h

@@ -1,109 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_BUNDLE_ADJUSTMENT_H_
-#define SVO_BUNDLE_ADJUSTMENT_H_
-
-#include <svo/global.h>
-
-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<FramePtr>* 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_

CPU_version/svo/include/svo/depth_filter.h

@@ -1,171 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_DEPTH_FILTER_H_
-#define SVO_DEPTH_FILTER_H_
-
-#include <queue>
-#include <boost/thread.hpp>
-#include <boost/function.hpp>
-#include <vikit/performance_monitor.h>
-#include <svo/global.h>
-#include <svo/feature_detection.h>
-#include <svo/matcher.h>
-
-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<boost::mutex> lock_t;
-  typedef boost::function<void ( Point*, double )> 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<Seed>& seeds);
-
-  /// Return a reference to the seeds. This is NOT THREAD SAFE!
-  //std::list<Seed>& getSeeds() { return seeds_; } Bug 238
-  std::list<Seed, Eigen::aligned_allocator<Seed>>& 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<Seed> seeds_;
-  std::list<Seed, Eigen::aligned_allocator<Seed>> 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<FramePtr> 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_

CPU_version/svo/include/svo/feature.h

@@ -1,75 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_FEATURE_H_
-#define SVO_FEATURE_H_
-
-#include <svo/frame.h>
-
-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_

CPU_version/svo/include/svo/feature_alignment.h

@@ -1,63 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_FEATURE_ALIGNMENT_H_
-#define SVO_FEATURE_ALIGNMENT_H_
-
-#include <svo/global.h>
-
-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_

CPU_version/svo/include/svo/feature_detection.h

@@ -1,107 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_FEATURE_DETECTION_H_
-#define SVO_FEATURE_DETECTION_H_
-
-#include <svo/global.h>
-#include <svo/frame.h>
-
-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<Corner> 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<bool> grid_occupancy_;
-
-  void resetGrid();
-
-  inline int getCellIndex(int x, int y, int level)
-  {
-    const int scale = (1<<level);
-    return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
-  }
-};
-typedef boost::shared_ptr<AbstractDetector> 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_

CPU_version/svo/include/svo/frame.h

@@ -1,154 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_FRAME_H_
-#define SVO_FRAME_H_
-
-#include <sophus/se3.h>
-#include <vikit/math_utils.h>
-#include <vikit/abstract_camera.h>
-#include <boost/noncopyable.hpp>
-#include <svo/global.h>
-
-namespace g2o {
-class VertexSE3Expmap;
-}
-typedef g2o::VertexSE3Expmap g2oFrameSE3;
-
-namespace svo {
-
-class Point;
-struct Feature;
-
-typedef list<Feature*> Features;
-typedef vector<cv::Mat> 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<double, 6, 6>          Cov_;                   //!< Covariance.
-  ImgPyr                        img_pyr_;               //!< Image Pyramid.
-  Features                      fts_;                   //!< List of features in the image.
-  vector<Feature*>              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<double,2,6>& 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_

CPU_version/svo/include/svo/frame_handler_base.h

@@ -1,123 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_FRAME_HANDLER_BASE_H_
-#define SVO_FRAME_HANDLER_BASE_H_
-
-#include <queue>
-#include <vikit/timer.h>
-#include <vikit/ringbuffer.h>
-#include <boost/noncopyable.hpp>
-#include <boost/function.hpp>
-#include <boost/thread.hpp>
-#include <svo/global.h>
-#include <svo/map.h>
-
-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<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
-  vk::RingBuffer<size_t> 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_

CPU_version/svo/include/svo/frame_handler_mono.h

@@ -1,104 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_FRAME_HANDLER_H_
-#define SVO_FRAME_HANDLER_H_
-
-#include <set>
-#include <vikit/abstract_camera.h>
-#include <svo/frame_handler_base.h>
-#include <svo/reprojector.h>
-#include <svo/initialization.h>
-#include <svo/imu_integration.h>
-
-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<FramePtr>& coreKeyframes() { return core_kfs_; }
-
-  /// Return the feature track to visualize the KLT tracking during initialization.
-  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
-  const vector<cv::Point2f>& 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_Integration> 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<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
-  vector< pair<FramePtr,size_t> > 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_

CPU_version/svo/include/svo/global.h

@@ -1,101 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_GLOBAL_H_
-#define SVO_GLOBAL_H_
-
-#include <list>
-#include <vector>
-#include <string>
-#include <stdint.h>
-#include <stdio.h>
-#include <math.h>
-
-#include <Eigen/Core>
-#include <opencv2/opencv.hpp>
-#include <sophus/se3.h>
-#include <vikit/performance_monitor.h>
-#include <boost/shared_ptr.hpp>
-#include<Eigen/StdVector>
-#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 <ros/console.h>
-  #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] "<<x<<"\033[0;0m"<<std::endl;
-  #ifdef SVO_DEBUG_OUTPUT
-    #define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
-  #else
-    #define SVO_DEBUG_STREAM(x)
-  #endif
-  #define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
-  #define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
-  #include <chrono> // 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<std::chrono::system_clock> __log_stream_throttle__now__ = \
-      std::chrono::system_clock::now(); \
-      if (__log_stream_throttle__last_hit__ + rate <= \
-          std::chrono::duration_cast<std::chrono::seconds>( \
-          __log_stream_throttle__now__.time_since_epoch()).count()) { \
-        __log_stream_throttle__last_hit__ = \
-        std::chrono::duration_cast<std::chrono::seconds>( \
-        __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<Frame> FramePtr;
-} // namespace svo
-
-#endif // SVO_GLOBAL_H_

CPU_version/svo/include/svo/imu_integration.h

@@ -1,121 +0,0 @@
-//
-// Created by gufe on 4/9/21.
-//
-
-#ifndef SVO_IMU_INTEGRATION_H
-#define SVO_IMU_INTEGRATION_H
-#include <gtsam/navigation/CombinedImuFactor.h>
-#include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/nonlinear/ISAM2.h>
-#include <sophus/se3.h>
-#include <svo/frame.h>
-
-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 <typename T>
-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 <typename T>
-enumerate_impl<T> enumerate(T & t)
-{
-    return enumerate_impl<T>(t);
-}
-
-class Imu_Integration{
-public:
-    Imu_Integration(Sophus::SE3& SE_init);
-    ~Imu_Integration();
-    bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
-    bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
-    bool update(double* imu= nullptr);
-    bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
-private:
-    std::shared_ptr<gtsam::NavState> statePtr;
-    std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
-    std::shared_ptr<gtsam::Values> valuesPtr;
-    std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
-    boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
-    std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
-    boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
-    std::chrono::steady_clock::time_point t_1;
-    std::uint32_t imu_factor_id=0;
-    uint imu_n=0;
-    std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
-    bool syn=false;
-};
-#endif //SVO_IMU_INTEGRATION_H

CPU_version/svo/include/svo/initialization.h

@@ -1,83 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_INITIALIZATION_H
-#define SVO_INITIALIZATION_H
-
-#include <svo/global.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() {};
-  ~KltHomographyInit() {};
-  InitResult addFirstFrame(FramePtr frame_ref);
-  InitResult addSecondFrame(FramePtr frame_ref);
-  void reset();
-
-protected:
-  vector<cv::Point2f> px_ref_;      //!< keypoints to be tracked in reference frame.
-  vector<cv::Point2f> px_cur_;      //!< tracked keypoints in current frame.
-  vector<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
-  vector<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
-  vector<double> disparities_;      //!< disparity between first and second frame.
-  vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
-  vector<Vector3d> 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<cv::Point2f>& px_vec,
-    vector<Vector3d>& f_vec);
-
-/// Compute optical flow (Lucas Kanade) for selected keypoints.
-void trackKlt(
-    FramePtr frame_ref,
-    FramePtr frame_cur,
-    vector<cv::Point2f>& px_ref,
-    vector<cv::Point2f>& px_cur,
-    vector<Vector3d>& f_ref,
-    vector<Vector3d>& f_cur,
-    vector<double>& disparities);
-
-void computeHomography(
-    const vector<Vector3d>& f_ref,
-    const vector<Vector3d>& f_cur,
-    double focal_length,
-    double reprojection_threshold,
-    vector<int>& inliers,
-    vector<Vector3d>& xyz_in_cur,
-    SE3& T_cur_from_ref);
-
-} // namespace initialization
-} // namespace svo
-
-#endif // SVO_INITIALIZATION_H

CPU_version/svo/include/svo/map.h

@@ -1,136 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_MAP_H_
-#define SVO_MAP_H_
-
-#include <queue>
-#include <boost/noncopyable.hpp>
-#include <boost/thread.hpp>
-#include <svo/global.h>
-
-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<Point*, Feature*> PointCandidate;
-  typedef list<PointCandidate> 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<FramePtr,double> >& 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_

CPU_version/svo/include/svo/matcher.h

@@ -1,130 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_MATCHER_H_
-#define SVO_MATCHER_H_
-
-#include <svo/global.h>
-
-namespace vk {
-  class AbstractCamera;
-  namespace patch_score {
-    template<int HALF_PATCH_SIZE> 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<halfpatch_size_> 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_

CPU_version/svo/include/svo/point.h

@@ -1,108 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_POINT_H_
-#define SVO_POINT_H_
-
-#include <boost/noncopyable.hpp>
-#include <svo/global.h>
-
-namespace g2o {
-  class VertexSBAPointXYZ;
-}
-typedef g2o::VertexSBAPointXYZ g2oPoint;
-
-namespace svo {
-
-class Feature;
-
-typedef Matrix<double, 2, 3> 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<Feature*>              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_

CPU_version/svo/include/svo/pose_optimizer.h

@@ -1,50 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_POSE_OPTIMIZER_H_
-#define SVO_POSE_OPTIMIZER_H_
-
-#include <svo/global.h>
-
-namespace svo {
-
-using namespace Eigen;
-using namespace Sophus;
-using namespace std;
-
-typedef Matrix<double,6,6> Matrix6d;
-typedef Matrix<double,2,6> Matrix26d;
-typedef Matrix<double,6,1> 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_

CPU_version/svo/include/svo/reprojector.h

@@ -1,101 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_REPROJECTION_H_
-#define SVO_REPROJECTION_H_
-
-#include <svo/global.h>
-#include <svo/matcher.h>
-
-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<FramePtr,std::size_t> >& 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<Candidate > Cell;
-  typedef std::vector<Cell*> CandidateGrid;
-
-  /// The grid stores a set of candidate matches. For every grid cell we try to find one match.
-  struct Grid
-  {
-    CandidateGrid cells;
-    vector<int> 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_

CPU_version/svo/include/svo/sparse_img_align.h

@@ -1,83 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef SVO_SPARSE_IMG_ALIGN_H_
-#define SVO_SPARSE_IMG_ALIGN_H_
-
-#include <vikit/nlls_solver.h>
-#include <vikit/performance_monitor.h>
-#include <svo/global.h>
-
-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<double, 6, 6> 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<double, 6, Dynamic, ColMajor> jacobian_cache_;
-  bool have_ref_patch_cache_;
-  cv::Mat ref_patch_cache_;
-  std::vector<bool> 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_

CPU_version/svo/include/svo/svoVisionFactor.h

@@ -1,70 +0,0 @@
-//
-// Created by root on 4/13/21.
-//
-
-#ifndef SVO_SVOVISIONFACTOR_H
-#define SVO_SVOVISIONFACTOR_H
-#pragma once
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/base/Matrix.h>
-#include <gtsam/base/Vector.h>
-#include <gtsam/geometry/Pose3.h>
-#include <vikit/robust_cost.h>
-#include <vikit/math_utils.h>
-#include <svo/frame.h>
-
-namespace svo {
-
-    class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
-
-    private:
-        // measurement information
-        gtsam::Point3 feature;   ///<Unit-bearing vector of the feature.
-        gtsam::Point3 landmark;   ///<Unit-bearing vector of the feature.
-        boost::optional<gtsam::Point3> 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<VisionFactor> 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<gtsam::Point3> body_P_sensor = boost::none) :
-                gtsam::NoiseModelFactor1<gtsam::Pose3>(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<gtsam::Matrix&> 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<<level);
-            if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
-            return e;
-        }
-        // The second is a 'clone' function that allows the factor to be copied. Under most
-        // circumstances, the following code that employs the default copy constructor should
-        // work fine.
-        virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-            return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-                    gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); }
-
-    };
-
-} // namespace gtsamexamples
-#endif //SVO_SVOVISIONFACTOR_H

CPU_version/svo/package.xml

@@ -1,27 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>svo</name>
-  <version>0.1.0</version>
-  <description>
-    Monocular Visual Odometry
-  </description>
-  <author>Christian Forster</author>
-  <maintainer email="[email protected]">Christian Forster</maintainer>
-  <license>GPLv3</license>
-
-  <!-- Dependencies which this package needs to build itself. -->
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <!-- Dependencies needed to compile this package. -->
-  <build_depend>cmake_modules</build_depend> <!-- for FindEigen.cmake -->
-  <build_depend>roscpp</build_depend>
-  <build_depend>roslib</build_depend>
-  <build_depend>vikit_common</build_depend>
-  <build_depend>vikit_ros</build_depend>
-
-  <!-- Dependencies needed after this package is compiled. -->
-  <run_depend>roscpp</run_depend>
-  <run_depend>roslib</run_depend>
-  <run_depend>vikit_common</run_depend>
-  <run_depend>vikit_ros</run_depend>
-</package>

CPU_version/svo/src/bundle_adjustment.cpp

@@ -1,435 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <vikit/math_utils.h>
-#include <boost/thread.hpp>
-#include <g2o/core/sparse_optimizer.h>
-#include <g2o/core/block_solver.h>
-#include <g2o/core/solver.h>
-#include <g2o/core/robust_kernel_impl.h>
-#include <g2o/core/optimization_algorithm_levenberg.h>
-#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
-#include <g2o/solvers/csparse/linear_solver_csparse.h>
-#include <g2o/solvers/dense/linear_solver_dense.h>
-#include <g2o/types/sba/types_six_dof_expmap.h>
-#include <g2o/solvers/structure_only/structure_only_solver.h>
-#include <svo/bundle_adjustment.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
-#include <svo/map.h>
-
-#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<EdgeContainerSE3> 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<EdgeContainerSE3>::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<FramePtr>* 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<EdgeContainerSE3> edges;
-  set<Point*> mps;
-  list<Frame*> 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<FramePtr>::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<Point*>::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<Feature*>::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<g2o::OptimizableGraph::Vertex*>(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<FramePtr>::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<Frame*>::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it)
-    (*it)->v_kf_ = NULL;
-
-  // Update Mappoints
-  for(set<Point*>::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<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
-  {
-    if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<<it->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<EdgeContainerSE3> edges;
-  list< pair<FramePtr,Feature*> > 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<FramePtr>::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<FramePtr,Feature*>(*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<FramePtr>::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<FramePtr,Feature*> >::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<EdgeContainerSE3>::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<g2o::BlockSolver_6_3::PoseMatrixType>();
-  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
-
-  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<g2o::BlockSolverX::PoseMatrixType>();
-  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
-  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<g2o::OptimizableGraph::Vertex*>(v_point));
-  e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(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

CPU_version/svo/src/config.cpp

@@ -1,102 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifdef SVO_USE_ROS
-#include <vikit/params_helper.h>
-#endif
-#include <svo/config.h>
-
-namespace svo {
-
-Config::Config() :
-#ifdef SVO_USE_ROS
-    trace_name(vk::getParam<string>("svo/trace_name", "svo")),
-    trace_dir(vk::getParam<string>("svo/trace_dir", "/tmp")),
-    n_pyr_levels(vk::getParam<int>("svo/n_pyr_levels", 3)),
-    use_imu(vk::getParam<bool>("svo/use_imu", false)),
-    core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
-    map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
-    grid_size(vk::getParam<int>("svo/grid_size", 30)),
-    init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
-    init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
-    init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),
-    klt_max_level(vk::getParam<int>("svo/klt_max_level", 4)),
-    klt_min_level(vk::getParam<int>("svo/klt_min_level", 2)),
-    reproj_thresh(vk::getParam<double>("svo/reproj_thresh", 2.0)),
-    poseoptim_thresh(vk::getParam<double>("svo/poseoptim_thresh", 2.0)),
-    poseoptim_num_iter(vk::getParam<int>("svo/poseoptim_num_iter", 10)),
-    structureoptim_max_pts(vk::getParam<int>("svo/structureoptim_max_pts", 20)),
-    structureoptim_num_iter(vk::getParam<int>("svo/structureoptim_num_iter", 5)),
-    loba_thresh(vk::getParam<double>("svo/loba_thresh", 2.0)),
-    loba_robust_huber_width(vk::getParam<double>("svo/loba_robust_huber_width", 1.0)),
-    loba_num_iter(vk::getParam<int>("svo/loba_num_iter", 0)),
-    kfselect_mindist(vk::getParam<double>("svo/kfselect_mindist", 0.12)),
-    triang_min_corner_score(vk::getParam<double>("svo/triang_min_corner_score", 20.0)),
-    triang_half_patch_size(vk::getParam<int>("svo/triang_half_patch_size", 4)),
-    subpix_n_iter(vk::getParam<int>("svo/subpix_n_iter", 10)),
-    max_n_kfs(vk::getParam<int>("svo/max_n_kfs", 10)),
-    img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
-    max_fts(vk::getParam<int>("svo/max_fts", 120)),
-    quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
-    quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)),
-    ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)),
-    GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)),
-    iuc(vk::getParam<double>("svo/IUC", 1e-1)),
-    abc(vk::getParam<double>("svo/ABC", 1e-1)),
-    gbc(vk::getParam<double>("svo/GBC", 1e-1)),
-    ebp(vk::getParam<double>("svo/EBP", 1e-2)),
-    nrp(vk::getParam<double>("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
-

CPU_version/svo/src/depth_filter.cpp

@@ -1,352 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <algorithm>
-#include <vikit/math_utils.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/vision.h>
-#include <boost/bind.hpp>
-#include <boost/math/distributions/normal.hpp>
-#include <svo/global.h>
-#include <svo/depth_filter.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/matcher.h>
-#include <svo/config.h>
-#include <svo/feature_detection.h>
-
-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 "<<new_features.size()<<" new seeds");
-  seeds_updating_halt_ = false;
-}
-
-void DepthFilter::removeKeyframe(FramePtr frame)
-{
-  seeds_updating_halt_ = true;
-  lock_t lock(seeds_mut_);
-  std::list<Seed>::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<Seed>::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<int>())) {
-      ++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<Seed>& seeds)
-{
-  lock_t lock(seeds_mut_);
-  for(std::list<Seed>::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<float> 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

CPU_version/svo/src/feature_alignment.cpp

@@ -1,601 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifdef __SSE2__
-#include <emmintrin.h>
-#endif
-#ifdef __ARM_NEON__
-#include <arm_neon.h>
-#endif
-#include <svo/feature_alignment.h>
-
-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<patch_size; ++y)
-  {
-    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
-    for(int x=0; x<patch_size; ++x, ++it, ++it_dv)
-    {
-      Vector2f J;
-      J[0] = 0.5*(dir[0]*(it[1] - it[-1]) + dir[1]*(it[ref_step] - it[-ref_step]));
-      J[1] = 1;
-      *it_dv = J[0];
-      H += J*J.transpose();
-    }
-  }
-  h_inv = 1.0/H(0,0)*patch_size*patch_size;
-  Matrix2f Hinv = H.inverse();
-  float mean_diff = 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];
-  float chi2 = 0;
-  Vector2f update; update.setZero();
-  for(int iter = 0; iter<n_iter; ++iter)
-  {
-    int u_r = floor(u);
-    int v_r = floor(v);
-    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= 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<patch_size; ++y)
-    {
-      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
-      for(int x=0; x<patch_size; ++x, ++it, ++it_ref, ++it_ref_dv)
-      {
-        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
-        float res = search_pixel - *it_ref + mean_diff;
-        Jres[0] -= res*(*it_ref_dv);
-        Jres[1] -= res;
-        new_chi2 += res*res;
-      }
-    }
-
-    if(iter > 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<patch_size_; ++y)
-  {
-    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
-    for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
-    {
-      Vector3f J;
-      J[0] = 0.5 * (it[1] - it[-1]);
-      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
-      J[2] = 1;
-      *it_dx = J[0];
-      *it_dy = J[1];
-      H += J*J.transpose();
-    }
-  }
-  Matrix3f Hinv = H.inverse();
-  float mean_diff = 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];
-//  float chi2 = 0;
-  Vector3f update; update.setZero();
-  for(int iter = 0; iter<n_iter; ++iter)
-  {
-    int u_r = floor(u);
-    int v_r = floor(v);
-    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= 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<patch_size_; ++y)
-    {
-      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
-      for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
-      {
-        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
-        float res = search_pixel - *it_ref + mean_diff;
-        Jres[0] -= res*(*it_ref_dx);
-        Jres[1] -= res*(*it_ref_dy);
-        Jres[2] -= res;
-//        new_chi2 += res*res;
-      }
-    }
-
-
-/*
-    if(iter > 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<patch_size; ++y)
-  {
-    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
-    for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
-    {
-      int16_t dx = static_cast<int16_t>(it[1]) - it[-1];
-      int16_t dy = static_cast<int16_t>(it[ref_step]) - it[-ref_step];
-      *it_dx = dx;
-      *it_dy = dy;  // we are missing a factor 1/2
-      A11 += static_cast<float>(dx*dx); // we are missing a factor 1/4
-      A12 += static_cast<float>(dx*dy);
-      A22 += static_cast<float>(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<n_iter; ++iter)
-  {
-    int u_r = floor(u);
-    int v_r = floor(v);
-    if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= 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<int>((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS));
-    int wTR = static_cast<int>(subpix_x * (1.0f-subpix_y)*(1 << W_BITS));
-    int wBL = static_cast<int>((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<patch_size; ++y)
-    {
-      const uint8_t* it  = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
-
-      // Iptr is aligned!
-      //__m128i diff0 = _mm_load_si128((const __m128i*)(ref_patch + y*8));
-      __m128i diff = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(ref_patch + y*8)), z);
-
-      // load the lower 64 bits and unpack [8u 0 8u 0..]
-      __m128i v00 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it)), z);
-      __m128i v01 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + 1)), z);
-      __m128i v10 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step)), z);
-      __m128i v11 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step + 1)), z);
-
-      // interpolate top row and bottom row
-      // _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers in a with the lower 4 signed or unsigned 16-bit integers in b.
-      // _mm_madd_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
-      __m128i t0 = _mm_add_epi32(_mm_madd_epi16(_mm_unpacklo_epi16(v00, v01), qw0),
-                                 _mm_madd_epi16(_mm_unpacklo_epi16(v10, v11), qw1));
-      __m128i t1 = _mm_add_epi32(_mm_madd_epi16(_mm_unpackhi_epi16(v00, v01), qw0),
-                                 _mm_madd_epi16(_mm_unpackhi_epi16(v10, v11), qw1));
-
-      // _mm_srai_epi32: Shifts the 4 signed 32-bit integers in A right by count bits while shifting in the sign bit.
-      t0 = _mm_srai_epi32(_mm_add_epi32(t0, qdelta), W_BITS); // adding qdelta is for rounding closest int
-      t1 = _mm_srai_epi32(_mm_add_epi32(t1, qdelta), W_BITS);
-
-      // compute 8xres:
-      // _mm_packs_epi32: Packs the 8 signed 32-bit integers from a and b into signed 16-bit integers and saturates.
-      diff = _mm_subs_epi16(_mm_packs_epi32(t0, t1), diff);
-
-      // load gradient dX and dY, both are aligned!
-      v00 = _mm_load_si128((const __m128i*)(ref_patch_dx + y*patch_size)); // [dx1, dx2, dx3, dx4 ...]
-      v01 = _mm_load_si128((const __m128i*)(ref_patch_dy + y*patch_size)); // [dy1, dy2, dy3, dy4 ...]
-
-      // _mm_mulhi_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
-      v10 = _mm_mullo_epi16(v00, diff); // Packs the lower 16 bits of the 8 signed 32-bit results. [15:0]
-      v11 = _mm_mulhi_epi16(v00, diff); // Packs the upper 16 bits of the 8 signed 32-bit results. [31:16]
-
-      // _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers with the lower 4 signed or unsigned 16-bit integers in b.
-      v00 = _mm_unpacklo_epi16(v10, v11);
-      v10 = _mm_unpackhi_epi16(v10, v11);
-
-      // convert to float and add to dx
-      qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v00));
-      qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v10));
-
-      // same with dY
-      v10 = _mm_mullo_epi16(v01, diff);
-      v11 = _mm_mulhi_epi16(v01, diff);
-      v00 = _mm_unpacklo_epi16(v10, v11);
-      v10 = _mm_unpackhi_epi16(v10, v11);
-      qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v00));
-      qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v10));
-    }
-
-    float __attribute__((__aligned__(16))) buf[4];
-    _mm_store_ps(buf, qb0);
-    b1 += buf[0]+buf[1]+buf[2]+buf[3];
-    _mm_store_ps(buf, qb1);
-    b2 += buf[0]+buf[1]+buf[2]+buf[3];
-#endif
-
-    // compute -A^-1*b
-    update_u = ((A12*b2 - A22*b1) * Dinv)*2; // * 2 to compensate because above, we did not compute the derivative correctly
-    update_v = ((A12*b1 - A11*b2) * Dinv)*2;
-    u += update_u;
-    v += update_v;
-
-#if SUBPIX_VERBOSE
-    cout << "Iter " << iter << ":"
-         << "\t u=" << u << ", v=" << v
-         << "\t update = " << update_u << ", " << update_v
-         << "\t new chi2 = " << new_chi2 << endl;
-#endif
-
-    if(update_u*update_u+update_v*update_v < min_update_squared)
-    {
-#if SUBPIX_VERBOSE
-      cout << "converged." << endl;
-#endif
-      converged=true;
-      break;
-    }
-    chi2 = new_chi2;
-  }
-
-  cur_px_estimate << u, v;
-  return converged;
-}
-
-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)
-{
-  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;
-  Matrix3f H; H.setZero();
-  for(int y=0; y<patch_size; ++y)
-  {
-    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
-    for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
-    {
-      *it_dx = static_cast<int16_t>(it[1] - it[-1]);
-      *it_dy = static_cast<int16_t>(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<n_iter; ++iter)
-  {
-    int u_r = floor(u);
-    int v_r = floor(v);
-    if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= 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<uint16_t>((1.0f-subpix_x)*(1.0f-subpix_y)*(1<<SHIFT_BITS));
-    const uint16_t wTR = static_cast<uint16_t>(subpix_x*(1.0f-subpix_y)*(1<<SHIFT_BITS));
-    const uint16_t wBL = static_cast<uint16_t>((1.0f-subpix_x)*subpix_y*(1<<SHIFT_BITS));
-    const uint16_t wBR = static_cast<uint16_t>((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<patch_size; ++y)
-    {
-      const uint8_t* it  = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
-
-      // load and convert from uint8 to uint16
-      uint16x8_t v00 = vmovl_u8( vld1_u8( it ) );
-      uint16x8_t v01 = vmovl_u8( vld1_u8( it + 1 ) );
-      uint16x8_t v10 = vmovl_u8( vld1_u8( it + cur_step ) );
-      uint16x8_t v11 = vmovl_u8( vld1_u8( it + cur_step + 1 ) );
-
-      // vector multiply by scalar
-      v00 = vmulq_n_u16( v00, wTL );
-      v01 = vmulq_n_u16( v01, wTR );
-      v10 = vmulq_n_u16( v10, wBL );
-      v11 = vmulq_n_u16( v11, wBR );
-
-      // add all results together
-      v00 = vaddq_u16( v00, vaddq_u16( v01, vaddq_u16( v10, v11 ) ) );
-
-      // descale: shift right by constant
-      v00 = vshrq_n_u16(v00, SHIFT_BITS);
-
-      // compute difference between reference and interpolated patch,
-      // use reinterpet-cast to make signed [-255,255]
-      int16x8_t res = vsubq_s16(vreinterpretq_s16_u16(v00), vreinterpretq_s16_u16(vmovl_u8(vld1_u8( ref_patch + y*8 ))));
-
-      // correct res with mean difference
-      res = vaddq_s16(res, vmean_diff);
-
-      // compute sum of the residual
-      vres_sum = vaddq_s16(vres_sum, res); 
-
-      // Vector multiply accumulate long: vmla -> 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

CPU_version/svo/src/feature_detection.cpp

@@ -1,118 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <svo/feature_detection.h>
-#include <svo/feature.h>
-#include <fast/fast.h>
-#include <vikit/vision.h>
-
-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<double>(img_width)/cell_size_)),
-        grid_n_rows_(ceil(static_cast<double>(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<int>(i->px[1]/cell_size_)*grid_n_cols_
-        + static_cast<int>(i->px[0]/cell_size_)) = true;
-  });
-}
-
-void AbstractDetector::setGridOccpuancy(const Vector2d& px)
-{
-  grid_occupancy_.at(
-      static_cast<int>(px[1]/cell_size_)*grid_n_cols_
-    + static_cast<int>(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<n_pyr_levels_; ++L)
-  {
-    const int scale = (1<<L);
-    vector<fast::fast_xy> 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<int> 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<int>((xy.y*scale)/cell_size_)*grid_n_cols_
-                  + static_cast<int>((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
-

CPU_version/svo/src/frame.cpp

@@ -1,190 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <stdexcept>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
-#include <vikit/math_utils.h>
-#include <vikit/vision.h>
-#include <vikit/performance_monitor.h>
-#include <fast/fast.h>
-
-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<n_levels; ++i)
-  {
-    pyr[i] = cv::Mat(pyr[i-1].rows/2, pyr[i-1].cols/2, CV_8U);
-    vk::halfSample(pyr[i-1], pyr[i]);
-  }
-}
-
-bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
-{
-  vector<double> depth_vec;
-  depth_vec.reserve(frame.fts_.size());
-  depth_min = std::numeric_limits<double>::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

CPU_version/svo/src/frame_handler_base.cpp

@@ -1,199 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <vikit/abstract_camera.h>
-#include <stdlib.h>
-#include <Eigen/StdVector>
-#include <boost/bind.hpp>
-#include <fstream>
-#include <svo/frame_handler_base.h>
-#include <svo/config.h>
-#include <svo/feature.h>
-#include <svo/matcher.h>
-#include <svo/map.h>
-#include <svo/point.h>
-
-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: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
-  SVO_LOG(dropout);
-
-  // save processing time to calculate fps
-  acc_frame_timings_.push_back(timer_.stop());
-  if(stage_ == STAGE_DEFAULT_FRAME)
-    acc_num_obs_.push_back(num_observations);
-  num_obs_last_ = num_observations;
-  SVO_STOP_TIMER("tot_time");
-
-#ifdef SVO_TRACE
-  g_permon->writeToFile();
-  {
-    boost::unique_lock<boost::mutex> 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<int>(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<Point*> 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<Point*>::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it)
-  {
-    (*it)->optimize(max_iter);
-    (*it)->last_structure_optim_ = frame->id_;
-  }
-}
-
-
-} // namespace svo

CPU_version/svo/src/frame_handler_mono.cpp

@@ -1,307 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <svo/config.h>
-#include <svo/frame_handler_mono.h>
-#include <svo/map.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/pose_optimizer.h>
-#include <svo/sparse_img_align.h>
-#include <vikit/performance_monitor.h>
-#include <svo/depth_filter.h>
-#ifdef USE_BUNDLE_ADJUSTMENT
-#include <svo/bundle_adjustment.h>
-#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<Imu_Integration>(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 = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<" \t Quality min: "<<Config::qualityMinFts()<<'\n';
-  if(reprojector_.n_matches_< Config::qualityMinFts())
-  {
-    new_frame_->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: "<<sfba_n_edges_final<<'\n';
-  if(sfba_n_edges_final < Config::qualityMinFts())
-    return RESULT_FAILURE;
-
-  // structure optimization
-  //SVO_START_TIMER("point_optimizer");
-  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
-  //SVO_STOP_TIMER("point_optimizer");
-
-  // select keyframe
-  core_kfs_.insert(new_frame_);
-  setTrackingQuality(sfba_n_edges_final);
-  if(tracking_quality_ == TRACKING_INSUFFICIENT)
-  {
-    new_frame_->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 {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "
-                     "Error {"<<loba_err_init<<", "<<loba_err_fin<<"}");
-  }
-#endif
-
-  // 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::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<FramePtr, size_t>::second, _1) >
-                    boost::bind(&pair<FramePtr, size_t>::second, _2));
-  std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
-}
-
-} // namespace svo

CPU_version/svo/src/imu_integration.cpp

@@ -1,153 +0,0 @@
-//
-// Created by gfuse on 4/9/21.
-//
-//#define NDEBUG
-#include "svo/imu_integration.h"
-#include <memory>
-#include <utility>
-#include "svo/feature.h"
-#include "svo/point.h"
-#include "svo/svoVisionFactor.h"
-#include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/geometry/Pose3.h>
-#include <vikit/params_helper.h>
-#include <svo/config.h>
-
-Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
-    graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
-    valuesPtr=std::make_shared<gtsam::Values>();
-    gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
-    parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(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::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
-    preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
-    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection
-    optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
-    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<double>(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<double>(in-t_1).count());
-        t_1=in;
-        ++imu_n;
-    }
-    return true;
-
-}
-bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
-    gtsam::Values result = optimizer.calculateEstimate();
-    // Overwrite the beginning of the preintegration for the next step.
-    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
-    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(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<svo::Frame>& new_frame){
-    gtsam::Values result = optimizer.optimize();
-    // Overwrite the beginning of the preintegration for the next step.
-    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
-    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(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<svo::Frame>& 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::NavState>(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<const gtsam::PreintegratedCombinedMeasurements&>(*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<gtsam::CombinedImuFactor>(imu_factor);
-        imu_n=0;
-    }
-
-    num_obs=0;
-    for(auto&& f:new_frame->fts_){
-       if(f->point==NULL)continue;
-        graphPtr->emplace_shared<svo::VisionFactor>(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<<f->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;
-}

CPU_version/svo/src/initialization.cpp

@@ -1,199 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <svo/config.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/initialization.h>
-#include <svo/feature_detection.h>
-#include <vikit/math_utils.h>
-#include <vikit/homography.h>
-
-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 "<<disparity<<"px average disparity.");
-  if(disparity < Config::initMinDisparity())
-    return NO_KEYFRAME;
-
-  computeHomography(
-      f_ref_, f_cur_,
-      frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
-      inliers_, xyz_in_cur_, T_cur_from_ref_);
-  SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
-
-  if(inliers_.size() < Config::initMinInliers())
-  {
-    SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
-    return FAILURE;
-  }
-
-  // Rescale the map such that the mean scene depth is equal to the specified scale
-  vector<double> depth_vec;
-  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
-    depth_vec.push_back((xyz_in_cur_[i]).z());
-  double scene_depth_median = vk::getMedian(depth_vec);
-  double scale = Config::mapScale()/scene_depth_median;
-  frame_cur->T_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<int>::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<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 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<cv::Point2f>& px_vec,
-    vector<Vector3d>& 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<cv::Point2f>& px_ref,
-    vector<cv::Point2f>& px_cur,
-    vector<Vector3d>& f_ref,
-    vector<Vector3d>& f_cur,
-    vector<double>& disparities)
-{
-  const double klt_win_size = 30.0;
-  const int klt_max_iter = 30;
-  const double klt_eps = 0.001;
-  vector<uchar> status;
-  vector<float> error;
-  vector<float> 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<cv::Point2f>::iterator px_ref_it = px_ref.begin();
-  vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
-  vector<Vector3d>::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<Vector3d>& f_ref,
-    const vector<Vector3d>& f_cur,
-    double focal_length,
-    double reprojection_threshold,
-    vector<int>& inliers,
-    vector<Vector3d>& xyz_in_cur,
-    SE3& T_cur_from_ref)
-{
-  vector<Vector2d > uv_ref(f_ref.size());
-  vector<Vector2d > uv_cur(f_cur.size());
-  for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
-  {
-    uv_ref[i] = vk::project2d(f_ref[i]);
-    uv_cur[i] = vk::project2d(f_cur[i]);
-  }
-  vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
-  Homography.computeSE3fromMatches();
-  vector<int> 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

CPU_version/svo/src/map.cpp

@@ -1,368 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <set>
-#include <svo/map.h>
-#include <svo/point.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <boost/bind.hpp>
-
-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<FramePtr,double> >& 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<FramePtr,double> > 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<FramePtr, double>::second, _1) <
-                 boost::bind(&std::pair<FramePtr, double>::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<boost::mutex> lock(mut_);
-  candidates_.push_back(PointCandidate(point, point->obs_.front()));
-}
-
-void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
-{
-  boost::unique_lock<boost::mutex> 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<boost::mutex> 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<boost::mutex> 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<boost::mutex> 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<Point*> 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

CPU_version/svo/src/matcher.cpp

@@ -1,323 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <cstdlib>
-#include <vikit/abstract_camera.h>
-#include <vikit/vision.h>
-#include <vikit/math_utils.h>
-#include <vikit/patch_score.h>
-#include <svo/matcher.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
-#include <svo/feature_alignment.h>
-
-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<<level_ref)));
-  Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
-  xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
-  xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
-  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
-  const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
-  const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
-  A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
-  A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
-}
-
-int getBestSearchLevel(
-    const Matrix2d& A_cur_ref,
-    const int max_level)
-{
-  // Compute patch level in other image
-  int search_level = 0;
-  double D = A_cur_ref.determinant();
-  while(D > 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<float>();
-  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<float>() / (1<<level_ref);
-  for (int y=0; y<patch_size; ++y)
-  {
-    for (int x=0; x<patch_size; ++x, ++patch_ptr)
-    {
-      Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
-      px_patch *= (1<<search_level);
-      const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
-      if (px[0]<0 || px[1]<0 || px[0]>=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<double,3,2> 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; y<patch_size_+1; ++y, ref_patch_ptr += patch_size_)
-  {
-    uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1;
-    for(int x=0; x<patch_size_; ++x)
-      ref_patch_ptr[x] = ref_patch_border_ptr[x];
-  }
-}
-
-bool Matcher::findMatchDirect(
-    const Point& pt,
-    const Frame& cur_frame,
-    Vector2d& px_cur)
-{
-  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
-    return false;
-
-  if(!ref_ftr_->frame->cam_->isInFrame(
-      ref_ftr_->px.cast<int>()/(1<<ref_ftr_->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<<search_level_));
-
-  bool success = false;
-  if(ref_ftr_->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<float>(),
-          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<<search_level_);
-  return success;
-}
-
-bool Matcher::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)
-{
-  SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
-  int zmssd_best = PatchScore::threshold();
-  Vector2d uv_best;
-
-  // Compute start and end of epipolar line in old_kf for match search, on unit plane!
-  Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
-  Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
-  epi_dir_ = A - B;
-
-  // Compute affine warp matrix
-  warp::getWarpMatrixAffine(
-      *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
-      d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
-
-  // feature pre-selection
-  reject_ = false;
-  if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering)
-  {
-    const Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
-    const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
-    if(cosangle < options_.epi_search_edgelet_max_angle) {
-      reject_ = true;
-      return false;
-    }
-  }
-
-  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
-
-  // Find length of search range on epipolar line
-  Vector2d px_A(cur_frame.cam_->world2cam(A));
-  Vector2d px_B(cur_frame.cam_->world2cam(B));
-  epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
-
-  // Warp reference patch at ref_level
-  warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
-                   ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
-  createPatchFromPatchWithBorder();
-
-  if(epi_length_ < 2.0)
-  {
-    px_cur_ = (px_A+px_B)/2.0;
-    Vector2d px_scaled(px_cur_/(1<<search_level_));
-    bool res;
-    if(options_.align_1d)
-      res = feature_alignment::align1D(
-          cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().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<<search_level_);
-      if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->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; i<n_steps; ++i, uv+=step)
-  {
-    Vector2d px(cur_frame.cam_->world2cam(uv));
-    Vector2i pxi(px[0]/(1<<search_level_)+0.5,
-                 px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
-
-    if(pxi == last_checked_pxi)
-      continue;
-    last_checked_pxi = pxi;
-
-    // check if the patch is full within the new frame
-    if(!cur_frame.cam_->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<<search_level_));
-      bool res;
-      if(options_.align_1d)
-        res = feature_alignment::align1D(
-            cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().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<<search_level_);
-        if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->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

CPU_version/svo/src/point.cpp

@@ -1,179 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <stdexcept>
-#include <vikit/math_utils.h>
-#include <svo/point.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
- 
-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<double,3,3>(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; i<n_iter; i++)
-  {
-    A.setZero();
-    b.setZero();
-    double new_chi2 = 0.0;
-
-    // compute residuals
-    for(auto it=obs_.begin(); it!=obs_.end(); ++it)
-    {
-      Matrix23d J;
-      const Vector3d p_in_f((*it)->frame->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

CPU_version/svo/src/pose_optimizer.cpp

@@ -1,165 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <stdexcept>
-#include <svo/pose_optimizer.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <vikit/robust_cost.h>
-#include <vikit/math_utils.h>
-
-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<double> 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<float> 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; iter<n_iter; iter++)
-  {
-    // overwrite scale
-    if(iter == 5)
-      scale = 0.85/frame->cam_->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

CPU_version/svo/src/reprojector.cpp

@@ -1,219 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <algorithm>
-#include <stdexcept>
-#include <svo/reprojector.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/map.h>
-#include <svo/config.h>
-#include <boost/bind.hpp>
-#include <boost/thread.hpp>
-#include <vikit/abstract_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/timer.h>
-
-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<double>(cam->width())/grid_.cell_size);
-  grid_.grid_n_rows = ceil(static_cast<double>(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; i<grid_.cells.size(); ++i)
-    grid_.cell_order[i] = i;
-  random_shuffle(grid_.cell_order.begin(), grid_.cell_order.end()); // maybe we should do it at every iteration!
-}
-
-void Reprojector::resetGrid()
-{
-  n_matches_ = 0;
-  n_trials_ = 0;
-  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ c->clear(); });
-}
-
-void Reprojector::reprojectMap(
-    FramePtr frame,
-    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
-{
-  resetGrid();
-
-  // Identify those Keyframes which share a common field of view.
-  SVO_START_TIMER("reproject_kfs");
-  list< pair<FramePtr,double> > close_kfs;
-  map_.getCloseKeyframes(frame, close_kfs);
-
-  // Sort KFs with overlap according to their closeness
-  close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
-                 boost::bind(&std::pair<FramePtr, double>::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 && n<options_.max_n_kfs; ++it_frame, ++n)
-  {
-    FramePtr ref_frame = it_frame->first;
-    overlap_kfs.push_back(pair<FramePtr,size_t>(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<boost::mutex> 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<grid_.cells.size(); ++i)
-  {
-    // we prefer good quality points over unkown quality (more likely to match)
-    // and unknown quality over candidates (position not optimized)
-    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
-      ++n_matches_;
-    if(n_matches_ > (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<int>(), 8)) // 8px is the patch size in the matcher
-  {
-    const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
-                + static_cast<int>(px[0]/grid_.cell_size);
-    grid_.cells.at(k)->push_back(Candidate(point, px));
-    return true;
-  }
-  return false;
-}
-
-} // namespace svo

CPU_version/svo/src/sparse_img_align.cpp

@@ -1,274 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <algorithm>
-#include <svo/sparse_img_align.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/config.h>
-#include <svo/point.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/vision.h>
-#include <vikit/math_utils.h>
-
-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<double, 6, 6> SparseImgAlign::getFisherInformation()
-{
-  double sigma_i_sq = 5e-4*255*255; // image noise
-  Matrix<double,6,6> 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<<level_);
-  const Vector3d ref_pos = ref_frame_->pos();
-  const double focal_length = ref_frame_->cam_->errorMultiplier2();
-  size_t feature_counter = 0;
-  std::vector<bool>::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<double,2,6> 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<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
-    for(int y=0; y<patch_size_; ++y)
-    {
-      uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_);
-      for(int x=0; x<patch_size_; ++x, ++ref_img_ptr, ++cache_ptr, ++pixel_counter)
-      {
-        // precompute interpolated reference patch color
-        *cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1];
-
-        // we use the inverse compositional: thereby we can take the gradient always at the same position
-        // get gradient of warped image (~gradient at warped position)
-        float dx = 0.5f * ((w_ref_tl*ref_img_ptr[1] + w_ref_tr*ref_img_ptr[2] + w_ref_bl*ref_img_ptr[stride+1] + w_ref_br*ref_img_ptr[stride+2])
-                          -(w_ref_tl*ref_img_ptr[-1] + w_ref_tr*ref_img_ptr[0] + w_ref_bl*ref_img_ptr[stride-1] + w_ref_br*ref_img_ptr[stride]));
-        float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1])
-                          -(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1]));
-
-        // cache the jacobian
-        jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
-            (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
-      }
-    }
-  }
-  have_ref_patch_cache_ = true;
-}
-
-double SparseImgAlign::computeResiduals(
-    const SE3& T_cur_from_ref,
-    bool linearize_system,
-    bool compute_weight_scale)
-{
-  // Warp the (cur)rent image such that it aligns with the (ref)erence image
-  const cv::Mat& cur_img = cur_frame_->img_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<float> 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<<level_);
-  const Vector3d ref_pos(ref_frame_->pos());
-  float chi2 = 0.0;
-  size_t feature_counter = 0; // is used to compute the index of the cached jacobian
-  std::vector<bool>::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<float>() * 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<float*>(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; y<patch_size_; ++y)
-    {
-      uint8_t* cur_img_ptr = (uint8_t*) cur_img.data + (v_cur_i+y-patch_halfsize_)*stride + (u_cur_i-patch_halfsize_);
-
-      for(int x=0; x<patch_size_; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr)
-      {
-        // compute residual
-        const float intensity_cur = w_cur_tl*cur_img_ptr[0] + w_cur_tr*cur_img_ptr[1] + w_cur_bl*cur_img_ptr[stride] + w_cur_br*cur_img_ptr[stride+1];
-        const float res = intensity_cur - (*ref_patch_cache_ptr);
-
-        // used to compute scale for robust cost
-        if(compute_weight_scale)
-          errors.push_back(fabsf(res));
-
-        // robustification
-        float weight = 1.0;
-        if(use_weights_) {
-          weight = weight_function_->value(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<float>((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
-

CPU_version/svo/test/README.md

@@ -1,24 +0,0 @@
-#### 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

CPU_version/svo/test/benchmark.csv

@@ -1,307 +0,0 @@
-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

CPU_version/svo/test/data/.gitignore

@@ -1,2 +0,0 @@
-.gitignore
-!.gitignore

CPU_version/svo/test/results/.gitignore

@@ -1,2 +0,0 @@
-.gitignore
-!.gitignore

CPU_version/svo/test/test_depth_filter.cpp

@@ -1,206 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <list>
-#include <vector>
-#include <string>
-#include <boost/bind.hpp>
-#include <boost/make_shared.hpp>
-#include <boost/shared_ptr.hpp>
-#include <vikit/pinhole_camera.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/atan_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/file_reader.h>
-#include <vikit/blender_utils.h>
-#include <vikit/timer.h>
-#include <iostream>
-#include <svo/feature_detection.h>
-#include <svo/depth_filter.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
-#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<ConvergedSeed> results_;
-  std::vector<double> 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<float>(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<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
-  std::vector<vk::blender_utils::file_format::ImageNameAndPose> 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<vk::blender_utils::file_format::ImageNameAndPose>::iterator it = sequence.begin();
-  vk::Timer t;
-  std::list<size_t> 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<svo::Frame>(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<svo::Frame>(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<double>::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<ConvergedSeed>::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<size_t>::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<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
-  {
-	cv::Vec3b c = frame_ref_->img_pyr_[0].at<cv::Vec3b>(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;
-}

CPU_version/svo/test/test_feature_alignment.cpp

@@ -1,124 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <string.h>
-#include <svo/global.h>
-#include <svo/feature_alignment.h>
-#include <vikit/timer.h>
-#include <vikit/aligned_mem.h>
-#include "test_utils.h"
-#if __ARM_NEON__
-#include <arm_neon.h>
-#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<uint8_t>(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<float>();
-    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;
-}

CPU_version/svo/test/test_feature_detection.cpp

@@ -1,74 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <string.h>
-#include <svo/global.h>
-#include <svo/config.h>
-#include <svo/frame.h>
-#include <svo/feature_detection.h>
-#include <svo/depth_filter.h>
-#include <svo/feature.h>
-#include <vikit/timer.h>
-#include <vikit/vision.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/atan_camera.h>
-#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;
-}

CPU_version/svo/test/test_matcher.cpp

@@ -1,200 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <algorithm>
-#include <vikit/pinhole_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/blender_utils.h>
-#include <svo/matcher.h>
-#include <svo/frame.h>
-#include <svo/config.h>
-#include <svo/feature.h>
-#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<double> errors; errors.reserve(depthmap.cols*depthmap.rows);
-  for(int y=4; y<depthmap.rows-4; ++y)
-  {
-    for(int x=4; x<depthmap.cols-4; ++x)
-    {
-      svo::Feature ftr(frame_ref_, Eigen::Vector2d(x,y), 0);
-      double depth_gt = depth_ref_.at<float>(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<float>(y,x) = depth_estimate;
-        error_map.at<float>(y,x) = fabsf(depth_estimate-depth_gt);
-        ++n_converged;
-        sum_error += error_map.at<float>(y,x);
-        errors.push_back(error_map.at<float>(y,x));
-      }
-      else
-        mask.at<uint8_t>(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<double>::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<<level_cur)+0.5, px_cur[1]/(1<<level_cur)+0.5);
-  cv::Mat cur_patch(patch_size, patch_size, CV_8U);
-  frame_cur_->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<<ref_ftr_->level), ref_ftr_->px[1]/(1<<ref_ftr_->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;
-}

CPU_version/svo/test/test_pipeline.cpp

@@ -1,98 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <svo/config.h>
-#include <svo/frame_handler_mono.h>
-#include <svo/map.h>
-#include <svo/frame.h>
-#include <vector>
-#include <string>
-#include <vikit/math_utils.h>
-#include <vikit/vision.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/atan_camera.h>
-#include <vikit/pinhole_camera.h>
-#include <opencv2/opencv.hpp>
-#include <sophus/se3.h>
-#include <iostream>
-#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;
-}
-

CPU_version/svo/test/test_pose_optimizer.cpp

@@ -1,110 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <algorithm>
-#include <vikit/pinhole_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/sample.h>
-#include <vikit/blender_utils.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/config.h>
-#include <svo/feature_detection.h>
-#include <svo/pose_optimizer.h>
-#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<float>(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;
-}

CPU_version/svo/test/test_sparse_img_align.cpp

@@ -1,163 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#include <vikit/pinhole_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/file_reader.h>
-#include <vikit/timer.h>
-#include <vikit/blender_utils.h>
-#include <iostream>
-#include <svo/feature_detection.h>
-#include <svo/sparse_img_align.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/config.h>
-#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<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
-  std::vector<vk::blender_utils::file_format::ImageNameAndPose> 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<vk::blender_utils::file_format::ImageNameAndPose>::iterator iter = sequence.begin();
-  std::list<double> 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<float>(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<double>::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;
-}

CPU_version/svo/test/test_utils.h

@@ -1,57 +0,0 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
-//
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-
-#ifndef TEST_UTILS_H_
-#define TEST_UTILS_H_
-
-#include <string.h>
-#include <cstdlib> // for getenv
-#ifdef SVO_USE_ROS
-# include <ros/package.h>
-# include <vikit/params_helper.h>
-#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<std::string>("svo/trace_dir", default_dir);
-#else
-  return "/tmp";
-#endif
-}
-
-} // namespace test_utils
-} // namespace svo
-
-
-#endif // TEST_UTILS_H_

CPU_version/svo_analysis/.gitignore

@@ -1 +0,0 @@
-*.pyc

CPU_version/svo_analysis/CMakeLists.txt

@@ -1,8 +0,0 @@
-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)

CPU_version/svo_analysis/README.md

@@ -1,27 +0,0 @@
-## 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.
-  

CPU_version/svo_analysis/comparisons/speed.yaml

@@ -1,14 +0,0 @@
-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

CPU_version/svo_analysis/comparisons/textures.yaml

@@ -1,22 +0,0 @@
-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

CPU_version/svo_analysis/experiments/.gitignore

@@ -1,2 +0,0 @@
-*.yaml
-!.gitignore

CPU_version/svo_analysis/package.xml

@@ -1,19 +0,0 @@
-<package>
-
-  <name>svo_analysis</name>
-  <version>0.0.1</version>
-  <description>
-    Python scripts to analyse performance of SVO.
-  </description>
-  <maintainer email="[email protected]">Christian Forster</maintainer>
-  <author>Christian Forster</author>
-  <license>BSD</license>
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>rospy</build_depend>
-
-  <run_depend>python-rospkg</run_depend>
-  <run_depend>python-yaml</run_depend>
-  <run_depend>rospy</run_depend>
-
-</package>

CPU_version/svo_analysis/results/.gitignore

@@ -1,2 +0,0 @@
-.gitignore
-!.gitignore

CPU_version/svo_analysis/scripts/benchmark.py

@@ -1,111 +0,0 @@
-#!/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)
-      
-      
-  
-  
-  
-  

CPU_version/svo_analysis/scripts/compare_results.py

@@ -1,150 +0,0 @@
-#!/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) 
-  
- 
-                 
-           

CPU_version/svo_analysis/scripts/comparison.py

@@ -1,159 +0,0 @@
-#!/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) 
-                
-           

CPU_version/svo_analysis/scripts/evaluate.py

@@ -1,58 +0,0 @@
-#!/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)

CPU_version/svo_analysis/setup.py

@@ -1,12 +0,0 @@
-#!/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)

CPU_version/svo_analysis/src/svo_analysis/analyse_dataset.py

@@ -1,41 +0,0 @@
-# -*- 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'
-
-  

CPU_version/svo_analysis/src/svo_analysis/analyse_depth.py

@@ -1,66 +0,0 @@
-#!/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)

CPU_version/svo_analysis/src/svo_analysis/analyse_logs.py

@@ -1,74 +0,0 @@
-#!/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))
-

CPU_version/svo_analysis/src/svo_analysis/analyse_timing.py

@@ -1,68 +0,0 @@
-#!/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")

CPU_version/svo_analysis/src/svo_analysis/analyse_trajectory.py

@@ -1,185 +0,0 @@
-#!/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)

CPU_version/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py

@@ -1,60 +0,0 @@
-#!/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()

CPU_version/svo_analysis/src/svo_analysis/hand_eye_calib.py

@@ -1,227 +0,0 @@
-# -*- 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))
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py

@@ -1,2 +0,0 @@
-# -*- coding: utf-8 -*-
-

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py

@@ -1,130 +0,0 @@
-#!/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)
-        

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_ate.py

@@ -1,194 +0,0 @@
-#!/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)
-        

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_rpe.py

@@ -1,382 +0,0 @@
-#!/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)<numpy.sqrt(param_max_pairs)):
-            pairs = [(i,j) for i in range(len(traj_est)) for j in range(len(traj_est))]
-        else:
-            pairs = [(random.randint(0,len(traj_est)-1),random.randint(0,len(traj_est)-1)) for i in range(param_max_pairs)]
-    else:
-        pairs = []
-        for i in range(len(traj_est)):
-            j = find_closest_index(index_est,index_est[i] + param_delta)
-            if j!=len(traj_est)-1: 
-                pairs.append((i,j))
-        if(param_max_pairs!=0 and len(pairs)>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)
-        
-

CPU_version/svo_msgs/.idea/.gitignore

@@ -1,8 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml
-# Datasource local storage ignored files
-/dataSources/
-/dataSources.local.xml
-# Editor-based HTTP Client requests
-/httpRequests/

CPU_version/svo_msgs/.idea/misc.xml

@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
-</project>

CPU_version/svo_msgs/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/svo_msgs.iml" filepath="$PROJECT_DIR$/.idea/svo_msgs.iml" />
-    </modules>
-  </component>
-</project>

CPU_version/svo_msgs/.idea/svo_msgs.iml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module classpath="CMake" type="CPP_MODULE" version="4" />

CPU_version/svo_msgs/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

CPU_version/svo_msgs/CMakeLists.txt

@@ -1,33 +0,0 @@
-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}
-)

CPU_version/svo_msgs/msg/DenseInput.msg

@@ -1,6 +0,0 @@
-Header header
-uint32 frame_id
-geometry_msgs/Pose pose
-sensor_msgs/Image image
-float32 min_depth
-float32 max_depth

CPU_version/svo_msgs/msg/Feature.msg

@@ -1,5 +0,0 @@
-float32 u
-float32 v
-float32 x
-float32 y
-float32 z

CPU_version/svo_msgs/msg/NbvTrajectory.msg

@@ -1,2 +0,0 @@
-Header header
-geometry_msgs/Pose[] trajectory

CPU_version/svo_msgs/package.xml

@@ -1,19 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>svo_msgs</name>
-  <version>0.0.0</version>
-  <description>Custom messages for svo</description>
-  <maintainer email="[email protected]">Christian Forster</maintainer>
-  <license>BSD</license>
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>message_generation</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  
-  <run_depend>message_runtime</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-
-  <export></export>
-</package>

CPU_version/vio_svo/.idea/.gitignore

@@ -1,8 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml
-# Datasource local storage ignored files
-/dataSources/
-/dataSources.local.xml
-# Editor-based HTTP Client requests
-/httpRequests/

CPU_version/vio_svo/.idea/misc.xml

@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
-</project>

CPU_version/vio_svo/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/svo_ros.iml" filepath="$PROJECT_DIR$/.idea/svo_ros.iml" />
-    </modules>
-  </component>
-</project>

CPU_version/vio_svo/.idea/svo_ros.iml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module classpath="CMake" type="CPP_MODULE" version="4" />