[
  {
    "path": ".gitignore",
    "content": "data.csv\n/.git/\ncamera_model/build/\nvins_estimator/build/\n"
  },
  {
    "path": ".kdev4/src.kdev4",
    "content": "[Buildset]\nBuildItems=@Variant(\\x00\\x00\\x00\\t\\x00\\x00\\x00\\x00\\x01\\x00\\x00\\x00\\x0b\\x00\\x00\\x00\\x00\\x01\\x00\\x00\\x00\\x1e\\x00R\\x00e\\x00m\\x00o\\x00v\\x00e\\x00_\\x00R\\x00O\\x00S\\x00_\\x00V\\x00I\\x00N\\x00S)\n\n[CMake]\nBuild Directory Count=1\nCurrent Build Directory Index=0\nProjectRootRelative=./\n\n[CMake][CMake Build Directory 0]\nBuild Directory Path=file:///home/solomon/merge_vins_version/build\nBuild Type=\nCMake Binary=file:///usr/bin/cmake\nEnvironment Profile=\nExtra Arguments=\nInstall Directory=file:///home/solomon/merge_vins_version/install\n\n[Launch]\nLaunch Configurations=Launch Configuration 0\n\n[Launch][Launch Configuration 0]\nConfigured Launch Modes=execute\nConfigured Launchers=nativeAppLauncher\nName=New Application Launcher\nType=Native Application\n\n[Launch][Launch Configuration 0][Data]\nArguments=\nDebugger Shell=\nDependencies=@Variant(\\x00\\x00\\x00\\t\\x00\\x00\\x00\\x00\\x00)\nDependency Action=Nothing\nDisplay Demangle Names=true\nDisplay Static Members=false\nEnvironmentGroup=default\nExecutable=file:///home/solomon/workspace/slam/VINS-MONO/catkin_ws/src/CMakeLists.txt\nExternal Terminal=konsole --noclose --workdir %workdir -e %exe\nGDB Path=file:///home/solomon/merge_vins_version/devel/lib/vins_estimator/vins_estimator\nProject Target=Remove_ROS_VINS,benchmark_publisher,benchmark_publisher\nRemote GDB Config Script=\nRemote GDB Run Script=\nRemote GDB Shell Script=\nStart With=ApplicationOutput\nUse External Terminal=false\nWorking Directory=file:///home/solomon/workspace/slam/VINS-MONO/catkin_ws/src\nisExecutable=true\n\n[Project]\nVersionControlSupport=kdevgit\n"
  },
  {
    "path": "LICENCE",
    "content": "                    GNU GENERAL PUBLIC LICENSE\n                       Version 3, 29 June 2007\n\n Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n                            Preamble\n\n  The GNU General Public License is a free, copyleft license for\nsoftware and other kinds of works.\n\n  The licenses for most software and other practical works are designed\nto take away your freedom to share and change the works.  By contrast,\nthe GNU General Public License is intended to guarantee your freedom to\nshare and change all versions of a program--to make sure it remains free\nsoftware for all its users.  We, the Free Software Foundation, use the\nGNU General Public License for most of our software; it applies also to\nany other work released this way by its authors.  You can apply it to\nyour programs, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthem if you wish), that you receive source code or can get it if you\nwant it, that you can change the software or use pieces of it in new\nfree programs, and that you know you can do these things.\n\n  To protect your rights, we need to prevent others from denying you\nthese rights or asking you to surrender the rights.  Therefore, you have\ncertain responsibilities if you distribute copies of the software, or if\nyou modify it: responsibilities to respect the freedom of others.\n\n  For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must pass on to the recipients the same\nfreedoms that you received.  You must make sure that they, too, receive\nor can get the source code.  And you must show them these terms so they\nknow their rights.\n\n  Developers that use the GNU GPL protect your rights with two steps:\n(1) assert copyright on the software, and (2) offer you this License\ngiving you legal permission to copy, distribute and/or modify it.\n\n  For the developers' and authors' protection, the GPL clearly explains\nthat there is no warranty for this free software.  For both users' and\nauthors' sake, the GPL requires that modified versions be marked as\nchanged, so that their problems will not be attributed erroneously to\nauthors of previous versions.\n\n  Some devices are designed to deny users access to install or run\nmodified versions of the software inside them, although the manufacturer\ncan do so.  This is fundamentally incompatible with the aim of\nprotecting users' freedom to change the software.  The systematic\npattern of such abuse occurs in the area of products for individuals to\nuse, which is precisely where it is most unacceptable.  Therefore, we\nhave designed this version of the GPL to prohibit the practice for those\nproducts.  If such problems arise substantially in other domains, we\nstand ready to extend this provision to those domains in future versions\nof the GPL, as needed to protect the freedom of users.\n\n  Finally, every program is threatened constantly by software patents.\nStates should not allow patents to restrict development and use of\nsoftware on general-purpose computers, but in those that do, we wish to\navoid the special danger that patents applied to a free program could\nmake it effectively proprietary.  To prevent this, the GPL assures that\npatents cannot be used to render the program non-free.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.\n\n                       TERMS AND CONDITIONS\n\n  0. Definitions.\n\n  \"This License\" refers to version 3 of the GNU General Public License.\n\n  \"Copyright\" also means copyright-like laws that apply to other kinds of\nworks, such as semiconductor masks.\n\n  \"The Program\" refers to any copyrightable work licensed under this\nLicense.  Each licensee is addressed as \"you\".  \"Licensees\" and\n\"recipients\" may be individuals or organizations.\n\n  To \"modify\" a work means to copy from or adapt all or part of the work\nin a fashion requiring copyright permission, other than the making of an\nexact copy.  The resulting work is called a \"modified version\" of the\nearlier work or a work \"based on\" the earlier work.\n\n  A \"covered work\" means either the unmodified Program or a work based\non the Program.\n\n  To \"propagate\" a work means to do anything with it that, without\npermission, would make you directly or secondarily liable for\ninfringement under applicable copyright law, except executing it on a\ncomputer or modifying a private copy.  Propagation includes copying,\ndistribution (with or without modification), making available to the\npublic, and in some countries other activities as well.\n\n  To \"convey\" a work means any kind of propagation that enables other\nparties to make or receive copies.  Mere interaction with a user through\na computer network, with no transfer of a copy, is not conveying.\n\n  An interactive user interface displays \"Appropriate Legal Notices\"\nto the extent that it includes a convenient and prominently visible\nfeature that (1) displays an appropriate copyright notice, and (2)\ntells the user that there is no warranty for the work (except to the\nextent that warranties are provided), that licensees may convey the\nwork under this License, and how to view a copy of this License.  If\nthe interface presents a list of user commands or options, such as a\nmenu, a prominent item in the list meets this criterion.\n\n  1. Source Code.\n\n  The \"source code\" for a work means the preferred form of the work\nfor making modifications to it.  \"Object code\" means any non-source\nform of a work.\n\n  A \"Standard Interface\" means an interface that either is an official\nstandard defined by a recognized standards body, or, in the case of\ninterfaces specified for a particular programming language, one that\nis widely used among developers working in that language.\n\n  The \"System Libraries\" of an executable work include anything, other\nthan the work as a whole, that (a) is included in the normal form of\npackaging a Major Component, but which is not part of that Major\nComponent, and (b) serves only to enable use of the work with that\nMajor Component, or to implement a Standard Interface for which an\nimplementation is available to the public in source code form.  A\n\"Major Component\", in this context, means a major essential component\n(kernel, window system, and so on) of the specific operating system\n(if any) on which the executable work runs, or a compiler used to\nproduce the work, or an object code interpreter used to run it.\n\n  The \"Corresponding Source\" for a work in object code form means all\nthe source code needed to generate, install, and (for an executable\nwork) run the object code and to modify the work, including scripts to\ncontrol those activities.  However, it does not include the work's\nSystem Libraries, or general-purpose tools or generally available free\nprograms which are used unmodified in performing those activities but\nwhich are not part of the work.  For example, Corresponding Source\nincludes interface definition files associated with source files for\nthe work, and the source code for shared libraries and dynamically\nlinked subprograms that the work is specifically designed to require,\nsuch as by intimate data communication or control flow between those\nsubprograms and other parts of the work.\n\n  The Corresponding Source need not include anything that users\ncan regenerate automatically from other parts of the Corresponding\nSource.\n\n  The Corresponding Source for a work in source code form is that\nsame work.\n\n  2. Basic Permissions.\n\n  All rights granted under this License are granted for the term of\ncopyright on the Program, and are irrevocable provided the stated\nconditions are met.  This License explicitly affirms your unlimited\npermission to run the unmodified Program.  The output from running a\ncovered work is covered by this License only if the output, given its\ncontent, constitutes a covered work.  This License acknowledges your\nrights of fair use or other equivalent, as provided by copyright law.\n\n  You may make, run and propagate covered works that you do not\nconvey, without conditions so long as your license otherwise remains\nin force.  You may convey covered works to others for the sole purpose\nof having them make modifications exclusively for you, or provide you\nwith facilities for running those works, provided that you comply with\nthe terms of this License in conveying all material for which you do\nnot control copyright.  Those thus making or running the covered works\nfor you must do so exclusively on your behalf, under your direction\nand control, on terms that prohibit them from making any copies of\nyour copyrighted material outside their relationship with you.\n\n  Conveying under any other circumstances is permitted solely under\nthe conditions stated below.  Sublicensing is not allowed; section 10\nmakes it unnecessary.\n\n  3. Protecting Users' Legal Rights From Anti-Circumvention Law.\n\n  No covered work shall be deemed part of an effective technological\nmeasure under any applicable law fulfilling obligations under article\n11 of the WIPO copyright treaty adopted on 20 December 1996, or\nsimilar laws prohibiting or restricting circumvention of such\nmeasures.\n\n  When you convey a covered work, you waive any legal power to forbid\ncircumvention of technological measures to the extent such circumvention\nis effected by exercising rights under this License with respect to\nthe covered work, and you disclaim any intention to limit operation or\nmodification of the work as a means of enforcing, against the work's\nusers, your or third parties' legal rights to forbid circumvention of\ntechnological measures.\n\n  4. Conveying Verbatim Copies.\n\n  You may convey verbatim copies of the Program's source code as you\nreceive it, in any medium, provided that you conspicuously and\nappropriately publish on each copy an appropriate copyright notice;\nkeep intact all notices stating that this License and any\nnon-permissive terms added in accord with section 7 apply to the code;\nkeep intact all notices of the absence of any warranty; and give all\nrecipients a copy of this License along with the Program.\n\n  You may charge any price or no price for each copy that you convey,\nand you may offer support or warranty protection for a fee.\n\n  5. Conveying Modified Source Versions.\n\n  You may convey a work based on the Program, or the modifications to\nproduce it from the Program, in the form of source code under the\nterms of section 4, provided that you also meet all of these conditions:\n\n    a) The work must carry prominent notices stating that you modified\n    it, and giving a relevant date.\n\n    b) The work must carry prominent notices stating that it is\n    released under this License and any conditions added under section\n    7.  This requirement modifies the requirement in section 4 to\n    \"keep intact all notices\".\n\n    c) You must license the entire work, as a whole, under this\n    License to anyone who comes into possession of a copy.  This\n    License will therefore apply, along with any applicable section 7\n    additional terms, to the whole of the work, and all its parts,\n    regardless of how they are packaged.  This License gives no\n    permission to license the work in any other way, but it does not\n    invalidate such permission if you have separately received it.\n\n    d) If the work has interactive user interfaces, each must display\n    Appropriate Legal Notices; however, if the Program has interactive\n    interfaces that do not display Appropriate Legal Notices, your\n    work need not make them do so.\n\n  A compilation of a covered work with other separate and independent\nworks, which are not by their nature extensions of the covered work,\nand which are not combined with it such as to form a larger program,\nin or on a volume of a storage or distribution medium, is called an\n\"aggregate\" if the compilation and its resulting copyright are not\nused to limit the access or legal rights of the compilation's users\nbeyond what the individual works permit.  Inclusion of a covered work\nin an aggregate does not cause this License to apply to the other\nparts of the aggregate.\n\n  6. Conveying Non-Source Forms.\n\n  You may convey a covered work in object code form under the terms\nof sections 4 and 5, provided that you also convey the\nmachine-readable Corresponding Source under the terms of this License,\nin one of these ways:\n\n    a) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by the\n    Corresponding Source fixed on a durable physical medium\n    customarily used for software interchange.\n\n    b) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by a\n    written offer, valid for at least three years and valid for as\n    long as you offer spare parts or customer support for that product\n    model, to give anyone who possesses the object code either (1) a\n    copy of the Corresponding Source for all the software in the\n    product that is covered by this License, on a durable physical\n    medium customarily used for software interchange, for a price no\n    more than your reasonable cost of physically performing this\n    conveying of source, or (2) access to copy the\n    Corresponding Source from a network server at no charge.\n\n    c) Convey individual copies of the object code with a copy of the\n    written offer to provide the Corresponding Source.  This\n    alternative is allowed only occasionally and noncommercially, and\n    only if you received the object code with such an offer, in accord\n    with subsection 6b.\n\n    d) Convey the object code by offering access from a designated\n    place (gratis or for a charge), and offer equivalent access to the\n    Corresponding Source in the same way through the same place at no\n    further charge.  You need not require recipients to copy the\n    Corresponding Source along with the object code.  If the place to\n    copy the object code is a network server, the Corresponding Source\n    may be on a different server (operated by you or a third party)\n    that supports equivalent copying facilities, provided you maintain\n    clear directions next to the object code saying where to find the\n    Corresponding Source.  Regardless of what server hosts the\n    Corresponding Source, you remain obligated to ensure that it is\n    available for as long as needed to satisfy these requirements.\n\n    e) Convey the object code using peer-to-peer transmission, provided\n    you inform other peers where the object code and Corresponding\n    Source of the work are being offered to the general public at no\n    charge under subsection 6d.\n\n  A separable portion of the object code, whose source code is excluded\nfrom the Corresponding Source as a System Library, need not be\nincluded in conveying the object code work.\n\n  A \"User Product\" is either (1) a \"consumer product\", which means any\ntangible personal property which is normally used for personal, family,\nor household purposes, or (2) anything designed or sold for incorporation\ninto a dwelling.  In determining whether a product is a consumer product,\ndoubtful cases shall be resolved in favor of coverage.  For a particular\nproduct received by a particular user, \"normally used\" refers to a\ntypical or common use of that class of product, regardless of the status\nof the particular user or of the way in which the particular user\nactually uses, or expects or is expected to use, the product.  A product\nis a consumer product regardless of whether the product has substantial\ncommercial, industrial or non-consumer uses, unless such uses represent\nthe only significant mode of use of the product.\n\n  \"Installation Information\" for a User Product means any methods,\nprocedures, authorization keys, or other information required to install\nand execute modified versions of a covered work in that User Product from\na modified version of its Corresponding Source.  The information must\nsuffice to ensure that the continued functioning of the modified object\ncode is in no case prevented or interfered with solely because\nmodification has been made.\n\n  If you convey an object code work under this section in, or with, or\nspecifically for use in, a User Product, and the conveying occurs as\npart of a transaction in which the right of possession and use of the\nUser Product is transferred to the recipient in perpetuity or for a\nfixed term (regardless of how the transaction is characterized), the\nCorresponding Source conveyed under this section must be accompanied\nby the Installation Information.  But this requirement does not apply\nif neither you nor any third party retains the ability to install\nmodified object code on the User Product (for example, the work has\nbeen installed in ROM).\n\n  The requirement to provide Installation Information does not include a\nrequirement to continue to provide support service, warranty, or updates\nfor a work that has been modified or installed by the recipient, or for\nthe User Product in which it has been modified or installed.  Access to a\nnetwork may be denied when the modification itself materially and\nadversely affects the operation of the network or violates the rules and\nprotocols for communication across the network.\n\n  Corresponding Source conveyed, and Installation Information provided,\nin accord with this section must be in a format that is publicly\ndocumented (and with an implementation available to the public in\nsource code form), and must require no special password or key for\nunpacking, reading or copying.\n\n  7. Additional Terms.\n\n  \"Additional permissions\" are terms that supplement the terms of this\nLicense by making exceptions from one or more of its conditions.\nAdditional permissions that are applicable to the entire Program shall\nbe treated as though they were included in this License, to the extent\nthat they are valid under applicable law.  If additional permissions\napply only to part of the Program, that part may be used separately\nunder those permissions, but the entire Program remains governed by\nthis License without regard to the additional permissions.\n\n  When you convey a copy of a covered work, you may at your option\nremove any additional permissions from that copy, or from any part of\nit.  (Additional permissions may be written to require their own\nremoval in certain cases when you modify the work.)  You may place\nadditional permissions on material, added by you to a covered work,\nfor which you have or can give appropriate copyright permission.\n\n  Notwithstanding any other provision of this License, for material you\nadd to a covered work, you may (if authorized by the copyright holders of\nthat material) supplement the terms of this License with terms:\n\n    a) Disclaiming warranty or limiting liability differently from the\n    terms of sections 15 and 16 of this License; or\n\n    b) Requiring preservation of specified reasonable legal notices or\n    author attributions in that material or in the Appropriate Legal\n    Notices displayed by works containing it; or\n\n    c) Prohibiting misrepresentation of the origin of that material, or\n    requiring that modified versions of such material be marked in\n    reasonable ways as different from the original version; or\n\n    d) Limiting the use for publicity purposes of names of licensors or\n    authors of the material; or\n\n    e) Declining to grant rights under trademark law for use of some\n    trade names, trademarks, or service marks; or\n\n    f) Requiring indemnification of licensors and authors of that\n    material by anyone who conveys the material (or modified versions of\n    it) with contractual assumptions of liability to the recipient, for\n    any liability that these contractual assumptions directly impose on\n    those licensors and authors.\n\n  All other non-permissive additional terms are considered \"further\nrestrictions\" within the meaning of section 10.  If the Program as you\nreceived it, or any part of it, contains a notice stating that it is\ngoverned by this License along with a term that is a further\nrestriction, you may remove that term.  If a license document contains\na further restriction but permits relicensing or conveying under this\nLicense, you may add to a covered work material governed by the terms\nof that license document, provided that the further restriction does\nnot survive such relicensing or conveying.\n\n  If you add terms to a covered work in accord with this section, you\nmust place, in the relevant source files, a statement of the\nadditional terms that apply to those files, or a notice indicating\nwhere to find the applicable terms.\n\n  Additional terms, permissive or non-permissive, may be stated in the\nform of a separately written license, or stated as exceptions;\nthe above requirements apply either way.\n\n  8. Termination.\n\n  You may not propagate or modify a covered work except as expressly\nprovided under this License.  Any attempt otherwise to propagate or\nmodify it is void, and will automatically terminate your rights under\nthis License (including any patent licenses granted under the third\nparagraph of section 11).\n\n  However, if you cease all violation of this License, then your\nlicense from a particular copyright holder is reinstated (a)\nprovisionally, unless and until the copyright holder explicitly and\nfinally terminates your license, and (b) permanently, if the copyright\nholder fails to notify you of the violation by some reasonable means\nprior to 60 days after the cessation.\n\n  Moreover, your license from a particular copyright holder is\nreinstated permanently if the copyright holder notifies you of the\nviolation by some reasonable means, this is the first time you have\nreceived notice of violation of this License (for any work) from that\ncopyright holder, and you cure the violation prior to 30 days after\nyour receipt of the notice.\n\n  Termination of your rights under this section does not terminate the\nlicenses of parties who have received copies or rights from you under\nthis License.  If your rights have been terminated and not permanently\nreinstated, you do not qualify to receive new licenses for the same\nmaterial under section 10.\n\n  9. Acceptance Not Required for Having Copies.\n\n  You are not required to accept this License in order to receive or\nrun a copy of the Program.  Ancillary propagation of a covered work\noccurring solely as a consequence of using peer-to-peer transmission\nto receive a copy likewise does not require acceptance.  However,\nnothing other than this License grants you permission to propagate or\nmodify any covered work.  These actions infringe copyright if you do\nnot accept this License.  Therefore, by modifying or propagating a\ncovered work, you indicate your acceptance of this License to do so.\n\n  10. Automatic Licensing of Downstream Recipients.\n\n  Each time you convey a covered work, the recipient automatically\nreceives a license from the original licensors, to run, modify and\npropagate that work, subject to this License.  You are not responsible\nfor enforcing compliance by third parties with this License.\n\n  An \"entity transaction\" is a transaction transferring control of an\norganization, or substantially all assets of one, or subdividing an\norganization, or merging organizations.  If propagation of a covered\nwork results from an entity transaction, each party to that\ntransaction who receives a copy of the work also receives whatever\nlicenses to the work the party's predecessor in interest had or could\ngive under the previous paragraph, plus a right to possession of the\nCorresponding Source of the work from the predecessor in interest, if\nthe predecessor has it or can get it with reasonable efforts.\n\n  You may not impose any further restrictions on the exercise of the\nrights granted or affirmed under this License.  For example, you may\nnot impose a license fee, royalty, or other charge for exercise of\nrights granted under this License, and you may not initiate litigation\n(including a cross-claim or counterclaim in a lawsuit) alleging that\nany patent claim is infringed by making, using, selling, offering for\nsale, or importing the Program or any portion of it.\n\n  11. Patents.\n\n  A \"contributor\" is a copyright holder who authorizes use under this\nLicense of the Program or a work on which the Program is based.  The\nwork thus licensed is called the contributor's \"contributor version\".\n\n  A contributor's \"essential patent claims\" are all patent claims\nowned or controlled by the contributor, whether already acquired or\nhereafter acquired, that would be infringed by some manner, permitted\nby this License, of making, using, or selling its contributor version,\nbut do not include claims that would be infringed only as a\nconsequence of further modification of the contributor version.  For\npurposes of this definition, \"control\" includes the right to grant\npatent sublicenses in a manner consistent with the requirements of\nthis License.\n\n  Each contributor grants you a non-exclusive, worldwide, royalty-free\npatent license under the contributor's essential patent claims, to\nmake, use, sell, offer for sale, import and otherwise run, modify and\npropagate the contents of its contributor version.\n\n  In the following three paragraphs, a \"patent license\" is any express\nagreement or commitment, however denominated, not to enforce a patent\n(such as an express permission to practice a patent or covenant not to\nsue for patent infringement).  To \"grant\" such a patent license to a\nparty means to make such an agreement or commitment not to enforce a\npatent against the party.\n\n  If you convey a covered work, knowingly relying on a patent license,\nand the Corresponding Source of the work is not available for anyone\nto copy, free of charge and under the terms of this License, through a\npublicly available network server or other readily accessible means,\nthen you must either (1) cause the Corresponding Source to be so\navailable, or (2) arrange to deprive yourself of the benefit of the\npatent license for this particular work, or (3) arrange, in a manner\nconsistent with the requirements of this License, to extend the patent\nlicense to downstream recipients.  \"Knowingly relying\" means you have\nactual knowledge that, but for the patent license, your conveying the\ncovered work in a country, or your recipient's use of the covered work\nin a country, would infringe one or more identifiable patents in that\ncountry that you have reason to believe are valid.\n\n  If, pursuant to or in connection with a single transaction or\narrangement, you convey, or propagate by procuring conveyance of, a\ncovered work, and grant a patent license to some of the parties\nreceiving the covered work authorizing them to use, propagate, modify\nor convey a specific copy of the covered work, then the patent license\nyou grant is automatically extended to all recipients of the covered\nwork and works based on it.\n\n  A patent license is \"discriminatory\" if it does not include within\nthe scope of its coverage, prohibits the exercise of, or is\nconditioned on the non-exercise of one or more of the rights that are\nspecifically granted under this License.  You may not convey a covered\nwork if you are a party to an arrangement with a third party that is\nin the business of distributing software, under which you make payment\nto the third party based on the extent of your activity of conveying\nthe work, and under which the third party grants, to any of the\nparties who would receive the covered work from you, a discriminatory\npatent license (a) in connection with copies of the covered work\nconveyed by you (or copies made from those copies), or (b) primarily\nfor and in connection with specific products or compilations that\ncontain the covered work, unless you entered into that arrangement,\nor that patent license was granted, prior to 28 March 2007.\n\n  Nothing in this License shall be construed as excluding or limiting\nany implied license or other defenses to infringement that may\notherwise be available to you under applicable patent law.\n\n  12. No Surrender of Others' Freedom.\n\n  If conditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot convey a\ncovered work so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you may\nnot convey it at all.  For example, if you agree to terms that obligate you\nto collect a royalty for further conveying from those to whom you convey\nthe Program, the only way you could satisfy both those terms and this\nLicense would be to refrain entirely from conveying the Program.\n\n  13. Use with the GNU Affero General Public License.\n\n  Notwithstanding any other provision of this License, you have\npermission to link or combine any covered work with a work licensed\nunder version 3 of the GNU Affero General Public License into a single\ncombined work, and to convey the resulting work.  The terms of this\nLicense will continue to apply to the part which is the covered work,\nbut the special requirements of the GNU Affero General Public License,\nsection 13, concerning interaction through a network will apply to the\ncombination as such.\n\n  14. Revised Versions of this License.\n\n  The Free Software Foundation may publish revised and/or new versions of\nthe GNU General Public License from time to time.  Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\n  Each version is given a distinguishing version number.  If the\nProgram specifies that a certain numbered version of the GNU General\nPublic License \"or any later version\" applies to it, you have the\noption of following the terms and conditions either of that numbered\nversion or of any later version published by the Free Software\nFoundation.  If the Program does not specify a version number of the\nGNU General Public License, you may choose any version ever published\nby the Free Software Foundation.\n\n  If the Program specifies that a proxy can decide which future\nversions of the GNU General Public License can be used, that proxy's\npublic statement of acceptance of a version permanently authorizes you\nto choose that version for the Program.\n\n  Later license versions may give you additional or different\npermissions.  However, no additional obligations are imposed on any\nauthor or copyright holder as a result of your choosing to follow a\nlater version.\n\n  15. Disclaimer of Warranty.\n\n  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY\nAPPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT\nHOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY\nOF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,\nTHE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM\nIS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF\nALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. Limitation of Liability.\n\n  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS\nTHE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY\nGENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE\nUSE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF\nDATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD\nPARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),\nEVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGES.\n\n  17. Interpretation of Sections 15 and 16.\n\n  If the disclaimer of warranty and limitation of liability provided\nabove cannot be given local legal effect according to their terms,\nreviewing courts shall apply local law that most closely approximates\nan absolute waiver of all civil liability in connection with the\nProgram, unless a warranty or assumption of liability accompanies a\ncopy of the Program in return for a fee.\n\n                     END OF TERMS AND CONDITIONS\n\n            How to Apply These Terms to Your New Programs\n\n  If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n  To do so, attach the following notices to the program.  It is safest\nto attach them to the start of each source file to most effectively\nstate the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n    {one line to give the program's name and a brief idea of what it does.}\n    Copyright (C) {year}  {name of author}\n\n    This program is free software: you can redistribute it and/or modify\n    it under the terms of the GNU General Public License as published by\n    the Free Software Foundation, either version 3 of the License, or\n    (at your option) any later version.\n\n    This program is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n    GNU General Public License for more details.\n\n    You should have received a copy of the GNU General Public License\n    along with this program.  If not, see <http://www.gnu.org/licenses/>.\n\nAlso add information on how to contact you by electronic and paper mail.\n\n  If the program does terminal interaction, make it output a short\nnotice like this when it starts in an interactive mode:\n\n    {project}  Copyright (C) {year}  {fullname}\n    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n    This is free software, and you are welcome to redistribute it\n    under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License.  Of course, your program's commands\nmight be different; for a GUI interface, you would use an \"about box\".\n\n  You should also get your employer (if you work as a programmer) or school,\nif any, to sign a \"copyright disclaimer\" for the program, if necessary.\nFor more information on this, and how to apply and follow the GNU GPL, see\n<http://www.gnu.org/licenses/>.\n\n  The GNU General Public License does not permit incorporating your program\ninto proprietary programs.  If your program is a subroutine library, you\nmay consider it more useful to permit linking proprietary applications with\nthe library.  If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.  But first, please read\n<http://www.gnu.org/philosophy/why-not-lgpl.html>.\n"
  },
  {
    "path": "README.md",
    "content": "# This  repositories is a version of HKUST-Aerial-Robotics/VINS-Mono without ROS mechanism. The project can be passed on the euroc dataset so far. [EuRoc](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) ASL data format, this version can run both on 32bit OS and 64 bit OS platform.\n\n# thanks to the reference program： \n# [HKUST-Aerial-Robotics/VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono)\n# What need to be installed\n```\n  1. Ceres Solver\n  2. Opencv 3.1\n  3. Eigen 3.2.0\n  4. boost\n  5. Pangolin\n```\n# How to build:\n```\n  1. mkdir VINS_Workspace\n  2. cd VINS_Workspace\n  3. git clone git@github.com:heguixiang/Remove_ROS_VINS.git\n  4. mv Remove_ROS_VINS src\n  5. ./generate.sh\n``` \n# How to run:\n```\n  1. cd VINS_Workspace\n  2. mkdir -p data/image/MH_01_easy\n  3. mkdir -p data/imu/\n  4. cd VINS_Workspace/data & git clone git@github.com:heguixiang/EuRoc-Timestamps.git\n  5. download the EuRoc dataset(e.g. Machine Hall 01)\n  6. cp the MH_01_easy/mav0/cam0/data to data/image/MH_01_easy\n  7. cp the MH_01_easy/mav0/imu0/data.csv to data/imu/\n  8. cd VINS_Workspace\n  9. ./src/vins_estimator/build/vins_estimator ./src/config/euroc/euroc_config.yaml ./data/image/MH_01_easy/data/ ./data/EuRoc-Timestamps/MH01.txt ./data/imu/data.csv\n```  \n"
  },
  {
    "path": "ar_demo/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(ar_demo)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11 -DEIGEN_DONT_PARALLELIZE\")\n#-DEIGEN_USE_MKL_ALL\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g\")\n\nfind_package(catkin REQUIRED COMPONENTS\n  roscpp\n  rospy\n  std_msgs\n  image_transport\n  sensor_msgs\n  cv_bridge\n  message_filters\n  camera_model\n)\nfind_package(OpenCV REQUIRED)\n\ncatkin_package(\n\n)\n\n\ninclude_directories(\n  ${catkin_INCLUDE_DIRS}\n)\n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3  REQUIRED)\ninclude_directories(\n  ${catkin_INCLUDE_DIRS}\n  ${EIGEN3_INCLUDE_DIR}\n)\n\nadd_executable(ar_demo_node src/ar_demo_node.cpp)\n\n target_link_libraries(ar_demo_node\n   ${catkin_LIBRARIES} ${OpenCV_LIBS}\n )\n\n\n"
  },
  {
    "path": "ar_demo/CMakeLists.txt.back",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(ar_demo)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11 -DEIGEN_DONT_PARALLELIZE\")\n#-DEIGEN_USE_MKL_ALL\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g\")\n\nfind_package(catkin REQUIRED COMPONENTS\n  roscpp\n  rospy\n  std_msgs\n  image_transport\n  sensor_msgs\n  cv_bridge\n  message_filters\n  camera_model\n)\nfind_package(OpenCV REQUIRED)\n\ncatkin_package(\n\n)\n\n\ninclude_directories(\n  ${catkin_INCLUDE_DIRS}\n)\n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3  REQUIRED)\ninclude_directories(\n  ${catkin_INCLUDE_DIRS}\n  ${EIGEN3_INCLUDE_DIR}\n)\n\nadd_executable(ar_demo_node src/ar_demo_node.cpp)\n\n target_link_libraries(ar_demo_node\n   ${catkin_LIBRARIES} ${OpenCV_LIBS}\n )\n\n\n"
  },
  {
    "path": "ar_demo/launch/3dm_bag.launch",
    "content": "<launch>\n\t<include file=\"$(find vins_estimator)/launch/3dm.launch\"/>\n\n    <node pkg=\"ar_demo\" type=\"ar_demo_node\" name=\"ar_demo_node\" output=\"screen\">\n        <remap from=\"~image_raw\" to=\"/mv_25001498/image_raw\"/>\n        <remap from=\"~camera_pose\" to=\"/vins_estimator/camera_pose\"/>\n        <remap from=\"~pointcloud\" to=\"/vins_estimator/point_cloud\"/>\n        <param name=\"calib_file\" type=\"string\" value=\"$(find feature_tracker)/../config/3dm/3dm_config.yaml\"/>\n        <param name=\"use_undistored_img\" type=\"bool\" value=\"false\"/>\n    </node>\n</launch>"
  },
  {
    "path": "ar_demo/launch/ar_A3.launch",
    "content": "<launch>\n\n    <include file=\"$(find vins_estimator)/launch/A3.launch\"/>\n\n    <node pkg=\"ar_demo\" type=\"ar_demo_node\" name=\"ar_demo_node\" output=\"screen\">\n        <remap from=\"~image_raw\" to=\"/djiros/image\"/>\n        <remap from=\"~camera_pose\" to=\"/vins_estimator/camera_pose\"/>\n        <remap from=\"~pointcloud\" to=\"/vins_estimator/point_cloud\"/>\n        <param name=\"calib_file\" type=\"string\" value=\"$(find feature_tracker)/../config/A3/A3_config.yaml\"/>\n        <param name=\"use_undistored_img\" type=\"bool\" value=\"false\"/>\n    </node>\n</launch>"
  },
  {
    "path": "ar_demo/launch/ar_rviz.launch",
    "content": "<launch>\n    <node name=\"rvizvisualisation\" pkg=\"rviz\" type=\"rviz\" output=\"log\" args=\"-d $(find ar_demo)/../config/AR_demo.rviz\" />\n</launch>"
  },
  {
    "path": "ar_demo/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>ar_demo</name>\n  <version>0.0.0</version>\n  <description>The ar_demo package</description>\n\n  <!-- One maintainer tag required, multiple allowed, one person per tag --> \n  <!-- Example:  -->\n  <!-- <maintainer email=\"jane.doe@example.com\">Jane Doe</maintainer> -->\n  <maintainer email=\"qintonguav@gmail.com\">tony-ws</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>TODO</license>\n\n\n  <!-- Url tags are optional, but mutiple are allowed, one per tag -->\n  <!-- Optional attribute type can be: website, bugtracker, or repository -->\n  <!-- Example: -->\n  <!-- <url type=\"website\">http://wiki.ros.org/AR_demo</url> -->\n\n\n  <!-- Author tags are optional, mutiple are allowed, one per tag -->\n  <!-- Authors do not have to be maintianers, but could be -->\n  <!-- Example: -->\n  <!-- <author email=\"jane.doe@example.com\">Jane Doe</author> -->\n\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <build_depend>roscpp</build_depend>\n  <build_depend>rospy</build_depend>\n  <build_depend>std_msgs</build_depend>\n  <build_depend>camera_model</build_depend>\n  <run_depend>roscpp</run_depend>\n  <run_depend>rospy</run_depend>\n  <run_depend>std_msgs</run_depend>\n  <run_depend>camera_model</run_depend>\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>"
  },
  {
    "path": "ar_demo/package.xml.back",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>ar_demo</name>\n  <version>0.0.0</version>\n  <description>The ar_demo package</description>\n\n  <!-- One maintainer tag required, multiple allowed, one person per tag --> \n  <!-- Example:  -->\n  <!-- <maintainer email=\"jane.doe@example.com\">Jane Doe</maintainer> -->\n  <maintainer email=\"qintonguav@gmail.com\">tony-ws</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>TODO</license>\n\n\n  <!-- Url tags are optional, but mutiple are allowed, one per tag -->\n  <!-- Optional attribute type can be: website, bugtracker, or repository -->\n  <!-- Example: -->\n  <!-- <url type=\"website\">http://wiki.ros.org/AR_demo</url> -->\n\n\n  <!-- Author tags are optional, mutiple are allowed, one per tag -->\n  <!-- Authors do not have to be maintianers, but could be -->\n  <!-- Example: -->\n  <!-- <author email=\"jane.doe@example.com\">Jane Doe</author> -->\n\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <build_depend>roscpp</build_depend>\n  <build_depend>rospy</build_depend>\n  <build_depend>std_msgs</build_depend>\n  <build_depend>camera_model</build_depend>\n  <run_depend>roscpp</run_depend>\n  <run_depend>rospy</run_depend>\n  <run_depend>std_msgs</run_depend>\n  <run_depend>camera_model</run_depend>\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>"
  },
  {
    "path": "ar_demo/src/ar_demo_node.cpp",
    "content": "#include <ros/ros.h>\n#include <std_msgs/ColorRGBA.h>\n#include <visualization_msgs/Marker.h>\n#include <visualization_msgs/MarkerArray.h>\n#include <nav_msgs/Odometry.h>\n#include <geometry_msgs/PoseStamped.h>\n#include <sensor_msgs/Image.h>\n#include <sensor_msgs/image_encodings.h>\n#include <sensor_msgs/PointCloud.h>\n#include <cv_bridge/cv_bridge.h>\n#include <image_transport/image_transport.h>\n#include <cmath>\n#include <opencv2/opencv.hpp>\n#include <opencv2/highgui/highgui.hpp>\n#include <eigen3/Eigen/Dense>\n#include <eigen3/Eigen/Geometry>\n#include <vector>\n#include <message_filters/subscriber.h>\n#include <message_filters/time_synchronizer.h>\n#include <message_filters/synchronizer.h>\n#include <message_filters/sync_policies/approximate_time.h>\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include <queue>\n#include <cmath>\n#include <algorithm> \n\nusing namespace std;\nusing namespace Eigen;\nusing namespace sensor_msgs;\nusing namespace message_filters;\nusing namespace camodocal;\n\nint ROW;\nint COL;\ndouble FOCAL_LENGTH;\nconst int axis_num = 0;\nconst int cube_num = 1;\nconst double box_length = 0.8;\nbool USE_UNDISTORED_IMG;\nbool pose_init = false;\nint img_cnt = 0;\n\nros::Publisher object_pub;\nimage_transport::Publisher pub_ARimage;\nVector3d Axis[6];\nVector3d Cube_center[3];\nvector<Vector3d> Cube_corner[3];\nvector<Vector3d> output_Axis[6];\nvector<Vector3d> output_Cube[3];\nvector<double> output_corner_dis[3];\ndouble Cube_center_depth[3];\nqueue<ImageConstPtr> img_buf;\ncamodocal::CameraPtr m_camera;\nbool look_ground = 0;\nstd_msgs::ColorRGBA line_color_r;\nstd_msgs::ColorRGBA line_color_g;\nstd_msgs::ColorRGBA line_color_b;\n\nvoid axis_generate(visualization_msgs::Marker &line_list, Vector3d &origin, int id)\n{\n\n    line_list.id = id;\n    line_list.header.frame_id = \"world\";\n    line_list.header.stamp = ros::Time::now();\n    line_list.action = visualization_msgs::Marker::ADD;\n    line_list.type = visualization_msgs::Marker::LINE_LIST;\n    line_list.scale.x = 0.1;\n    line_list.color.a = 1.0;\n    line_list.lifetime = ros::Duration();\n    \n    line_list.pose.orientation.w = 1.0;\n    line_list.color.b = 1.0;\n    geometry_msgs::Point p;\n    p.x = origin.x();\n    p.y = origin.y();\n    p.z = origin.z();\n    line_list.points.push_back(p);\n    line_list.colors.push_back(line_color_r);\n    p.x += 1.0;\n    line_list.points.push_back(p);\n    line_list.colors.push_back(line_color_r);\n    p.x -= 1.0;\n    line_list.points.push_back(p);\n    line_list.colors.push_back(line_color_g);\n    p.y += 1.0;\n    line_list.points.push_back(p);\n    line_list.colors.push_back(line_color_g);\n    p.y -= 1.0;\n    line_list.points.push_back(p);\n    line_list.colors.push_back(line_color_b);\n    p.z += 1.0;\n    line_list.points.push_back(p);\n    line_list.colors.push_back(line_color_b);\n}\n\nvoid cube_generate(visualization_msgs::Marker &marker, Vector3d &origin, int id)\n{\n\n    //uint32_t shape = visualization_msgs::Marker::CUBE;\n    marker.header.frame_id = \"world\";\n    marker.header.stamp = ros::Time::now();\n    marker.ns = \"basic_shapes\";\n    marker.id = 0;\n    //marker.type = shape;\n    marker.action = visualization_msgs::Marker::ADD;\n    marker.type = visualization_msgs::Marker::CUBE_LIST;\n    /*\n    marker.pose.position.x = origin.x();\n    marker.pose.position.y = origin.y();\n    marker.pose.position.z = origin.z();\n    marker.pose.orientation.x = 0.0;\n    marker.pose.orientation.y = 0.0;\n    marker.pose.orientation.z = 0.0;\n    marker.pose.orientation.w = 1.0;\n    */\n    marker.scale.x = box_length;\n    marker.scale.y = box_length;\n    marker.scale.z = box_length;\n\n    marker.color.r = 0.0f;\n    marker.color.g = 1.0f;\n    marker.color.b = 0.0f;\n    marker.color.a = 1.0;\n\n    marker.lifetime = ros::Duration();  \n    geometry_msgs::Point p;\n    p.x = origin.x();\n    p.y = origin.y();\n    p.z = origin.z();\n    marker.points.push_back(p);\n    marker.colors.push_back(line_color_r);\n    Cube_corner[id].clear();\n    Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() - box_length / 2, origin.z() - box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() - box_length / 2, origin.z() - box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() + box_length / 2, origin.z() - box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() + box_length / 2, origin.z() - box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() - box_length / 2, origin.z() + box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() - box_length / 2, origin.z() + box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() + box_length / 2, origin.z() + box_length / 2));\n    Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() + box_length / 2, origin.z() + box_length / 2));\n}\n\nvoid add_object()\n{\n    visualization_msgs::MarkerArray markerArray_msg;\n\n    visualization_msgs::Marker line_list;\n    visualization_msgs::Marker cube_list;\n\n    for (int i = 0; i < axis_num; i++)\n    {\n        axis_generate(line_list, Axis[i], i);\n        markerArray_msg.markers.push_back(line_list);\n    }\n\n    for (int i = 0; i <cube_num; i++)\n    {\n        cube_generate(cube_list, Cube_center[i], i);\n    }\n    //cube_generate(cube_list, Cube_center[2], 2);\n    markerArray_msg.markers.push_back(cube_list);\n\n    object_pub.publish(markerArray_msg);\n}\n\nvoid project_object(Vector3d camera_p, Quaterniond camera_q)\n{\n    for (int i = 0; i < axis_num; i++)\n    {\n        output_Axis[i].clear();\n        Vector3d local_point;\n        Vector2d local_uv;\n        local_point = camera_q.inverse() * (Axis[i] - camera_p);\n        m_camera->spaceToPlane(local_point, local_uv);\n\n        if (local_point.z() > 0)\n            //&& 0 <= local_uv.x() && local_uv.x() <= COL - 1 && 0 <= local_uv.y() && local_uv.y() <= ROW -1)\n        {\n            output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));\n\n            local_point = camera_q.inverse() * (Axis[i] + Vector3d(1, 0, 0) - camera_p);\n            m_camera->spaceToPlane(local_point, local_uv);\n            output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));\n\n            local_point = camera_q.inverse() * (Axis[i] + Vector3d(0, 1, 0) - camera_p);\n            m_camera->spaceToPlane(local_point, local_uv);\n            output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));\n\n            local_point = camera_q.inverse() * (Axis[i] + Vector3d(0, 0, 1) - camera_p);\n            m_camera->spaceToPlane(local_point, local_uv);\n            output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));\n\n        }\n    } \n\n    for (int i = 0; i < cube_num; i++)\n    {\n        output_Cube[i].clear();\n        output_corner_dis[i].clear();\n        Vector3d local_point;\n        Vector2d local_uv;\n        local_point = camera_q.inverse() * (Cube_center[i] - camera_p);\n        if (USE_UNDISTORED_IMG)\n        {\n            local_uv.x() = local_point(0) / local_point(2) * FOCAL_LENGTH + COL / 2;\n            local_uv.y() = local_point(1) / local_point(2) * FOCAL_LENGTH + ROW / 2;\n        }\n        else\n            m_camera->spaceToPlane(local_point, local_uv);\n        if (local_point.z() > box_length / 2)\n           //&& 0 <= local_uv.x() && local_uv.x() <= COL - 1 && 0 <= local_uv.y() && local_uv.y() <= ROW -1)\n        {\n            Cube_center_depth[i] = local_point.z();\n            for (int j = 0; j < 8; j++)\n            {\n                local_point = camera_q.inverse() * (Cube_corner[i][j] - camera_p);\n                output_corner_dis[i].push_back(local_point.norm());\n                if (USE_UNDISTORED_IMG)\n                {\n                    //ROS_INFO(\"directly project!\");\n                    local_uv.x() = local_point(0) / local_point(2) * FOCAL_LENGTH + COL / 2;\n                    local_uv.y() = local_point(1) / local_point(2) * FOCAL_LENGTH + ROW / 2;\n                }\n                else\n                {\n                    //ROS_INFO(\"camera model project!\");\n                    m_camera->spaceToPlane(local_point, local_uv);\n                    local_uv.x() = std::min(std::max(-5000.0, local_uv.x()),5000.0);\n                    local_uv.y() = std::min(std::max(-5000.0, local_uv.y()),5000.0);\n                }\n                output_Cube[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));\n            }\n        }\n        else\n        {\n            Cube_center_depth[i] = -1;\n        }\n\n    }   \n}\n\nvoid draw_object(cv::Mat &AR_image)\n{\n    for (int i = 0; i < axis_num; i++)\n    {\n        if(output_Axis[i].empty())\n            continue;\n        cv::Point2d origin(output_Axis[i][0].x(), output_Axis[i][0].y());\n        cv::Point2d axis_x(output_Axis[i][1].x(), output_Axis[i][1].y());\n        cv::Point2d axis_y(output_Axis[i][2].x(), output_Axis[i][2].y());\n        cv::Point2d axis_z(output_Axis[i][3].x(), output_Axis[i][3].y());\n        cv::line(AR_image, origin, axis_x, cv::Scalar(0, 0, 255), 2, 8, 0);\n        cv::line(AR_image, origin, axis_y, cv::Scalar(0, 255, 0), 2, 8, 0);\n        cv::line(AR_image, origin, axis_z, cv::Scalar(255, 0, 0), 2, 8, 0);\n    }\n\n    //depth sort  big---->small\n    int index[cube_num];\n    for (int i = 0; i < cube_num; i++)\n    {\n        index[i] = i;\n        //cout << \"i \" << i << \" init depth\" << Cube_center_depth[i] << endl;\n    }\n    for (int i = 0; i < cube_num; i++)\n        for (int j = 0; j < cube_num - i - 1; j++)\n        {\n            if (Cube_center_depth[j] < Cube_center_depth[j + 1])\n            {\n                double tmp = Cube_center_depth[j];\n                Cube_center_depth[j] = Cube_center_depth[j + 1];\n                Cube_center_depth[j + 1] = tmp;\n                int tmp_index = index[j];\n                index[j] = index[j + 1];\n                index[j + 1] = tmp_index;\n            }\n        }\n\n    for (int k = 0; k < cube_num; k++)\n    {\n        int i = index[k];\n        //cout << \"draw \" << i << \"depth \" << Cube_center_depth[i] << endl;\n        if (output_Cube[i].empty())\n            continue;\n        //draw color\n        cv::Point* p = new cv::Point[8];\n        p[0] = cv::Point(output_Cube[i][0].x(), output_Cube[i][0].y());\n        p[1] = cv::Point(output_Cube[i][1].x(), output_Cube[i][1].y());\n        p[2] = cv::Point(output_Cube[i][2].x(), output_Cube[i][2].y());\n        p[3] = cv::Point(output_Cube[i][3].x(), output_Cube[i][3].y());\n        p[4] = cv::Point(output_Cube[i][4].x(), output_Cube[i][4].y());\n        p[5] = cv::Point(output_Cube[i][5].x(), output_Cube[i][5].y());\n        p[6] = cv::Point(output_Cube[i][6].x(), output_Cube[i][6].y());\n        p[7] = cv::Point(output_Cube[i][7].x(), output_Cube[i][7].y());\n        \n        int npts[1] = {4};\n        float min_depth = 100000;\n        int min_index = 5;\n        for(int j= 0; j < (int)output_corner_dis[i].size(); j++)\n        {\n            if(output_corner_dis[i][j] < min_depth)\n            {\n                min_depth = output_corner_dis[i][j];\n                min_index = j;\n            }\n        }\n        \n        cv::Point plain[1][4];\n        const cv::Point* ppt[1] = {plain[0]};\n        //first draw large depth plane\n        int point_group[8][12] = {{0,1,5,4, 0,4,6,2, 0,1,3,2},\n            {0,1,5,4, 1,5,7,3, 0,1,3,2},\n            {2,3,7,6, 0,4,6,2, 0,1,3,2},\n            {2,3,7,6, 1,5,7,3, 0,1,3,2},\n            {0,1,5,4, 0,4,6,2, 4,5,7,6},\n            {0,1,5,4, 1,5,7,3, 4,5,7,6},\n            {2,3,7,6, 0,4,6,2, 4,5,7,6},\n            {2,3,7,6, 1,5,7,3, 4,5,7,6}};\n        \n        plain[0][0] = p[point_group[min_index][4]];\n        plain[0][1] = p[point_group[min_index][5]];\n        plain[0][2] = p[point_group[min_index][6]];\n        plain[0][3] = p[point_group[min_index][7]];\n        cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 200, 0));\n        \n        plain[0][0] = p[point_group[min_index][0]];\n        plain[0][1] = p[point_group[min_index][1]];\n        plain[0][2] = p[point_group[min_index][2]];\n        plain[0][3] = p[point_group[min_index][3]];\n        cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(200, 0, 0));\n        \n        if(output_corner_dis[i][point_group[min_index][2]] + output_corner_dis[i][point_group[min_index][3]] >\n           output_corner_dis[i][point_group[min_index][5]] + output_corner_dis[i][point_group[min_index][6]])\n        {\n            plain[0][0] = p[point_group[min_index][4]];\n            plain[0][1] = p[point_group[min_index][5]];\n            plain[0][2] = p[point_group[min_index][6]];\n            plain[0][3] = p[point_group[min_index][7]];\n            cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 200, 0));\n\n        }\n        plain[0][0] = p[point_group[min_index][8]];\n        plain[0][1] = p[point_group[min_index][9]];\n        plain[0][2] = p[point_group[min_index][10]];\n        plain[0][3] = p[point_group[min_index][11]];\n        cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 0, 200));\n        delete p;\n    }\n}\n\nvoid callback(const ImageConstPtr& img_msg, const geometry_msgs::PoseStamped::ConstPtr& pose_msg)\n{\n    //throw the first few unstable pose\n    if(img_cnt < 20)\n    {\n        img_cnt ++;\n        return;\n    }\n   //ROS_INFO(\"sync callback!\");\n   Vector3d camera_p(pose_msg->pose.position.x,\n                     pose_msg->pose.position.y,\n                     pose_msg->pose.position.z);\n   Quaterniond camera_q(pose_msg->pose.orientation.w,\n                        pose_msg->pose.orientation.x,\n                        pose_msg->pose.orientation.y,\n                        pose_msg->pose.orientation.z);\n\n   //test plane\n   Vector3d cam_z(0, 0, -1);\n   Vector3d w_cam_z = camera_q * cam_z;\n   //cout << \"angle \" << acos(w_cam_z.dot(Vector3d(0, 0, 1))) * 180.0 / M_PI << endl;\n   if (acos(w_cam_z.dot(Vector3d(0, 0, 1))) * 180.0 / M_PI < 90)\n   {\n        //ROS_WARN(\" look down\");\n        look_ground = 1;\n   }\n   else\n        look_ground = 0;\n\n   project_object(camera_p, camera_q);\n\n   cv_bridge::CvImagePtr bridge_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);\n   cv::Mat AR_image;\n   AR_image = bridge_ptr->image.clone();\n   cv::cvtColor(AR_image, AR_image, cv::COLOR_GRAY2RGB);\n   draw_object(AR_image);\n\n   sensor_msgs::ImagePtr AR_msg = cv_bridge::CvImage(img_msg->header, \"bgr8\", AR_image).toImageMsg();\n   pub_ARimage.publish(AR_msg);\n\n}\nvoid point_callback(const sensor_msgs::PointCloudConstPtr &point_msg)\n{\n    if (!look_ground)\n        return;\n    int height_range[30];\n    double height_sum[30];\n    for (int i = 0; i < 30; i++)\n    {\n        height_range[i] = 0;\n        height_sum[i] = 0;\n    }\n    for (unsigned int i = 0; i < point_msg->points.size(); i++)\n    {\n        //double x = point_msg->points[i].x;\n        //double y = point_msg->points[i].y;\n        double z = point_msg->points[i].z;\n        int index = (z + 2.0) / 0.1;\n        if (0 <= index && index < 30)\n        {\n            height_range[index]++;\n            height_sum[index] += z;\n        }\n        //cout << \"point \" << \" z \" << z << endl;\n    }\n    int max_num = 0;\n    int max_index = -1;\n    for (int i = 1; i < 29; i++)\n    {\n        if (max_num < height_range[i])\n        {\n            max_num = height_range[i];\n            max_index = i;\n        }\n    }\n    if (max_index == -1)\n        return;\n    int neigh_num = height_range[max_index - 1] + height_range[max_index] + height_range[max_index + 1];\n    double neigh_height = (height_sum[max_index - 1] + height_sum[max_index] + height_sum[max_index + 1]) / neigh_num;\n    //ROS_WARN(\"detect ground plain, height %f\", neigh_height);\n    if (neigh_num < (int)point_msg->points.size() / 2)\n    {\n        //ROS_INFO(\"points not enough\");\n        return;\n    }\n    //update height\n    for (int i = 0; i < cube_num; i++)\n    {\n        Cube_center[i].z() = neigh_height + box_length / 2.0;\n    }\n    add_object();\n\n}\nvoid img_callback(const ImageConstPtr& img_msg)\n{\n    if(pose_init)\n    {\n        img_buf.push(img_msg);\n    }\n    else\n        return;\n}\nvoid pose_callback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)\n{\n    if(!pose_init)\n    {\n        pose_init = true;\n        return;\n    }\n\n    if (img_buf.empty())\n    {\n        //ROS_WARN(\"image coming late\");\n        return;\n    }\n\n    while (img_buf.front()->header.stamp < pose_msg->header.stamp && !img_buf.empty())\n    {\n        img_buf.pop();\n    }\n\n    if (!img_buf.empty())\n    {\n        callback(img_buf.front(), pose_msg);\n        img_buf.pop();\n    }\n    //else\n    //    ROS_WARN(\"image coming late\");\n}\nint main( int argc, char** argv )\n{\n    ros::init(argc, argv, \"points_and_lines\");\n    ros::NodeHandle n(\"~\");\n    object_pub = n.advertise<visualization_msgs::MarkerArray>(\"AR_object\", 10);\n    n.getParam(\"use_undistored_img\", USE_UNDISTORED_IMG);\n    ros::Subscriber sub_img;\n    if (USE_UNDISTORED_IMG)\n    {\n        // the same as image crop\n        ROW = 600;\n        COL = 480;\n        FOCAL_LENGTH = 320.0;\n        sub_img = n.subscribe(\"image_undistored\", 100, img_callback);\n    }\n    else\n    {\n        ROW = 752;\n        COL = 480;\n        FOCAL_LENGTH = 460.0;\n        sub_img = n.subscribe(\"image_raw\", 100, img_callback);\n    }\n\n    Axis[0] = Vector3d(0, 2, -1.2);\n    Axis[1]= Vector3d(-10, 5, 0);\n    Axis[2] = Vector3d(3, 3, 3);\n    Axis[3] = Vector3d(-2, 2, 0);\n    Axis[4]= Vector3d(5, 10, -5);\n    Axis[5] = Vector3d(0, 10, -1);\n\n    Cube_center[0] = Vector3d(-2, 0, -1.2 + box_length / 2.0);\n    //Cube_center[0] = Vector3d(0, 3, -1.2 + box_length / 2.0);\n    Cube_center[1] = Vector3d(4, -2, -1.2 + box_length / 2.0);\n    Cube_center[2] = Vector3d(0, -2, -1.2 + box_length / 2.0);\n\n    ros::Subscriber pose_img = n.subscribe(\"camera_pose\", 100, pose_callback);\n    ros::Subscriber sub_point = n.subscribe(\"pointcloud\", 2000, point_callback);\n    image_transport::ImageTransport it(n);\n    pub_ARimage = it.advertise(\"AR_image\", 1000);\n\n    line_color_r.r = 1.0;\n    line_color_r.a = 1.0;\n    line_color_g.g = 1.0;\n    line_color_g.a = 1.0;\n    line_color_b.b = 1.0;\n    line_color_b.a = 1.0;\n\n    string calib_file;\n    n.getParam(\"calib_file\", calib_file);\n    ROS_INFO(\"reading paramerter of camera %s\", calib_file.c_str());\n    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n\n    ros::Rate r(100);\n    ros::Duration(1).sleep();\n    add_object();\n    add_object();\n    ros::spin();\n}\n\n"
  },
  {
    "path": "benchmark_publisher/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(benchmark_publisher)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11 -DEIGEN_DONT_PARALLELIZE\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g -rdynamic\")\n\n\nfind_package(catkin REQUIRED COMPONENTS\n    roscpp\n    tf\n    )\n\ncatkin_package()\ninclude_directories(${catkin_INCLUDE_DIRS}) \n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3  REQUIRED)\ninclude_directories(\n  ${catkin_INCLUDE_DIRS}\n  ${EIGEN3_INCLUDE_DIR}\n)\n\nadd_executable(benchmark_publisher\n    src/benchmark_publisher_node.cpp\n    )\n\ntarget_link_libraries(benchmark_publisher ${catkin_LIBRARIES})\n"
  },
  {
    "path": "benchmark_publisher/CMakeLists.txt.back",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(benchmark_publisher)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11 -DEIGEN_DONT_PARALLELIZE\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g -rdynamic\")\n\n\nfind_package(catkin REQUIRED COMPONENTS\n    roscpp\n    tf\n    )\n\ncatkin_package()\ninclude_directories(${catkin_INCLUDE_DIRS}) \n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3  REQUIRED)\ninclude_directories(\n  ${catkin_INCLUDE_DIRS}\n  ${EIGEN3_INCLUDE_DIR}\n)\n\nadd_executable(benchmark_publisher\n    src/benchmark_publisher_node.cpp\n    )\n\ntarget_link_libraries(benchmark_publisher ${catkin_LIBRARIES})\n"
  },
  {
    "path": "benchmark_publisher/launch/publish.launch",
    "content": "<launch>\n<!--MH_01_easy MH_02_easy MH_03_medium MH_04_difficult MH_05_difficult V1_01_easy V1_02_medium V1_03_difficult V2_01_easy V2_02_medium V2_03_difficult  -->\n\t<arg name=\"sequence_name\" default = \"MH_05_difficult\" />\n\n    <node name=\"benchmark_publisher\" pkg=\"benchmark_publisher\" type=\"benchmark_publisher\" output=\"screen\">\n        <param name=\"data_name\" type=\"string\" value=\"$(find benchmark_publisher)/config/$(arg sequence_name)/data.csv\" />\n        <remap from=\"~estimated_odometry\" to=\"/vins_estimator/odometry\" />\n    </node>\n<!--\n    <node pkg=\"rosbag\" type=\"play\" name=\"player\" output=\"log\" \n    args=\"/home/tony-ws/bag/ijrr_euroc_mav_dataset/$(arg sequence_name)/$(arg sequence_name).bag -r 2\" />\n-->\n</launch>\n"
  },
  {
    "path": "benchmark_publisher/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>benchmark_publisher</name>\n  <version>0.0.0</version>\n  <description>The benchmark_publisher package</description>\n\n  <!-- One maintainer tag required, multiple allowed, one person per tag --> \n  <!-- Example:  -->\n  <!-- <maintainer email=\"jane.doe@example.com\">Jane Doe</maintainer> -->\n  <maintainer email=\"dvorak@todo.todo\">dvorak</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>TODO</license>\n\n\n  <!-- Url tags are optional, but mutiple are allowed, one per tag -->\n  <!-- Optional attribute type can be: website, bugtracker, or repository -->\n  <!-- Example: -->\n  <!-- <url type=\"website\">http://wiki.ros.org/benchmark_publisher</url> -->\n\n\n  <!-- Author tags are optional, mutiple are allowed, one per tag -->\n  <!-- Authors do not have to be maintianers, but could be -->\n  <!-- Example: -->\n  <!-- <author email=\"jane.doe@example.com\">Jane Doe</author> -->\n\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <build_depend>roscpp</build_depend>\n  <run_depend>roscpp</run_depend>\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>"
  },
  {
    "path": "benchmark_publisher/package.xml.back",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>benchmark_publisher</name>\n  <version>0.0.0</version>\n  <description>The benchmark_publisher package</description>\n\n  <!-- One maintainer tag required, multiple allowed, one person per tag --> \n  <!-- Example:  -->\n  <!-- <maintainer email=\"jane.doe@example.com\">Jane Doe</maintainer> -->\n  <maintainer email=\"dvorak@todo.todo\">dvorak</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>TODO</license>\n\n\n  <!-- Url tags are optional, but mutiple are allowed, one per tag -->\n  <!-- Optional attribute type can be: website, bugtracker, or repository -->\n  <!-- Example: -->\n  <!-- <url type=\"website\">http://wiki.ros.org/benchmark_publisher</url> -->\n\n\n  <!-- Author tags are optional, mutiple are allowed, one per tag -->\n  <!-- Authors do not have to be maintianers, but could be -->\n  <!-- Example: -->\n  <!-- <author email=\"jane.doe@example.com\">Jane Doe</author> -->\n\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <build_depend>roscpp</build_depend>\n  <run_depend>roscpp</run_depend>\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>"
  },
  {
    "path": "benchmark_publisher/src/benchmark_publisher_node.cpp",
    "content": "#include <cstdio>\n#include <vector>\n#include <ros/ros.h>\n#include <nav_msgs/Odometry.h>\n#include <nav_msgs/Path.h>\n#include <geometry_msgs/PoseStamped.h>\n#include <tf/transform_broadcaster.h>\n#include <fstream>\n#include <eigen3/Eigen/Dense>\n\nusing namespace std;\nusing namespace Eigen;\n\nconst int SKIP = 50;\nstring benchmark_output_path;\nstring estimate_output_path;\ntemplate <typename T>\nT readParam(ros::NodeHandle &n, std::string name)\n{\n    T ans;\n    if (n.getParam(name, ans))\n    {\n        ROS_INFO_STREAM(\"Loaded \" << name << \": \" << ans);\n    }\n    else\n    {\n        ROS_ERROR_STREAM(\"Failed to load \" << name);\n        n.shutdown();\n    }\n    return ans;\n}\n\nstruct Data\n{\n    Data(FILE *f)\n    {\n        fscanf(f, \" %lf,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\", &t,\n               &px, &py, &pz,\n               &qw, &qx, &qy, &qz,\n               &vx, &vy, &vz,\n               &wx, &wy, &wz,\n               &ax, &ay, &az);\n        t /= 1e9;\n    }\n    double t;\n    float px, py, pz;\n    float qw, qx, qy, qz;\n    float vx, vy, vz;\n    float wx, wy, wz;\n    float ax, ay, az;\n};\nint idx = 1;\nvector<Data> benchmark;\n\nros::Publisher pub_odom;\nros::Publisher pub_path;\nnav_msgs::Path path;\n\nint init = 0;\nQuaterniond baseRgt;\nVector3d baseTgt;\ntf::Transform trans;\n\nvoid odom_callback(const nav_msgs::OdometryConstPtr &odom_msg)\n{\n    //ROS_INFO(\"odom callback!\");\n    if (odom_msg->header.stamp.toSec() > benchmark.back().t)\n      return;\n  \n    for (; idx < static_cast<int>(benchmark.size()) && benchmark[idx].t <= odom_msg->header.stamp.toSec(); idx++)\n        ;\n\n\n    if (init++ < SKIP)\n    {\n        baseRgt = Quaterniond(odom_msg->pose.pose.orientation.w,\n                              odom_msg->pose.pose.orientation.x,\n                              odom_msg->pose.pose.orientation.y,\n                              odom_msg->pose.pose.orientation.z) *\n                  Quaterniond(benchmark[idx - 1].qw,\n                              benchmark[idx - 1].qx,\n                              benchmark[idx - 1].qy,\n                              benchmark[idx - 1].qz).inverse();\n        baseTgt = Vector3d{odom_msg->pose.pose.position.x,\n                           odom_msg->pose.pose.position.y,\n                           odom_msg->pose.pose.position.z} -\n                  baseRgt * Vector3d{benchmark[idx - 1].px, benchmark[idx - 1].py, benchmark[idx - 1].pz};\n        return;\n    }\n\n    nav_msgs::Odometry odometry;\n    odometry.header.stamp = ros::Time(benchmark[idx - 1].t);\n    odometry.header.frame_id = \"world\";\n    odometry.child_frame_id = \"world\";\n\n    Vector3d tmp_T = baseTgt + baseRgt * Vector3d{benchmark[idx - 1].px, benchmark[idx - 1].py, benchmark[idx - 1].pz};\n    odometry.pose.pose.position.x = tmp_T.x();\n    odometry.pose.pose.position.y = tmp_T.y();\n    odometry.pose.pose.position.z = tmp_T.z();\n\n    Quaterniond tmp_R = baseRgt * Quaterniond{benchmark[idx - 1].qw,\n                                              benchmark[idx - 1].qx,\n                                              benchmark[idx - 1].qy,\n                                              benchmark[idx - 1].qz};\n    odometry.pose.pose.orientation.w = tmp_R.w();\n    odometry.pose.pose.orientation.x = tmp_R.x();\n    odometry.pose.pose.orientation.y = tmp_R.y();\n    odometry.pose.pose.orientation.z = tmp_R.z();\n\n    Vector3d tmp_V = baseRgt * Vector3d{benchmark[idx - 1].vx,\n                                        benchmark[idx - 1].vy,\n                                        benchmark[idx - 1].vz};\n    odometry.twist.twist.linear.x = tmp_V.x();\n    odometry.twist.twist.linear.y = tmp_V.y();\n    odometry.twist.twist.linear.z = tmp_V.z();\n    pub_odom.publish(odometry);\n\n    geometry_msgs::PoseStamped pose_stamped;\n    pose_stamped.header = odometry.header;\n    pose_stamped.pose = odometry.pose.pose;\n    path.header = odometry.header;\n    path.poses.push_back(pose_stamped);\n    pub_path.publish(path);\n}\n\nint main(int argc, char **argv)\n{\n    ros::init(argc, argv, \"benchmark_publisher\");\n    ros::NodeHandle n(\"~\");\n\n    string csv_file = readParam<string>(n, \"data_name\");\n    std::cout << \"load ground truth \" << csv_file << std::endl;\n    FILE *f = fopen(csv_file.c_str(), \"r\");\n    if (f==NULL)\n    {\n      ROS_WARN(\"can't load ground truth; wrong path\");\n      //std::cerr << \"can't load ground truth; wrong path \" << csv_file << std::endl;\n      return 0;\n    }\n    char tmp[10000];\n    fgets(tmp, 10000, f);\n    while (!feof(f))\n        benchmark.emplace_back(f);\n    fclose(f);\n    benchmark.pop_back();\n    ROS_INFO(\"Data loaded: %d\", (int)benchmark.size());\n\n    pub_odom = n.advertise<nav_msgs::Odometry>(\"odometry\", 1000);\n    pub_path = n.advertise<nav_msgs::Path>(\"path\", 1000);\n\n    ros::Subscriber sub_odom = n.subscribe(\"estimated_odometry\", 1000, odom_callback);\n    \n    ros::Rate r(20);\n    ros::spin();\n}\n"
  },
  {
    "path": "camera_model/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(camera_model)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -fPIC\")\n\n#find_package(catkin REQUIRED COMPONENTS\n   # roscpp\n   # std_msgs\n#    )\n\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\ninclude_directories(${Boost_INCLUDE_DIRS})\n\nfind_package(OpenCV REQUIRED)\n\n# set(EIGEN_INCLUDE_DIR \"/usr/local/include/eigen3\")\nfind_package(Ceres REQUIRED)\ninclude_directories(${CERES_INCLUDE_DIRS})\n\n\n#catkin_package(\n#   INCLUDE_DIRS include\n#   LIBRARIES camera_model\n   # CATKIN_DEPENDS roscpp std_msgs\n#    DEPENDS system_lib\n#   )\n\n#include_directories(\n#   ${catkin_INCLUDE_DIRS}\n#    )\n\ninclude_directories(\"include\")\n\n\n# add_executable(Calibration \n#     src/intrinsic_calib.cc\n#     src/chessboard/Chessboard.cc\n#     src/calib/CameraCalibration.cc\n#     src/camera_models/Camera.cc\n#     src/camera_models/CameraFactory.cc\n#     src/camera_models/CostFunctionFactory.cc\n#     src/camera_models/PinholeCamera.cc\n#     src/camera_models/CataCamera.cc\n#     src/camera_models/EquidistantCamera.cc\n#     src/camera_models/ScaramuzzaCamera.cc\n#     src/sparse_graph/Transform.cc\n#     src/gpl/gpl.cc\n#     src/gpl/EigenQuaternionParameterization.cc)\n\nadd_library(camera_model STATIC\n    src/chessboard/Chessboard.cc\n    src/calib/CameraCalibration.cc\n    src/camera_models/Camera.cc\n    src/camera_models/CameraFactory.cc\n    src/camera_models/CostFunctionFactory.cc\n    src/camera_models/PinholeCamera.cc\n    src/camera_models/CataCamera.cc\n    src/camera_models/EquidistantCamera.cc\n    src/camera_models/ScaramuzzaCamera.cc\n    src/sparse_graph/Transform.cc\n    src/gpl/gpl.cc\n    src/gpl/EigenQuaternionParameterization.cc)\n\n# target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})\ntarget_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})\n"
  },
  {
    "path": "camera_model/include/camodocal/calib/CameraCalibration.h",
    "content": "#ifndef CAMERACALIBRATION_H\n#define CAMERACALIBRATION_H\n\n#include <opencv2/core/core.hpp>\n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace camodocal\n{\n\nclass CameraCalibration\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    CameraCalibration();\n\n    CameraCalibration(Camera::ModelType modelType,\n                      const std::string& cameraName,\n                      const cv::Size& imageSize,\n                      const cv::Size& boardSize,\n                      float squareSize);\n\n    void clear(void);\n\n    void addChessboardData(const std::vector<cv::Point2f>& corners);\n\n    bool calibrate(void);\n\n    int sampleCount(void) const;\n    std::vector<std::vector<cv::Point2f> >& imagePoints(void);\n    const std::vector<std::vector<cv::Point2f> >& imagePoints(void) const;\n    std::vector<std::vector<cv::Point3f> >& scenePoints(void);\n    const std::vector<std::vector<cv::Point3f> >& scenePoints(void) const;\n    CameraPtr& camera(void);\n    const CameraConstPtr camera(void) const;\n\n    Eigen::Matrix2d& measurementCovariance(void);\n    const Eigen::Matrix2d& measurementCovariance(void) const;\n\n    cv::Mat& cameraPoses(void);\n    const cv::Mat& cameraPoses(void) const;\n\n    void drawResults(std::vector<cv::Mat>& images) const;\n\n    void writeParams(const std::string& filename) const;\n\n    bool writeChessboardData(const std::string& filename) const;\n    bool readChessboardData(const std::string& filename);\n\n    void setVerbose(bool verbose);\n\nprivate:\n    bool calibrateHelper(CameraPtr& camera,\n                         std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const;\n\n    void optimize(CameraPtr& camera,\n                  std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const;\n\n    template<typename T>\n    void readData(std::ifstream& ifs, T& data) const;\n\n    template<typename T>\n    void writeData(std::ofstream& ofs, T data) const;\n\n    cv::Size m_boardSize;\n    float m_squareSize;\n\n    CameraPtr m_camera;\n    cv::Mat m_cameraPoses;\n\n    std::vector<std::vector<cv::Point2f> > m_imagePoints;\n    std::vector<std::vector<cv::Point3f> > m_scenePoints;\n\n    Eigen::Matrix2d m_measurementCovariance;\n\n    bool m_verbose;\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/Camera.h",
    "content": "#ifndef CAMERA_H\n#define CAMERA_H\n\n#include <boost/shared_ptr.hpp>\n#include <eigen3/Eigen/Dense>\n#include <opencv2/core/core.hpp>\n#include <vector>\n\nnamespace camodocal\n{\n\nclass Camera\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    enum ModelType\n    {\n        KANNALA_BRANDT,\n        MEI,\n        PINHOLE,\n        SCARAMUZZA\n    };\n\n    class Parameters\n    {\n    public:\n        EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        Parameters(ModelType modelType);\n\n        Parameters(ModelType modelType, const std::string& cameraName,\n                   int w, int h);\n\n        ModelType& modelType(void);\n        std::string& cameraName(void);\n        int& imageWidth(void);\n        int& imageHeight(void);\n\n        ModelType modelType(void) const;\n        const std::string& cameraName(void) const;\n        int imageWidth(void) const;\n        int imageHeight(void) const;\n\n        int nIntrinsics(void) const;\n\n        virtual bool readFromYamlFile(const std::string& filename) = 0;\n        virtual void writeToYamlFile(const std::string& filename) const = 0;\n\n    protected:\n        ModelType m_modelType;\n        int m_nIntrinsics;\n        std::string m_cameraName;\n        int m_imageWidth;\n        int m_imageHeight;\n    };\n\n    virtual ModelType modelType(void) const = 0;\n    virtual const std::string& cameraName(void) const = 0;\n    virtual int imageWidth(void) const = 0;\n    virtual int imageHeight(void) const = 0;\n\n    virtual cv::Mat& mask(void);\n    virtual const cv::Mat& mask(void) const;\n\n    virtual void estimateIntrinsics(const cv::Size& boardSize,\n                                    const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                                    const std::vector< std::vector<cv::Point2f> >& imagePoints) = 0;\n    virtual void estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,\n                                    const std::vector<cv::Point2f>& imagePoints,\n                                    cv::Mat& rvec, cv::Mat& tvec) const;\n\n    // Lift points from the image plane to the sphere\n    virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n    //%output P\n\n    // Lift points from the image plane to the projective space\n    virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n    //%output P\n\n    // Projects 3D points to the image plane (Pi function)\n    virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0;\n    //%output p\n\n    // Projects 3D points to the image plane (Pi function)\n    // and calculates jacobian\n    //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n    //                          Eigen::Matrix<double,2,3>& J) const = 0;\n    //%output p\n    //%output J\n\n    virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0;\n    //%output p\n\n    //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0;\n    virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n                                            float fx = -1.0f, float fy = -1.0f,\n                                            cv::Size imageSize = cv::Size(0, 0),\n                                            float cx = -1.0f, float cy = -1.0f,\n                                            cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0;\n\n    virtual int parameterCount(void) const = 0;\n\n    virtual void readParameters(const std::vector<double>& parameters) = 0;\n    virtual void writeParameters(std::vector<double>& parameters) const = 0;\n\n    virtual void writeParametersToYamlFile(const std::string& filename) const = 0;\n\n    virtual std::string parametersToString(void) const = 0;\n\n    /**\n     * \\brief Calculates the reprojection distance between points\n     *\n     * \\param P1 first 3D point coordinates\n     * \\param P2 second 3D point coordinates\n     * \\return euclidean distance in the plane\n     */\n    double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const;\n\n    double reprojectionError(const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                             const std::vector< std::vector<cv::Point2f> >& imagePoints,\n                             const std::vector<cv::Mat>& rvecs,\n                             const std::vector<cv::Mat>& tvecs,\n                             cv::OutputArray perViewErrors = cv::noArray()) const;\n\n    double reprojectionError(const Eigen::Vector3d& P,\n                             const Eigen::Quaterniond& camera_q,\n                             const Eigen::Vector3d& camera_t,\n                             const Eigen::Vector2d& observed_p) const;\n\n    void projectPoints(const std::vector<cv::Point3f>& objectPoints,\n                       const cv::Mat& rvec,\n                       const cv::Mat& tvec,\n                       std::vector<cv::Point2f>& imagePoints) const;\nprotected:\n    cv::Mat m_mask;\n};\n\ntypedef boost::shared_ptr<Camera> CameraPtr;\ntypedef boost::shared_ptr<const Camera> CameraConstPtr;\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/CameraFactory.h",
    "content": "#ifndef CAMERAFACTORY_H\n#define CAMERAFACTORY_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace camodocal\n{\n\nclass CameraFactory\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    CameraFactory();\n\n    static boost::shared_ptr<CameraFactory> instance(void);\n\n    CameraPtr generateCamera(Camera::ModelType modelType,\n                             const std::string& cameraName,\n                             cv::Size imageSize) const;\n\n    CameraPtr generateCameraFromYamlFile(const std::string& filename);\n\nprivate:\n    static boost::shared_ptr<CameraFactory> m_instance;\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/CataCamera.h",
    "content": "#ifndef CATACAMERA_H\r\n#define CATACAMERA_H\r\n\r\n#include <opencv2/core/core.hpp>\r\n#include <string>\r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n/**\r\n * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration\r\n * from Planar Grids, ICRA 2007\r\n */\r\n\r\nclass CataCamera: public Camera\r\n{\r\npublic:\r\n    class Parameters: public Camera::Parameters\r\n    {\r\n    public:\r\n        Parameters();\r\n        Parameters(const std::string& cameraName,\r\n                   int w, int h,\r\n                   double xi,\r\n                   double k1, double k2, double p1, double p2,\r\n                   double gamma1, double gamma2, double u0, double v0);\r\n\r\n        double& xi(void);\r\n        double& k1(void);\r\n        double& k2(void);\r\n        double& p1(void);\r\n        double& p2(void);\r\n        double& gamma1(void);\r\n        double& gamma2(void);\r\n        double& u0(void);\r\n        double& v0(void);\r\n\r\n        double xi(void) const;\r\n        double k1(void) const;\r\n        double k2(void) const;\r\n        double p1(void) const;\r\n        double p2(void) const;\r\n        double gamma1(void) const;\r\n        double gamma2(void) const;\r\n        double u0(void) const;\r\n        double v0(void) const;\r\n\r\n        bool readFromYamlFile(const std::string& filename);\r\n        void writeToYamlFile(const std::string& filename) const;\r\n\r\n        Parameters& operator=(const Parameters& other);\r\n        friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n    private:\r\n        double m_xi;\r\n        double m_k1;\r\n        double m_k2;\r\n        double m_p1;\r\n        double m_p2;\r\n        double m_gamma1;\r\n        double m_gamma2;\r\n        double m_u0;\r\n        double m_v0;\r\n    };\r\n\r\n    CataCamera();\r\n\r\n    /**\r\n    * \\brief Constructor from the projection model parameters\r\n    */\r\n    CataCamera(const std::string& cameraName,\r\n               int imageWidth, int imageHeight,\r\n               double xi, double k1, double k2, double p1, double p2,\r\n               double gamma1, double gamma2, double u0, double v0);\r\n    /**\r\n    * \\brief Constructor from the projection model parameters\r\n    */\r\n    CataCamera(const Parameters& params);\r\n\r\n    Camera::ModelType modelType(void) const;\r\n    const std::string& cameraName(void) const;\r\n    int imageWidth(void) const;\r\n    int imageHeight(void) const;\r\n\r\n    void estimateIntrinsics(const cv::Size& boardSize,\r\n                            const std::vector< std::vector<cv::Point3f> >& objectPoints,\r\n                            const std::vector< std::vector<cv::Point2f> >& imagePoints);\r\n\r\n    // Lift points from the image plane to the sphere\r\n    void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n    //%output P\r\n\r\n    // Lift points from the image plane to the projective space\r\n    void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n    //%output P\r\n\r\n    // Projects 3D points to the image plane (Pi function)\r\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n    //%output p\r\n\r\n    // Projects 3D points to the image plane (Pi function)\r\n    // and calculates jacobian\r\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n                      Eigen::Matrix<double,2,3>& J) const;\r\n    //%output p\r\n    //%output J\r\n\r\n    void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n    //%output p\r\n\r\n    template <typename T>\r\n    static void spaceToPlane(const T* const params,\r\n                             const T* const q, const T* const t,\r\n                             const Eigen::Matrix<T, 3, 1>& P,\r\n                             Eigen::Matrix<T, 2, 1>& p);\r\n\r\n    void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\r\n    void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\r\n                    Eigen::Matrix2d& J) const;\r\n\r\n    void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n    cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n                                    float fx = -1.0f, float fy = -1.0f,\r\n                                    cv::Size imageSize = cv::Size(0, 0),\r\n                                    float cx = -1.0f, float cy = -1.0f,\r\n                                    cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n    int parameterCount(void) const;\r\n\r\n    const Parameters& getParameters(void) const;\r\n    void setParameters(const Parameters& parameters);\r\n\r\n    void readParameters(const std::vector<double>& parameterVec);\r\n    void writeParameters(std::vector<double>& parameterVec) const;\r\n\r\n    void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n    std::string parametersToString(void) const;\r\n\r\nprivate:\r\n    Parameters mParameters;\r\n\r\n    double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n    bool m_noDistortion;\r\n};\r\n\r\ntypedef boost::shared_ptr<CataCamera> CataCameraPtr;\r\ntypedef boost::shared_ptr<const CataCamera> CataCameraConstPtr;\r\n\r\ntemplate <typename T>\r\nvoid\r\nCataCamera::spaceToPlane(const T* const params,\r\n                         const T* const q, const T* const t,\r\n                         const Eigen::Matrix<T, 3, 1>& P,\r\n                         Eigen::Matrix<T, 2, 1>& p)\r\n{\r\n    T P_w[3];\r\n    P_w[0] = T(P(0));\r\n    P_w[1] = T(P(1));\r\n    P_w[2] = T(P(2));\r\n\r\n    // Convert quaternion from Eigen convention (x, y, z, w)\r\n    // to Ceres convention (w, x, y, z)\r\n    T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n    T P_c[3];\r\n    ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n    P_c[0] += t[0];\r\n    P_c[1] += t[1];\r\n    P_c[2] += t[2];\r\n\r\n    // project 3D object point to the image plane\r\n    T xi = params[0];\r\n    T k1 = params[1];\r\n    T k2 = params[2];\r\n    T p1 = params[3];\r\n    T p2 = params[4];\r\n    T gamma1 = params[5];\r\n    T gamma2 = params[6];\r\n    T alpha = T(0); //cameraParams.alpha();\r\n    T u0 = params[7];\r\n    T v0 = params[8];\r\n\r\n    // Transform to model plane\r\n    T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);\r\n    P_c[0] /= len;\r\n    P_c[1] /= len;\r\n    P_c[2] /= len;\r\n\r\n    T u = P_c[0] / (P_c[2] + xi);\r\n    T v = P_c[1] / (P_c[2] + xi);\r\n\r\n    T rho_sqr = u * u + v * v;\r\n    T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;\r\n    T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);\r\n    T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;\r\n\r\n    u = L * u + du;\r\n    v = L * v + dv;\r\n    p(0) = gamma1 * (u + alpha * v) + u0;\r\n    p(1) = gamma2 * v + v0;\r\n}\r\n\r\n}\r\n\r\n#endif\r\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/CostFunctionFactory.h",
    "content": "#ifndef COSTFUNCTIONFACTORY_H\n#define COSTFUNCTIONFACTORY_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n\n#include \"camodocal/camera_models/Camera.h\"\n\nnamespace ceres\n{\n    class CostFunction;\n}\n\nnamespace camodocal\n{\n\nenum\n{\n    CAMERA_INTRINSICS =         1 << 0,\n    CAMERA_POSE =               1 << 1,\n    POINT_3D =                  1 << 2,\n    ODOMETRY_INTRINSICS =       1 << 3,\n    ODOMETRY_3D_POSE =          1 << 4,\n    ODOMETRY_6D_POSE =          1 << 5,\n    CAMERA_ODOMETRY_TRANSFORM = 1 << 6\n};\n\nclass CostFunctionFactory\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    CostFunctionFactory();\n\n    static boost::shared_ptr<CostFunctionFactory> instance(void);\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n                                              const Eigen::Vector3d& observed_P,\n                                              const Eigen::Vector2d& observed_p,\n                                              int flags) const;\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n                                              const Eigen::Vector3d& observed_P,\n                                              const Eigen::Vector2d& observed_p,\n                                              const Eigen::Matrix2d& sqrtPrecisionMat,\n                                              int flags) const;\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n                                              const Eigen::Vector2d& observed_p,\n                                              int flags, bool optimize_cam_odo_z = true) const;\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n                                              const Eigen::Vector2d& observed_p,\n                                              const Eigen::Matrix2d& sqrtPrecisionMat,\n                                              int flags, bool optimize_cam_odo_z = true) const;\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n                                              const Eigen::Vector3d& odo_pos,\n                                              const Eigen::Vector3d& odo_att,\n                                              const Eigen::Vector2d& observed_p,\n                                              int flags, bool optimize_cam_odo_z = true) const;\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,\n                                              const Eigen::Quaterniond& cam_odo_q,\n                                              const Eigen::Vector3d& cam_odo_t,\n                                              const Eigen::Vector3d& odo_pos,\n                                              const Eigen::Vector3d& odo_att,\n                                              const Eigen::Vector2d& observed_p,\n                                              int flags) const;\n\n    ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft,\n                                              const CameraConstPtr& cameraRight,\n                                              const Eigen::Vector3d& observed_P,\n                                              const Eigen::Vector2d& observed_p_left,\n                                              const Eigen::Vector2d& observed_p_right) const;\n\nprivate:\n    static boost::shared_ptr<CostFunctionFactory> m_instance;\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/EquidistantCamera.h",
    "content": "#ifndef EQUIDISTANTCAMERA_H\r\n#define EQUIDISTANTCAMERA_H\r\n\r\n#include <opencv2/core/core.hpp>\r\n#include <string>\r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n/**\r\n * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method\r\n * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006\r\n */\r\n\r\nclass EquidistantCamera: public Camera\r\n{\r\npublic:\r\n    class Parameters: public Camera::Parameters\r\n    {\r\n    public:\r\n        Parameters();\r\n        Parameters(const std::string& cameraName,\r\n                   int w, int h,\r\n                   double k2, double k3, double k4, double k5,\r\n                   double mu, double mv,\r\n                   double u0, double v0);\r\n\r\n        double& k2(void);\r\n        double& k3(void);\r\n        double& k4(void);\r\n        double& k5(void);\r\n        double& mu(void);\r\n        double& mv(void);\r\n        double& u0(void);\r\n        double& v0(void);\r\n\r\n        double k2(void) const;\r\n        double k3(void) const;\r\n        double k4(void) const;\r\n        double k5(void) const;\r\n        double mu(void) const;\r\n        double mv(void) const;\r\n        double u0(void) const;\r\n        double v0(void) const;\r\n\r\n        bool readFromYamlFile(const std::string& filename);\r\n        void writeToYamlFile(const std::string& filename) const;\r\n\r\n        Parameters& operator=(const Parameters& other);\r\n        friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n    private:\r\n        // projection\r\n        double m_k2;\r\n        double m_k3;\r\n        double m_k4;\r\n        double m_k5;\r\n\r\n        double m_mu;\r\n        double m_mv;\r\n        double m_u0;\r\n        double m_v0;\r\n    };\r\n\r\n    EquidistantCamera();\r\n\r\n    /**\r\n    * \\brief Constructor from the projection model parameters\r\n    */\r\n    EquidistantCamera(const std::string& cameraName,\r\n                      int imageWidth, int imageHeight,\r\n                      double k2, double k3, double k4, double k5,\r\n                      double mu, double mv,\r\n                      double u0, double v0);\r\n    /**\r\n    * \\brief Constructor from the projection model parameters\r\n    */\r\n    EquidistantCamera(const Parameters& params);\r\n\r\n    Camera::ModelType modelType(void) const;\r\n    const std::string& cameraName(void) const;\r\n    int imageWidth(void) const;\r\n    int imageHeight(void) const;\r\n\r\n    void estimateIntrinsics(const cv::Size& boardSize,\r\n                            const std::vector< std::vector<cv::Point3f> >& objectPoints,\r\n                            const std::vector< std::vector<cv::Point2f> >& imagePoints);\r\n\r\n    // Lift points from the image plane to the sphere\r\n    virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n    //%output P\r\n\r\n    // Lift points from the image plane to the projective space\r\n    void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n    //%output P\r\n\r\n    // Projects 3D points to the image plane (Pi function)\r\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n    //%output p\r\n\r\n    // Projects 3D points to the image plane (Pi function)\r\n    // and calculates jacobian\r\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n                      Eigen::Matrix<double,2,3>& J) const;\r\n    //%output p\r\n    //%output J\r\n\r\n    void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n    //%output p\r\n\r\n    template <typename T>\r\n    static void spaceToPlane(const T* const params,\r\n                             const T* const q, const T* const t,\r\n                             const Eigen::Matrix<T, 3, 1>& P,\r\n                             Eigen::Matrix<T, 2, 1>& p);\r\n\r\n    void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n    cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n                                    float fx = -1.0f, float fy = -1.0f,\r\n                                    cv::Size imageSize = cv::Size(0, 0),\r\n                                    float cx = -1.0f, float cy = -1.0f,\r\n                                    cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n    int parameterCount(void) const;\r\n\r\n    const Parameters& getParameters(void) const;\r\n    void setParameters(const Parameters& parameters);\r\n\r\n    void readParameters(const std::vector<double>& parameterVec);\r\n    void writeParameters(std::vector<double>& parameterVec) const;\r\n\r\n    void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n    std::string parametersToString(void) const;\r\n\r\nprivate:\r\n    template<typename T>\r\n    static T r(T k2, T k3, T k4, T k5, T theta);\r\n\r\n\r\n    void fitOddPoly(const std::vector<double>& x, const std::vector<double>& y,\r\n                    int n, std::vector<double>& coeffs) const;\r\n\r\n    void backprojectSymmetric(const Eigen::Vector2d& p_u,\r\n                              double& theta, double& phi) const;\r\n\r\n    Parameters mParameters;\r\n\r\n    double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n};\r\n\r\ntypedef boost::shared_ptr<EquidistantCamera> EquidistantCameraPtr;\r\ntypedef boost::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;\r\n\r\ntemplate<typename T>\r\nT\r\nEquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)\r\n{\r\n    // k1 = 1\r\n    return theta +\r\n           k2 * theta * theta * theta +\r\n           k3 * theta * theta * theta * theta * theta +\r\n           k4 * theta * theta * theta * theta * theta * theta * theta +\r\n           k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;\r\n}\r\n\r\ntemplate <typename T>\r\nvoid\r\nEquidistantCamera::spaceToPlane(const T* const params,\r\n                                const T* const q, const T* const t,\r\n                                const Eigen::Matrix<T, 3, 1>& P,\r\n                                Eigen::Matrix<T, 2, 1>& p)\r\n{\r\n    T P_w[3];\r\n    P_w[0] = T(P(0));\r\n    P_w[1] = T(P(1));\r\n    P_w[2] = T(P(2));\r\n\r\n    // Convert quaternion from Eigen convention (x, y, z, w)\r\n    // to Ceres convention (w, x, y, z)\r\n    T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n    T P_c[3];\r\n    ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n    P_c[0] += t[0];\r\n    P_c[1] += t[1];\r\n    P_c[2] += t[2];\r\n\r\n    // project 3D object point to the image plane;\r\n    T k2 = params[0];\r\n    T k3 = params[1];\r\n    T k4 = params[2];\r\n    T k5 = params[3];\r\n    T mu = params[4];\r\n    T mv = params[5];\r\n    T u0 = params[6];\r\n    T v0 = params[7];\r\n\r\n    T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);\r\n    T theta = acos(P_c[2] / len);\r\n    T phi = atan2(P_c[1], P_c[0]);\r\n\r\n    Eigen::Matrix<T,2,1> p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix<T,2,1>(cos(phi), sin(phi));\r\n\r\n    p(0) = mu * p_u(0) + u0;\r\n    p(1) = mv * p_u(1) + v0;\r\n}\r\n\r\n}\r\n\r\n#endif\r\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/PinholeCamera.h",
    "content": "#ifndef PINHOLECAMERA_H\n#define PINHOLECAMERA_H\n\n#include <opencv2/core/core.hpp>\n#include <string>\n\n#include \"ceres/rotation.h\"\n#include \"Camera.h\"\n\nnamespace camodocal\n{\n\nclass PinholeCamera: public Camera\n{\npublic:\n    class Parameters: public Camera::Parameters\n    {\n    public:\n        Parameters();\n        Parameters(const std::string& cameraName,\n                   int w, int h,\n                   double k1, double k2, double p1, double p2,\n                   double fx, double fy, double cx, double cy);\n\n        double& k1(void);\n        double& k2(void);\n        double& p1(void);\n        double& p2(void);\n        double& fx(void);\n        double& fy(void);\n        double& cx(void);\n        double& cy(void);\n\n        double xi(void) const;\n        double k1(void) const;\n        double k2(void) const;\n        double p1(void) const;\n        double p2(void) const;\n        double fx(void) const;\n        double fy(void) const;\n        double cx(void) const;\n        double cy(void) const;\n\n        bool readFromYamlFile(const std::string& filename);\n        void writeToYamlFile(const std::string& filename) const;\n\n        Parameters& operator=(const Parameters& other);\n        friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\n\n    private:\n        double m_k1;\n        double m_k2;\n        double m_p1;\n        double m_p2;\n        double m_fx;\n        double m_fy;\n        double m_cx;\n        double m_cy;\n    };\n\n    PinholeCamera();\n\n    /**\n    * \\brief Constructor from the projection model parameters\n    */\n    PinholeCamera(const std::string& cameraName,\n                  int imageWidth, int imageHeight,\n                  double k1, double k2, double p1, double p2,\n                  double fx, double fy, double cx, double cy);\n    /**\n    * \\brief Constructor from the projection model parameters\n    */\n    PinholeCamera(const Parameters& params);\n\n    Camera::ModelType modelType(void) const;\n    const std::string& cameraName(void) const;\n    int imageWidth(void) const;\n    int imageHeight(void) const;\n\n    void estimateIntrinsics(const cv::Size& boardSize,\n                            const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                            const std::vector< std::vector<cv::Point2f> >& imagePoints);\n\n    // Lift points from the image plane to the sphere\n    virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\n    //%output P\n\n    // Lift points from the image plane to the projective space\n    void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\n    //%output P\n\n    // Projects 3D points to the image plane (Pi function)\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\n    //%output p\n\n    // Projects 3D points to the image plane (Pi function)\n    // and calculates jacobian\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n                      Eigen::Matrix<double,2,3>& J) const;\n    //%output p\n    //%output J\n\n    void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\n    //%output p\n\n    template <typename T>\n    static void spaceToPlane(const T* const params,\n                             const T* const q, const T* const t,\n                             const Eigen::Matrix<T, 3, 1>& P,\n                             Eigen::Matrix<T, 2, 1>& p);\n\n    void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\n    void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n                    Eigen::Matrix2d& J) const;\n\n    void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\n    cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n                                    float fx = -1.0f, float fy = -1.0f,\n                                    cv::Size imageSize = cv::Size(0, 0),\n                                    float cx = -1.0f, float cy = -1.0f,\n                                    cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\n\n    int parameterCount(void) const;\n\n    const Parameters& getParameters(void) const;\n    void setParameters(const Parameters& parameters);\n\n    void readParameters(const std::vector<double>& parameterVec);\n    void writeParameters(std::vector<double>& parameterVec) const;\n\n    void writeParametersToYamlFile(const std::string& filename) const;\n\n    std::string parametersToString(void) const;\n\nprivate:\n    Parameters mParameters;\n\n    double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\n    bool m_noDistortion;\n};\n\ntypedef boost::shared_ptr<PinholeCamera> PinholeCameraPtr;\ntypedef boost::shared_ptr<const PinholeCamera> PinholeCameraConstPtr;\n\ntemplate <typename T>\nvoid\nPinholeCamera::spaceToPlane(const T* const params,\n                            const T* const q, const T* const t,\n                            const Eigen::Matrix<T, 3, 1>& P,\n                            Eigen::Matrix<T, 2, 1>& p)\n{\n    T P_w[3];\n    P_w[0] = T(P(0));\n    P_w[1] = T(P(1));\n    P_w[2] = T(P(2));\n\n    // Convert quaternion from Eigen convention (x, y, z, w)\n    // to Ceres convention (w, x, y, z)\n    T q_ceres[4] = {q[3], q[0], q[1], q[2]};\n\n    T P_c[3];\n    ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\n\n    P_c[0] += t[0];\n    P_c[1] += t[1];\n    P_c[2] += t[2];\n\n    // project 3D object point to the image plane\n    T k1 = params[0];\n    T k2 = params[1];\n    T p1 = params[2];\n    T p2 = params[3];\n    T fx = params[4];\n    T fy = params[5];\n    T alpha = T(0); //cameraParams.alpha();\n    T cx = params[6];\n    T cy = params[7];\n\n    // Transform to model plane\n    T u = P_c[0] / P_c[2];\n    T v = P_c[1] / P_c[2];\n\n    T rho_sqr = u * u + v * v;\n    T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;\n    T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);\n    T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;\n\n    u = L * u + du;\n    v = L * v + dv;\n    p(0) = fx * (u + alpha * v) + cx;\n    p(1) = fy * v + cy;\n}\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h",
    "content": "#ifndef SCARAMUZZACAMERA_H\r\n#define SCARAMUZZACAMERA_H\r\n\r\n#include <opencv2/core/core.hpp>\r\n#include <string>\r\n\r\n#include \"ceres/rotation.h\"\r\n#include \"Camera.h\"\r\n\r\nnamespace camodocal\r\n{\r\n\r\n#define SCARAMUZZA_POLY_SIZE 5\r\n#define SCARAMUZZA_INV_POLY_SIZE 20\r\n\r\n#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/)\r\n\r\n/**\r\n * Scaramuzza Camera (Omnidirectional)\r\n * https://sites.google.com/site/scarabotix/ocamcalib-toolbox\r\n */\r\n\r\nclass OCAMCamera: public Camera\r\n{\r\npublic:\r\n    class Parameters: public Camera::Parameters\r\n    {\r\n    public:\r\n        Parameters();\r\n\r\n        double& C(void) { return m_C; }\r\n        double& D(void) { return m_D; }\r\n        double& E(void) { return m_E; }\r\n\r\n        double& center_x(void) { return m_center_x; }\r\n        double& center_y(void) { return m_center_y; }\r\n\r\n        double& poly(int idx) { return m_poly[idx]; }\r\n        double& inv_poly(int idx) { return m_inv_poly[idx]; }\r\n\r\n        double C(void) const { return m_C; }\r\n        double D(void) const { return m_D; }\r\n        double E(void) const { return m_E; }\r\n\r\n        double center_x(void) const { return m_center_x; }\r\n        double center_y(void) const { return m_center_y; }\r\n\r\n        double poly(int idx) const { return m_poly[idx]; }\r\n        double inv_poly(int idx) const { return m_inv_poly[idx]; }\r\n\r\n        bool readFromYamlFile(const std::string& filename);\r\n        void writeToYamlFile(const std::string& filename) const;\r\n\r\n        Parameters& operator=(const Parameters& other);\r\n        friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n\r\n    private:\r\n        double m_poly[SCARAMUZZA_POLY_SIZE];\r\n        double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n        double m_C;\r\n        double m_D;\r\n        double m_E;\r\n        double m_center_x;\r\n        double m_center_y;\r\n    };\r\n\r\n    OCAMCamera();\r\n\r\n    /**\r\n    * \\brief Constructor from the projection model parameters\r\n    */\r\n    OCAMCamera(const Parameters& params);\r\n\r\n    Camera::ModelType modelType(void) const;\r\n    const std::string& cameraName(void) const;\r\n    int imageWidth(void) const;\r\n    int imageHeight(void) const;\r\n\r\n    void estimateIntrinsics(const cv::Size& boardSize,\r\n                            const std::vector< std::vector<cv::Point3f> >& objectPoints,\r\n                            const std::vector< std::vector<cv::Point2f> >& imagePoints);\r\n\r\n    // Lift points from the image plane to the sphere\r\n    void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n    //%output P\r\n\r\n    // Lift points from the image plane to the projective space\r\n    void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;\r\n    //%output P\r\n\r\n    // Projects 3D points to the image plane (Pi function)\r\n    void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;\r\n    //%output p\r\n\r\n    // Projects 3D points to the image plane (Pi function)\r\n    // and calculates jacobian\r\n    //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\r\n    //                  Eigen::Matrix<double,2,3>& J) const;\r\n    //%output p\r\n    //%output J\r\n\r\n    void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;\r\n    //%output p\r\n\r\n    template <typename T>\r\n    static void spaceToPlane(const T* const params,\r\n                             const T* const q, const T* const t,\r\n                             const Eigen::Matrix<T, 3, 1>& P,\r\n                             Eigen::Matrix<T, 2, 1>& p);\r\n    template <typename T>\r\n    static void spaceToSphere(const T* const params,\r\n                              const T* const q, const T* const t,\r\n                              const Eigen::Matrix<T, 3, 1>& P,\r\n                              Eigen::Matrix<T, 3, 1>& P_s);\r\n    template <typename T>\r\n    static void LiftToSphere(const T* const params,\r\n                              const Eigen::Matrix<T, 2, 1>& p,\r\n                              Eigen::Matrix<T, 3, 1>& P);\r\n\r\n    template <typename T>\r\n    static void SphereToPlane(const T* const params, const Eigen::Matrix<T, 3, 1>& P,\r\n                               Eigen::Matrix<T, 2, 1>& p);\r\n\r\n\r\n    void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;\r\n    cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\r\n                                    float fx = -1.0f, float fy = -1.0f,\r\n                                    cv::Size imageSize = cv::Size(0, 0),\r\n                                    float cx = -1.0f, float cy = -1.0f,\r\n                                    cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;\r\n\r\n    int parameterCount(void) const;\r\n\r\n    const Parameters& getParameters(void) const;\r\n    void setParameters(const Parameters& parameters);\r\n\r\n    void readParameters(const std::vector<double>& parameterVec);\r\n    void writeParameters(std::vector<double>& parameterVec) const;\r\n\r\n    void writeParametersToYamlFile(const std::string& filename) const;\r\n\r\n    std::string parametersToString(void) const;\r\n\r\nprivate:\r\n    Parameters mParameters;\r\n\r\n    double m_inv_scale;\r\n};\r\n\r\ntypedef boost::shared_ptr<OCAMCamera> OCAMCameraPtr;\r\ntypedef boost::shared_ptr<const OCAMCamera> OCAMCameraConstPtr;\r\n\r\ntemplate <typename T>\r\nvoid\r\nOCAMCamera::spaceToPlane(const T* const params,\r\n                         const T* const q, const T* const t,\r\n                         const Eigen::Matrix<T, 3, 1>& P,\r\n                         Eigen::Matrix<T, 2, 1>& p)\r\n{\r\n    T P_c[3];\r\n    {\r\n        T P_w[3];\r\n        P_w[0] = T(P(0));\r\n        P_w[1] = T(P(1));\r\n        P_w[2] = T(P(2));\r\n\r\n        // Convert quaternion from Eigen convention (x, y, z, w)\r\n        // to Ceres convention (w, x, y, z)\r\n        T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n        ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n        P_c[0] += t[0];\r\n        P_c[1] += t[1];\r\n        P_c[2] += t[2];\r\n    }\r\n\r\n    T c = params[0];\r\n    T d = params[1];\r\n    T e = params[2];\r\n    T xc[2] = { params[3], params[4] };\r\n\r\n    //T poly[SCARAMUZZA_POLY_SIZE];\r\n    //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n    //    poly[i] = params[5+i];\r\n\r\n    T inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n    for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n        inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];\r\n\r\n    T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];\r\n    T norm = T(0.0);\r\n    if (norm_sqr > T(0.0))\r\n        norm = sqrt(norm_sqr);\r\n\r\n    T theta = atan2(-P_c[2], norm);\r\n    T rho = T(0.0);\r\n    T theta_i = T(1.0);\r\n\r\n    for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n    {\r\n        rho += theta_i * inv_poly[i];\r\n        theta_i *= theta;\r\n    }\r\n\r\n    T invNorm = T(1.0) / norm;\r\n    T xn[2] = {\r\n        P_c[0] * invNorm * rho,\r\n        P_c[1] * invNorm * rho\r\n    };\r\n\r\n    p(0) = xn[0] * c + xn[1] * d + xc[0];\r\n    p(1) = xn[0] * e + xn[1]     + xc[1];\r\n}\r\n\r\ntemplate <typename T>\r\nvoid\r\nOCAMCamera::spaceToSphere(const T* const params,\r\n                          const T* const q, const T* const t,\r\n                          const Eigen::Matrix<T, 3, 1>& P,\r\n                          Eigen::Matrix<T, 3, 1>& P_s)\r\n{\r\n    T P_c[3];\r\n    {\r\n        T P_w[3];\r\n        P_w[0] = T(P(0));\r\n        P_w[1] = T(P(1));\r\n        P_w[2] = T(P(2));\r\n\r\n        // Convert quaternion from Eigen convention (x, y, z, w)\r\n        // to Ceres convention (w, x, y, z)\r\n        T q_ceres[4] = {q[3], q[0], q[1], q[2]};\r\n\r\n        ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);\r\n\r\n        P_c[0] += t[0];\r\n        P_c[1] += t[1];\r\n        P_c[2] += t[2];\r\n    }\r\n\r\n    //T poly[SCARAMUZZA_POLY_SIZE];\r\n    //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n    //    poly[i] = params[5+i];\r\n\r\n    T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];\r\n    T norm = T(0.0);\r\n    if (norm_sqr > T(0.0))\r\n        norm = sqrt(norm_sqr);\r\n\r\n    P_s(0) = P_c[0] / norm;\r\n    P_s(1) = P_c[1] / norm;\r\n    P_s(2) = P_c[2] / norm;\r\n}\r\n\r\ntemplate <typename T>\r\nvoid\r\nOCAMCamera::LiftToSphere(const T* const params,\r\n                          const Eigen::Matrix<T, 2, 1>& p,\r\n                          Eigen::Matrix<T, 3, 1>& P)\r\n{\r\n    T c = params[0];\r\n    T d = params[1];\r\n    T e = params[2];\r\n    T cc[2] = { params[3], params[4] };\r\n    T poly[SCARAMUZZA_POLY_SIZE];\r\n    for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n       poly[i] = params[5+i];\r\n\r\n    // Relative to Center\r\n    T p_2d[2];\r\n    p_2d[0] = T(p(0));\r\n    p_2d[1] = T(p(1));\r\n\r\n    T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]};\r\n\r\n    T inv_scale = T(1.0) / (c - d * e);\r\n\r\n    // Affine Transformation\r\n    T xc_a[2];\r\n\r\n    xc_a[0] = inv_scale * (xc[0] - d * xc[1]);\r\n    xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);\r\n\r\n    T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];\r\n    T phi = sqrt(norm_sqr);\r\n    T phi_i = T(1.0);\r\n    T z = T(0.0);\r\n\r\n    for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)\r\n    {\r\n        if (i!=1) {\r\n            z += phi_i * poly[i];\r\n        }\r\n        phi_i *= phi;\r\n    }\r\n\r\n    T p_3d[3];\r\n    p_3d[0] = xc[0];\r\n    p_3d[1] = xc[1];\r\n    p_3d[2] = -z;\r\n\r\n    T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];\r\n    T p_3d_norm = sqrt(p_3d_norm_sqr);\r\n\r\n    P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;\r\n}\r\n\r\ntemplate <typename T>\r\nvoid OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix<T, 3, 1>& P,\r\n                               Eigen::Matrix<T, 2, 1>& p) {\r\n    T P_c[3];\r\n    {\r\n        P_c[0] = T(P(0));\r\n        P_c[1] = T(P(1));\r\n        P_c[2] = T(P(2));\r\n    }\r\n\r\n    T c = params[0];\r\n    T d = params[1];\r\n    T e = params[2];\r\n    T xc[2] = {params[3], params[4]};\r\n\r\n    T inv_poly[SCARAMUZZA_INV_POLY_SIZE];\r\n    for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\r\n        inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];\r\n\r\n    T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];\r\n    T norm = T(0.0);\r\n    if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr);\r\n\r\n    T theta = atan2(-P_c[2], norm);\r\n    T rho = T(0.0);\r\n    T theta_i = T(1.0);\r\n\r\n    for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {\r\n        rho += theta_i * inv_poly[i];\r\n        theta_i *= theta;\r\n    }\r\n\r\n    T invNorm = T(1.0) / norm;\r\n    T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};\r\n\r\n    p(0) = xn[0] * c + xn[1] * d + xc[0];\r\n    p(1) = xn[0] * e + xn[1] + xc[1];\r\n}\r\n}\r\n\r\n#endif\r\n"
  },
  {
    "path": "camera_model/include/camodocal/chessboard/Chessboard.h",
    "content": "#ifndef CHESSBOARD_H\n#define CHESSBOARD_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n\nnamespace camodocal\n{\n\n// forward declarations\nclass ChessboardCorner;\ntypedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;\nclass ChessboardQuad;\ntypedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;\n\nclass Chessboard\n{\npublic:\n    Chessboard(cv::Size boardSize, cv::Mat& image);\n\n    void findCorners(bool useOpenCV = false);\n    const std::vector<cv::Point2f>& getCorners(void) const;\n    bool cornersFound(void) const;\n\n    const cv::Mat& getImage(void) const;\n    const cv::Mat& getSketch(void) const;\n\nprivate:\n    bool findChessboardCorners(const cv::Mat& image,\n                               const cv::Size& patternSize,\n                               std::vector<cv::Point2f>& corners,\n                               int flags, bool useOpenCV);\n\n    bool findChessboardCornersImproved(const cv::Mat& image,\n                                       const cv::Size& patternSize,\n                                       std::vector<cv::Point2f>& corners,\n                                       int flags);\n\n    void cleanFoundConnectedQuads(std::vector<ChessboardQuadPtr>& quadGroup, cv::Size patternSize);\n\n    void findConnectedQuads(std::vector<ChessboardQuadPtr>& quads,\n                            std::vector<ChessboardQuadPtr>& group,\n                            int group_idx, int dilation);\n\n//    int checkQuadGroup(std::vector<ChessboardQuadPtr>& quadGroup,\n//                       std::vector<ChessboardCornerPtr>& outCorners,\n//                       cv::Size patternSize);\n\n    void labelQuadGroup(std::vector<ChessboardQuadPtr>& quad_group,\n                        cv::Size patternSize, bool firstRun);\n\n    void findQuadNeighbors(std::vector<ChessboardQuadPtr>& quads, int dilation);\n\n    int augmentBestRun(std::vector<ChessboardQuadPtr>& candidateQuads, int candidateDilation,\n                       std::vector<ChessboardQuadPtr>& existingQuads, int existingDilation);\n\n    void generateQuads(std::vector<ChessboardQuadPtr>& quads,\n                       cv::Mat& image, int flags,\n                       int dilation, bool firstRun);\n\n    bool checkQuadGroup(std::vector<ChessboardQuadPtr>& quads,\n                        std::vector<ChessboardCornerPtr>& corners,\n                        cv::Size patternSize);\n\n    void getQuadrangleHypotheses(const std::vector< std::vector<cv::Point> >& contours,\n                                 std::vector< std::pair<float, int> >& quads,\n                                 int classId) const;\n\n    bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const;\n\n    bool checkBoardMonotony(std::vector<ChessboardCornerPtr>& corners,\n                            cv::Size patternSize);\n\n    bool matchCorners(ChessboardQuadPtr& quad1, int corner1,\n                      ChessboardQuadPtr& quad2, int corner2) const;\n\n    cv::Mat mImage;\n    cv::Mat mSketch;\n    std::vector<cv::Point2f> mCorners;\n    cv::Size mBoardSize;\n    bool mCornersFound;\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/chessboard/ChessboardCorner.h",
    "content": "#ifndef CHESSBOARDCORNER_H\n#define CHESSBOARDCORNER_H\n\n#include <boost/shared_ptr.hpp>\n#include <opencv2/core/core.hpp>\n\nnamespace camodocal\n{\n\nclass ChessboardCorner;\ntypedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;\n\nclass ChessboardCorner\n{\npublic:\n    ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}\n\n    float meanDist(int &n) const\n    {\n        float sum = 0;\n        n = 0;\n        for (int i = 0; i < 4; ++i)\n        {\n            if (neighbors[i].get())\n            {\n                float dx = neighbors[i]->pt.x - pt.x;\n                float dy = neighbors[i]->pt.y - pt.y;\n                sum += sqrt(dx*dx + dy*dy);\n                n++;\n            }\n        }\n        return sum / std::max(n, 1);\n    }\n\n    cv::Point2f pt;                     // X and y coordinates\n    int row;                            // Row and column of the corner\n    int column;                         // in the found pattern\n    bool needsNeighbor;                 // Does the corner require a neighbor?\n    int count;                          // number of corner neighbors\n    ChessboardCornerPtr neighbors[4];   // pointer to all corner neighbors\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/chessboard/ChessboardQuad.h",
    "content": "#ifndef CHESSBOARDQUAD_H\n#define CHESSBOARDQUAD_H\n\n#include <boost/shared_ptr.hpp>\n\n#include \"camodocal/chessboard/ChessboardCorner.h\"\n\nnamespace camodocal\n{\n\nclass ChessboardQuad;\ntypedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;\n\nclass ChessboardQuad\n{\npublic:\n    ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}\n\n    int count;                         // Number of quad neighbors\n    int group_idx;                     // Quad group ID\n    float edge_len;                    // Smallest side length^2\n    ChessboardCornerPtr corners[4];    // Coordinates of quad corners\n    ChessboardQuadPtr neighbors[4];    // Pointers of quad neighbors\n    bool labeled;                      // Has this corner been labeled?\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/chessboard/Spline.h",
    "content": "/*  dynamo:- Event driven molecular dynamics simulator\n    http://www.marcusbannerman.co.uk/dynamo\n    Copyright (C) 2011  Marcus N Campbell Bannerman <m.bannerman@gmail.com>\n\n    This program is free software: you can redistribute it and/or\n    modify it under the terms of the GNU General Public License\n    version 3 as published by the Free Software Foundation.\n\n    This program is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n    GNU General Public License for more details.\n\n    You should have received a copy of the GNU General Public License\n    along with this program.  If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#pragma once\n\n#include <boost/numeric/ublas/vector.hpp>\n#include <boost/numeric/ublas/vector_proxy.hpp>\n#include <boost/numeric/ublas/matrix.hpp>\n#include <boost/numeric/ublas/triangular.hpp>\n#include <boost/numeric/ublas/lu.hpp>\n#include <exception>\n\nnamespace ublas = boost::numeric::ublas;\n\nclass Spline : private std::vector<std::pair<double, double> >\n{\npublic:\n  //The boundary conditions available\n  enum BC_type {\n\tFIXED_1ST_DERIV_BC,\n\tFIXED_2ND_DERIV_BC,\n\tPARABOLIC_RUNOUT_BC\n  };\n\n  enum Spline_type {\n\tLINEAR,\n\tCUBIC\n  };\n\n  //Constructor takes the boundary conditions as arguments, this\n  //sets the first derivative (gradient) at the lower and upper\n  //end points\n  Spline():\n\t_valid(false),\n\t_BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC),\n\t_BCLowVal(0), _BCHighVal(0),\n\t_type(CUBIC)\n  {}\n\n  typedef std::vector<std::pair<double, double> > base;\n  typedef base::const_iterator const_iterator;\n\n  //Standard STL read-only container stuff\n  const_iterator begin() const { return base::begin(); }\n  const_iterator end() const { return base::end(); }\n  void clear() { _valid = false; base::clear(); _data.clear(); }\n  size_t size() const { return base::size(); }\n  size_t max_size() const { return base::max_size(); }\n  size_t capacity() const { return base::capacity(); }\n  bool empty() const { return base::empty(); }\n\n  //Add a point to the spline, and invalidate it so its\n  //recalculated on the next access\n  inline void addPoint(double x, double y)\n  {\n\t_valid = false;\n\tbase::push_back(std::pair<double, double>(x,y));\n  }\n\n  //Reset the boundary conditions\n  inline void setLowBC(BC_type BC, double val = 0)\n  { _BCLow = BC; _BCLowVal = val; _valid = false; }\n\n  inline void setHighBC(BC_type BC, double val = 0)\n  { _BCHigh = BC; _BCHighVal = val; _valid = false; }\n\n  void setType(Spline_type type) { _type = type; _valid = false; }\n\n  //Check if the spline has been calculated, then generate the\n  //spline interpolated value\n  double operator()(double xval)\n  {\n\tif (!_valid) generate();\n\n\t//Special cases when we're outside the range of the spline points\n\tif (xval <= x(0)) return lowCalc(xval);\n\tif (xval >= x(size()-1)) return highCalc(xval);\n\n\t//Check all intervals except the last one\n\tfor (std::vector<SplineData>::const_iterator iPtr = _data.begin();\n\t\t iPtr != _data.end()-1; ++iPtr)\n\t\tif ((xval >= iPtr->x) && (xval <= (iPtr+1)->x))\n\t\t  return splineCalc(iPtr, xval);\n\n\treturn splineCalc(_data.end() - 1, xval);\n  }\n\nprivate:\n\n  ///////PRIVATE DATA MEMBERS\n  struct SplineData { double x,a,b,c,d; };\n  //vector of calculated spline data\n  std::vector<SplineData> _data;\n  //Second derivative at each point\n  ublas::vector<double> _ddy;\n  //Tracks whether the spline parameters have been calculated for\n  //the current set of points\n  bool _valid;\n  //The boundary conditions\n  BC_type _BCLow, _BCHigh;\n  //The values of the boundary conditions\n  double _BCLowVal, _BCHighVal;\n\n  Spline_type _type;\n\n  ///////PRIVATE FUNCTIONS\n  //Function to calculate the value of a given spline at a point xval\n  inline double splineCalc(std::vector<SplineData>::const_iterator i, double xval)\n  {\n\tconst double lx = xval - i->x;\n\treturn ((i->a * lx + i->b) * lx + i->c) * lx + i->d;\n  }\n\n  inline double lowCalc(double xval)\n  {\n\tconst double lx = xval - x(0);\n\n\tif (_type == LINEAR)\n\t  return lx * _BCHighVal + y(0);\n\n\tconst double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;\n\n\tswitch(_BCLow)\n\t  {\n\t  case FIXED_1ST_DERIV_BC:\n\t\treturn lx * _BCLowVal + y(0);\n\t  case FIXED_2ND_DERIV_BC:\n\t\t  return lx * lx * _BCLowVal + firstDeriv * lx + y(0);\n\t  case PARABOLIC_RUNOUT_BC:\n\t\treturn lx * lx * _ddy[0] + lx * firstDeriv  + y(0);\n\t  }\n\tthrow std::runtime_error(\"Unknown BC\");\n  }\n\n  inline double highCalc(double xval)\n  {\n\tconst double lx = xval - x(size() - 1);\n\n\tif (_type == LINEAR)\n\t  return lx * _BCHighVal + y(size() - 1);\n\n\tconst double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2);\n\n\tswitch(_BCHigh)\n\t  {\n\t  case FIXED_1ST_DERIV_BC:\n\t\treturn lx * _BCHighVal + y(size() - 1);\n\t  case FIXED_2ND_DERIV_BC:\n\t\treturn lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);\n\t  case PARABOLIC_RUNOUT_BC:\n\t\treturn lx * lx * _ddy[size()-1] + lx * firstDeriv  + y(size() - 1);\n\t  }\n\tthrow std::runtime_error(\"Unknown BC\");\n  }\n\n  //These just provide access to the point data in a clean way\n  inline double x(size_t i) const { return operator[](i).first; }\n  inline double y(size_t i) const { return operator[](i).second; }\n  inline double h(size_t i) const { return x(i+1) - x(i); }\n\n  //Invert a arbitrary matrix using the boost ublas library\n  template<class T>\n  bool InvertMatrix(ublas::matrix<T> A,\n\t\tublas::matrix<T>& inverse)\n  {\n\tusing namespace ublas;\n\n\t// create a permutation matrix for the LU-factorization\n\tpermutation_matrix<std::size_t> pm(A.size1());\n\n\t// perform LU-factorization\n\tint res = lu_factorize(A,pm);\n\t\tif( res != 0 ) return false;\n\n\t// create identity matrix of \"inverse\"\n\tinverse.assign(ublas::identity_matrix<T>(A.size1()));\n\n\t// backsubstitute to get the inverse\n\tlu_substitute(A, pm, inverse);\n\n\treturn true;\n  }\n\n  //This function will recalculate the spline parameters and store\n  //them in _data, ready for spline interpolation\n  void generate()\n  {\n\tif (size() < 2)\n\t  throw std::runtime_error(\"Spline requires at least 2 points\");\n\n\t//If any spline points are at the same x location, we have to\n\t//just slightly seperate them\n\t{\n\t  bool testPassed(false);\n\t  while (!testPassed)\n\t\t{\n\t\t  testPassed = true;\n\t\t  std::sort(base::begin(), base::end());\n\n\t\t  for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr)\n\t\tif (iPtr->first == (iPtr+1)->first)\n\t\t  {\n\t\t\tif ((iPtr+1)->first != 0)\n\t\t\t  (iPtr+1)->first += (iPtr+1)->first\n\t\t\t* std::numeric_limits<double>::epsilon() * 10;\n\t\t\telse\n\t\t\t  (iPtr+1)->first = std::numeric_limits<double>::epsilon() * 10;\n\t\t\ttestPassed = false;\n\t\t\tbreak;\n\t\t  }\n\t\t}\n\t}\n\n\tconst size_t e = size() - 1;\n\n\tswitch (_type)\n\t  {\n\t  case LINEAR:\n\t\t{\n\t\t  _data.resize(e);\n\t\t  for (size_t i(0); i < e; ++i)\n\t\t{\n\t\t  _data[i].x = x(i);\n\t\t  _data[i].a = 0;\n\t\t  _data[i].b = 0;\n\t\t  _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i));\n\t\t  _data[i].d = y(i);\n\t\t}\n\t\t  break;\n\t\t}\n\t  case CUBIC:\n\t\t{\n\t\t  ublas::matrix<double> A(size(), size());\n\t\t  for (size_t yv(0); yv <= e; ++yv)\n\t\tfor (size_t xv(0); xv <= e; ++xv)\n\t\t  A(xv,yv) = 0;\n\n\t\t  for (size_t i(1); i < e; ++i)\n\t\t{\n\t\t  A(i-1,i) = h(i-1);\n\t\t  A(i,i) = 2 * (h(i-1) + h(i));\n\t\t  A(i+1,i) = h(i);\n\t\t}\n\n\t\t  ublas::vector<double> C(size());\n\t\t  for (size_t xv(0); xv <= e; ++xv)\n\t\tC(xv) = 0;\n\n\t\t  for (size_t i(1); i < e; ++i)\n\t\tC(i) = 6 *\n\t\t  ((y(i+1) - y(i)) / h(i)\n\t\t   - (y(i) - y(i-1)) / h(i-1));\n\n\t\t  //Boundary conditions\n\t\t  switch(_BCLow)\n\t\t{\n\t\tcase FIXED_1ST_DERIV_BC:\n\t\t  C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);\n\t\t  A(0,0) = 2 * h(0);\n\t\t  A(1,0) = h(0);\n\t\t  break;\n\t\tcase FIXED_2ND_DERIV_BC:\n\t\t  C(0) = _BCLowVal;\n\t\t  A(0,0) = 1;\n\t\t  break;\n\t\tcase PARABOLIC_RUNOUT_BC:\n\t\t  C(0) = 0; A(0,0) = 1; A(1,0) = -1;\n\t\t  break;\n\t\t}\n\n\t\t  switch(_BCHigh)\n\t\t{\n\t\tcase FIXED_1ST_DERIV_BC:\n\t\t  C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1));\n\t\t  A(e,e) = 2 * h(e - 1);\n\t\t  A(e-1,e) = h(e - 1);\n\t\t  break;\n\t\tcase FIXED_2ND_DERIV_BC:\n\t\t  C(e) = _BCHighVal;\n\t\t  A(e,e) = 1;\n\t\t  break;\n\t\tcase PARABOLIC_RUNOUT_BC:\n\t\t  C(e) = 0; A(e,e) = 1; A(e-1,e) = -1;\n\t\t  break;\n\t\t}\n\n\t\t  ublas::matrix<double> AInv(size(), size());\n\t\t  InvertMatrix(A,AInv);\n\n\t\t  _ddy = ublas::prod(C, AInv);\n\n\t\t  _data.resize(size()-1);\n\t\t  for (size_t i(0); i < e; ++i)\n\t\t{\n\t\t  _data[i].x = x(i);\n\t\t  _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i));\n\t\t  _data[i].b = _ddy(i) / 2;\n\t\t  _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3;\n\t\t  _data[i].d = y(i);\n\t\t}\n\t\t}\n\t  }\n\t_valid = true;\n  }\n};\n"
  },
  {
    "path": "camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h",
    "content": "#ifndef EIGENQUATERNIONPARAMETERIZATION_H\n#define EIGENQUATERNIONPARAMETERIZATION_H\n\n#include \"ceres/local_parameterization.h\"\n\nnamespace camodocal\n{\n\nclass EigenQuaternionParameterization : public ceres::LocalParameterization\n{\npublic:\n    virtual ~EigenQuaternionParameterization() {}\n    virtual bool Plus(const double* x,\n                      const double* delta,\n                      double* x_plus_delta) const;\n    virtual bool ComputeJacobian(const double* x,\n                                 double* jacobian) const;\n    virtual int GlobalSize() const { return 4; }\n    virtual int LocalSize() const { return 3; }\n\nprivate:\n    template<typename T>\n    void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;\n};\n\n\ntemplate<typename T>\nvoid\nEigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const\n{\n    zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];\n    zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];\n    zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];\n    zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];\n}\n\n}\n\n#endif\n\n"
  },
  {
    "path": "camera_model/include/camodocal/gpl/EigenUtils.h",
    "content": "#ifndef EIGENUTILS_H\n#define EIGENUTILS_H\n\n#include <eigen3/Eigen/Dense>\n\n#include \"ceres/rotation.h\"\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\n// Returns the 3D cross product skew symmetric matrix of a given 3D vector\ntemplate<typename T>\nEigen::Matrix<T, 3, 3> skew(const Eigen::Matrix<T, 3, 1>& vec)\n{\n    return (Eigen::Matrix<T, 3, 3>() << T(0), -vec(2), vec(1),\n                                        vec(2), T(0), -vec(0),\n                                        -vec(1), vec(0), T(0)).finished();\n}\n\ntemplate<typename Derived>\ntypename Eigen::MatrixBase<Derived>::PlainObject sqrtm(const Eigen::MatrixBase<Derived>& A)\n{\n    Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> es(A);\n\n    return es.operatorSqrt();\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 3, 3> AngleAxisToRotationMatrix(const Eigen::Matrix<T, 3, 1>& rvec)\n{\n    T angle = rvec.norm();\n    if (angle == T(0))\n    {\n        return Eigen::Matrix<T, 3, 3>::Identity();\n    }\n\n    Eigen::Matrix<T, 3, 1> axis;\n    axis = rvec.normalized();\n\n    Eigen::Matrix<T, 3, 3> rmat;\n    rmat = Eigen::AngleAxis<T>(angle, axis);\n\n    return rmat;\n}\n\ntemplate<typename T>\nEigen::Quaternion<T> AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1>& rvec)\n{\n    Eigen::Matrix<T, 3, 3> rmat = AngleAxisToRotationMatrix<T>(rvec);\n\n    return Eigen::Quaternion<T>(rmat);\n}\n\ntemplate<typename T>\nvoid AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1>& rvec, T* q)\n{\n    Eigen::Quaternion<T> quat = AngleAxisToQuaternion<T>(rvec);\n\n    q[0] = quat.x();\n    q[1] = quat.y();\n    q[2] = quat.z();\n    q[3] = quat.w();\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 3, 1> RotationToAngleAxis(const Eigen::Matrix<T, 3, 3> & rmat)\n{\n    Eigen::AngleAxis<T> angleaxis; \n    angleaxis.fromRotationMatrix(rmat); \n    return angleaxis.angle() * angleaxis.axis(); \n    \n}\n\ntemplate<typename T>\nvoid QuaternionToAngleAxis(const T* const q, Eigen::Matrix<T, 3, 1>& rvec)\n{\n    Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]);\n\n    Eigen::Matrix<T, 3, 3> rmat = quat.toRotationMatrix();\n\n    Eigen::AngleAxis<T> angleaxis;\n    angleaxis.fromRotationMatrix(rmat);\n\n    rvec = angleaxis.angle() * angleaxis.axis();\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 3, 3> QuaternionToRotation(const T* const q)\n{\n    T R[9];\n    ceres::QuaternionToRotation(q, R);\n\n    Eigen::Matrix<T, 3, 3> rmat;\n    for (int i = 0; i < 3; ++i)\n    {\n        for (int j = 0; j < 3; ++j)\n        {\n            rmat(i,j) = R[i * 3 + j];\n        }\n    }\n\n    return rmat;\n}\n\ntemplate<typename T>\nvoid QuaternionToRotation(const T* const q, T* rot)\n{\n    ceres::QuaternionToRotation(q, rot);\n}\n\ntemplate<typename T>\nEigen::Matrix<T,4,4> QuaternionMultMatLeft(const Eigen::Quaternion<T>& q)\n{\n    return (Eigen::Matrix<T,4,4>() << q.w(), -q.z(), q.y(), q.x(),\n                                      q.z(), q.w(), -q.x(), q.y(),\n                                      -q.y(), q.x(), q.w(), q.z(),\n                                      -q.x(), -q.y(), -q.z(), q.w()).finished();\n}\n\ntemplate<typename T>\nEigen::Matrix<T,4,4> QuaternionMultMatRight(const Eigen::Quaternion<T>& q)\n{\n    return (Eigen::Matrix<T,4,4>() << q.w(), q.z(), -q.y(), q.x(),\n                                      -q.z(), q.w(), q.x(), q.y(),\n                                      q.y(), -q.x(), q.w(), q.z(),\n                                      -q.x(), -q.y(), -q.z(), q.w()).finished();\n}\n\n/// @param theta - rotation about screw axis\n/// @param d - projection of tvec on the rotation axis\n/// @param l - screw axis direction\n/// @param m - screw axis moment\ntemplate<typename T>\nvoid AngleAxisAndTranslationToScrew(const Eigen::Matrix<T, 3, 1>& rvec,\n                                    const Eigen::Matrix<T, 3, 1>& tvec,\n                                    T& theta, T& d,\n                                    Eigen::Matrix<T, 3, 1>& l,\n                                    Eigen::Matrix<T, 3, 1>& m)\n{\n\n    theta = rvec.norm();\n    if (theta == 0)\n    {\n        l.setZero(); \n        m.setZero(); \n        std::cout << \"Warning: Undefined screw! Returned 0. \" << std::endl; \n    }\n\n    l = rvec.normalized();\n\n    Eigen::Matrix<T, 3, 1> t = tvec;\n\n    d = t.transpose() * l;\n\n    // point on screw axis - projection of origin on screw axis\n    Eigen::Matrix<T, 3, 1> c;\n    c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));\n\n    // c and hence the screw axis is not defined if theta is either 0 or M_PI\n    m = c.cross(l);\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 3, 3> RPY2mat(T roll, T pitch, T yaw)\n{\n    Eigen::Matrix<T, 3, 3> m;\n\n    T cr = cos(roll);\n    T sr = sin(roll);\n    T cp = cos(pitch);\n    T sp = sin(pitch);\n    T cy = cos(yaw);\n    T sy = sin(yaw);\n\n    m(0,0) = cy * cp;\n    m(0,1) = cy * sp * sr - sy * cr;\n    m(0,2) = cy * sp * cr + sy * sr;\n    m(1,0) = sy * cp;\n    m(1,1) = sy * sp * sr + cy * cr;\n    m(1,2) = sy * sp * cr - cy * sr;\n    m(2,0) = - sp;\n    m(2,1) = cp * sr;\n    m(2,2) = cp * cr;\n    return m; \n}\n\ntemplate<typename T>\nvoid mat2RPY(const Eigen::Matrix<T, 3, 3>& m, T& roll, T& pitch, T& yaw)\n{\n    roll = atan2(m(2,1), m(2,2));\n    pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));\n    yaw = atan2(m(1,0), m(0,0));\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 4, 4> homogeneousTransform(const Eigen::Matrix<T, 3, 3>& R, const Eigen::Matrix<T, 3, 1>& t)\n{\n    Eigen::Matrix<T, 4, 4> H;\n    H.setIdentity();\n\n    H.block(0,0,3,3) = R;\n    H.block(0,3,3,1) = t;\n\n    return H;\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 4, 4> poseWithCartesianTranslation(const T* const q, const T* const p)\n{\n    Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();\n\n    T rotation[9];\n    ceres::QuaternionToRotation(q, rotation);\n    for (int i = 0; i < 3; ++i)\n    {\n        for (int j = 0; j < 3; ++j)\n        {\n            pose(i,j) = rotation[i * 3 + j];\n        }\n    }\n\n    pose(0,3) = p[0];\n    pose(1,3) = p[1];\n    pose(2,3) = p[2];\n\n    return pose;\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 4, 4> poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0))\n{\n    Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();\n\n    T rotation[9];\n    ceres::QuaternionToRotation(q, rotation);\n    for (int i = 0; i < 3; ++i)\n    {\n        for (int j = 0; j < 3; ++j)\n        {\n            pose(i,j) = rotation[i * 3 + j];\n        }\n    }\n\n    T theta = p[0];\n    T phi = p[1];\n    pose(0,3) = sin(theta) * cos(phi) * scale;\n    pose(1,3) = sin(theta) * sin(phi) * scale;\n    pose(2,3) = cos(theta) * scale;\n\n    return pose;\n}\n\n// Returns the Sampson error of a given essential matrix and 2 image points\ntemplate<typename T>\nT sampsonError(const Eigen::Matrix<T, 3, 3>& E,\n               const Eigen::Matrix<T, 3, 1>& p1,\n               const Eigen::Matrix<T, 3, 1>& p2)\n{\n    Eigen::Matrix<T, 3, 1> Ex1 = E * p1;\n    Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;\n\n    T x2tEx1 = p2.dot(Ex1);\n\n    // compute Sampson error\n    T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));\n\n    return err;\n}\n\n// Returns the Sampson error of a given rotation/translation and 2 image points\ntemplate<typename T>\nT sampsonError(const Eigen::Matrix<T, 3, 3>& R,\n               const Eigen::Matrix<T, 3, 1>& t,\n               const Eigen::Matrix<T, 3, 1>& p1,\n               const Eigen::Matrix<T, 3, 1>& p2)\n{\n    // construct essential matrix\n    Eigen::Matrix<T, 3, 3> E = skew(t) * R;\n\n    Eigen::Matrix<T, 3, 1> Ex1 = E * p1;\n    Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;\n\n    T x2tEx1 = p2.dot(Ex1);\n\n    // compute Sampson error\n    T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));\n\n    return err;\n}\n\n// Returns the Sampson error of a given rotation/translation and 2 image points\ntemplate<typename T>\nT sampsonError(const Eigen::Matrix<T, 4, 4>& H,\n               const Eigen::Matrix<T, 3, 1>& p1,\n               const Eigen::Matrix<T, 3, 1>& p2)\n{\n    Eigen::Matrix<T, 3, 3> R = H.block(0, 0, 3, 3);\n    Eigen::Matrix<T, 3, 1> t = H.block(0, 3, 3, 1);\n\n    return sampsonError(R, t, p1, p2);\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 3, 1>\ntransformPoint(const Eigen::Matrix<T, 4, 4>& H, const Eigen::Matrix<T, 3, 1>& P)\n{\n    Eigen::Matrix<T, 3, 1> P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);\n\n    return P_trans;\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 4, 4>\nestimate3DRigidTransform(const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points1,\n                         const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points2)\n{\n    // compute centroids\n    Eigen::Matrix<T, 3, 1> c1, c2;\n    c1.setZero(); c2.setZero();\n\n    for (size_t i = 0; i < points1.size(); ++i)\n    {\n        c1 += points1.at(i);\n        c2 += points2.at(i);\n    }\n\n    c1 /= points1.size();\n    c2 /= points1.size();\n\n    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());\n    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());\n    for (size_t i = 0; i < points1.size(); ++i)\n    {\n        X.col(i) = points1.at(i) - c1;\n        Y.col(i) = points2.at(i) - c2;\n    }\n\n    Eigen::Matrix<T, 3, 3> H = X * Y.transpose();\n\n    Eigen::JacobiSVD< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);\n\n    Eigen::Matrix<T, 3, 3> U = svd.matrixU();\n    Eigen::Matrix<T, 3, 3> V = svd.matrixV();\n    if (U.determinant() * V.determinant() < 0.0)\n    {\n        V.col(2) *= -1.0;\n    }\n\n    Eigen::Matrix<T, 3, 3> R = V * U.transpose();\n    Eigen::Matrix<T, 3, 1> t = c2 - R * c1;\n\n    return homogeneousTransform(R, t);\n}\n\ntemplate<typename T>\nEigen::Matrix<T, 4, 4>\nestimate3DRigidSimilarityTransform(const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points1,\n                                   const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points2)\n{\n    // compute centroids\n    Eigen::Matrix<T, 3, 1> c1, c2;\n    c1.setZero(); c2.setZero();\n\n    for (size_t i = 0; i < points1.size(); ++i)\n    {\n        c1 += points1.at(i);\n        c2 += points2.at(i);\n    }\n\n    c1 /= points1.size();\n    c2 /= points1.size();\n\n    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());\n    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());\n    for (size_t i = 0; i < points1.size(); ++i)\n    {\n        X.col(i) = points1.at(i) - c1;\n        Y.col(i) = points2.at(i) - c2;\n    }\n\n    Eigen::Matrix<T, 3, 3> H = X * Y.transpose();\n\n    Eigen::JacobiSVD< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);\n\n    Eigen::Matrix<T, 3, 3> U = svd.matrixU();\n    Eigen::Matrix<T, 3, 3> V = svd.matrixV();\n    if (U.determinant() * V.determinant() < 0.0)\n    {\n        V.col(2) *= -1.0;\n    }\n\n    Eigen::Matrix<T, 3, 3> R = V * U.transpose();\n\n    std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > > rotatedPoints1(points1.size());\n    for (size_t i = 0; i < points1.size(); ++i)\n    {\n        rotatedPoints1.at(i) = R * (points1.at(i) - c1);\n    }\n\n    double sum_ss = 0.0, sum_tt = 0.0;\n    for (size_t i = 0; i < points1.size(); ++i)\n    {\n        sum_ss += (points1.at(i) - c1).squaredNorm();\n        sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));\n    }\n\n    double scale = sum_tt / sum_ss;\n\n    Eigen::Matrix<T, 3, 3> sR = scale * R;\n    Eigen::Matrix<T, 3, 1> t = c2 - sR * c1;\n\n    return homogeneousTransform(sR, t);\n}\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/include/camodocal/gpl/gpl.h",
    "content": "#ifndef GPL_H\r\n#define GPL_H\r\n\r\n#include <algorithm>\r\n#include <cmath>\r\n#include <opencv2/core/core.hpp>\r\n\r\nnamespace camodocal\r\n{\r\n\r\ntemplate<class T>\r\nconst T clamp(const T& v, const T& a, const T& b)\r\n{\r\n\treturn std::min(b, std::max(a, v));\r\n}\r\n\r\ndouble hypot3(double x, double y, double z);\r\nfloat hypot3f(float x, float y, float z);\r\n\r\ntemplate<class T>\r\nconst T normalizeTheta(const T& theta)\r\n{\r\n\tT normTheta = theta;\r\n\r\n\twhile (normTheta < - M_PI)\r\n\t{\r\n\t\tnormTheta += 2.0 * M_PI;\r\n\t}\r\n\twhile (normTheta > M_PI)\r\n\t{\r\n\t\tnormTheta -= 2.0 * M_PI;\r\n\t}\r\n\r\n\treturn normTheta;\r\n}\r\n\r\ndouble d2r(double deg);\r\nfloat d2r(float deg);\r\ndouble r2d(double rad);\r\nfloat r2d(float rad);\r\n\r\ndouble sinc(double theta);\r\n\r\ntemplate<class T>\r\nconst T square(const T& x)\r\n{\r\n\treturn x * x;\r\n}\r\n\r\ntemplate<class T>\r\nconst T cube(const T& x)\r\n{\r\n\treturn x * x * x;\r\n}\r\n\r\ntemplate<class T>\r\nconst T random(const T& a, const T& b)\r\n{\r\n\treturn static_cast<double>(rand()) / RAND_MAX * (b - a) + a;\r\n}\r\n\r\ntemplate<class T>\r\nconst T randomNormal(const T& sigma)\r\n{\r\n    T x1, x2, w;\r\n\r\n    do\r\n    {\r\n        x1 = 2.0 * random(0.0, 1.0) - 1.0;\r\n        x2 = 2.0 * random(0.0, 1.0) - 1.0;\r\n        w = x1 * x1 + x2 * x2;\r\n    }\r\n    while (w >= 1.0 || w == 0.0);\r\n\r\n    w = sqrt((-2.0 * log(w)) / w);\r\n\r\n    return x1 * w * sigma;\r\n}\r\n\r\nunsigned long long timeInMicroseconds(void);\r\n\r\ndouble timeInSeconds(void);\r\n\r\nvoid colorDepthImage(cv::Mat& imgDepth,\r\n                     cv::Mat& imgColoredDepth,\r\n                     float minRange, float maxRange);\r\n\r\nbool colormap(const std::string& name, unsigned char idx,\r\n              float& r, float& g, float& b);\r\n\r\nstd::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1);\r\nstd::vector<cv::Point2i> bresCircle(int x0, int y0, int r);\r\n\r\nvoid fitCircle(const std::vector<cv::Point2d>& points,\r\n               double& centerX, double& centerY, double& radius);\r\n\r\nstd::vector<cv::Point2d> intersectCircles(double x1, double y1, double r1,\r\n                                          double x2, double y2, double r2);\r\n\r\nvoid LLtoUTM(double latitude, double longitude,\r\n             double& utmNorthing, double& utmEasting,\r\n             std::string& utmZone);\r\nvoid UTMtoLL(double utmNorthing, double utmEasting,\r\n             const std::string& utmZone,\r\n             double& latitude, double& longitude);\r\n\r\nlong int timestampDiff(uint64_t t1, uint64_t t2);\r\n\r\n}\r\n\r\n#endif\r\n"
  },
  {
    "path": "camera_model/include/camodocal/sparse_graph/Transform.h",
    "content": "#ifndef TRANSFORM_H\n#define TRANSFORM_H\n\n#include <boost/shared_ptr.hpp>\n#include <eigen3/Eigen/Dense>\n#include <stdint.h>\n\nnamespace camodocal\n{\n\nclass Transform\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    Transform();\n    Transform(const Eigen::Matrix4d& H);\n\n    Eigen::Quaterniond& rotation(void);\n    const Eigen::Quaterniond& rotation(void) const;\n    double* rotationData(void);\n    const double* const rotationData(void) const;\n\n    Eigen::Vector3d& translation(void);\n    const Eigen::Vector3d& translation(void) const;\n    double* translationData(void);\n    const double* const translationData(void) const;\n\n    Eigen::Matrix4d toMatrix(void) const;\n\nprivate:\n    Eigen::Quaterniond m_q;\n    Eigen::Vector3d m_t;\n};\n\n}\n\n#endif\n"
  },
  {
    "path": "camera_model/instruction",
    "content": "rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/\n"
  },
  {
    "path": "camera_model/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>camera_model</name>\n  <version>0.0.0</version>\n  <description>The camera_model package</description>\n\n  <!-- One maintainer tag required, multiple allowed, one person per tag --> \n  <!-- Example:  -->\n  <!-- <maintainer email=\"jane.doe@example.com\">Jane Doe</maintainer> -->\n  <maintainer email=\"dvorak@todo.todo\">dvorak</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>TODO</license>\n\n\n  <!-- Url tags are optional, but mutiple are allowed, one per tag -->\n  <!-- Optional attribute type can be: website, bugtracker, or repository -->\n  <!-- Example: -->\n  <!-- <url type=\"website\">http://wiki.ros.org/camera_model</url> -->\n\n\n  <!-- Author tags are optional, mutiple are allowed, one per tag -->\n  <!-- Authors do not have to be maintianers, but could be -->\n  <!-- Example: -->\n  <!-- <author email=\"jane.doe@example.com\">Jane Doe</author> -->\n\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <!--build_depend>roscpp</build_depend-->\n  <!--build_depend>std_msgs</build_depend-->\n  <!--run_depend>roscpp</run_depend-->\n  <!--run_depend>std_msgs</run_depend-->\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>"
  },
  {
    "path": "camera_model/readme.md",
    "content": "part of [camodocal](https://github.com/hengli/camodocal)\n\n[Google Ceres](http://ceres-solver.org) is needed.\n\n# Calibration:\n\nUse [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera.\n\n# Undistortion:\n\nSee [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: \n\n - liftProjective: Lift points from the image plane to the projective space.\n - spaceToPlane: Projects 3D points to the image plane (Pi function)\n\n"
  },
  {
    "path": "camera_model/src/calib/CameraCalibration.cc",
    "content": "#include \"camodocal/calib/CameraCalibration.h\"\n\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#include <iomanip>\n#include <iostream>\n#include <algorithm>\n#include <fstream>\n#include <opencv2/core/core.hpp>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n#include <opencv2/calib3d/calib3d.hpp>\n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/sparse_graph/Transform.h\"\n#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n#include \"camodocal/gpl/EigenUtils.h\"\n#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\nnamespace camodocal\n{\n\nCameraCalibration::CameraCalibration()\n : m_boardSize(cv::Size(0,0))\n , m_squareSize(0.0f)\n , m_verbose(false)\n{\n\n}\n\nCameraCalibration::CameraCalibration(const Camera::ModelType modelType,\n                                     const std::string& cameraName,\n                                     const cv::Size& imageSize,\n                                     const cv::Size& boardSize,\n                                     float squareSize)\n : m_boardSize(boardSize)\n , m_squareSize(squareSize)\n , m_verbose(false)\n{\n    m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize);\n}\n\nvoid\nCameraCalibration::clear(void)\n{\n    m_imagePoints.clear();\n    m_scenePoints.clear();\n}\n\nvoid\nCameraCalibration::addChessboardData(const std::vector<cv::Point2f>& corners)\n{\n    m_imagePoints.push_back(corners);\n\n    std::vector<cv::Point3f> scenePointsInView;\n    for (int i = 0; i < m_boardSize.height; ++i)\n    {\n        for (int j = 0; j < m_boardSize.width; ++j)\n        {\n            scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0));\n        }\n    }\n    m_scenePoints.push_back(scenePointsInView);\n}\n\nbool\nCameraCalibration::calibrate(void)\n{\n    int imageCount = m_imagePoints.size();\n\n    // compute intrinsic camera parameters and extrinsic parameters for each of the views\n    std::vector<cv::Mat> rvecs;\n    std::vector<cv::Mat> tvecs;\n    bool ret = calibrateHelper(m_camera, rvecs, tvecs);\n\n    m_cameraPoses = cv::Mat(imageCount, 6, CV_64F);\n    for (int i = 0; i < imageCount; ++i)\n    {\n        m_cameraPoses.at<double>(i,0) = rvecs.at(i).at<double>(0);\n        m_cameraPoses.at<double>(i,1) = rvecs.at(i).at<double>(1);\n        m_cameraPoses.at<double>(i,2) = rvecs.at(i).at<double>(2);\n        m_cameraPoses.at<double>(i,3) = tvecs.at(i).at<double>(0);\n        m_cameraPoses.at<double>(i,4) = tvecs.at(i).at<double>(1);\n        m_cameraPoses.at<double>(i,5) = tvecs.at(i).at<double>(2);\n    }\n\n    // Compute measurement covariance.\n    std::vector<std::vector<cv::Point2f> > errVec(m_imagePoints.size());\n    Eigen::Vector2d errSum = Eigen::Vector2d::Zero();\n    size_t errCount = 0;\n    for (size_t i = 0; i < m_imagePoints.size(); ++i)\n    {\n        std::vector<cv::Point2f> estImagePoints;\n        m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),\n                                estImagePoints);\n\n        for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n        {\n            cv::Point2f pObs = m_imagePoints.at(i).at(j);\n            cv::Point2f pEst = estImagePoints.at(j);\n\n            cv::Point2f err = pObs - pEst;\n\n            errVec.at(i).push_back(err);\n\n            errSum += Eigen::Vector2d(err.x, err.y);\n        }\n\n        errCount += m_imagePoints.at(i).size();\n    }\n\n    Eigen::Vector2d errMean = errSum / static_cast<double>(errCount);\n\n    Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero();\n    for (size_t i = 0; i < errVec.size(); ++i)\n    {\n        for (size_t j = 0; j < errVec.at(i).size(); ++j)\n        {\n            cv::Point2f err = errVec.at(i).at(j);\n            double d0 = err.x - errMean(0);\n            double d1 = err.y - errMean(1);\n\n            measurementCovariance(0,0) += d0 * d0;\n            measurementCovariance(0,1) += d0 * d1;\n            measurementCovariance(1,1) += d1 * d1;\n        }\n    }\n    measurementCovariance /= static_cast<double>(errCount);\n    measurementCovariance(1,0) = measurementCovariance(0,1);\n\n    m_measurementCovariance = measurementCovariance;\n\n    return ret;\n}\n\nint\nCameraCalibration::sampleCount(void) const\n{\n    return m_imagePoints.size();\n}\n\nstd::vector<std::vector<cv::Point2f> >&\nCameraCalibration::imagePoints(void)\n{\n    return m_imagePoints;\n}\n\nconst std::vector<std::vector<cv::Point2f> >&\nCameraCalibration::imagePoints(void) const\n{\n    return m_imagePoints;\n}\n\nstd::vector<std::vector<cv::Point3f> >&\nCameraCalibration::scenePoints(void)\n{\n    return m_scenePoints;\n}\n\nconst std::vector<std::vector<cv::Point3f> >&\nCameraCalibration::scenePoints(void) const\n{\n    return m_scenePoints;\n}\n\nCameraPtr&\nCameraCalibration::camera(void)\n{\n    return m_camera;\n}\n\nconst CameraConstPtr\nCameraCalibration::camera(void) const\n{\n    return m_camera;\n}\n\nEigen::Matrix2d&\nCameraCalibration::measurementCovariance(void)\n{\n    return m_measurementCovariance;\n}\n\nconst Eigen::Matrix2d&\nCameraCalibration::measurementCovariance(void) const\n{\n    return m_measurementCovariance;\n}\n\ncv::Mat&\nCameraCalibration::cameraPoses(void)\n{\n    return m_cameraPoses;\n}\n\nconst cv::Mat&\nCameraCalibration::cameraPoses(void) const\n{\n    return m_cameraPoses;\n}\n\nvoid\nCameraCalibration::drawResults(std::vector<cv::Mat>& images) const\n{\n    std::vector<cv::Mat> rvecs, tvecs;\n\n    for (size_t i = 0; i < images.size(); ++i)\n    {\n        cv::Mat rvec(3, 1, CV_64F);\n        rvec.at<double>(0) = m_cameraPoses.at<double>(i,0);\n        rvec.at<double>(1) = m_cameraPoses.at<double>(i,1);\n        rvec.at<double>(2) = m_cameraPoses.at<double>(i,2);\n\n        cv::Mat tvec(3, 1, CV_64F);\n        tvec.at<double>(0) = m_cameraPoses.at<double>(i,3);\n        tvec.at<double>(1) = m_cameraPoses.at<double>(i,4);\n        tvec.at<double>(2) = m_cameraPoses.at<double>(i,5);\n\n        rvecs.push_back(rvec);\n        tvecs.push_back(tvec);\n    }\n\n    int drawShiftBits = 4;\n    int drawMultiplier = 1 << drawShiftBits;\n\n    cv::Scalar green(0, 255, 0);\n    cv::Scalar red(0, 0, 255);\n\n    for (size_t i = 0; i < images.size(); ++i)\n    {\n        cv::Mat& image = images.at(i);\n        if (image.channels() == 1)\n        {\n            cv::cvtColor(image, image, CV_GRAY2RGB);\n        }\n\n        std::vector<cv::Point2f> estImagePoints;\n        m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),\n                                estImagePoints);\n\n        float errorSum = 0.0f;\n        float errorMax = std::numeric_limits<float>::min();\n\n        for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n        {\n            cv::Point2f pObs = m_imagePoints.at(i).at(j);\n            cv::Point2f pEst = estImagePoints.at(j);\n\n            cv::circle(image,\n                       cv::Point(cvRound(pObs.x * drawMultiplier),\n                                 cvRound(pObs.y * drawMultiplier)),\n                       5, green, 2, CV_AA, drawShiftBits);\n\n            cv::circle(image,\n                       cv::Point(cvRound(pEst.x * drawMultiplier),\n                                 cvRound(pEst.y * drawMultiplier)),\n                       5, red, 2, CV_AA, drawShiftBits);\n\n            float error = cv::norm(pObs - pEst);\n\n            errorSum += error;\n            if (error > errorMax)\n            {\n                errorMax = error;\n            }\n        }\n\n        std::ostringstream oss;\n        oss << \"Reprojection error: avg = \" << errorSum / m_imagePoints.at(i).size()\n            << \"   max = \" << errorMax;\n\n        cv::putText(image, oss.str(), cv::Point(10, image.rows - 10),\n                    cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),\n                    1, CV_AA);\n    }\n}\n\nvoid\nCameraCalibration::writeParams(const std::string& filename) const\n{\n    m_camera->writeParametersToYamlFile(filename);\n}\n\nbool\nCameraCalibration::writeChessboardData(const std::string& filename) const\n{\n    std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary);\n    if (!ofs.is_open())\n    {\n        return false;\n    }\n\n    writeData(ofs, m_boardSize.width);\n    writeData(ofs, m_boardSize.height);\n    writeData(ofs, m_squareSize);\n\n    writeData(ofs, m_measurementCovariance(0,0));\n    writeData(ofs, m_measurementCovariance(0,1));\n    writeData(ofs, m_measurementCovariance(1,0));\n    writeData(ofs, m_measurementCovariance(1,1));\n\n    writeData(ofs, m_cameraPoses.rows);\n    writeData(ofs, m_cameraPoses.cols);\n    writeData(ofs, m_cameraPoses.type());\n    for (int i = 0; i < m_cameraPoses.rows; ++i)\n    {\n        for (int j = 0; j < m_cameraPoses.cols; ++j)\n        {\n            writeData(ofs, m_cameraPoses.at<double>(i,j));\n        }\n    }\n\n    writeData(ofs, m_imagePoints.size());\n    for (size_t i = 0; i < m_imagePoints.size(); ++i)\n    {\n        writeData(ofs, m_imagePoints.at(i).size());\n        for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n        {\n            const cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n\n            writeData(ofs, ipt.x);\n            writeData(ofs, ipt.y);\n        }\n    }\n\n    writeData(ofs, m_scenePoints.size());\n    for (size_t i = 0; i < m_scenePoints.size(); ++i)\n    {\n        writeData(ofs, m_scenePoints.at(i).size());\n        for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)\n        {\n            const cv::Point3f& spt = m_scenePoints.at(i).at(j);\n\n            writeData(ofs, spt.x);\n            writeData(ofs, spt.y);\n            writeData(ofs, spt.z);\n        }\n    }\n\n    return true;\n}\n\nbool\nCameraCalibration::readChessboardData(const std::string& filename)\n{\n    std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);\n    if (!ifs.is_open())\n    {\n        return false;\n    }\n\n    readData(ifs, m_boardSize.width);\n    readData(ifs, m_boardSize.height);\n    readData(ifs, m_squareSize);\n\n    readData(ifs, m_measurementCovariance(0,0));\n    readData(ifs, m_measurementCovariance(0,1));\n    readData(ifs, m_measurementCovariance(1,0));\n    readData(ifs, m_measurementCovariance(1,1));\n\n    int rows, cols, type;\n    readData(ifs, rows);\n    readData(ifs, cols);\n    readData(ifs, type);\n    m_cameraPoses = cv::Mat(rows, cols, type);\n\n    for (int i = 0; i < m_cameraPoses.rows; ++i)\n    {\n        for (int j = 0; j < m_cameraPoses.cols; ++j)\n        {\n            readData(ifs, m_cameraPoses.at<double>(i,j));\n        }\n    }\n\n    size_t nImagePointSets;\n    readData(ifs, nImagePointSets);\n\n    m_imagePoints.clear();\n    m_imagePoints.resize(nImagePointSets);\n    for (size_t i = 0; i < m_imagePoints.size(); ++i)\n    {\n        size_t nImagePoints;\n        readData(ifs, nImagePoints);\n        m_imagePoints.at(i).resize(nImagePoints);\n\n        for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n        {\n            cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n            readData(ifs, ipt.x);\n            readData(ifs, ipt.y);\n        }\n    }\n\n    size_t nScenePointSets;\n    readData(ifs, nScenePointSets);\n\n    m_scenePoints.clear();\n    m_scenePoints.resize(nScenePointSets);\n    for (size_t i = 0; i < m_scenePoints.size(); ++i)\n    {\n        size_t nScenePoints;\n        readData(ifs, nScenePoints);\n        m_scenePoints.at(i).resize(nScenePoints);\n\n        for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j)\n        {\n            cv::Point3f& spt = m_scenePoints.at(i).at(j);\n            readData(ifs, spt.x);\n            readData(ifs, spt.y);\n            readData(ifs, spt.z);\n        }\n    }\n\n    return true;\n}\n\nvoid\nCameraCalibration::setVerbose(bool verbose)\n{\n    m_verbose = verbose;\n}\n\nbool\nCameraCalibration::calibrateHelper(CameraPtr& camera,\n                                   std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const\n{\n    rvecs.assign(m_scenePoints.size(), cv::Mat());\n    tvecs.assign(m_scenePoints.size(), cv::Mat());\n\n    // STEP 1: Estimate intrinsics\n    camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints);\n\n    // STEP 2: Estimate extrinsics\n    for (size_t i = 0; i < m_scenePoints.size(); ++i)\n    {\n        camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i));\n    }\n\n    if (m_verbose)\n    {\n        std::cout << \"[\" << camera->cameraName() << \"] \"\n                  << \"# INFO: \" << \"Initial reprojection error: \"\n                  << std::fixed << std::setprecision(3)\n                  << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs)\n                  << \" pixels\" << std::endl;\n    }\n\n    // STEP 3: optimization using ceres\n    optimize(camera, rvecs, tvecs);\n\n    if (m_verbose)\n    {\n        double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs);\n        std::cout << \"[\" << camera->cameraName() << \"] \" << \"# INFO: Final reprojection error: \"\n                  << err << \" pixels\" << std::endl;\n        std::cout << \"[\" << camera->cameraName() << \"] \" << \"# INFO: \"\n                  << camera->parametersToString() << std::endl;\n    }\n\n    return true;\n}\n\nvoid\nCameraCalibration::optimize(CameraPtr& camera,\n                            std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const\n{\n    // Use ceres to do optimization\n    ceres::Problem problem;\n\n    std::vector<Transform, Eigen::aligned_allocator<Transform> > transformVec(rvecs.size());\n    for (size_t i = 0; i < rvecs.size(); ++i)\n    {\n        Eigen::Vector3d rvec;\n        cv::cv2eigen(rvecs.at(i), rvec);\n\n        transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized());\n        transformVec.at(i).translation() << tvecs[i].at<double>(0),\n                                            tvecs[i].at<double>(1),\n                                            tvecs[i].at<double>(2);\n    }\n\n    std::vector<double> intrinsicCameraParams;\n    m_camera->writeParameters(intrinsicCameraParams);\n\n    // create residuals for each observation\n    for (size_t i = 0; i < m_imagePoints.size(); ++i)\n    {\n        for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j)\n        {\n            const cv::Point3f& spt = m_scenePoints.at(i).at(j);\n            const cv::Point2f& ipt = m_imagePoints.at(i).at(j);\n\n            ceres::CostFunction* costFunction =\n                CostFunctionFactory::instance()->generateCostFunction(camera,\n                                                                      Eigen::Vector3d(spt.x, spt.y, spt.z),\n                                                                      Eigen::Vector2d(ipt.x, ipt.y),\n                                                                      CAMERA_INTRINSICS | CAMERA_POSE);\n\n            ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0);\n            problem.AddResidualBlock(costFunction, lossFunction,\n                                     intrinsicCameraParams.data(),\n                                     transformVec.at(i).rotationData(),\n                                     transformVec.at(i).translationData());\n        }\n\n        ceres::LocalParameterization* quaternionParameterization =\n            new EigenQuaternionParameterization;\n\n        problem.SetParameterization(transformVec.at(i).rotationData(),\n                                    quaternionParameterization);\n    }\n\n    std::cout << \"begin ceres\" << std::endl;\n    ceres::Solver::Options options;\n    options.max_num_iterations = 1000;\n    options.num_threads = 1;\n\n    if (m_verbose)\n    {\n        options.minimizer_progress_to_stdout = true;\n    }\n\n    ceres::Solver::Summary summary;\n    ceres::Solve(options, &problem, &summary);\n    std::cout << \"end ceres\" << std::endl;\n\n    if (m_verbose)\n    {\n        std::cout << summary.FullReport() << std::endl;\n    }\n\n    camera->readParameters(intrinsicCameraParams);\n\n    for (size_t i = 0; i < rvecs.size(); ++i)\n    {\n        Eigen::AngleAxisd aa(transformVec.at(i).rotation());\n\n        Eigen::Vector3d rvec = aa.angle() * aa.axis();\n        cv::eigen2cv(rvec, rvecs.at(i));\n\n        cv::Mat& tvec = tvecs.at(i);\n        tvec.at<double>(0) = transformVec.at(i).translation()(0);\n        tvec.at<double>(1) = transformVec.at(i).translation()(1);\n        tvec.at<double>(2) = transformVec.at(i).translation()(2);\n    }\n}\n\ntemplate<typename T>\nvoid\nCameraCalibration::readData(std::ifstream& ifs, T& data) const\n{\n    char* buffer = new char[sizeof(T)];\n\n    ifs.read(buffer, sizeof(T));\n\n    data = *(reinterpret_cast<T*>(buffer));\n\n    delete[] buffer;\n}\n\ntemplate<typename T>\nvoid\nCameraCalibration::writeData(std::ofstream& ofs, T data) const\n{\n    char* pData = reinterpret_cast<char*>(&data);\n\n    ofs.write(pData, sizeof(T));\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/camera_models/Camera.cc",
    "content": "#include \"camodocal/camera_models/Camera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include <opencv2/calib3d/calib3d.hpp>\n\nnamespace camodocal\n{\n\nCamera::Parameters::Parameters(ModelType modelType)\n : m_modelType(modelType)\n , m_imageWidth(0)\n , m_imageHeight(0)\n{\n    switch (modelType)\n    {\n    case KANNALA_BRANDT:\n        m_nIntrinsics = 8;\n        break;\n    case PINHOLE:\n        m_nIntrinsics = 8;\n        break;\n    case SCARAMUZZA:\n        m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;\n        break;\n    case MEI:\n    default:\n        m_nIntrinsics = 9;\n    }\n}\n\nCamera::Parameters::Parameters(ModelType modelType,\n                               const std::string& cameraName,\n                               int w, int h)\n : m_modelType(modelType)\n , m_cameraName(cameraName)\n , m_imageWidth(w)\n , m_imageHeight(h)\n{\n    switch (modelType)\n    {\n    case KANNALA_BRANDT:\n        m_nIntrinsics = 8;\n        break;\n    case PINHOLE:\n        m_nIntrinsics = 8;\n        break;\n    case SCARAMUZZA:\n        m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;\n        break;\n    case MEI:\n    default:\n        m_nIntrinsics = 9;\n    }\n}\n\nCamera::ModelType&\nCamera::Parameters::modelType(void)\n{\n    return m_modelType;\n}\n\nstd::string&\nCamera::Parameters::cameraName(void)\n{\n    return m_cameraName;\n}\n\nint&\nCamera::Parameters::imageWidth(void)\n{\n    return m_imageWidth;\n}\n\nint&\nCamera::Parameters::imageHeight(void)\n{\n    return m_imageHeight;\n}\n\nCamera::ModelType\nCamera::Parameters::modelType(void) const\n{\n    return m_modelType;\n}\n\nconst std::string&\nCamera::Parameters::cameraName(void) const\n{\n    return m_cameraName;\n}\n\nint\nCamera::Parameters::imageWidth(void) const\n{\n    return m_imageWidth;\n}\n\nint\nCamera::Parameters::imageHeight(void) const\n{\n    return m_imageHeight;\n}\n\nint\nCamera::Parameters::nIntrinsics(void) const\n{\n    return m_nIntrinsics;\n}\n\ncv::Mat&\nCamera::mask(void)\n{\n    return m_mask;\n}\n\nconst cv::Mat&\nCamera::mask(void) const\n{\n    return m_mask;\n}\n\nvoid\nCamera::estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,\n                           const std::vector<cv::Point2f>& imagePoints,\n                           cv::Mat& rvec, cv::Mat& tvec) const\n{\n    std::vector<cv::Point2f> Ms(imagePoints.size());\n    for (size_t i = 0; i < Ms.size(); ++i)\n    {\n        Eigen::Vector3d P;\n        liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);\n\n        P /= P(2);\n\n        Ms.at(i).x = P(0);\n        Ms.at(i).y = P(1);\n    }\n\n    // assume unit focal length, zero principal point, and zero distortion\n    cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);\n}\n\ndouble\nCamera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const\n{\n    Eigen::Vector2d p1, p2;\n\n    spaceToPlane(P1, p1);\n    spaceToPlane(P2, p2);\n\n    return (p1 - p2).norm();\n}\n\ndouble\nCamera::reprojectionError(const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                          const std::vector< std::vector<cv::Point2f> >& imagePoints,\n                          const std::vector<cv::Mat>& rvecs,\n                          const std::vector<cv::Mat>& tvecs,\n                          cv::OutputArray _perViewErrors) const\n{\n    int imageCount = objectPoints.size();\n    size_t pointsSoFar = 0;\n    double totalErr = 0.0;\n\n    bool computePerViewErrors = _perViewErrors.needed();\n    cv::Mat perViewErrors;\n    if (computePerViewErrors)\n    {\n        _perViewErrors.create(imageCount, 1, CV_64F);\n        perViewErrors = _perViewErrors.getMat();\n    }\n\n    for (int i = 0; i < imageCount; ++i)\n    {\n        size_t pointCount = imagePoints.at(i).size();\n\n        pointsSoFar += pointCount;\n\n        std::vector<cv::Point2f> estImagePoints;\n        projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i),\n                      estImagePoints);\n\n        double err = 0.0;\n        for (size_t j = 0; j < imagePoints.at(i).size(); ++j)\n        {\n            err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));\n        }\n\n        if (computePerViewErrors)\n        {\n            perViewErrors.at<double>(i) = err / pointCount;\n        }\n\n        totalErr += err;\n    }\n\n    return totalErr / pointsSoFar;\n}\n\ndouble\nCamera::reprojectionError(const Eigen::Vector3d& P,\n                          const Eigen::Quaterniond& camera_q,\n                          const Eigen::Vector3d& camera_t,\n                          const Eigen::Vector2d& observed_p) const\n{\n    Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;\n\n    Eigen::Vector2d p;\n    spaceToPlane(P_cam, p);\n\n    return (p - observed_p).norm();\n}\n\nvoid\nCamera::projectPoints(const std::vector<cv::Point3f>& objectPoints,\n                      const cv::Mat& rvec,\n                      const cv::Mat& tvec,\n                      std::vector<cv::Point2f>& imagePoints) const\n{\n    // project 3D object points to the image plane\n    imagePoints.reserve(objectPoints.size());\n\n    //double\n    cv::Mat R0;\n    cv::Rodrigues(rvec, R0);\n\n    Eigen::MatrixXd R(3,3);\n    R << R0.at<double>(0,0), R0.at<double>(0,1), R0.at<double>(0,2),\n         R0.at<double>(1,0), R0.at<double>(1,1), R0.at<double>(1,2),\n         R0.at<double>(2,0), R0.at<double>(2,1), R0.at<double>(2,2);\n\n    Eigen::Vector3d t;\n    t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);\n\n    for (size_t i = 0; i < objectPoints.size(); ++i)\n    {\n        const cv::Point3f& objectPoint = objectPoints.at(i);\n\n        // Rotate and translate\n        Eigen::Vector3d P;\n        P << objectPoint.x, objectPoint.y, objectPoint.z;\n\n        P = R * P + t;\n\n        Eigen::Vector2d p;\n        spaceToPlane(P, p);\n\n        imagePoints.push_back(cv::Point2f(p(0), p(1)));\n    }\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/camera_models/CameraFactory.cc",
    "content": "#include \"camodocal/camera_models/CameraFactory.h\"\n\n#include <boost/algorithm/string.hpp>\n\n\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include \"ceres/ceres.h\"\n\nnamespace camodocal\n{\n\nboost::shared_ptr<CameraFactory> CameraFactory::m_instance;\n\nCameraFactory::CameraFactory()\n{\n\n}\n\nboost::shared_ptr<CameraFactory>\nCameraFactory::instance(void)\n{\n    if (m_instance.get() == 0)\n    {\n        m_instance.reset(new CameraFactory);\n    }\n\n    return m_instance;\n}\n\nCameraPtr\nCameraFactory::generateCamera(Camera::ModelType modelType,\n                              const std::string& cameraName,\n                              cv::Size imageSize) const\n{\n    switch (modelType)\n    {\n    case Camera::KANNALA_BRANDT:\n    {\n        EquidistantCameraPtr camera(new EquidistantCamera);\n\n        EquidistantCamera::Parameters params = camera->getParameters();\n        params.cameraName() = cameraName;\n        params.imageWidth() = imageSize.width;\n        params.imageHeight() = imageSize.height;\n        camera->setParameters(params);\n        return camera;\n    }\n    case Camera::PINHOLE:\n    {\n        PinholeCameraPtr camera(new PinholeCamera);\n\n        PinholeCamera::Parameters params = camera->getParameters();\n        params.cameraName() = cameraName;\n        params.imageWidth() = imageSize.width;\n        params.imageHeight() = imageSize.height;\n        camera->setParameters(params);\n        return camera;\n    }\n    case Camera::SCARAMUZZA:\n    {\n        OCAMCameraPtr camera(new OCAMCamera);\n\n        OCAMCamera::Parameters params = camera->getParameters();\n        params.cameraName() = cameraName;\n        params.imageWidth() = imageSize.width;\n        params.imageHeight() = imageSize.height;\n        camera->setParameters(params);\n        return camera;\n    }\n    case Camera::MEI:\n    default:\n    {\n        CataCameraPtr camera(new CataCamera);\n\n        CataCamera::Parameters params = camera->getParameters();\n        params.cameraName() = cameraName;\n        params.imageWidth() = imageSize.width;\n        params.imageHeight() = imageSize.height;\n        camera->setParameters(params);\n        return camera;\n    }\n    }\n}\n\nCameraPtr\nCameraFactory::generateCameraFromYamlFile(const std::string& filename)\n{\n    cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n    if (!fs.isOpened())\n    {\n        return CameraPtr();\n    }\n\n    Camera::ModelType modelType = Camera::MEI;\n    if (!fs[\"model_type\"].isNone())\n    {\n        std::string sModelType;\n        fs[\"model_type\"] >> sModelType;\n\n        if (boost::iequals(sModelType, \"kannala_brandt\"))\n        {\n            modelType = Camera::KANNALA_BRANDT;\n        }\n        else if (boost::iequals(sModelType, \"mei\"))\n        {\n            modelType = Camera::MEI;\n        }\n        else if (boost::iequals(sModelType, \"scaramuzza\"))\n        {\n            modelType = Camera::SCARAMUZZA;\n        }\n        else if (boost::iequals(sModelType, \"pinhole\"))\n        {\n            modelType = Camera::PINHOLE;\n        }\n        else\n        {\n            std::cerr << \"# ERROR: Unknown camera model: \" << sModelType << std::endl;\n            return CameraPtr();\n        }\n    }\n\n    switch (modelType)\n    {\n    case Camera::KANNALA_BRANDT:\n    {\n        EquidistantCameraPtr camera(new EquidistantCamera);\n\n        EquidistantCamera::Parameters params = camera->getParameters();\n        params.readFromYamlFile(filename);\n        camera->setParameters(params);\n        return camera;\n    }\n    case Camera::PINHOLE:\n    {\n        PinholeCameraPtr camera(new PinholeCamera);\n\n        PinholeCamera::Parameters params = camera->getParameters();\n        params.readFromYamlFile(filename);\n        camera->setParameters(params);\n        return camera;\n    }\n    case Camera::SCARAMUZZA:\n    {\n        OCAMCameraPtr camera(new OCAMCamera);\n\n        OCAMCamera::Parameters params = camera->getParameters();\n        params.readFromYamlFile(filename);\n        camera->setParameters(params);\n        return camera;\n    }\n    case Camera::MEI:\n    default:\n    {\n        CataCameraPtr camera(new CataCamera);\n\n        CataCamera::Parameters params = camera->getParameters();\n        params.readFromYamlFile(filename);\n        camera->setParameters(params);\n        return camera;\n    }\n    }\n\n    return CameraPtr();\n}\n\n}\n\n"
  },
  {
    "path": "camera_model/src/camera_models/CataCamera.cc",
    "content": "#include \"camodocal/camera_models/CataCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#include <iomanip>\n#include <iostream>\n#include <opencv2/calib3d/calib3d.hpp>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nCataCamera::Parameters::Parameters()\n : Camera::Parameters(MEI)\n , m_xi(0.0)\n , m_k1(0.0)\n , m_k2(0.0)\n , m_p1(0.0)\n , m_p2(0.0)\n , m_gamma1(0.0)\n , m_gamma2(0.0)\n , m_u0(0.0)\n , m_v0(0.0)\n{\n\n}\n\nCataCamera::Parameters::Parameters(const std::string& cameraName,\n                                   int w, int h,\n                                   double xi,\n                                   double k1, double k2,\n                                   double p1, double p2,\n                                   double gamma1, double gamma2,\n                                   double u0, double v0)\n : Camera::Parameters(MEI, cameraName, w, h)\n , m_xi(xi)\n , m_k1(k1)\n , m_k2(k2)\n , m_p1(p1)\n , m_p2(p2)\n , m_gamma1(gamma1)\n , m_gamma2(gamma2)\n , m_u0(u0)\n , m_v0(v0)\n{\n}\n\ndouble&\nCataCamera::Parameters::xi(void)\n{\n    return m_xi;\n}\n\ndouble&\nCataCamera::Parameters::k1(void)\n{\n    return m_k1;\n}\n\ndouble&\nCataCamera::Parameters::k2(void)\n{\n    return m_k2;\n}\n\ndouble&\nCataCamera::Parameters::p1(void)\n{\n    return m_p1;\n}\n\ndouble&\nCataCamera::Parameters::p2(void)\n{\n    return m_p2;\n}\n\ndouble&\nCataCamera::Parameters::gamma1(void)\n{\n    return m_gamma1;\n}\n\ndouble&\nCataCamera::Parameters::gamma2(void)\n{\n    return m_gamma2;\n}\n\ndouble&\nCataCamera::Parameters::u0(void)\n{\n    return m_u0;\n}\n\ndouble&\nCataCamera::Parameters::v0(void)\n{\n    return m_v0;\n}\n\ndouble\nCataCamera::Parameters::xi(void) const\n{\n    return m_xi;\n}\n\ndouble\nCataCamera::Parameters::k1(void) const\n{\n    return m_k1;\n}\n\ndouble\nCataCamera::Parameters::k2(void) const\n{\n    return m_k2;\n}\n\ndouble\nCataCamera::Parameters::p1(void) const\n{\n    return m_p1;\n}\n\ndouble\nCataCamera::Parameters::p2(void) const\n{\n    return m_p2;\n}\n\ndouble\nCataCamera::Parameters::gamma1(void) const\n{\n    return m_gamma1;\n}\n\ndouble\nCataCamera::Parameters::gamma2(void) const\n{\n    return m_gamma2;\n}\n\ndouble\nCataCamera::Parameters::u0(void) const\n{\n    return m_u0;\n}\n\ndouble\nCataCamera::Parameters::v0(void) const\n{\n    return m_v0;\n}\n\nbool\nCataCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n    cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n    if (!fs.isOpened())\n    {\n        return false;\n    }\n\n    if (!fs[\"model_type\"].isNone())\n    {\n        std::string sModelType;\n        fs[\"model_type\"] >> sModelType;\n\n        if (sModelType.compare(\"MEI\") != 0)\n        {\n            return false;\n        }\n    }\n\n    m_modelType = MEI;\n    fs[\"camera_name\"] >> m_cameraName;\n    m_imageWidth = static_cast<int>(fs[\"image_width\"]);\n    m_imageHeight = static_cast<int>(fs[\"image_height\"]);\n\n    cv::FileNode n = fs[\"mirror_parameters\"];\n    m_xi = static_cast<double>(n[\"xi\"]);\n\n    n = fs[\"distortion_parameters\"];\n    m_k1 = static_cast<double>(n[\"k1\"]);\n    m_k2 = static_cast<double>(n[\"k2\"]);\n    m_p1 = static_cast<double>(n[\"p1\"]);\n    m_p2 = static_cast<double>(n[\"p2\"]);\n\n    n = fs[\"projection_parameters\"];\n    m_gamma1 = static_cast<double>(n[\"gamma1\"]);\n    m_gamma2 = static_cast<double>(n[\"gamma2\"]);\n    m_u0 = static_cast<double>(n[\"u0\"]);\n    m_v0 = static_cast<double>(n[\"v0\"]);\n\n    return true;\n}\n\nvoid\nCataCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n    cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n    fs << \"model_type\" << \"MEI\";\n    fs << \"camera_name\" << m_cameraName;\n    fs << \"image_width\" << m_imageWidth;\n    fs << \"image_height\" << m_imageHeight;\n\n    // mirror: xi\n    fs << \"mirror_parameters\";\n    fs << \"{\" << \"xi\" << m_xi << \"}\";\n\n    // radial distortion: k1, k2\n    // tangential distortion: p1, p2\n    fs << \"distortion_parameters\";\n    fs << \"{\" << \"k1\" << m_k1\n              << \"k2\" << m_k2\n              << \"p1\" << m_p1\n              << \"p2\" << m_p2 << \"}\";\n\n    // projection: gamma1, gamma2, u0, v0\n    fs << \"projection_parameters\";\n    fs << \"{\" << \"gamma1\" << m_gamma1\n              << \"gamma2\" << m_gamma2\n              << \"u0\" << m_u0\n              << \"v0\" << m_v0 << \"}\";\n\n    fs.release();\n}\n\nCataCamera::Parameters&\nCataCamera::Parameters::operator=(const CataCamera::Parameters& other)\n{\n    if (this != &other)\n    {\n        m_modelType = other.m_modelType;\n        m_cameraName = other.m_cameraName;\n        m_imageWidth = other.m_imageWidth;\n        m_imageHeight = other.m_imageHeight;\n        m_xi = other.m_xi;\n        m_k1 = other.m_k1;\n        m_k2 = other.m_k2;\n        m_p1 = other.m_p1;\n        m_p2 = other.m_p2;\n        m_gamma1 = other.m_gamma1;\n        m_gamma2 = other.m_gamma2;\n        m_u0 = other.m_u0;\n        m_v0 = other.m_v0;\n    }\n\n    return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const CataCamera::Parameters& params)\n{\n    out << \"Camera Parameters:\" << std::endl;\n    out << \"    model_type \" << \"MEI\" << std::endl;\n    out << \"   camera_name \" << params.m_cameraName << std::endl;\n    out << \"   image_width \" << params.m_imageWidth << std::endl;\n    out << \"  image_height \" << params.m_imageHeight << std::endl;\n\n    out << \"Mirror Parameters\" << std::endl;\n    out << std::fixed << std::setprecision(10);\n    out << \"            xi \" << params.m_xi << std::endl;\n\n    // radial distortion: k1, k2\n    // tangential distortion: p1, p2\n    out << \"Distortion Parameters\" << std::endl;\n    out << \"            k1 \" << params.m_k1 << std::endl\n        << \"            k2 \" << params.m_k2 << std::endl\n        << \"            p1 \" << params.m_p1 << std::endl\n        << \"            p2 \" << params.m_p2 << std::endl;\n\n    // projection: gamma1, gamma2, u0, v0\n    out << \"Projection Parameters\" << std::endl;\n    out << \"        gamma1 \" << params.m_gamma1 << std::endl\n        << \"        gamma2 \" << params.m_gamma2 << std::endl\n        << \"            u0 \" << params.m_u0 << std::endl\n        << \"            v0 \" << params.m_v0 << std::endl;\n\n    return out;\n}\n\nCataCamera::CataCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n , m_noDistortion(true)\n{\n\n}\n\nCataCamera::CataCamera(const std::string& cameraName,\n                       int imageWidth, int imageHeight,\n                       double xi, double k1, double k2, double p1, double p2,\n                       double gamma1, double gamma2, double u0, double v0)\n : mParameters(cameraName, imageWidth, imageHeight,\n               xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0)\n{\n    if ((mParameters.k1() == 0.0) &&\n        (mParameters.k2() == 0.0) &&\n        (mParameters.p1() == 0.0) &&\n        (mParameters.p2() == 0.0))\n    {\n        m_noDistortion = true;\n    }\n    else\n    {\n        m_noDistortion = false;\n    }\n\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.gamma1();\n    m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n    m_inv_K22 = 1.0 / mParameters.gamma2();\n    m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nCataCamera::CataCamera(const CataCamera::Parameters& params)\n : mParameters(params)\n{\n    if ((mParameters.k1() == 0.0) &&\n        (mParameters.k2() == 0.0) &&\n        (mParameters.p1() == 0.0) &&\n        (mParameters.p2() == 0.0))\n    {\n        m_noDistortion = true;\n    }\n    else\n    {\n        m_noDistortion = false;\n    }\n\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.gamma1();\n    m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n    m_inv_K22 = 1.0 / mParameters.gamma2();\n    m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nCamera::ModelType\nCataCamera::modelType(void) const\n{\n    return mParameters.modelType();\n}\n\nconst std::string&\nCataCamera::cameraName(void) const\n{\n    return mParameters.cameraName();\n}\n\nint\nCataCamera::imageWidth(void) const\n{\n    return mParameters.imageWidth();\n}\n\nint\nCataCamera::imageHeight(void) const\n{\n    return mParameters.imageHeight();\n}\n\nvoid\nCataCamera::estimateIntrinsics(const cv::Size& boardSize,\n                               const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                               const std::vector< std::vector<cv::Point2f> >& imagePoints)\n{\n    Parameters params = getParameters();\n\n    double u0 = params.imageWidth() / 2.0;\n    double v0 = params.imageHeight() / 2.0;\n\n    double gamma0 = 0.0;\n    double minReprojErr = std::numeric_limits<double>::max();\n\n    std::vector<cv::Mat> rvecs, tvecs;\n    rvecs.assign(objectPoints.size(), cv::Mat());\n    tvecs.assign(objectPoints.size(), cv::Mat());\n\n    params.xi() = 1.0;\n    params.k1() = 0.0;\n    params.k2() = 0.0;\n    params.p1() = 0.0;\n    params.p2() = 0.0;\n    params.u0() = u0;\n    params.v0() = v0;\n\n    // Initialize gamma (focal length)\n    // Use non-radial line image and xi = 1\n    for (size_t i = 0; i < imagePoints.size(); ++i)\n    {\n        for (int r = 0; r < boardSize.height; ++r)\n        {\n            cv::Mat P(boardSize.width, 4, CV_64F);\n            for (int c = 0; c < boardSize.width; ++c)\n            {\n                const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c);\n\n                double u = imagePoint.x - u0;\n                double v = imagePoint.y - v0;\n\n                P.at<double>(c, 0) = u;\n                P.at<double>(c, 1) = v;\n                P.at<double>(c, 2) = 0.5;\n                P.at<double>(c, 3) = -0.5 * (square(u) + square(v));\n            }\n\n            cv::Mat C;\n            cv::SVD::solveZ(P, C);\n\n            double t = square(C.at<double>(0)) + square(C.at<double>(1)) + C.at<double>(2) * C.at<double>(3);\n            if (t < 0.0)\n            {\n                continue;\n            }\n\n            // check that line image is not radial\n            double d = sqrt(1.0 / t);\n            double nx = C.at<double>(0) * d;\n            double ny = C.at<double>(1) * d;\n            if (hypot(nx, ny) > 0.95)\n            {\n                continue;\n            }\n\n            double gamma = sqrt(C.at<double>(2) / C.at<double>(3));\n\n            params.gamma1() = gamma;\n            params.gamma2() = gamma;\n            setParameters(params);\n\n            for (size_t j = 0; j < objectPoints.size(); ++j)\n            {\n                estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j));\n            }\n\n            double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());\n\n            if (reprojErr < minReprojErr)\n            {\n                minReprojErr = reprojErr;\n                gamma0 = gamma;\n            }\n        }\n    }\n\n    if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max())\n    {\n        std::cout << \"[\" << params.cameraName() << \"] \"\n                  << \"# INFO: CataCamera model fails with given data. \" << std::endl;\n\n        return;\n    }\n\n    params.gamma1() = gamma0;\n    params.gamma2() = gamma0;\n    setParameters(params);\n}\n\n/** \n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nCataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n    double lambda;\n\n    // Lift points to normalised plane\n    mx_d = m_inv_K11 * p(0) + m_inv_K13;\n    my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n    if (m_noDistortion)\n    {\n        mx_u = mx_d;\n        my_u = my_d;\n    }\n    else\n    {\n        // Apply inverse distortion model\n        if (0)\n        {\n            double k1 = mParameters.k1();\n            double k2 = mParameters.k2();\n            double p1 = mParameters.p1();\n            double p2 = mParameters.p2();\n\n            // Inverse distortion model\n            // proposed by Heikkila\n            mx2_d = mx_d*mx_d;\n            my2_d = my_d*my_d;\n            mxy_d = mx_d*my_d;\n            rho2_d = mx2_d+my2_d;\n            rho4_d = rho2_d*rho2_d;\n            radDist_d = k1*rho2_d+k2*rho4_d;\n            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n            mx_u = mx_d - inv_denom_d*Dx_d;\n            my_u = my_d - inv_denom_d*Dy_d;\n        }\n        else\n        {\n            // Recursive distortion model\n            int n = 6;\n            Eigen::Vector2d d_u;\n            distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n            // Approximate value\n            mx_u = mx_d - d_u(0);\n            my_u = my_d - d_u(1);\n\n            for (int i = 1; i < n; ++i)\n            {\n                distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n                mx_u = mx_d - d_u(0);\n                my_u = my_d - d_u(1);\n            }\n        }\n    }\n\n    // Lift normalised points to the sphere (inv_hslash)\n    double xi = mParameters.xi();\n    if (xi == 1.0)\n    {\n        lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0);\n        P << lambda * mx_u, lambda * my_u, lambda - 1.0;\n    }\n    else\n    {\n        lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) / (1.0 + mx_u * mx_u + my_u * my_u);\n        P << lambda * mx_u, lambda * my_u, lambda - xi;\n    }\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nCataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n    //double lambda;\n\n    // Lift points to normalised plane\n    mx_d = m_inv_K11 * p(0) + m_inv_K13;\n    my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n    if (m_noDistortion)\n    {\n        mx_u = mx_d;\n        my_u = my_d;\n    }\n    else\n    {\n        if (0)\n        {\n            double k1 = mParameters.k1();\n            double k2 = mParameters.k2();\n            double p1 = mParameters.p1();\n            double p2 = mParameters.p2();\n\n            // Apply inverse distortion model\n            // proposed by Heikkila\n            mx2_d = mx_d*mx_d;\n            my2_d = my_d*my_d;\n            mxy_d = mx_d*my_d;\n            rho2_d = mx2_d+my2_d;\n            rho4_d = rho2_d*rho2_d;\n            radDist_d = k1*rho2_d+k2*rho4_d;\n            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n            mx_u = mx_d - inv_denom_d*Dx_d;\n            my_u = my_d - inv_denom_d*Dy_d;\n        }\n        else\n        {\n            // Recursive distortion model\n            int n = 8;\n            Eigen::Vector2d d_u;\n            distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n            // Approximate value\n            mx_u = mx_d - d_u(0);\n            my_u = my_d - d_u(1);\n\n            for (int i = 1; i < n; ++i)\n            {\n                distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n                mx_u = mx_d - d_u(0);\n                my_u = my_d - d_u(1);\n            }\n        }\n    }\n\n    // Obtain a projective ray\n    double xi = mParameters.xi();\n    if (xi == 1.0)\n    {\n        P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0;\n    }\n    else\n    {\n        // Reuse variable\n        rho2_d = mx_u * mx_u + my_u * my_u;\n        P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d));\n    }\n}\n\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nCataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n    Eigen::Vector2d p_u, p_d;\n\n    // Project points to the normalised plane\n    double z = P(2) + mParameters.xi() * P.norm();\n    p_u << P(0) / z, P(1) / z;\n\n    if (m_noDistortion)\n    {\n        p_d = p_u;\n    }\n    else\n    {\n        // Apply distortion\n        Eigen::Vector2d d_u;\n        distortion(p_u, d_u);\n        p_d = p_u + d_u;\n    }\n\n    // Apply generalised projection matrix\n    p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n         mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\n#if 0\n/** \n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nCataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n                        Eigen::Matrix<double,2,3>& J) const\n{\n    double xi = mParameters.xi();\n\n    Eigen::Vector2d p_u, p_d;\n    double norm, inv_denom;\n    double dxdmx, dydmx, dxdmy, dydmy;\n\n    norm = P.norm();\n    // Project points to the normalised plane\n    inv_denom = 1.0 / (P(2) + xi * norm);\n    p_u << inv_denom * P(0), inv_denom * P(1);\n\n    // Calculate jacobian\n    inv_denom = inv_denom * inv_denom / norm;\n    double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2)));\n    double dvdx = -inv_denom * xi * P(0) * P(1);\n    double dudy = dvdx;\n    double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2)));\n    inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable\n    double dudz = P(0) * inv_denom;\n    double dvdz = P(1) * inv_denom;\n\n    if (m_noDistortion)\n    {\n        p_d = p_u;\n    }\n    else\n    {\n        // Apply distortion\n        Eigen::Vector2d d_u;\n        distortion(p_u, d_u);\n        p_d = p_u + d_u;\n    }\n\n    double gamma1 = mParameters.gamma1();\n    double gamma2 = mParameters.gamma2();\n\n    // Make the product of the jacobians\n    // and add projection matrix jacobian\n    inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse\n    dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy);\n    dudx = inv_denom;\n\n    inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse\n    dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy);\n    dudy = inv_denom;\n\n    inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse\n    dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy);\n    dudz = inv_denom;\n    \n    // Apply generalised projection matrix\n    p << gamma1 * p_d(0) + mParameters.u0(),\n         gamma2 * p_d(1) + mParameters.v0();\n\n    J << dudx, dudy, dudz,\n         dvdx, dvdy, dvdz;\n}\n#endif\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nCataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n    Eigen::Vector2d p_d;\n\n    if (m_noDistortion)\n    {\n        p_d = p_u;\n    }\n    else\n    {\n        // Apply distortion\n        Eigen::Vector2d d_u;\n        distortion(p_u, d_u);\n        p_d = p_u + d_u;\n    }\n\n    // Apply generalised projection matrix\n    p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n         mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\n/** \n * \\brief Apply distortion to input point (from the normalised plane)\n *  \n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nCataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n{\n    double k1 = mParameters.k1();\n    double k2 = mParameters.k2();\n    double p1 = mParameters.p1();\n    double p2 = mParameters.p2();\n\n    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n    mx2_u = p_u(0) * p_u(0);\n    my2_u = p_u(1) * p_u(1);\n    mxy_u = p_u(0) * p_u(1);\n    rho2_u = mx2_u + my2_u;\n    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n}\n\n/** \n * \\brief Apply distortion to input point (from the normalised plane)\n *        and calculate Jacobian\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nCataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n                       Eigen::Matrix2d& J) const\n{\n    double k1 = mParameters.k1();\n    double k2 = mParameters.k2();\n    double p1 = mParameters.p1();\n    double p2 = mParameters.p2();\n\n    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n    mx2_u = p_u(0) * p_u(0);\n    my2_u = p_u(1) * p_u(1);\n    mxy_u = p_u(0) * p_u(1);\n    rho2_u = mx2_u + my2_u;\n    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n    double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0);\n    double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1);\n    double dxdmy = dydmx;\n    double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0);\n\n    J << dxdmx, dxdmy,\n         dydmx, dydmy;\n}\n\nvoid\nCataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n    cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n            double xi = mParameters.xi();\n            double d2 = mx_u * mx_u + my_u * my_u;\n\n            Eigen::Vector3d P;\n            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));\n\n            Eigen::Vector2d p;\n            spaceToPlane(P, p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nCataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n                                    float fx, float fy,\n                                    cv::Size imageSize,\n                                    float cx, float cy,\n                                    cv::Mat rmat) const\n{\n    if (imageSize == cv::Size(0, 0))\n    {\n        imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n    }\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n    Eigen::Matrix3f K_rect;\n\n    if (cx == -1.0f && cy == -1.0f)\n    {\n        K_rect << fx, 0, imageSize.width / 2,\n                  0, fy, imageSize.height / 2,\n                  0, 0, 1;\n    }\n    else\n    {\n        K_rect << fx, 0, cx,\n                  0, fy, cy,\n                  0, 0, 1;\n    }\n\n    if (fx == -1.0f || fy == -1.0f)\n    {\n        K_rect(0,0) = mParameters.gamma1();\n        K_rect(1,1) = mParameters.gamma2();\n    }\n\n    Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n    Eigen::Matrix3f R, R_inv;\n    cv::cv2eigen(rmat, R);\n    R_inv = R.inverse();\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            Eigen::Vector3f xo;\n            xo << u, v, 1;\n\n            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n            Eigen::Vector2d p;\n            spaceToPlane(uo.cast<double>(), p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n    cv::Mat K_rect_cv;\n    cv::eigen2cv(K_rect, K_rect_cv);\n    return K_rect_cv;\n}\n\nint\nCataCamera::parameterCount(void) const\n{\n    return 9;\n}\n\nconst CataCamera::Parameters&\nCataCamera::getParameters(void) const\n{\n    return mParameters;\n}\n\nvoid\nCataCamera::setParameters(const CataCamera::Parameters& parameters)\n{\n    mParameters = parameters;\n\n    if ((mParameters.k1() == 0.0) &&\n        (mParameters.k2() == 0.0) &&\n        (mParameters.p1() == 0.0) &&\n        (mParameters.p2() == 0.0))\n    {\n        m_noDistortion = true;\n    }\n    else\n    {\n        m_noDistortion = false;\n    }\n\n    m_inv_K11 = 1.0 / mParameters.gamma1();\n    m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n    m_inv_K22 = 1.0 / mParameters.gamma2();\n    m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n}\n\nvoid\nCataCamera::readParameters(const std::vector<double>& parameterVec)\n{\n    if ((int)parameterVec.size() != parameterCount())\n    {\n        return;\n    }\n\n    Parameters params = getParameters();\n\n    params.xi() = parameterVec.at(0);\n    params.k1() = parameterVec.at(1);\n    params.k2() = parameterVec.at(2);\n    params.p1() = parameterVec.at(3);\n    params.p2() = parameterVec.at(4);\n    params.gamma1() = parameterVec.at(5);\n    params.gamma2() = parameterVec.at(6);\n    params.u0() = parameterVec.at(7);\n    params.v0() = parameterVec.at(8);\n\n    setParameters(params);\n}\n\nvoid\nCataCamera::writeParameters(std::vector<double>& parameterVec) const\n{\n    parameterVec.resize(parameterCount());\n    parameterVec.at(0) = mParameters.xi();\n    parameterVec.at(1) = mParameters.k1();\n    parameterVec.at(2) = mParameters.k2();\n    parameterVec.at(3) = mParameters.p1();\n    parameterVec.at(4) = mParameters.p2();\n    parameterVec.at(5) = mParameters.gamma1();\n    parameterVec.at(6) = mParameters.gamma2();\n    parameterVec.at(7) = mParameters.u0();\n    parameterVec.at(8) = mParameters.v0();\n}\n\nvoid\nCataCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n    mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nCataCamera::parametersToString(void) const\n{\n    std::ostringstream oss;\n    oss << mParameters;\n\n    return oss.str();\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/camera_models/CostFunctionFactory.cc",
    "content": "#include \"camodocal/camera_models/CostFunctionFactory.h\"\n\n#include \"ceres/ceres.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\nnamespace camodocal\n{\n\ntemplate<typename T>\nvoid\nworldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo,\n                       const T* const p_odo, const T* const att_odo,\n                       T* q, T* t, bool optimize_cam_odo_z = true)\n{\n    Eigen::Quaternion<T> q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2)));\n    Eigen::Quaternion<T> q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0));\n    Eigen::Quaternion<T> q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0));\n\n    Eigen::Quaternion<T> q_zyx_inv = q_x_inv * q_y_inv * q_z_inv;\n\n    T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()};\n\n    T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]};\n\n    T q0[4];\n    ceres::QuaternionProduct(q_odo_cam, q_odo, q0);\n\n    T t0[3];\n    T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]};\n\n    ceres::QuaternionRotatePoint(q_odo, t_odo, t0);\n\n    t0[0] += t_cam_odo[0];\n    t0[1] += t_cam_odo[1];\n\n    if (optimize_cam_odo_z)\n    {\n        t0[2] += t_cam_odo[2];\n    }\n\n    ceres::QuaternionRotatePoint(q_odo_cam, t0, t);\n    t[0] = -t[0];\n    t[1] = -t[1];\n    t[2] = -t[2];\n\n    // Convert quaternion from Ceres convention (w, x, y, z)\n    // to Eigen convention (x, y, z, w)\n    q[0] = q0[1];\n    q[1] = q0[2];\n    q[2] = q0[3];\n    q[3] = q0[0];\n}\n\ntemplate<class CameraT>\nclass ReprojectionError1\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    ReprojectionError1(const Eigen::Vector3d& observed_P,\n                       const Eigen::Vector2d& observed_p)\n        : m_observed_P(observed_P), m_observed_p(observed_p)\n        , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {}\n\n    ReprojectionError1(const Eigen::Vector3d& observed_P,\n                       const Eigen::Vector2d& observed_p,\n                       const Eigen::Matrix2d& sqrtPrecisionMat)\n        : m_observed_P(observed_P), m_observed_p(observed_p)\n        , m_sqrtPrecisionMat(sqrtPrecisionMat) {}\n\n    ReprojectionError1(const std::vector<double>& intrinsic_params,\n                       const Eigen::Vector3d& observed_P,\n                       const Eigen::Vector2d& observed_p)\n        : m_intrinsic_params(intrinsic_params)\n        , m_observed_P(observed_P), m_observed_p(observed_p) {}\n\n    // variables: camera intrinsics and camera extrinsics\n    template <typename T>\n    bool operator()(const T* const intrinsic_params,\n                    const T* const q,\n                    const T* const t,\n                    T* residuals) const\n    {\n        Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();\n\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n        Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();\n\n        Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;\n\n        residuals[0] = e_weighted(0);\n        residuals[1] = e_weighted(1);\n\n        return true;\n    }\n\n    // variables: camera-odometry transforms and odometry poses\n    template <typename T>\n    bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n                    const T* const p_odo, const T* const att_odo,\n                    T* residuals) const\n    {\n        T q[4], t[3];\n        worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t);\n\n        Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();\n\n        std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n\n        // project 3D object point to the image plane\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n        residuals[0] = predicted_p(0) - T(m_observed_p(0));\n        residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n        return true;\n    }\n\n//private:\n    // camera intrinsics\n    std::vector<double> m_intrinsic_params;\n\n    // observed 3D point\n    Eigen::Vector3d m_observed_P;\n\n    // observed 2D point\n    Eigen::Vector2d m_observed_p;\n\n    // square root of precision matrix\n    Eigen::Matrix2d m_sqrtPrecisionMat;\n};\n\n// variables: camera extrinsics, 3D point\ntemplate<class CameraT>\nclass ReprojectionError2\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    ReprojectionError2(const std::vector<double>& intrinsic_params,\n                       const Eigen::Vector2d& observed_p)\n        : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {}\n\n    template <typename T>\n    bool operator()(const T* const q, const T* const t,\n                    const T* const point, T* residuals) const\n    {\n        Eigen::Matrix<T, 3, 1> P;\n        P(0) = T(point[0]);\n        P(1) = T(point[1]);\n        P(2) = T(point[2]);\n\n        std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n\n        // project 3D object point to the image plane\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n        residuals[0] = predicted_p(0) - T(m_observed_p(0));\n        residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n        return true;\n    }\n\nprivate:\n    // camera intrinsics\n    std::vector<double> m_intrinsic_params;\n\n    // observed 2D point\n    Eigen::Vector2d m_observed_p;\n};\n\ntemplate<class CameraT>\nclass ReprojectionError3\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    ReprojectionError3(const Eigen::Vector2d& observed_p)\n        : m_observed_p(observed_p)\n        , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())\n        , m_optimize_cam_odo_z(true) {}\n\n    ReprojectionError3(const Eigen::Vector2d& observed_p,\n                       const Eigen::Matrix2d& sqrtPrecisionMat)\n        : m_observed_p(observed_p)\n        , m_sqrtPrecisionMat(sqrtPrecisionMat)\n        , m_optimize_cam_odo_z(true) {}\n\n    ReprojectionError3(const std::vector<double>& intrinsic_params,\n                       const Eigen::Vector2d& observed_p)\n        : m_intrinsic_params(intrinsic_params)\n        , m_observed_p(observed_p)\n        , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity())\n        , m_optimize_cam_odo_z(true) {}\n\n    ReprojectionError3(const std::vector<double>& intrinsic_params,\n                       const Eigen::Vector2d& observed_p,\n                       const Eigen::Matrix2d& sqrtPrecisionMat)\n        : m_intrinsic_params(intrinsic_params)\n        , m_observed_p(observed_p)\n        , m_sqrtPrecisionMat(sqrtPrecisionMat)\n        , m_optimize_cam_odo_z(true) {}\n\n\n    ReprojectionError3(const std::vector<double>& intrinsic_params,\n                       const Eigen::Vector3d& odo_pos,\n                       const Eigen::Vector3d& odo_att,\n                       const Eigen::Vector2d& observed_p,\n                       bool optimize_cam_odo_z)\n        : m_intrinsic_params(intrinsic_params)\n        , m_odo_pos(odo_pos), m_odo_att(odo_att)\n        , m_observed_p(observed_p)\n        , m_optimize_cam_odo_z(optimize_cam_odo_z) {}\n\n    ReprojectionError3(const std::vector<double>& intrinsic_params,\n                       const Eigen::Quaterniond& cam_odo_q,\n                       const Eigen::Vector3d& cam_odo_t,\n                       const Eigen::Vector3d& odo_pos,\n                       const Eigen::Vector3d& odo_att,\n                       const Eigen::Vector2d& observed_p)\n        : m_intrinsic_params(intrinsic_params)\n        , m_cam_odo_q(cam_odo_q), m_cam_odo_t(cam_odo_t)\n        , m_odo_pos(odo_pos), m_odo_att(odo_att)\n        , m_observed_p(observed_p)\n        , m_optimize_cam_odo_z(true) {}\n\n    // variables: camera intrinsics, camera-to-odometry transform,\n    //            odometry extrinsics, 3D point\n    template <typename T>\n    bool operator()(const T* const intrinsic_params,\n                    const T* const q_cam_odo, const T* const t_cam_odo,\n                    const T* const p_odo, const T* const att_odo,\n                    const T* const point, T* residuals) const\n    {\n        T q[4], t[3];\n        worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n        Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);\n\n        // project 3D object point to the image plane\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n        Eigen::Matrix<T, 2, 1> err = predicted_p - m_observed_p.cast<T>();\n        Eigen::Matrix<T, 2, 1> err_weighted = m_sqrtPrecisionMat.cast<T>() * err;\n\n        residuals[0] = err_weighted(0);\n        residuals[1] = err_weighted(1);\n\n        return true;\n    }\n\n    // variables: camera-to-odometry transform, 3D point\n    template <typename T>\n    bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n                    const T* const point, T* residuals) const\n    {\n        T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};\n        T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};\n        T q[4], t[3];\n\n        worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n        std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n        Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);\n\n        // project 3D object point to the image plane\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n        residuals[0] = predicted_p(0) - T(m_observed_p(0));\n        residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n        return true;\n    }\n\n    // variables: camera-to-odometry transform, odometry extrinsics, 3D point\n    template <typename T>\n    bool operator()(const T* const q_cam_odo, const T* const t_cam_odo,\n                    const T* const p_odo, const T* const att_odo,\n                    const T* const point, T* residuals) const\n    {\n        T q[4], t[3];\n        worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n        std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n        Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);\n\n        // project 3D object point to the image plane\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n        Eigen::Matrix<T, 2, 1> err = predicted_p - m_observed_p.cast<T>();\n        Eigen::Matrix<T, 2, 1> err_weighted = m_sqrtPrecisionMat.cast<T>() * err;\n\n        residuals[0] = err_weighted(0);\n        residuals[1] = err_weighted(1);\n\n        return true;\n    }\n\n    // variables: 3D point\n    template <typename T>\n    bool operator()(const T* const point, T* residuals) const\n    {\n        T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), T(m_cam_odo_q.coeffs()(2)), T(m_cam_odo_q.coeffs()(3))};\n        T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))};\n        T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};\n        T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};\n        T q[4], t[3];\n\n        worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);\n\n        std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());\n        Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);\n\n        // project 3D object point to the image plane\n        Eigen::Matrix<T, 2, 1> predicted_p;\n        CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);\n\n        residuals[0] = predicted_p(0) - T(m_observed_p(0));\n        residuals[1] = predicted_p(1) - T(m_observed_p(1));\n\n        return true;\n    }\n\nprivate:\n    // camera intrinsics\n    std::vector<double> m_intrinsic_params;\n\n    // observed camera-odometry transform\n    Eigen::Quaterniond m_cam_odo_q;\n    Eigen::Vector3d m_cam_odo_t;\n\n    // observed odometry\n    Eigen::Vector3d m_odo_pos;\n    Eigen::Vector3d m_odo_att;\n\n    // observed 2D point\n    Eigen::Vector2d m_observed_p;\n\n    Eigen::Matrix2d m_sqrtPrecisionMat;\n\n    bool m_optimize_cam_odo_z;\n};\n\n// variables: camera intrinsics and camera extrinsics\ntemplate<class CameraT>\nclass StereoReprojectionError\n{\npublic:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    StereoReprojectionError(const Eigen::Vector3d& observed_P,\n                            const Eigen::Vector2d& observed_p_l,\n                            const Eigen::Vector2d& observed_p_r)\n        : m_observed_P(observed_P)\n        , m_observed_p_l(observed_p_l)\n        , m_observed_p_r(observed_p_r)\n    {\n\n    }\n\n    template <typename T>\n    bool operator()(const T* const intrinsic_params_l,\n                    const T* const intrinsic_params_r,\n                    const T* const q_l,\n                    const T* const t_l,\n                    const T* const q_l_r,\n                    const T* const t_l_r,\n                    T* residuals) const\n    {\n        Eigen::Matrix<T, 3, 1> P;\n        P(0) = T(m_observed_P(0));\n        P(1) = T(m_observed_P(1));\n        P(2) = T(m_observed_P(2));\n\n        Eigen::Matrix<T, 2, 1> predicted_p_l;\n        CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l);\n\n        Eigen::Quaternion<T> q_r = Eigen::Quaternion<T>(q_l_r) * Eigen::Quaternion<T>(q_l);\n\n        Eigen::Matrix<T, 3, 1> t_r;\n        t_r(0) = t_l[0];\n        t_r(1) = t_l[1];\n        t_r(2) = t_l[2];\n\n        t_r = Eigen::Quaternion<T>(q_l_r) * t_r;\n        t_r(0) += t_l_r[0];\n        t_r(1) += t_l_r[1];\n        t_r(2) += t_l_r[2];\n\n        Eigen::Matrix<T, 2, 1> predicted_p_r;\n        CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r);\n\n        residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0));\n        residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1));\n        residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0));\n        residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1));\n\n        return true;\n    }\n\nprivate:\n    // observed 3D point\n    Eigen::Vector3d m_observed_P;\n\n    // observed 2D point\n    Eigen::Vector2d m_observed_p_l;\n    Eigen::Vector2d m_observed_p_r;\n};\n\ntemplate <class CameraT>\nclass ComprehensionError {\n  public:\n    EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p)\n        : m_observed_P(observed_P),\n          m_observed_p(observed_p),\n          m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {\n    }\n\n    template <typename T>\n    bool operator()(const T* const intrinsic_params, const T* const q, const T* const t,\n                    T* residuals) const {\n        {\n            Eigen::Matrix<T, 2, 1> p = m_observed_p.cast<T>();\n        \n            Eigen::Matrix<T, 3, 1> predicted_img_P;\n            CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P);\n                \n            Eigen::Matrix<T, 2, 1> predicted_p;\n            CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p);\n\n            Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();\n\n            Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;\n\n            residuals[0] = e_weighted(0);\n            residuals[1] = e_weighted(1);\n        }\n\n        {\n            Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();\n\n            Eigen::Matrix<T, 2, 1> predicted_p;\n            CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);\n\n            Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();\n\n            Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;\n\n            residuals[2] = e_weighted(0);\n            residuals[3] = e_weighted(1);\n        }\n\n        return true;\n    }\n\n    // private:\n    // camera intrinsics\n    // std::vector<double> m_intrinsic_params;\n\n    // observed 3D point\n    Eigen::Vector3d m_observed_P;\n\n    // observed 2D point\n    Eigen::Vector2d m_observed_p;\n\n    // square root of precision matrix\n    Eigen::Matrix2d m_sqrtPrecisionMat;\n};\n\nboost::shared_ptr<CostFunctionFactory> CostFunctionFactory::m_instance;\n\nCostFunctionFactory::CostFunctionFactory()\n{\n\n}\n\nboost::shared_ptr<CostFunctionFactory>\nCostFunctionFactory::instance(void)\n{\n    if (m_instance.get() == 0)\n    {\n        m_instance.reset(new CostFunctionFactory);\n    }\n\n    return m_instance;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n        const Eigen::Vector3d& observed_P,\n        const Eigen::Vector2d& observed_p,\n        int flags) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    std::vector<double> intrinsic_params;\n    camera->writeParameters(intrinsic_params);\n\n    switch (flags)\n    {\n    case CAMERA_INTRINSICS | CAMERA_POSE:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 8, 4, 3>(\n                new ReprojectionError1<EquidistantCamera>(observed_P, observed_p));\n            break;\n        case Camera::PINHOLE:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 8, 4, 3>(\n                new ReprojectionError1<PinholeCamera>(observed_P, observed_p));\n            break;\n        case Camera::MEI:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 9, 4, 3>(\n                new ReprojectionError1<CataCamera>(observed_P, observed_p));\n            break;\n        case Camera::SCARAMUZZA:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ComprehensionError<OCAMCamera>, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n                new ComprehensionError<OCAMCamera>(observed_P, observed_p));\n                // new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n                // new ReprojectionError1<OCAMCamera>(observed_P, observed_p));\n            break;\n        }\n        break;\n    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 4, 3, 3, 3>(\n                new ReprojectionError1<EquidistantCamera>(intrinsic_params, observed_P, observed_p));\n            break;\n        case Camera::PINHOLE:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 4, 3, 3, 3>(\n                new ReprojectionError1<PinholeCamera>(intrinsic_params, observed_P, observed_p));\n            break;\n        case Camera::MEI:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 4, 3, 3, 3>(\n                new ReprojectionError1<CataCamera>(intrinsic_params, observed_P, observed_p));\n            break;\n        case Camera::SCARAMUZZA:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, 4, 3, 3, 3>(\n                new ReprojectionError1<OCAMCamera>(intrinsic_params, observed_P, observed_p));\n            break;\n        }\n        break;\n    }\n\n    return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n        const Eigen::Vector3d& observed_P,\n        const Eigen::Vector2d& observed_p,\n        const Eigen::Matrix2d& sqrtPrecisionMat,\n        int flags) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    std::vector<double> intrinsic_params;\n    camera->writeParameters(intrinsic_params);\n\n    switch (flags)\n    {\n    case CAMERA_INTRINSICS | CAMERA_POSE:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 8, 4, 3>(\n                new ReprojectionError1<EquidistantCamera>(observed_P, observed_p, sqrtPrecisionMat));\n            break;\n        case Camera::PINHOLE:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 8, 4, 3>(\n                new ReprojectionError1<PinholeCamera>(observed_P, observed_p, sqrtPrecisionMat));\n            break;\n        case Camera::MEI:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 9, 4, 3>(\n                new ReprojectionError1<CataCamera>(observed_P, observed_p, sqrtPrecisionMat));\n            break;\n        case Camera::SCARAMUZZA:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(\n                new ReprojectionError1<OCAMCamera>(observed_P, observed_p, sqrtPrecisionMat));\n            break;\n        }\n        break;\n    }\n\n    return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n        const Eigen::Vector2d& observed_p,\n        int flags, bool optimize_cam_odo_z) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    std::vector<double> intrinsic_params;\n    camera->writeParameters(intrinsic_params);\n\n    switch (flags)\n    {\n    case CAMERA_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError2<EquidistantCamera>, 2, 4, 3, 3>(\n                new ReprojectionError2<EquidistantCamera>(intrinsic_params, observed_p));\n            break;\n        case Camera::PINHOLE:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError2<PinholeCamera>, 2, 4, 3, 3>(\n                new ReprojectionError2<PinholeCamera>(intrinsic_params, observed_p));\n            break;\n        case Camera::MEI:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError2<CataCamera>, 2, 4, 3, 3>(\n                new ReprojectionError2<CataCamera>(intrinsic_params, observed_p));\n            break;\n        case Camera::SCARAMUZZA:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError2<OCAMCamera>, 2, 4, 3, 3>(\n                new ReprojectionError2<OCAMCamera>(intrinsic_params, observed_p));\n            break;\n        }\n        break;\n    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        }\n        break;\n    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));\n            }\n            break;\n        }\n        break;\n    case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<EquidistantCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<CataCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<CataCamera>(observed_p));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3>(\n                    new ReprojectionError3<OCAMCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3>(\n                    new ReprojectionError3<OCAMCamera>(observed_p));\n            }\n            break;\n        }\n        break;\n    case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<EquidistantCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(observed_p));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(observed_p));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(observed_p));\n            }\n            break;\n        }\n        break;\n    }\n\n    return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n        const Eigen::Vector2d& observed_p,\n        const Eigen::Matrix2d& sqrtPrecisionMat,\n        int flags, bool optimize_cam_odo_z) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    std::vector<double> intrinsic_params;\n    camera->writeParameters(intrinsic_params);\n\n    switch (flags)\n    {\n    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));\n            }\n            break;\n        }\n        break;\n    case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<EquidistantCamera>(observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<CataCamera>(observed_p, sqrtPrecisionMat));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(observed_p, sqrtPrecisionMat));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(observed_p, sqrtPrecisionMat));\n            }\n            break;\n        }\n        break;\n    }\n\n    return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n        const Eigen::Vector3d& odo_pos,\n        const Eigen::Vector3d& odo_att,\n        const Eigen::Vector2d& observed_p,\n        int flags, bool optimize_cam_odo_z) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    std::vector<double> intrinsic_params;\n    camera->writeParameters(intrinsic_params);\n\n    switch (flags)\n    {\n    case CAMERA_ODOMETRY_TRANSFORM | POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3>(\n                    new ReprojectionError3<EquidistantCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 2, 3>(\n                    new ReprojectionError3<EquidistantCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            break;\n        case Camera::PINHOLE:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3>(\n                    new ReprojectionError3<PinholeCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            break;\n        case Camera::MEI:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3>(\n                    new ReprojectionError3<CataCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            break;\n        case Camera::SCARAMUZZA:\n            if (optimize_cam_odo_z)\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            else\n            {\n                costFunction =\n                    new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3>(\n                    new ReprojectionError3<OCAMCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));\n            }\n            break;\n        }\n        break;\n    }\n\n    return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,\n        const Eigen::Quaterniond& cam_odo_q,\n        const Eigen::Vector3d& cam_odo_t,\n        const Eigen::Vector3d& odo_pos,\n        const Eigen::Vector3d& odo_att,\n        const Eigen::Vector2d& observed_p,\n        int flags) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    std::vector<double> intrinsic_params;\n    camera->writeParameters(intrinsic_params);\n\n    switch (flags)\n    {\n    case POINT_3D:\n        switch (camera->modelType())\n        {\n        case Camera::KANNALA_BRANDT:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 3>(\n                new ReprojectionError3<EquidistantCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n            break;\n        case Camera::PINHOLE:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 3>(\n                new ReprojectionError3<PinholeCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n            break;\n        case Camera::MEI:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 3>(\n                new ReprojectionError3<CataCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n            break;\n        case Camera::SCARAMUZZA:\n            costFunction =\n                new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 3>(\n                new ReprojectionError3<OCAMCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));\n            break;\n        }\n        break;\n    }\n\n    return costFunction;\n}\n\nceres::CostFunction*\nCostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL,\n        const CameraConstPtr& cameraR,\n        const Eigen::Vector3d& observed_P,\n        const Eigen::Vector2d& observed_p_l,\n        const Eigen::Vector2d& observed_p_r) const\n{\n    ceres::CostFunction* costFunction = 0;\n\n    if (cameraL->modelType() != cameraR->modelType())\n    {\n        return costFunction;\n    }\n\n    switch (cameraL->modelType())\n    {\n    case Camera::KANNALA_BRANDT:\n        costFunction =\n            new ceres::AutoDiffCostFunction<StereoReprojectionError<EquidistantCamera>, 4, 8, 8, 4, 3, 4, 3>(\n            new StereoReprojectionError<EquidistantCamera>(observed_P, observed_p_l, observed_p_r));\n        break;\n    case Camera::PINHOLE:\n        costFunction =\n            new ceres::AutoDiffCostFunction<StereoReprojectionError<PinholeCamera>, 4, 8, 8, 4, 3, 4, 3>(\n            new StereoReprojectionError<PinholeCamera>(observed_P, observed_p_l, observed_p_r));\n        break;\n    case Camera::MEI:\n        costFunction =\n            new ceres::AutoDiffCostFunction<StereoReprojectionError<CataCamera>, 4, 9, 9, 4, 3, 4, 3>(\n            new StereoReprojectionError<CataCamera>(observed_P, observed_p_l, observed_p_r));\n        break;\n    case Camera::SCARAMUZZA:\n        costFunction =\n            new ceres::AutoDiffCostFunction<StereoReprojectionError<OCAMCamera>, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>(\n            new StereoReprojectionError<OCAMCamera>(observed_P, observed_p_l, observed_p_r));\n        break;\n    }\n\n    return costFunction;\n}\n\n}\n\n"
  },
  {
    "path": "camera_model/src/camera_models/EquidistantCamera.cc",
    "content": "#include \"camodocal/camera_models/EquidistantCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#include <iomanip>\n#include <iostream>\n#include <opencv2/calib3d/calib3d.hpp>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nEquidistantCamera::Parameters::Parameters()\n : Camera::Parameters(KANNALA_BRANDT)\n , m_k2(0.0)\n , m_k3(0.0)\n , m_k4(0.0)\n , m_k5(0.0)\n , m_mu(0.0)\n , m_mv(0.0)\n , m_u0(0.0)\n , m_v0(0.0)\n{\n\n}\n\nEquidistantCamera::Parameters::Parameters(const std::string& cameraName,\n                                          int w, int h,\n                                          double k2, double k3, double k4, double k5,\n                                          double mu, double mv,\n                                          double u0, double v0)\n : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h)\n , m_k2(k2)\n , m_k3(k3)\n , m_k4(k4)\n , m_k5(k5)\n , m_mu(mu)\n , m_mv(mv)\n , m_u0(u0)\n , m_v0(v0)\n{\n\n}\n\ndouble&\nEquidistantCamera::Parameters::k2(void)\n{\n    return m_k2;\n}\n\ndouble&\nEquidistantCamera::Parameters::k3(void)\n{\n    return m_k3;\n}\n\ndouble&\nEquidistantCamera::Parameters::k4(void)\n{\n    return m_k4;\n}\n\ndouble&\nEquidistantCamera::Parameters::k5(void)\n{\n    return m_k5;\n}\n\ndouble&\nEquidistantCamera::Parameters::mu(void)\n{\n    return m_mu;\n}\n\ndouble&\nEquidistantCamera::Parameters::mv(void)\n{\n    return m_mv;\n}\n\ndouble&\nEquidistantCamera::Parameters::u0(void)\n{\n    return m_u0;\n}\n\ndouble&\nEquidistantCamera::Parameters::v0(void)\n{\n    return m_v0;\n}\n\ndouble\nEquidistantCamera::Parameters::k2(void) const\n{\n    return m_k2;\n}\n\ndouble\nEquidistantCamera::Parameters::k3(void) const\n{\n    return m_k3;\n}\n\ndouble\nEquidistantCamera::Parameters::k4(void) const\n{\n    return m_k4;\n}\n\ndouble\nEquidistantCamera::Parameters::k5(void) const\n{\n    return m_k5;\n}\n\ndouble\nEquidistantCamera::Parameters::mu(void) const\n{\n    return m_mu;\n}\n\ndouble\nEquidistantCamera::Parameters::mv(void) const\n{\n    return m_mv;\n}\n\ndouble\nEquidistantCamera::Parameters::u0(void) const\n{\n    return m_u0;\n}\n\ndouble\nEquidistantCamera::Parameters::v0(void) const\n{\n    return m_v0;\n}\n\nbool\nEquidistantCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n    cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n    if (!fs.isOpened())\n    {\n        return false;\n    }\n\n    if (!fs[\"model_type\"].isNone())\n    {\n        std::string sModelType;\n        fs[\"model_type\"] >> sModelType;\n\n        if (sModelType.compare(\"KANNALA_BRANDT\") != 0)\n        {\n            return false;\n        }\n    }\n\n    m_modelType = KANNALA_BRANDT;\n    fs[\"camera_name\"] >> m_cameraName;\n    m_imageWidth = static_cast<int>(fs[\"image_width\"]);\n    m_imageHeight = static_cast<int>(fs[\"image_height\"]);\n\n    cv::FileNode n = fs[\"projection_parameters\"];\n    m_k2 = static_cast<double>(n[\"k2\"]);\n    m_k3 = static_cast<double>(n[\"k3\"]);\n    m_k4 = static_cast<double>(n[\"k4\"]);\n    m_k5 = static_cast<double>(n[\"k5\"]);\n    m_mu = static_cast<double>(n[\"mu\"]);\n    m_mv = static_cast<double>(n[\"mv\"]);\n    m_u0 = static_cast<double>(n[\"u0\"]);\n    m_v0 = static_cast<double>(n[\"v0\"]);\n\n    return true;\n}\n\nvoid\nEquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n    cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n    fs << \"model_type\" << \"KANNALA_BRANDT\";\n    fs << \"camera_name\" << m_cameraName;\n    fs << \"image_width\" << m_imageWidth;\n    fs << \"image_height\" << m_imageHeight;\n\n    // projection: k2, k3, k4, k5, mu, mv, u0, v0\n    fs << \"projection_parameters\";\n    fs << \"{\" << \"k2\" << m_k2\n              << \"k3\" << m_k3\n              << \"k4\" << m_k4\n              << \"k5\" << m_k5\n              << \"mu\" << m_mu\n              << \"mv\" << m_mv\n              << \"u0\" << m_u0\n              << \"v0\" << m_v0 << \"}\";\n\n    fs.release();\n}\n\nEquidistantCamera::Parameters&\nEquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other)\n{\n    if (this != &other)\n    {\n        m_modelType = other.m_modelType;\n        m_cameraName = other.m_cameraName;\n        m_imageWidth = other.m_imageWidth;\n        m_imageHeight = other.m_imageHeight;\n        m_k2 = other.m_k2;\n        m_k3 = other.m_k3;\n        m_k4 = other.m_k4;\n        m_k5 = other.m_k5;\n        m_mu = other.m_mu;\n        m_mv = other.m_mv;\n        m_u0 = other.m_u0;\n        m_v0 = other.m_v0;\n    }\n\n    return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const EquidistantCamera::Parameters& params)\n{\n    out << \"Camera Parameters:\" << std::endl;\n    out << \"    model_type \" << \"KANNALA_BRANDT\" << std::endl;\n    out << \"   camera_name \" << params.m_cameraName << std::endl;\n    out << \"   image_width \" << params.m_imageWidth << std::endl;\n    out << \"  image_height \" << params.m_imageHeight << std::endl;\n\n    // projection: k2, k3, k4, k5, mu, mv, u0, v0\n    out << \"Projection Parameters\" << std::endl;\n    out << \"            k2 \" << params.m_k2 << std::endl\n        << \"            k3 \" << params.m_k3 << std::endl\n        << \"            k4 \" << params.m_k4 << std::endl\n        << \"            k5 \" << params.m_k5 << std::endl\n        << \"            mu \" << params.m_mu << std::endl\n        << \"            mv \" << params.m_mv << std::endl\n        << \"            u0 \" << params.m_u0 << std::endl\n        << \"            v0 \" << params.m_v0 << std::endl;\n\n    return out;\n}\n\nEquidistantCamera::EquidistantCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n{\n\n}\n\nEquidistantCamera::EquidistantCamera(const std::string& cameraName,\n                                     int imageWidth, int imageHeight,\n                                     double k2, double k3, double k4, double k5,\n                                     double mu, double mv,\n                                     double u0, double v0)\n : mParameters(cameraName, imageWidth, imageHeight,\n               k2, k3, k4, k5, mu, mv, u0, v0)\n{\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.mu();\n    m_inv_K13 = -mParameters.u0() / mParameters.mu();\n    m_inv_K22 = 1.0 / mParameters.mv();\n    m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nEquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params)\n : mParameters(params)\n{\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.mu();\n    m_inv_K13 = -mParameters.u0() / mParameters.mu();\n    m_inv_K22 = 1.0 / mParameters.mv();\n    m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nCamera::ModelType\nEquidistantCamera::modelType(void) const\n{\n    return mParameters.modelType();\n}\n\nconst std::string&\nEquidistantCamera::cameraName(void) const\n{\n    return mParameters.cameraName();\n}\n\nint\nEquidistantCamera::imageWidth(void) const\n{\n    return mParameters.imageWidth();\n}\n\nint\nEquidistantCamera::imageHeight(void) const\n{\n    return mParameters.imageHeight();\n}\n\nvoid\nEquidistantCamera::estimateIntrinsics(const cv::Size& boardSize,\n                                      const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                                      const std::vector< std::vector<cv::Point2f> >& imagePoints)\n{\n    Parameters params = getParameters();\n\n    double u0 = params.imageWidth() / 2.0;\n    double v0 = params.imageHeight() / 2.0;\n\n    double minReprojErr = std::numeric_limits<double>::max();\n\n    std::vector<cv::Mat> rvecs, tvecs;\n    rvecs.assign(objectPoints.size(), cv::Mat());\n    tvecs.assign(objectPoints.size(), cv::Mat());\n\n    params.k2() = 0.0;\n    params.k3() = 0.0;\n    params.k4() = 0.0;\n    params.k5() = 0.0;\n    params.u0() = u0;\n    params.v0() = v0;\n\n    // Initialize focal length\n    // C. Hughes, P. Denny, M. Glavin, and E. Jones,\n    // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point\n    // Extraction, PAMI 2010\n    // Find circles from rows of chessboard corners, and for each pair\n    // of circles, find vanishing points: v1 and v2.\n    // f = ||v1 - v2|| / PI;\n    double f0 = 0.0;\n    for (size_t i = 0; i < imagePoints.size(); ++i)\n    {\n        std::vector<Eigen::Vector2d> center(boardSize.height);\n        double radius[boardSize.height];\n        for (int r = 0; r < boardSize.height; ++r)\n        {\n            std::vector<cv::Point2d> circle;\n            for (int c = 0; c < boardSize.width; ++c)\n            {\n                circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));\n            }\n\n            fitCircle(circle, center[r](0), center[r](1), radius[r]);\n        }\n\n        for (int j = 0; j < boardSize.height; ++j)\n        {\n            for (int k = j + 1; k < boardSize.height; ++k)\n            {\n                // find distance between pair of vanishing points which\n                // correspond to intersection points of 2 circles\n                std::vector<cv::Point2d> ipts;\n                ipts = intersectCircles(center[j](0), center[j](1), radius[j],\n                                        center[k](0), center[k](1), radius[k]);\n\n                if (ipts.size() < 2)\n                {\n                    continue;\n                }\n\n                double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;\n\n                params.mu() = f;\n                params.mv() = f;\n\n                setParameters(params);\n\n                for (size_t l = 0; l < objectPoints.size(); ++l)\n                {\n                    estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l));\n                }\n\n                double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray());\n\n                if (reprojErr < minReprojErr)\n                {\n                    minReprojErr = reprojErr;\n                    f0 = f;\n                }\n            }\n        }\n    }\n\n    if (f0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max())\n    {\n        std::cout << \"[\" << params.cameraName() << \"] \"\n                  << \"# INFO: kannala-Brandt model fails with given data. \" << std::endl;\n\n        return;\n    }\n\n    params.mu() = f0;\n    params.mv() = f0;\n\n    setParameters(params);\n}\n\n/**\n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nEquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    liftProjective(p, P);\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nEquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    // Lift points to normalised plane\n    Eigen::Vector2d p_u;\n    p_u << m_inv_K11 * p(0) + m_inv_K13,\n           m_inv_K22 * p(1) + m_inv_K23;\n\n    // Obtain a projective ray\n    double theta, phi;\n    backprojectSymmetric(p_u, theta, phi);\n\n    P(0) = sin(theta) * cos(phi);\n    P(1) = sin(theta) * sin(phi);\n    P(2) = cos(theta);\n}\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nEquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n    double theta = acos(P(2) / P.norm());\n    double phi = atan2(P(1), P(0));\n\n    Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));\n\n    // Apply generalised projection matrix\n    p << mParameters.mu() * p_u(0) + mParameters.u0(),\n         mParameters.mv() * p_u(1) + mParameters.v0();\n}\n\n\n/** \n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nEquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n                                Eigen::Matrix<double,2,3>& J) const\n{\n    double theta = acos(P(2) / P.norm());\n    double phi = atan2(P(1), P(0));\n\n    Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));\n\n    // Apply generalised projection matrix\n    p << mParameters.mu() * p_u(0) + mParameters.u0(),\n         mParameters.mv() * p_u(1) + mParameters.v0();\n}\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nEquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n//    Eigen::Vector2d p_d;\n//\n//    if (m_noDistortion)\n//    {\n//        p_d = p_u;\n//    }\n//    else\n//    {\n//        // Apply distortion\n//        Eigen::Vector2d d_u;\n//        distortion(p_u, d_u);\n//        p_d = p_u + d_u;\n//    }\n//\n//    // Apply generalised projection matrix\n//    p << mParameters.gamma1() * p_d(0) + mParameters.u0(),\n//         mParameters.gamma2() * p_d(1) + mParameters.v0();\n}\n\nvoid\nEquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n    cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n            double theta, phi;\n            backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi);\n\n            Eigen::Vector3d P;\n            P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta);\n\n            Eigen::Vector2d p;\n            spaceToPlane(P, p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nEquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n                                           float fx, float fy,\n                                           cv::Size imageSize,\n                                           float cx, float cy,\n                                           cv::Mat rmat) const\n{\n    if (imageSize == cv::Size(0, 0))\n    {\n        imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n    }\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n    Eigen::Matrix3f K_rect;\n\n    if (cx == -1.0f && cy == -1.0f)\n    {\n        K_rect << fx, 0, imageSize.width / 2,\n                  0, fy, imageSize.height / 2,\n                  0, 0, 1;\n    }\n    else\n    {\n        K_rect << fx, 0, cx,\n                  0, fy, cy,\n                  0, 0, 1;\n    }\n\n    if (fx == -1.0f || fy == -1.0f)\n    {\n        K_rect(0,0) = mParameters.mu();\n        K_rect(1,1) = mParameters.mv();\n    }\n\n    Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n    Eigen::Matrix3f R, R_inv;\n    cv::cv2eigen(rmat, R);\n    R_inv = R.inverse();\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            Eigen::Vector3f xo;\n            xo << u, v, 1;\n\n            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n            Eigen::Vector2d p;\n            spaceToPlane(uo.cast<double>(), p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n    cv::Mat K_rect_cv;\n    cv::eigen2cv(K_rect, K_rect_cv);\n    return K_rect_cv;\n}\n\nint\nEquidistantCamera::parameterCount(void) const\n{\n    return 8;\n}\n\nconst EquidistantCamera::Parameters&\nEquidistantCamera::getParameters(void) const\n{\n    return mParameters;\n}\n\nvoid\nEquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters)\n{\n    mParameters = parameters;\n\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.mu();\n    m_inv_K13 = -mParameters.u0() / mParameters.mu();\n    m_inv_K22 = 1.0 / mParameters.mv();\n    m_inv_K23 = -mParameters.v0() / mParameters.mv();\n}\n\nvoid\nEquidistantCamera::readParameters(const std::vector<double>& parameterVec)\n{\n    if (parameterVec.size() != parameterCount())\n    {\n        return;\n    }\n\n    Parameters params = getParameters();\n\n    params.k2() = parameterVec.at(0);\n    params.k3() = parameterVec.at(1);\n    params.k4() = parameterVec.at(2);\n    params.k5() = parameterVec.at(3);\n    params.mu() = parameterVec.at(4);\n    params.mv() = parameterVec.at(5);\n    params.u0() = parameterVec.at(6);\n    params.v0() = parameterVec.at(7);\n\n    setParameters(params);\n}\n\nvoid\nEquidistantCamera::writeParameters(std::vector<double>& parameterVec) const\n{\n    parameterVec.resize(parameterCount());\n    parameterVec.at(0) = mParameters.k2();\n    parameterVec.at(1) = mParameters.k3();\n    parameterVec.at(2) = mParameters.k4();\n    parameterVec.at(3) = mParameters.k5();\n    parameterVec.at(4) = mParameters.mu();\n    parameterVec.at(5) = mParameters.mv();\n    parameterVec.at(6) = mParameters.u0();\n    parameterVec.at(7) = mParameters.v0();\n}\n\nvoid\nEquidistantCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n    mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nEquidistantCamera::parametersToString(void) const\n{\n    std::ostringstream oss;\n    oss << mParameters;\n\n    return oss.str();\n}\n\nvoid\nEquidistantCamera::fitOddPoly(const std::vector<double>& x, const std::vector<double>& y,\n                              int n, std::vector<double>& coeffs) const\n{\n    std::vector<int> pows;\n    for (int i = 1; i <= n; i += 2)\n    {\n        pows.push_back(i);\n    }\n\n    Eigen::MatrixXd X(x.size(), pows.size());\n    Eigen::MatrixXd Y(y.size(), 1);\n    for (size_t i = 0; i < x.size(); ++i)\n    {\n        for (size_t j = 0; j < pows.size(); ++j)\n        {\n            X(i,j) = pow(x.at(i), pows.at(j));\n        }\n        Y(i,0) = y.at(i);\n    }\n\n    Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y;\n\n    coeffs.resize(A.rows());\n    for (int i = 0; i < A.rows(); ++i)\n    {\n        coeffs.at(i) = A(i,0);\n    }\n}\n\nvoid\nEquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u,\n                                        double& theta, double& phi) const\n{\n    double tol = 1e-10;\n    double p_u_norm = p_u.norm();\n\n    if (p_u_norm < 1e-10)\n    {\n        phi = 0.0;\n    }\n    else\n    {\n        phi = atan2(p_u(1), p_u(0));\n    }\n\n    int npow = 9;\n    if (mParameters.k5() == 0.0)\n    {\n        npow -= 2;\n    }\n    if (mParameters.k4() == 0.0)\n    {\n        npow -= 2;\n    }\n    if (mParameters.k3() == 0.0)\n    {\n        npow -= 2;\n    }\n    if (mParameters.k2() == 0.0)\n    {\n        npow -= 2;\n    }\n\n    Eigen::MatrixXd coeffs(npow + 1, 1);\n    coeffs.setZero();\n    coeffs(0) = -p_u_norm;\n    coeffs(1) = 1.0;\n\n    if (npow >= 3)\n    {\n        coeffs(3) = mParameters.k2();\n    }\n    if (npow >= 5)\n    {\n        coeffs(5) = mParameters.k3();\n    }\n    if (npow >= 7)\n    {\n        coeffs(7) = mParameters.k4();\n    }\n    if (npow >= 9)\n    {\n        coeffs(9) = mParameters.k5();\n    }\n\n    if (npow == 1)\n    {\n        theta = p_u_norm;\n    }\n    else\n    {\n        // Get eigenvalues of companion matrix corresponding to polynomial.\n        // Eigenvalues correspond to roots of polynomial.\n        Eigen::MatrixXd A(npow, npow);\n        A.setZero();\n        A.block(1, 0, npow - 1, npow - 1).setIdentity();\n        A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow);\n\n        Eigen::EigenSolver<Eigen::MatrixXd> es(A);\n        Eigen::MatrixXcd eigval = es.eigenvalues();\n\n        std::vector<double> thetas;\n        for (int i = 0; i < eigval.rows(); ++i)\n        {\n            if (fabs(eigval(i).imag()) > tol)\n            {\n                continue;\n            }\n\n            double t = eigval(i).real();\n\n            if (t < -tol)\n            {\n                continue;\n            }\n            else if (t < 0.0)\n            {\n                t = 0.0;\n            }\n\n            thetas.push_back(t);\n        }\n\n        if (thetas.empty())\n        {\n            theta = p_u_norm;\n        }\n        else\n        {\n            theta = *std::min_element(thetas.begin(), thetas.end());\n        }\n    }\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/camera_models/PinholeCamera.cc",
    "content": "#include \"camodocal/camera_models/PinholeCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#include <iomanip>\n#include <opencv2/calib3d/calib3d.hpp>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n#include <iostream>\nusing namespace std;\n\n#include \"camodocal/gpl/gpl.h\"\n\nnamespace camodocal\n{\n\nPinholeCamera::Parameters::Parameters()\n : Camera::Parameters(PINHOLE)\n , m_k1(0.0)\n , m_k2(0.0)\n , m_p1(0.0)\n , m_p2(0.0)\n , m_fx(0.0)\n , m_fy(0.0)\n , m_cx(0.0)\n , m_cy(0.0)\n{\n\n}\n\nPinholeCamera::Parameters::Parameters(const std::string& cameraName,\n                                      int w, int h,\n                                      double k1, double k2,\n                                      double p1, double p2,\n                                      double fx, double fy,\n                                      double cx, double cy)\n : Camera::Parameters(PINHOLE, cameraName, w, h)\n , m_k1(k1)\n , m_k2(k2)\n , m_p1(p1)\n , m_p2(p2)\n , m_fx(fx)\n , m_fy(fy)\n , m_cx(cx)\n , m_cy(cy)\n{\n}\n\ndouble&\nPinholeCamera::Parameters::k1(void)\n{\n    return m_k1;\n}\n\ndouble&\nPinholeCamera::Parameters::k2(void)\n{\n    return m_k2;\n}\n\ndouble&\nPinholeCamera::Parameters::p1(void)\n{\n    return m_p1;\n}\n\ndouble&\nPinholeCamera::Parameters::p2(void)\n{\n    return m_p2;\n}\n\ndouble&\nPinholeCamera::Parameters::fx(void)\n{\n    return m_fx;\n}\n\ndouble&\nPinholeCamera::Parameters::fy(void)\n{\n    return m_fy;\n}\n\ndouble&\nPinholeCamera::Parameters::cx(void)\n{\n    return m_cx;\n}\n\ndouble&\nPinholeCamera::Parameters::cy(void)\n{\n    return m_cy;\n}\n\ndouble\nPinholeCamera::Parameters::k1(void) const\n{\n    return m_k1;\n}\n\ndouble\nPinholeCamera::Parameters::k2(void) const\n{\n    return m_k2;\n}\n\ndouble\nPinholeCamera::Parameters::p1(void) const\n{\n    return m_p1;\n}\n\ndouble\nPinholeCamera::Parameters::p2(void) const\n{\n    return m_p2;\n}\n\ndouble\nPinholeCamera::Parameters::fx(void) const\n{\n    return m_fx;\n}\n\ndouble\nPinholeCamera::Parameters::fy(void) const\n{\n    return m_fy;\n}\n\ndouble\nPinholeCamera::Parameters::cx(void) const\n{\n    return m_cx;\n}\n\ndouble\nPinholeCamera::Parameters::cy(void) const\n{\n    return m_cy;\n}\n\nbool\nPinholeCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n    cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n    if (!fs.isOpened())\n    {\n        return false;\n    }\n\n    if (!fs[\"model_type\"].isNone())\n    {\n        std::string sModelType;\n        fs[\"model_type\"] >> sModelType;\n\n        if (sModelType.compare(\"PINHOLE\") != 0)\n        {\n            return false;\n        }\n    }\n\n    m_modelType = PINHOLE;\n    fs[\"camera_name\"] >> m_cameraName;\n    m_imageWidth = static_cast<int>(fs[\"image_width\"]);\n    m_imageHeight = static_cast<int>(fs[\"image_height\"]);\n\n    cv::FileNode n = fs[\"distortion_parameters\"];\n    m_k1 = static_cast<double>(n[\"k1\"]);\n    m_k2 = static_cast<double>(n[\"k2\"]);\n    m_p1 = static_cast<double>(n[\"p1\"]);\n    m_p2 = static_cast<double>(n[\"p2\"]);\n\n    n = fs[\"projection_parameters\"];\n    m_fx = static_cast<double>(n[\"fx\"]);\n    m_fy = static_cast<double>(n[\"fy\"]);\n    m_cx = static_cast<double>(n[\"cx\"]);\n    m_cy = static_cast<double>(n[\"cy\"]);\n  \n    cout << \"m_k1:\" << m_k1 << \" m_k2\" << m_k2 << \" m_p1:\"<<m_p1 << \" m_p2:\" << m_p2<< endl;\n    cout << \"m_fx:\" << m_fx << \" m_fy\" << m_fy << \" m_cx:\"<<m_cx << \" m_cy:\" << m_cy<< endl;\n    return true;\n}\n\nvoid\nPinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n    cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n    fs << \"model_type\" << \"PINHOLE\";\n    fs << \"camera_name\" << m_cameraName;\n    fs << \"image_width\" << m_imageWidth;\n    fs << \"image_height\" << m_imageHeight;\n\n    // radial distortion: k1, k2\n    // tangential distortion: p1, p2\n    fs << \"distortion_parameters\";\n    fs << \"{\" << \"k1\" << m_k1\n              << \"k2\" << m_k2\n              << \"p1\" << m_p1\n              << \"p2\" << m_p2 << \"}\";\n\n    // projection: fx, fy, cx, cy\n    fs << \"projection_parameters\";\n    fs << \"{\" << \"fx\" << m_fx\n              << \"fy\" << m_fy\n              << \"cx\" << m_cx\n              << \"cy\" << m_cy << \"}\";\n\n    fs.release();\n}\n\nPinholeCamera::Parameters&\nPinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other)\n{\n    if (this != &other)\n    {\n        m_modelType = other.m_modelType;\n        m_cameraName = other.m_cameraName;\n        m_imageWidth = other.m_imageWidth;\n        m_imageHeight = other.m_imageHeight;\n        m_k1 = other.m_k1;\n        m_k2 = other.m_k2;\n        m_p1 = other.m_p1;\n        m_p2 = other.m_p2;\n        m_fx = other.m_fx;\n        m_fy = other.m_fy;\n        m_cx = other.m_cx;\n        m_cy = other.m_cy;\n    }\n\n    return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const PinholeCamera::Parameters& params)\n{\n    out << \"Camera Parameters:\" << std::endl;\n    out << \"    model_type \" << \"PINHOLE\" << std::endl;\n    out << \"   camera_name \" << params.m_cameraName << std::endl;\n    out << \"   image_width \" << params.m_imageWidth << std::endl;\n    out << \"  image_height \" << params.m_imageHeight << std::endl;\n\n    // radial distortion: k1, k2\n    // tangential distortion: p1, p2\n    out << \"Distortion Parameters\" << std::endl;\n    out << \"            k1 \" << params.m_k1 << std::endl\n        << \"            k2 \" << params.m_k2 << std::endl\n        << \"            p1 \" << params.m_p1 << std::endl\n        << \"            p2 \" << params.m_p2 << std::endl;\n\n    // projection: fx, fy, cx, cy\n    out << \"Projection Parameters\" << std::endl;\n    out << \"            fx \" << params.m_fx << std::endl\n        << \"            fy \" << params.m_fy << std::endl\n        << \"            cx \" << params.m_cx << std::endl\n        << \"            cy \" << params.m_cy << std::endl;\n\n    return out;\n}\n\nPinholeCamera::PinholeCamera()\n : m_inv_K11(1.0)\n , m_inv_K13(0.0)\n , m_inv_K22(1.0)\n , m_inv_K23(0.0)\n , m_noDistortion(true)\n{\n\n}\n\nPinholeCamera::PinholeCamera(const std::string& cameraName,\n                             int imageWidth, int imageHeight,\n                             double k1, double k2, double p1, double p2,\n                             double fx, double fy, double cx, double cy)\n : mParameters(cameraName, imageWidth, imageHeight,\n               k1, k2, p1, p2, fx, fy, cx, cy)\n{\n    if ((mParameters.k1() == 0.0) &&\n        (mParameters.k2() == 0.0) &&\n        (mParameters.p1() == 0.0) &&\n        (mParameters.p2() == 0.0))\n    {\n        m_noDistortion = true;\n    }\n    else\n    {\n        m_noDistortion = false;\n    }\n\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.fx();\n    m_inv_K13 = -mParameters.cx() / mParameters.fx();\n    m_inv_K22 = 1.0 / mParameters.fy();\n    m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nPinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params)\n : mParameters(params)\n{\n    if ((mParameters.k1() == 0.0) &&\n        (mParameters.k2() == 0.0) &&\n        (mParameters.p1() == 0.0) &&\n        (mParameters.p2() == 0.0))\n    {\n        m_noDistortion = true;\n    }\n    else\n    {\n        m_noDistortion = false;\n    }\n\n    // Inverse camera projection matrix parameters\n    m_inv_K11 = 1.0 / mParameters.fx();\n    m_inv_K13 = -mParameters.cx() / mParameters.fx();\n    m_inv_K22 = 1.0 / mParameters.fy();\n    m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nCamera::ModelType\nPinholeCamera::modelType(void) const\n{\n    return mParameters.modelType();\n}\n\nconst std::string&\nPinholeCamera::cameraName(void) const\n{\n    return mParameters.cameraName();\n}\n\nint\nPinholeCamera::imageWidth(void) const\n{\n    return mParameters.imageWidth();\n}\n\nint\nPinholeCamera::imageHeight(void) const\n{\n    return mParameters.imageHeight();\n}\n\nvoid\nPinholeCamera::estimateIntrinsics(const cv::Size& boardSize,\n                                  const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                                  const std::vector< std::vector<cv::Point2f> >& imagePoints)\n{\n    // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000\n\n    Parameters params = getParameters();\n\n    params.k1() = 0.0;\n    params.k2() = 0.0;\n    params.p1() = 0.0;\n    params.p2() = 0.0;\n\n    double cx = params.imageWidth() / 2.0;\n    double cy = params.imageHeight() / 2.0;\n    params.cx() = cx;\n    params.cy() = cy;\n\n    size_t nImages = imagePoints.size();\n\n    cv::Mat A(nImages * 2, 2, CV_64F);\n    cv::Mat b(nImages * 2, 1, CV_64F);\n\n    for (size_t i = 0; i < nImages; ++i)\n    {\n        const std::vector<cv::Point3f>& oPoints = objectPoints.at(i);\n\n        std::vector<cv::Point2f> M(oPoints.size());\n        for (size_t j = 0; j < M.size(); ++j)\n        {\n            M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y);\n        }\n\n        cv::Mat H = cv::findHomography(M, imagePoints.at(i));\n\n        H.at<double>(0,0) -= H.at<double>(2,0) * cx;\n        H.at<double>(0,1) -= H.at<double>(2,1) * cx;\n        H.at<double>(0,2) -= H.at<double>(2,2) * cx;\n        H.at<double>(1,0) -= H.at<double>(2,0) * cy;\n        H.at<double>(1,1) -= H.at<double>(2,1) * cy;\n        H.at<double>(1,2) -= H.at<double>(2,2) * cy;\n\n        double h[3], v[3], d1[3], d2[3];\n        double n[4] = {0,0,0,0};\n\n        for (int j = 0; j < 3; ++j)\n        {\n            double t0 = H.at<double>(j,0);\n            double t1 = H.at<double>(j,1);\n            h[j] = t0; v[j] = t1;\n            d1[j] = (t0 + t1) * 0.5;\n            d2[j] = (t0 - t1) * 0.5;\n            n[0] += t0 * t0; n[1] += t1 * t1;\n            n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];\n        }\n\n        for (int j = 0; j < 4; ++j)\n        {\n            n[j] = 1.0 / sqrt(n[j]);\n        }\n\n        for (int j = 0; j < 3; ++j)\n        {\n            h[j] *= n[0]; v[j] *= n[1];\n            d1[j] *= n[2]; d2[j] *= n[3];\n        }\n\n        A.at<double>(i * 2, 0) = h[0] * v[0];\n        A.at<double>(i * 2, 1) = h[1] * v[1];\n        A.at<double>(i * 2 + 1, 0) = d1[0] * d2[0];\n        A.at<double>(i * 2 + 1, 1) = d1[1] * d2[1];\n        b.at<double>(i * 2, 0) = -h[2] * v[2];\n        b.at<double>(i * 2 + 1, 0) = -d1[2] * d2[2];\n    }\n\n    cv::Mat f(2, 1, CV_64F);\n    cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);\n\n    params.fx() = sqrt(fabs(1.0 / f.at<double>(0)));\n    params.fy() = sqrt(fabs(1.0 / f.at<double>(1)));\n\n    setParameters(params);\n}\n\n/**\n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nPinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    liftProjective(p, P);\n\n    P.normalize();\n}\n\n/**\n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nPinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n    //double lambda;\n\n    // Lift points to normalised plane\n    mx_d = m_inv_K11 * p(0) + m_inv_K13;\n    my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n    if (m_noDistortion)\n    {\n        mx_u = mx_d;\n        my_u = my_d;\n    }\n    else\n    {\n        if (0)\n        {\n            double k1 = mParameters.k1();\n            double k2 = mParameters.k2();\n            double p1 = mParameters.p1();\n            double p2 = mParameters.p2();\n\n            // Apply inverse distortion model\n            // proposed by Heikkila\n            mx2_d = mx_d*mx_d;\n            my2_d = my_d*my_d;\n            mxy_d = mx_d*my_d;\n            rho2_d = mx2_d+my2_d;\n            rho4_d = rho2_d*rho2_d;\n            radDist_d = k1*rho2_d+k2*rho4_d;\n            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n            mx_u = mx_d - inv_denom_d*Dx_d;\n            my_u = my_d - inv_denom_d*Dy_d;\n        }\n        else\n        {\n            // Recursive distortion model\n            int n = 8;\n            Eigen::Vector2d d_u;\n            distortion(Eigen::Vector2d(mx_d, my_d), d_u);\n            // Approximate value\n            mx_u = mx_d - d_u(0);\n            my_u = my_d - d_u(1);\n\n            for (int i = 1; i < n; ++i)\n            {\n                distortion(Eigen::Vector2d(mx_u, my_u), d_u);\n                mx_u = mx_d - d_u(0);\n                my_u = my_d - d_u(1);\n            }\n        }\n    }\n\n    // Obtain a projective ray\n    //cout<<\"lift:mx_u:\"<<mx_u<<\",my_u:\"<<my_u<<endl;\n    P << mx_u, my_u, 1.0;\n}\n\n\n/**\n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nPinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n    Eigen::Vector2d p_u, p_d;\n\n    // Project points to the normalised plane\n    p_u << P(0) / P(2), P(1) / P(2);\n\n    if (m_noDistortion)\n    {\n        p_d = p_u;\n    }\n    else\n    {\n        // Apply distortion\n        Eigen::Vector2d d_u;\n        distortion(p_u, d_u);\n        p_d = p_u + d_u;\n    }\n\n    // Apply generalised projection matrix\n    p << mParameters.fx() * p_d(0) + mParameters.cx(),\n         mParameters.fy() * p_d(1) + mParameters.cy();\n}\n\n#if 0\n/**\n * \\brief Project a 3D point to the image plane and calculate Jacobian\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nPinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,\n                            Eigen::Matrix<double,2,3>& J) const\n{\n    Eigen::Vector2d p_u, p_d;\n    double norm, inv_denom;\n    double dxdmx, dydmx, dxdmy, dydmy;\n\n    norm = P.norm();\n    // Project points to the normalised plane\n    inv_denom = 1.0 / P(2);\n    p_u << inv_denom * P(0), inv_denom * P(1);\n\n    // Calculate jacobian\n    double dudx = inv_denom;\n    double dvdx = 0.0;\n    double dudy = 0.0;\n    double dvdy = inv_denom;\n    inv_denom = - inv_denom * inv_denom;\n    double dudz = P(0) * inv_denom;\n    double dvdz = P(1) * inv_denom;\n\n    if (m_noDistortion)\n    {\n        p_d = p_u;\n    }\n    else\n    {\n        // Apply distortion\n        Eigen::Vector2d d_u;\n        distortion(p_u, d_u);\n        p_d = p_u + d_u;\n    }\n\n    double fx = mParameters.fx();\n    double fy = mParameters.fy();\n\n    // Make the product of the jacobians\n    // and add projection matrix jacobian\n    inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse\n    dvdx = fy * (dudx * dydmx + dvdx * dydmy);\n    dudx = inv_denom;\n\n    inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse\n    dvdy = fy * (dudy * dydmx + dvdy * dydmy);\n    dudy = inv_denom;\n\n    inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse\n    dvdz = fy * (dudz * dydmx + dvdz * dydmy);\n    dudz = inv_denom;\n\n    // Apply generalised projection matrix\n    p << fx * p_d(0) + mParameters.cx(),\n         fy * p_d(1) + mParameters.cy();\n\n    J << dudx, dudy, dudz,\n         dvdx, dvdy, dvdz;\n}\n#endif\n\n/**\n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nPinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n    Eigen::Vector2d p_d;\n\n    if (m_noDistortion)\n    {\n        p_d = p_u;\n    }\n    else\n    {\n        // Apply distortion\n        Eigen::Vector2d d_u;\n        distortion(p_u, d_u);\n        p_d = p_u + d_u;\n    }\n\n    // Apply generalised projection matrix\n    p << mParameters.fx() * p_d(0) + mParameters.cx(),\n         mParameters.fy() * p_d(1) + mParameters.cy();\n}\n\n/**\n * \\brief Apply distortion to input point (from the normalised plane)\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nPinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n{\n    double k1 = mParameters.k1();\n    double k2 = mParameters.k2();\n    double p1 = mParameters.p1();\n    double p2 = mParameters.p2();\n\n    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n    mx2_u = p_u(0) * p_u(0);\n    my2_u = p_u(1) * p_u(1);\n    mxy_u = p_u(0) * p_u(1);\n    rho2_u = mx2_u + my2_u;\n    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n}\n\n/**\n * \\brief Apply distortion to input point (from the normalised plane)\n *        and calculate Jacobian\n *\n * \\param p_u undistorted coordinates of point on the normalised plane\n * \\return to obtain the distorted point: p_d = p_u + d_u\n */\nvoid\nPinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,\n                          Eigen::Matrix2d& J) const\n{\n    double k1 = mParameters.k1();\n    double k2 = mParameters.k2();\n    double p1 = mParameters.p1();\n    double p2 = mParameters.p2();\n\n    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n    mx2_u = p_u(0) * p_u(0);\n    my2_u = p_u(1) * p_u(1);\n    mxy_u = p_u(0) * p_u(1);\n    rho2_u = mx2_u + my2_u;\n    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n    double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0);\n    double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1);\n    double dxdmy = dydmx;\n    double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0);\n\n    J << dxdmx, dxdmy,\n         dydmx, dydmy;\n}\n\nvoid\nPinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n    cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n            Eigen::Vector3d P;\n            P << mx_u, my_u, 1.0;\n\n            Eigen::Vector2d p;\n            spaceToPlane(P, p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n\ncv::Mat\nPinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n                                       float fx, float fy,\n                                       cv::Size imageSize,\n                                       float cx, float cy,\n                                       cv::Mat rmat) const\n{\n    if (imageSize == cv::Size(0, 0))\n    {\n        imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n    }\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n    Eigen::Matrix3f R, R_inv;\n    cv::cv2eigen(rmat, R);\n    R_inv = R.inverse();\n\n    // assume no skew\n    Eigen::Matrix3f K_rect;\n\n    if (cx == -1.0f || cy == -1.0f)\n    {\n        K_rect << fx, 0, imageSize.width / 2,\n                  0, fy, imageSize.height / 2,\n                  0, 0, 1;\n    }\n    else\n    {\n        K_rect << fx, 0, cx,\n                  0, fy, cy,\n                  0, 0, 1;\n    }\n\n    if (fx == -1.0f || fy == -1.0f)\n    {\n        K_rect(0,0) = mParameters.fx();\n        K_rect(1,1) = mParameters.fy();\n    }\n\n    Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            Eigen::Vector3f xo;\n            xo << u, v, 1;\n\n            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n            Eigen::Vector2d p;\n            spaceToPlane(uo.cast<double>(), p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n    cv::Mat K_rect_cv;\n    cv::eigen2cv(K_rect, K_rect_cv);\n    return K_rect_cv;\n}\n\nint\nPinholeCamera::parameterCount(void) const\n{\n    return 8;\n}\n\nconst PinholeCamera::Parameters&\nPinholeCamera::getParameters(void) const\n{\n    return mParameters;\n}\n\nvoid\nPinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)\n{\n    mParameters = parameters;\n\n    if ((mParameters.k1() == 0.0) &&\n        (mParameters.k2() == 0.0) &&\n        (mParameters.p1() == 0.0) &&\n        (mParameters.p2() == 0.0))\n    {\n        m_noDistortion = true;\n    }\n    else\n    {\n        m_noDistortion = false;\n    }\n\n    m_inv_K11 = 1.0 / mParameters.fx();\n    m_inv_K13 = -mParameters.cx() / mParameters.fx();\n    m_inv_K22 = 1.0 / mParameters.fy();\n    m_inv_K23 = -mParameters.cy() / mParameters.fy();\n}\n\nvoid\nPinholeCamera::readParameters(const std::vector<double>& parameterVec)\n{\n    if ((int)parameterVec.size() != parameterCount())\n    {\n        return;\n    }\n\n    Parameters params = getParameters();\n\n    params.k1() = parameterVec.at(0);\n    params.k2() = parameterVec.at(1);\n    params.p1() = parameterVec.at(2);\n    params.p2() = parameterVec.at(3);\n    params.fx() = parameterVec.at(4);\n    params.fy() = parameterVec.at(5);\n    params.cx() = parameterVec.at(6);\n    params.cy() = parameterVec.at(7);\n\n    setParameters(params);\n}\n\nvoid\nPinholeCamera::writeParameters(std::vector<double>& parameterVec) const\n{\n    parameterVec.resize(parameterCount());\n    parameterVec.at(0) = mParameters.k1();\n    parameterVec.at(1) = mParameters.k2();\n    parameterVec.at(2) = mParameters.p1();\n    parameterVec.at(3) = mParameters.p2();\n    parameterVec.at(4) = mParameters.fx();\n    parameterVec.at(5) = mParameters.fy();\n    parameterVec.at(6) = mParameters.cx();\n    parameterVec.at(7) = mParameters.cy();\n}\n\nvoid\nPinholeCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n    mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nPinholeCamera::parametersToString(void) const\n{\n    std::ostringstream oss;\n    oss << mParameters;\n\n    return oss.str();\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/camera_models/ScaramuzzaCamera.cc",
    "content": "#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\n#include <cmath>\n#include <cstdio>\n#include <eigen3/Eigen/Dense>\n#include <eigen3/Eigen/SVD>\n#include <iomanip>\n#include <iostream>\n#include <opencv2/calib3d/calib3d.hpp>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n#include <boost/lexical_cast.hpp>\n#include <boost/algorithm/string.hpp>\n\n#include \"camodocal/gpl/gpl.h\"\n\n\nEigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order) {\n    assert(poly_order > 0);\n    assert(xVec.size() > poly_order);\n    assert(xVec.size() == yVec.size());\n\n    Eigen::MatrixXd A(xVec.size(), poly_order+1);\n    Eigen::VectorXd B(xVec.size());\n\n    for(int i = 0; i < xVec.size(); ++i) {\n        const double x = xVec(i);\n        const double y = yVec(i);\n\n        double x_pow_k = 1.0;\n\n        for(int k=0; k<=poly_order; ++k) {\n            A(i,k) = x_pow_k;\n            x_pow_k *= x;\n        }\n\n        B(i) = y;\n    }\n\n    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);\n    Eigen::VectorXd x = svd.solve(B);\n\n    return x;\n}\n\nnamespace camodocal\n{\n\nOCAMCamera::Parameters::Parameters()\n : Camera::Parameters(SCARAMUZZA)\n , m_C(0.0)\n , m_D(0.0)\n , m_E(0.0)\n , m_center_x(0.0)\n , m_center_y(0.0)\n{\n    memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE);\n    memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);\n}\n\n\n\nbool\nOCAMCamera::Parameters::readFromYamlFile(const std::string& filename)\n{\n    cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n    if (!fs.isOpened())\n    {\n        return false;\n    }\n\n    if (!fs[\"model_type\"].isNone())\n    {\n        std::string sModelType;\n        fs[\"model_type\"] >> sModelType;\n\n        if (!boost::iequals(sModelType, \"scaramuzza\"))\n        {\n            return false;\n        }\n    }\n\n    m_modelType = SCARAMUZZA;\n    fs[\"camera_name\"] >> m_cameraName;\n    m_imageWidth = static_cast<int>(fs[\"image_width\"]);\n    m_imageHeight = static_cast<int>(fs[\"image_height\"]);\n\n    cv::FileNode n = fs[\"poly_parameters\"];\n    for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n        m_poly[i] = static_cast<double>(n[std::string(\"p\") + boost::lexical_cast<std::string>(i)]);\n\n    n = fs[\"inv_poly_parameters\"];\n    for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n        m_inv_poly[i] = static_cast<double>(n[std::string(\"p\") + boost::lexical_cast<std::string>(i)]);\n\n    n = fs[\"affine_parameters\"];\n    m_C = static_cast<double>(n[\"ac\"]);\n    m_D = static_cast<double>(n[\"ad\"]);\n    m_E = static_cast<double>(n[\"ae\"]);\n\n    m_center_x = static_cast<double>(n[\"cx\"]);\n    m_center_y = static_cast<double>(n[\"cy\"]);\n\n    return true;\n}\n\nvoid\nOCAMCamera::Parameters::writeToYamlFile(const std::string& filename) const\n{\n    cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n    fs << \"model_type\" << \"scaramuzza\";\n    fs << \"camera_name\" << m_cameraName;\n    fs << \"image_width\" << m_imageWidth;\n    fs << \"image_height\" << m_imageHeight;\n\n    fs << \"poly_parameters\";\n    fs << \"{\";\n    for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n        fs << std::string(\"p\") + boost::lexical_cast<std::string>(i) << m_poly[i];\n    fs << \"}\";\n\n    fs << \"inv_poly_parameters\";\n    fs << \"{\";\n    for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n        fs << std::string(\"p\") + boost::lexical_cast<std::string>(i) << m_inv_poly[i];\n    fs << \"}\";\n\n    fs << \"affine_parameters\";\n    fs << \"{\" << \"ac\" << m_C\n              << \"ad\" << m_D\n              << \"ae\" << m_E\n              << \"cx\" << m_center_x\n              << \"cy\" << m_center_y << \"}\";\n\n    fs.release();\n}\n\nOCAMCamera::Parameters&\nOCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other)\n{\n    if (this != &other)\n    {\n        m_modelType = other.m_modelType;\n        m_cameraName = other.m_cameraName;\n        m_imageWidth = other.m_imageWidth;\n        m_imageHeight = other.m_imageHeight;\n        m_C = other.m_C;\n        m_D = other.m_D;\n        m_E = other.m_E;\n        m_center_x = other.m_center_x;\n        m_center_y = other.m_center_y;\n\n        memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);\n        memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);\n    }\n\n    return *this;\n}\n\nstd::ostream&\noperator<< (std::ostream& out, const OCAMCamera::Parameters& params)\n{\n    out << \"Camera Parameters:\" << std::endl;\n    out << \"    model_type \" << \"scaramuzza\" << std::endl;\n    out << \"   camera_name \" << params.m_cameraName << std::endl;\n    out << \"   image_width \" << params.m_imageWidth << std::endl;\n    out << \"  image_height \" << params.m_imageHeight << std::endl;\n\n    out << std::fixed << std::setprecision(10);\n\n    out << \"Poly Parameters\" << std::endl;\n    for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n        out << std::string(\"p\") + boost::lexical_cast<std::string>(i) << \": \" << params.m_poly[i] << std::endl;\n\n    out << \"Inverse Poly Parameters\" << std::endl;\n    for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n        out << std::string(\"p\") + boost::lexical_cast<std::string>(i) << \": \" << params.m_inv_poly[i] << std::endl;\n\n    out << \"Affine Parameters\" << std::endl;\n    out << \"            ac \" << params.m_C << std::endl\n        << \"            ad \" << params.m_D << std::endl\n        << \"            ae \" << params.m_E << std::endl;\n    out << \"            cx \" << params.m_center_x << std::endl\n        << \"            cy \" << params.m_center_y << std::endl;\n\n    return out;\n}\n\nOCAMCamera::OCAMCamera()\n : m_inv_scale(0.0)\n{\n\n}\n\nOCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params)\n : mParameters(params)\n{\n    m_inv_scale = 1.0 / (params.C() - params.D() * params.E());\n}\n\nCamera::ModelType\nOCAMCamera::modelType(void) const\n{\n    return mParameters.modelType();\n}\n\nconst std::string&\nOCAMCamera::cameraName(void) const\n{\n    return mParameters.cameraName();\n}\n\nint\nOCAMCamera::imageWidth(void) const\n{\n    return mParameters.imageWidth();\n}\n\nint\nOCAMCamera::imageHeight(void) const\n{\n    return mParameters.imageHeight();\n}\n\nvoid\nOCAMCamera::estimateIntrinsics(const cv::Size& boardSize,\n                               const std::vector< std::vector<cv::Point3f> >& objectPoints,\n                               const std::vector< std::vector<cv::Point2f> >& imagePoints)\n{\n    // std::cout << \"OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED\" << std::endl;\n    // throw std::string(\"OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED\");\n\n    // Reference: Page 30 of\n    // \" Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635.\"\n    // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf\n    // Matlab code: calibrate.m\n\n    // First, estimate every image's extrinsics parameters\n    std::vector< Eigen::Matrix3d > RList;\n    std::vector< Eigen::Vector3d > TList;\n\n    RList.reserve(imagePoints.size());\n    TList.reserve(imagePoints.size());\n\n    // i-th image\n    for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index)\n    {\n        const std::vector<cv::Point3f>& objPts = objectPoints.at(image_index);\n        const std::vector<cv::Point2f>& imgPts = imagePoints.at(image_index);\n\n        assert(objPts.size() == imgPts.size());\n        assert(objPts.size() == static_cast<unsigned int>(boardSize.width * boardSize.height));\n\n        Eigen::MatrixXd M(objPts.size(), 6);\n\n        for(size_t corner_index = 0; corner_index < objPts.size(); ++corner_index) {\n            double X = objPts.at(corner_index).x;\n            double Y = objPts.at(corner_index).y;\n            assert(objPts.at(corner_index).z==0.0);\n            \n            double u = imgPts.at(corner_index).x;\n            double v = imgPts.at(corner_index).y;\n\n            M(corner_index, 0) = -v * X;\n            M(corner_index, 1) = -v * Y;\n            M(corner_index, 2) =  u * X;\n            M(corner_index, 3) =  u * Y;\n            M(corner_index, 4) = -v;\n            M(corner_index, 5) =  u;\n        }\n\n        Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);\n        assert(svd.matrixV().cols() == 6);\n        Eigen::VectorXd h = -svd.matrixV().col(5);\n\n        // scaled version of R and T\n        const double sr11 = h(0);\n        const double sr12 = h(1);\n        const double sr21 = h(2);\n        const double sr22 = h(3);\n        const double st1  = h(4);\n        const double st2  = h(5);\n\n        const double AA = square(sr11*sr12 + sr21*sr22);\n        const double BB = square(sr11) + square(sr21);\n        const double CC = square(sr12) + square(sr22);\n\n        const double sr32_squared_1 = (- (CC-BB) + sqrt(square(CC-BB) + 4.0 * AA)) / 2.0;\n        const double sr32_squared_2 = (- (CC-BB) - sqrt(square(CC-BB) + 4.0 * AA)) / 2.0;\n\n// printf(\"rst = %.12f\\n\", sr32_squared_1*sr32_squared_1 + (CC-BB)*sr32_squared_1 - AA);\n\n        std::vector<double> sr32_squared_values;\n        if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1);\n        if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2);\n        assert(!sr32_squared_values.empty());\n\n        std::vector<double> sr32_values;\n        std::vector<double> sr31_values;\n        for (auto sr32_squared : sr32_squared_values) {\n            for(int sign = -1; sign <= 1; sign += 2) {\n                const double sr32 = static_cast<double>(sign) * std::sqrt(sr32_squared);\n                sr32_values.push_back( sr32 );\n                if (sr32_squared == 0.0) {\n                    // sr31 can be calculated through norm equality, \n                    // but it has positive and negative posibilities\n                    // positive one\n                    sr31_values.push_back(std::sqrt(CC-BB));\n                    // negative one\n                    sr32_values.push_back( sr32 );\n                    sr31_values.push_back(-std::sqrt(CC-BB));\n                    \n                    break; // skip the same situation\n                } else {\n                    // sr31 can be calculated throught dot product == 0\n                    sr31_values.push_back(- (sr11*sr12 + sr21*sr22) / sr32);\n                }\n            }\n        }\n\n        // std::cout << \"h= \" << std::setprecision(12) << h.transpose() << std::endl;\n        // std::cout << \"length: \" << sr32_values.size() << \" & \" << sr31_values.size() << std::endl;\n\n        assert(!sr31_values.empty());\n        assert(sr31_values.size() == sr32_values.size());\n        \n        std::vector<Eigen::Matrix3d> H_values;\n        for(size_t i=0;i<sr31_values.size(); ++i) {\n            const double sr31 = sr31_values.at(i);\n            const double sr32 = sr32_values.at(i);\n            const double lambda = 1.0 / sqrt(sr11*sr11 + sr21*sr21 + sr31*sr31);\n            Eigen::Matrix3d H;\n            H.setZero();\n            H(0,0) = sr11; H(0,1) = sr12; H(0,2) = st1;\n            H(1,0) = sr21; H(1,1) = sr22; H(1,2) = st2;\n            H(2,0) = sr31; H(2,1) = sr32; H(2,2) = 0;\n\n            H_values.push_back( lambda * H);\n            H_values.push_back(-lambda * H);\n        }\n\n        for(auto& H : H_values) {\n            // std::cout << \"H=\\n\" << H << std::endl;\n            Eigen::Matrix3d R;\n            R.col(0) = H.col(0);\n            R.col(1) = H.col(1);\n            R.col(2) = H.col(0).cross(H.col(1));\n            // std::cout << \"R33 = \" << R(2,2) << std::endl;\n        }\n        \n        std::vector<Eigen::Matrix3d> H_candidates;\n\n        for (auto& H : H_values)\n        {\n            Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4);\n            Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size());\n            A_mat.setZero();\n            B_vec.setZero();\n\n            size_t line_index = 0;\n\n            // iterate images\n            const double& r11 = H(0,0);\n            const double& r12 = H(0,1);\n            // const double& r13 = H(0,2);\n            const double& r21 = H(1,0);\n            const double& r22 = H(1,1);\n            // const double& r23 = H(1,2);\n            const double& r31 = H(2,0);\n            const double& r32 = H(2,1);\n            // const double& r33 = H(2,2);\n            const double& t1  = H(0);\n            const double& t2  = H(1);\n                \n            // iterate chessboard corners in the image\n            for(size_t j=0; j<imagePoints.at(image_index).size(); ++j) {\n                assert(line_index == 2 * j);\n\n                const double& X = objectPoints.at(image_index).at(j).x;\n                const double& Y = objectPoints.at(image_index).at(j).y;\n                const double& u = imagePoints.at(image_index).at(j).x;\n                const double& v = imagePoints.at(image_index).at(j).y;\n\n                double A = r21 * X + r22 * Y + t2;\n                double B = v * (r31 * X + r32 * Y);\n                double C = r11 * X + r12 * Y + t1;\n                double D = u * (r31 * X + r32 * Y);\n                double rou = std::sqrt(u*u + v*v);\n\n                \n                A_mat(line_index+0, 0) = A;\n                A_mat(line_index+1, 0) = C;\n                A_mat(line_index+0, 1) = A * rou;\n                A_mat(line_index+1, 1) = C * rou;\n                A_mat(line_index+0, 2) = A * rou * rou;\n                A_mat(line_index+1, 2) = C * rou * rou;\n                \n                A_mat(line_index+0, 3) = -v;\n                A_mat(line_index+1, 3) = -u;\n                B_vec(line_index+0) = B;\n                B_vec(line_index+1) = D;\n\n                line_index += 2;\n            }\n\n            assert(line_index == static_cast<unsigned int>(A_mat.rows()));\n\n            // pseudo-inverse for polynomial parameters and all t3s\n            {\n                Eigen::JacobiSVD<Eigen::MatrixXd> svd(A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);\n\n                Eigen::VectorXd x = svd.solve(B_vec);\n\n                // std::cout << \"x(poly and t3) = \" << x << std::endl;\n\n                if (x(2) > 0 && x(3) > 0) {\n                    H_candidates.push_back(H);\n                }\n            }\n        }\n\n        // printf(\"H_candidates.size()=%zu\\n\", H_candidates.size());\n        assert(H_candidates.size()==1);\n\n        Eigen::Matrix3d& H = H_candidates.front();\n\n        Eigen::Matrix3d R; \n        R.col(0) = H.col(0); \n        R.col(1) = H.col(1); \n        R.col(2) = H.col(0).cross(H.col(1)); \n\n        Eigen::Vector3d T = H.col(2);\n        RList.push_back(R);\n        TList.push_back(T);\n\n        // std::cout << \"#\" << image_index << \" frame\" << \" R =\" << R << \" \\nT = \" << T.transpose() << std::endl;\n    }\n\n    // Second, estimate camera intrinsic parameters and all t3\n    Eigen::MatrixXd A_mat(2 * imagePoints.size() * imagePoints.at(0).size(), SCARAMUZZA_POLY_SIZE-1 + imagePoints.size());\n    Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size());\n    A_mat.setZero();\n    B_vec.setZero();\n\n    size_t line_index = 0;\n\n    // iterate images\n    for(size_t i = 0; i < imagePoints.size(); ++i) {\n        const double& r11 = RList.at(i)(0,0);\n        const double& r12 = RList.at(i)(0,1);\n        // const double& r13 = RList.at(i)(0,2);\n        const double& r21 = RList.at(i)(1,0);\n        const double& r22 = RList.at(i)(1,1);\n        // const double& r23 = RList.at(i)(1,2);\n        const double& r31 = RList.at(i)(2,0);\n        const double& r32 = RList.at(i)(2,1);\n        // const double& r33 = RList.at(i)(2,2);\n        const double& t1  = TList.at(i)(0);\n        const double& t2  = TList.at(i)(1);\n        \n        // iterate chessboard corners in the image\n        for(size_t j=0; j<imagePoints.at(i).size(); ++j) {\n            assert(line_index == 2 * (i * imagePoints.at(0).size() + j));\n\n            const double& X = objectPoints.at(i).at(j).x;\n            const double& Y = objectPoints.at(i).at(j).y;\n            const double& u = imagePoints.at(i).at(j).x;\n            const double& v = imagePoints.at(i).at(j).y;\n\n            double A = r21 * X + r22 * Y + t2;\n            double B = v * (r31 * X + r32 * Y);\n            double C = r11 * X + r12 * Y + t1;\n            double D = u * (r31 * X + r32 * Y);\n            double rou = std::sqrt(u*u + v*v);\n\n            for(int k=1;k<=SCARAMUZZA_POLY_SIZE-1;++k) {\n                double pow_rou = 0.0;\n                if (k == 1) {\n                    pow_rou = 1.0;\n                }\n                else {\n                    pow_rou = std::pow(rou, k);\n                }\n\n                A_mat(line_index+0, k-1) = A * pow_rou;\n                A_mat(line_index+1, k-1) = C * pow_rou;\n            }\n            \n            A_mat(line_index+0, SCARAMUZZA_POLY_SIZE-1+i) = -v;\n            A_mat(line_index+1, SCARAMUZZA_POLY_SIZE-1+i) = -u;\n            B_vec(line_index+0) = B;\n            B_vec(line_index+1) = D;\n\n            line_index += 2;\n        }\n    }\n\n    assert(line_index == static_cast<unsigned int>(A_mat.rows()));\n\n    Eigen::Matrix<double, SCARAMUZZA_POLY_SIZE, 1> poly_coeff;\n    // pseudo-inverse for polynomial parameters and all t3s\n    {\n        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);\n\n        Eigen::VectorXd x = svd.solve(B_vec);\n\n        poly_coeff[0] = x(0);\n        poly_coeff[1] = 0.0;\n        for(int i=1;i<poly_coeff.size()-1;++i) {\n            poly_coeff[i+1] = x(i);\n        }\n        assert(x.size() == static_cast<unsigned int>(SCARAMUZZA_POLY_SIZE-1+TList.size()));\n    }\n\n    Parameters params = getParameters();\n\n    // Affine matrix A is constructed as [C D; E 1]\n    params.C() = 1.0;\n    params.D() = 0.0;\n    params.E() = 0.0;\n\n    params.center_x() = params.imageWidth() / 2.0;\n    params.center_y() = params.imageHeight() / 2.0;\n    \n    for(size_t i=0; i<SCARAMUZZA_POLY_SIZE; ++i) {\n        params.poly(i) = poly_coeff[i];\n    }\n\n    // params.poly(0) = -216.9657476318;\n    // params.poly(1) = 0.0;\n    // params.poly(2) = 0.0017866911;\n    // params.poly(3) = -0.0000019866;\n    // params.poly(4) =  0.0000000077;\n\n    \n    // inv_poly\n    {\n        std::vector<double> rou_vec;\n        std::vector<double> z_vec;\n        for(double rou = 0.0; rou <= (params.imageWidth() + params.imageHeight())/2; rou += 0.1) {\n            double rou_pow_k = 1.0;\n            double z = 0.0;\n\n            for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++)\n            {\n                z += rou_pow_k * params.poly(k);\n                rou_pow_k *= rou;\n            }\n\n            rou_vec.push_back(rou);\n            z_vec.push_back(z);\n        }\n\n        assert(rou_vec.size() == z_vec.size());\n        Eigen::VectorXd xVec(rou_vec.size());\n        Eigen::VectorXd yVec(rou_vec.size());\n\n        for(size_t i=0; i<rou_vec.size(); ++i) {\n            xVec(i) = std::atan2(-z_vec.at(i), rou_vec.at(i));\n            yVec(i) = rou_vec.at(i);\n        }\n\n        // use lower order poly to eliminate over-fitting cause by noisy/inaccurate data\n        const int poly_fit_order = 4;\n        Eigen::VectorXd inv_poly_coeff = polyfit(xVec, yVec, poly_fit_order);\n        \n        for(int i=0; i<=poly_fit_order; ++i) {\n            params.inv_poly(i) = inv_poly_coeff(i);\n        }\n    }\n\n    setParameters(params);\n\n    std::cout << \"initial params:\\n\" << params << std::endl; \n}\n\n/** \n * \\brief Lifts a point from the image plane to the unit sphere\n *\n * \\param p image coordinates\n * \\param P coordinates of the point on the sphere\n */\nvoid\nOCAMCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    liftProjective(p, P);\n    P.normalize();\n}\n\n/** \n * \\brief Lifts a point from the image plane to its projective ray\n *\n * \\param p image coordinates\n * \\param P coordinates of the projective ray\n */\nvoid\nOCAMCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n{\n    // Relative to Center\n    Eigen::Vector2d xc(p[0] - mParameters.center_x(), p[1] - mParameters.center_y());\n\n    // Affine Transformation\n    // xc_a = inv(A) * xc;\n    Eigen::Vector2d xc_a(\n        m_inv_scale * (xc[0] - mParameters.D() * xc[1]),\n        m_inv_scale * (-mParameters.E() * xc[0] + mParameters.C() * xc[1])\n    );\n\n    double phi = std::sqrt(xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]);\n    double phi_i = 1.0;\n    double z = 0.0;\n\n    for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)\n    {\n        z += phi_i * mParameters.poly(i);\n        phi_i *= phi;\n    }\n\n    P << xc[0], xc[1], -z;\n}\n\n\n/** \n * \\brief Project a 3D point (\\a x,\\a y,\\a z) to the image plane in (\\a u,\\a v)\n *\n * \\param P 3D point coordinates\n * \\param p return value, contains the image point coordinates\n */\nvoid\nOCAMCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n{\n    double norm = std::sqrt(P[0] * P[0] + P[1] * P[1]);\n    double theta = std::atan2(-P[2], norm);\n    double rho = 0.0;\n    double theta_i = 1.0;\n\n    for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n    {\n        rho += theta_i * mParameters.inv_poly(i);\n        theta_i *= theta;\n    }\n\n    double invNorm = 1.0 / norm;\n    Eigen::Vector2d xn(\n        P[0] * invNorm * rho,\n        P[1] * invNorm * rho\n    );\n\n    p << xn[0] * mParameters.C() + xn[1] * mParameters.D() + mParameters.center_x(),\n         xn[0] * mParameters.E() + xn[1]                   + mParameters.center_y();\n}\n\n\n/** \n * \\brief Projects an undistorted 2D point p_u to the image plane\n *\n * \\param p_u 2D point coordinates\n * \\return image point coordinates\n */\nvoid\nOCAMCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n{\n    Eigen::Vector3d P(p_u[0], p_u[1], 1.0);\n    spaceToPlane(P, p);\n}\n\n\n#if 0\nvoid\nOCAMCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const\n{\n    cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;\n            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;\n\n            double xi = mParameters.xi();\n            double d2 = mx_u * mx_u + my_u * my_u;\n\n            Eigen::Vector3d P;\n            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));\n\n            Eigen::Vector2d p;\n            spaceToPlane(P, p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n}\n#endif\n\ncv::Mat\nOCAMCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,\n                                    float fx, float fy,\n                                    cv::Size imageSize,\n                                    float cx, float cy,\n                                    cv::Mat rmat) const\n{\n    if (imageSize == cv::Size(0, 0))\n    {\n        imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());\n    }\n\n    cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n    cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);\n\n    Eigen::Matrix3f K_rect;\n\n    K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx,\n              0, fy, cy < 0 ? imageSize.height / 2 : cy,\n              0, 0, 1;\n\n    if (fx < 0 || fy < 0)\n    {\n        throw std::string(std::string(__FUNCTION__) + \": Focal length must be specified\");\n    }\n\n    Eigen::Matrix3f K_rect_inv = K_rect.inverse();\n\n    Eigen::Matrix3f R, R_inv;\n    cv::cv2eigen(rmat, R);\n    R_inv = R.inverse();\n\n    for (int v = 0; v < imageSize.height; ++v)\n    {\n        for (int u = 0; u < imageSize.width; ++u)\n        {\n            Eigen::Vector3f xo;\n            xo << u, v, 1;\n\n            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;\n\n            Eigen::Vector2d p;\n            spaceToPlane(uo.cast<double>(), p);\n\n            mapX.at<float>(v,u) = p(0);\n            mapY.at<float>(v,u) = p(1);\n        }\n    }\n\n    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);\n\n    cv::Mat K_rect_cv;\n    cv::eigen2cv(K_rect, K_rect_cv);\n    return K_rect_cv;\n}\n\nint\nOCAMCamera::parameterCount(void) const\n{\n    return SCARAMUZZA_CAMERA_NUM_PARAMS;\n}\n\nconst OCAMCamera::Parameters&\nOCAMCamera::getParameters(void) const\n{\n    return mParameters;\n}\n\nvoid\nOCAMCamera::setParameters(const OCAMCamera::Parameters& parameters)\n{\n    mParameters = parameters;\n\n    m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());\n}\n\nvoid\nOCAMCamera::readParameters(const std::vector<double>& parameterVec)\n{\n    if ((int)parameterVec.size() != parameterCount())\n    {\n        return;\n    }\n\n    Parameters params = getParameters();\n\n    params.C() = parameterVec.at(0);\n    params.D() = parameterVec.at(1);\n    params.E() = parameterVec.at(2);\n    params.center_x() = parameterVec.at(3);\n    params.center_y() = parameterVec.at(4);\n    for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n        params.poly(i) = parameterVec.at(5+i);\n    for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n        params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);\n\n    setParameters(params);\n}\n\nvoid\nOCAMCamera::writeParameters(std::vector<double>& parameterVec) const\n{\n    parameterVec.resize(parameterCount());\n    parameterVec.at(0) = mParameters.C();\n    parameterVec.at(1) = mParameters.D();\n    parameterVec.at(2) = mParameters.E();\n    parameterVec.at(3) = mParameters.center_x();\n    parameterVec.at(4) = mParameters.center_y();\n    for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)\n        parameterVec.at(5+i) = mParameters.poly(i);\n    for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)\n        parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);\n}\n\nvoid\nOCAMCamera::writeParametersToYamlFile(const std::string& filename) const\n{\n    mParameters.writeToYamlFile(filename);\n}\n\nstd::string\nOCAMCamera::parametersToString(void) const\n{\n    std::ostringstream oss;\n    oss << mParameters;\n\n    return oss.str();\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/chessboard/Chessboard.cc",
    "content": "#include \"camodocal/chessboard/Chessboard.h\"\n\n#include <opencv2/calib3d/calib3d.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n\n#include \"camodocal/chessboard/ChessboardQuad.h\"\n#include \"camodocal/chessboard/Spline.h\"\n\n#define MAX_CONTOUR_APPROX  7\n\nnamespace camodocal\n{\n\nChessboard::Chessboard(cv::Size boardSize, cv::Mat& image)\n : mBoardSize(boardSize)\n , mCornersFound(false)\n{\n    if (image.channels() == 1)\n    {\n        cv::cvtColor(image, mSketch, CV_GRAY2BGR);\n        image.copyTo(mImage);\n    }\n    else\n    {\n        image.copyTo(mSketch);\n        cv::cvtColor(image, mImage, CV_BGR2GRAY);\n    }\n}\n\nvoid\nChessboard::findCorners(bool useOpenCV)\n{\n    mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners,\n                                          CV_CALIB_CB_ADAPTIVE_THRESH +\n                                          CV_CALIB_CB_NORMALIZE_IMAGE +\n                                          CV_CALIB_CB_FILTER_QUADS +\n                                          CV_CALIB_CB_FAST_CHECK,\n                                          useOpenCV);\n\n    if (mCornersFound)\n    {\n        // draw chessboard corners\n        cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound);\n    }\n}\n\nconst std::vector<cv::Point2f>&\nChessboard::getCorners(void) const\n{\n    return mCorners;\n}\n\nbool\nChessboard::cornersFound(void) const\n{\n    return mCornersFound;\n}\n\nconst cv::Mat&\nChessboard::getImage(void) const\n{\n    return mImage;\n}\n\nconst cv::Mat&\nChessboard::getSketch(void) const\n{\n    return mSketch;\n}\n\nbool\nChessboard::findChessboardCorners(const cv::Mat& image,\n                                  const cv::Size& patternSize,\n                                  std::vector<cv::Point2f>& corners,\n                                  int flags, bool useOpenCV)\n{\n    if (useOpenCV)\n    {\n        return cv::findChessboardCorners(image, patternSize, corners, flags);\n    }\n    else\n    {\n        return findChessboardCornersImproved(image, patternSize, corners, flags);\n    }\n}\n\nbool\nChessboard::findChessboardCornersImproved(const cv::Mat& image,\n                                          const cv::Size& patternSize,\n                                          std::vector<cv::Point2f>& corners,\n                                          int flags)\n{\n    /************************************************************************************\\\n        This is improved variant of chessboard corner detection algorithm that\n        uses a graph of connected quads. It is based on the code contributed\n        by Vladimir Vezhnevets and Philip Gruebele.\n        Here is the copyright notice from the original Vladimir's code:\n        ===============================================================\n\n        The algorithms developed and implemented by Vezhnevets Vldimir\n        aka Dead Moroz (vvp@graphics.cs.msu.ru)\n        See http://graphics.cs.msu.su/en/research/calibration/opencv.html\n        for detailed information.\n\n        Reliability additions and modifications made by Philip Gruebele.\n        <a href=\"mailto:pgruebele@cox.net\">pgruebele@cox.net</a>\n\n        Some improvements were made by:\n        1) Martin Rufli - increased chance of correct corner matching\n           Rufli, M., Scaramuzza, D., and Siegwart, R. (2008),\n           Automatic Detection of Checkerboards on Blurred and Distorted Images,\n           Proceedings of the IEEE/RSJ International Conference on\n           Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008.\n        2) Lionel Heng - post-detection checks and better thresholding\n\n    \\************************************************************************************/\n\n    //int bestDilation        = -1;\n    const int minDilations    =  0;\n    const int maxDilations    =  7;\n\n    std::vector<ChessboardQuadPtr> outputQuadGroup;\n\n    if (image.depth() != CV_8U || image.channels() == 2)\n    {\n        return false;\n    }\n\n    if (patternSize.width < 2 || patternSize.height < 2)\n    {\n        return false;\n    }\n\n    if (patternSize.width > 127 || patternSize.height > 127)\n    {\n        return false;\n    }\n\n    cv::Mat img = image;\n\n    // Image histogram normalization and\n    // BGR to Grayscale image conversion (if applicable)\n    // MARTIN: Set to \"false\"\n    if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE))\n    {\n        cv::Mat norm_img(image.rows, image.cols, CV_8UC1);\n\n        if (image.channels() != 1)\n        {\n            cv::cvtColor(image, norm_img, CV_BGR2GRAY);\n            img = norm_img;\n        }\n\n        if (flags & CV_CALIB_CB_NORMALIZE_IMAGE)\n        {\n            cv::equalizeHist(image, norm_img);\n            img = norm_img;\n        }\n    }\n\n    if (flags & CV_CALIB_CB_FAST_CHECK)\n    {\n        if (!checkChessboard(img, patternSize))\n        {\n            return false;\n        }\n    }\n\n    // PART 1: FIND LARGEST PATTERN\n    //-----------------------------------------------------------------------\n    // Checker patterns are tried to be found by dilating the background and\n    // then applying a canny edge finder on the closed contours (checkers).\n    // Try one dilation run, but if the pattern is not found, repeat until\n    // max_dilations is reached.\n\n    int prevSqrSize = 0;\n    bool found = false;\n    std::vector<ChessboardCornerPtr> outputCorners;\n\n    for (int k = 0; k < 6; ++k)\n    {\n        for (int dilations = minDilations; dilations <= maxDilations; ++dilations)\n        {\n            if (found)\n            {\n                break;\n            }\n\n            cv::Mat thresh_img;\n\n            // convert the input grayscale image to binary (black-n-white)\n            if (flags & CV_CALIB_CB_ADAPTIVE_THRESH)\n            {\n                int blockSize = lround(prevSqrSize == 0 ?\n                    std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1;\n\n                // convert to binary\n                cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k/2)*5);\n            }\n            else\n            {\n                // empiric threshold level\n                double mean = (cv::mean(img))[0];\n                int thresh_level = lround(mean - 10);\n                thresh_level = std::max(thresh_level, 10);\n\n                cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY);\n            }\n\n            // MARTIN's Code\n            // Use both a rectangular and a cross kernel. In this way, a more\n            // homogeneous dilation is performed, which is crucial for small,\n            // distorted checkers. Use the CROSS kernel first, since its action\n            // on the image is more subtle\n            cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3,3), cv::Point(1,1));\n            cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3,3), cv::Point(1,1));\n\n            if (dilations >= 1)\n                cv::dilate(thresh_img, thresh_img, kernel1);\n            if (dilations >= 2)\n                cv::dilate(thresh_img, thresh_img, kernel2);\n            if (dilations >= 3)\n                cv::dilate(thresh_img, thresh_img, kernel1);\n            if (dilations >= 4)\n                cv::dilate(thresh_img, thresh_img, kernel2);\n            if (dilations >= 5)\n                cv::dilate(thresh_img, thresh_img, kernel1);\n            if (dilations >= 6)\n                cv::dilate(thresh_img, thresh_img, kernel2);\n\n            // In order to find rectangles that go to the edge, we draw a white\n            // line around the image edge. Otherwise FindContours will miss those\n            // clipped rectangle contours. The border color will be the image mean,\n            // because otherwise we risk screwing up filters like cvSmooth()\n            cv::rectangle(thresh_img, cv::Point(0,0),\n                          cv::Point(thresh_img.cols - 1, thresh_img.rows - 1),\n                          CV_RGB(255,255,255), 3, 8);\n\n            // Generate quadrangles in the following function\n            std::vector<ChessboardQuadPtr> quads;\n\n            generateQuads(quads, thresh_img, flags, dilations, true);\n            if (quads.empty())\n            {\n                continue;\n            }\n\n            // The following function finds and assigns neighbor quads to every\n            // quadrangle in the immediate vicinity fulfilling certain\n            // prerequisites\n            findQuadNeighbors(quads, dilations);\n\n            // The connected quads will be organized in groups. The following loop\n            // increases a \"group_idx\" identifier.\n            // The function \"findConnectedQuads assigns all connected quads\n            // a unique group ID.\n            // If more quadrangles were assigned to a given group (i.e. connected)\n            // than are expected by the input variable \"patternSize\", the\n            // function \"cleanFoundConnectedQuads\" erases the surplus\n            // quadrangles by minimizing the convex hull of the remaining pattern.\n\n            for (int group_idx = 0; ; ++group_idx)\n            {\n                std::vector<ChessboardQuadPtr> quadGroup;\n\n                findConnectedQuads(quads, quadGroup, group_idx, dilations);\n\n                if (quadGroup.empty())\n                {\n                    break;\n                }\n\n                cleanFoundConnectedQuads(quadGroup, patternSize);\n\n                // The following function labels all corners of every quad\n                // with a row and column entry.\n                // \"count\" specifies the number of found quads in \"quad_group\"\n                // with group identifier \"group_idx\"\n                // The last parameter is set to \"true\", because this is the\n                // first function call and some initializations need to be\n                // made.\n                labelQuadGroup(quadGroup, patternSize, true);\n\n                found = checkQuadGroup(quadGroup, outputCorners, patternSize);\n\n                float sumDist = 0;\n                int total = 0;\n\n                for (int i = 0; i < (int)outputCorners.size(); ++i)\n                {\n                    int ni = 0;\n                    float avgi = outputCorners.at(i)->meanDist(ni);\n                    sumDist += avgi * ni;\n                    total += ni;\n                }\n                prevSqrSize = lround(sumDist / std::max(total, 1));\n\n                if (found && !checkBoardMonotony(outputCorners, patternSize))\n                {\n                    found = false;\n                }\n            }\n        }\n    }\n\n    if (!found)\n    {\n        return false;\n    }\n    else\n    {\n        corners.clear();\n        corners.reserve(outputCorners.size());\n        for (size_t i = 0; i < outputCorners.size(); ++i)\n        {\n            corners.push_back(outputCorners.at(i)->pt);\n        }\n\n        cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1),\n                         cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));\n\n        return true;\n    }\n}\n\n//===========================================================================\n// ERASE OVERHEAD\n//===========================================================================\n// If we found too many connected quads, remove those which probably do not\n// belong.\nvoid\nChessboard::cleanFoundConnectedQuads(std::vector<ChessboardQuadPtr>& quadGroup,\n                                     cv::Size patternSize)\n{\n    cv::Point2f center(0.0f, 0.0f);\n\n    // Number of quads this pattern should contain\n    int count = ((patternSize.width + 1)*(patternSize.height + 1) + 1)/2;\n\n    // If we have more quadrangles than we should, try to eliminate duplicates\n    // or ones which don't belong to the pattern rectangle. Else go to the end\n    // of the function\n    if ((int)quadGroup.size() <= count)\n    {\n        return;\n    }\n\n    // Create an array of quadrangle centers\n    std::vector<cv::Point2f> centers;\n    centers.resize(quadGroup.size());\n\n    for (size_t i = 0; i < quadGroup.size(); ++i)\n    {\n        cv::Point2f ci(0.0f, 0.0f);\n        ChessboardQuadPtr& q = quadGroup[i];\n\n        for (int j = 0; j < 4; ++j)\n        {\n            ci += q->corners[j]->pt;\n        }\n\n        ci *= 0.25f;\n\n        // Centers(i), is the geometric center of quad(i)\n        // Center, is the center of all found quads\n        centers[i] = ci;\n\n        center += ci;\n    }\n\n    center *= 1.0f / quadGroup.size();\n\n    // If we have more quadrangles than we should, we try to eliminate bad\n    // ones based on minimizing the bounding box. We iteratively remove the\n    // point which reduces the size of the bounding box of the blobs the most\n    // (since we want the rectangle to be as small as possible) remove the\n    // quadrange that causes the biggest reduction in pattern size until we\n    // have the correct number\n    while ((int)quadGroup.size() > count)\n    {\n        double minBoxArea = DBL_MAX;\n        int minBoxAreaIndex = -1;\n\n        // For each point, calculate box area without that point\n        for (size_t skip = 0; skip < quadGroup.size(); ++skip)\n        {\n            // get bounding rectangle\n            cv::Point2f temp = centers[skip];\n            centers[skip] = center;\n\n            std::vector<cv::Point2f> hull;\n            cv::convexHull(centers, hull, true, true);\n            centers[skip] = temp;\n\n            double hull_area = fabs(cv::contourArea(hull));\n\n            // remember smallest box area\n            if (hull_area < minBoxArea)\n            {\n                minBoxArea = hull_area;\n                minBoxAreaIndex = skip;\n            }\n        }\n\n        ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex];\n\n        // remove any references to this quad as a neighbor\n        for (size_t i = 0; i < quadGroup.size(); ++i)\n        {\n            ChessboardQuadPtr& q = quadGroup.at(i);\n            for (int j = 0; j < 4; ++j)\n            {\n                if (q->neighbors[j] == q0)\n                {\n                    q->neighbors[j].reset();\n                    q->count--;\n                    for (int k = 0; k < 4; ++k)\n                    {\n                        if (q0->neighbors[k] == q)\n                        {\n                            q0->neighbors[k].reset();\n                            q0->count--;\n                            break;\n                        }\n                    }\n                    break;\n                }\n            }\n        }\n        // remove the quad\n        quadGroup.at(minBoxAreaIndex) = quadGroup.back();\n        centers.at(minBoxAreaIndex) = centers.back();\n        quadGroup.pop_back();\n        centers.pop_back();\n    }\n}\n\n//===========================================================================\n// FIND COONECTED QUADS\n//===========================================================================\nvoid\nChessboard::findConnectedQuads(std::vector<ChessboardQuadPtr>& quads,\n                               std::vector<ChessboardQuadPtr>& group,\n                               int group_idx, int dilation)\n{\n    ChessboardQuadPtr q;\n\n    // Scan the array for a first unlabeled quad\n    for (size_t i = 0; i < quads.size(); ++i)\n    {\n        ChessboardQuadPtr& quad = quads.at(i);\n\n        if (quad->count > 0 && quad->group_idx < 0)\n        {\n            q = quad;\n            break;\n        }\n    }\n\n    if (q.get() == 0)\n    {\n        return;\n    }\n\n    // Recursively find a group of connected quads starting from the seed quad\n\n    std::vector<ChessboardQuadPtr> stack;\n    stack.push_back(q);\n\n    group.push_back(q);\n    q->group_idx = group_idx;\n\n    while (!stack.empty())\n    {\n        q = stack.back();\n        stack.pop_back();\n\n        for (int i = 0; i < 4; ++i)\n        {\n            ChessboardQuadPtr& neighbor = q->neighbors[i];\n\n            // If he neighbor exists and the neighbor has more than 0\n            // neighbors and the neighbor has not been classified yet.\n            if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0)\n            {\n                stack.push_back(neighbor);\n                group.push_back(neighbor);\n                neighbor->group_idx = group_idx;\n            }\n        }\n    }\n}\n\nvoid\nChessboard::labelQuadGroup(std::vector<ChessboardQuadPtr>& quadGroup,\n                           cv::Size patternSize, bool firstRun)\n{\n    // If this is the first function call, a seed quad needs to be selected\n    if (firstRun)\n    {\n        // Search for the (first) quad with the maximum number of neighbors\n        // (usually 4). This will be our starting point.\n        int mark = -1;\n        int maxNeighborCount = 0;\n        for (size_t i = 0; i < quadGroup.size(); ++i)\n        {\n            ChessboardQuadPtr& q = quadGroup.at(i);\n            if (q->count > maxNeighborCount)\n            {\n                mark = i;\n                maxNeighborCount = q->count;\n\n                if (maxNeighborCount == 4)\n                {\n                    break;\n                }\n            }\n        }\n\n        // Mark the starting quad's (per definition) upper left corner with\n        //(0,0) and then proceed clockwise\n        // The following labeling sequence enshures a \"right coordinate system\"\n        ChessboardQuadPtr& q = quadGroup.at(mark);\n\n        q->labeled = true;\n\n        q->corners[0]->row = 0;\n        q->corners[0]->column = 0;\n        q->corners[1]->row = 0;\n        q->corners[1]->column = 1;\n        q->corners[2]->row = 1;\n        q->corners[2]->column = 1;\n        q->corners[3]->row = 1;\n        q->corners[3]->column = 0;\n    }\n\n\n    // Mark all other corners with their respective row and column\n    bool flagChanged = true;\n    while (flagChanged)\n    {\n        // First reset the flag to \"false\"\n        flagChanged = false;\n\n        // Go through all quads top down is faster, since unlabeled quads will\n        // be inserted at the end of the list\n        for (int i = quadGroup.size() - 1; i >= 0; --i)\n        {\n            ChessboardQuadPtr& quad = quadGroup.at(i);\n\n            // Check whether quad \"i\" has been labeled already\n             if (!quad->labeled)\n            {\n                // Check its neighbors, whether some of them have been labeled\n                // already\n                for (int j = 0; j < 4; j++)\n                {\n                    // Check whether the neighbor exists (i.e. is not the NULL\n                    // pointer)\n                    if (quad->neighbors[j])\n                    {\n                        ChessboardQuadPtr& quadNeighbor = quad->neighbors[j];\n\n                        // Only proceed, if neighbor \"j\" was labeled\n                        if (quadNeighbor->labeled)\n                        {\n                            // For every quad it could happen to pass here\n                            // multiple times. We therefore \"break\" later.\n                            // Check whitch of the neighbors corners is\n                            // connected to the current quad\n                            int connectedNeighborCornerId = -1;\n                            for (int k = 0; k < 4; ++k)\n                            {\n                                if (quadNeighbor->neighbors[k] == quad)\n                                {\n                                    connectedNeighborCornerId = k;\n\n                                    // there is only one, therefore\n                                    break;\n                                }\n                            }\n\n\n                            // For the following calculations we need the row\n                            // and column of the connected neighbor corner and\n                            // all other corners of the connected quad \"j\",\n                            // clockwise (CW)\n                            ChessboardCornerPtr& conCorner        = quadNeighbor->corners[connectedNeighborCornerId];\n                            ChessboardCornerPtr& conCornerCW1     = quadNeighbor->corners[(connectedNeighborCornerId+1)%4];\n                            ChessboardCornerPtr& conCornerCW2     = quadNeighbor->corners[(connectedNeighborCornerId+2)%4];\n                            ChessboardCornerPtr& conCornerCW3     = quadNeighbor->corners[(connectedNeighborCornerId+3)%4];\n\n                            quad->corners[j]->row            =    conCorner->row;\n                            quad->corners[j]->column        =    conCorner->column;\n                            quad->corners[(j+1)%4]->row        =    conCorner->row - conCornerCW2->row + conCornerCW3->row;\n                            quad->corners[(j+1)%4]->column    =    conCorner->column - conCornerCW2->column + conCornerCW3->column;\n                            quad->corners[(j+2)%4]->row        =    conCorner->row + conCorner->row - conCornerCW2->row;\n                            quad->corners[(j+2)%4]->column    =    conCorner->column + conCorner->column - conCornerCW2->column;\n                            quad->corners[(j+3)%4]->row        =    conCorner->row - conCornerCW2->row + conCornerCW1->row;\n                            quad->corners[(j+3)%4]->column    =    conCorner->column - conCornerCW2->column + conCornerCW1->column;\n\n                            // Mark this quad as labeled\n                            quad->labeled = true;\n\n                            // Changes have taken place, set the flag\n                            flagChanged = true;\n\n                            // once is enough!\n                            break;\n                        }\n                    }\n                }\n            }\n        }\n    }\n\n\n    // All corners are marked with row and column\n    // Record the minimal and maximal row and column indices\n    // It is unlikely that more than 8bit checkers are used per dimension, if there are\n    // an error would have been thrown at the beginning of \"cvFindChessboardCorners2\"\n    int min_row        =  127;\n    int max_row        = -127;\n    int min_column    =  127;\n    int max_column    = -127;\n\n    for (int i = 0; i < (int)quadGroup.size(); ++i)\n    {\n        ChessboardQuadPtr& q = quadGroup.at(i);\n\n        for (int j = 0; j < 4; ++j)\n        {\n            ChessboardCornerPtr& c = q->corners[j];\n\n            if (c->row > max_row)\n            {\n                max_row = c->row;\n            }\n            if (c->row < min_row)\n            {\n                min_row = c->row;\n            }\n            if (c->column > max_column)\n            {\n                max_column = c->column;\n            }\n            if (c->column < min_column)\n            {\n                min_column = c->column;\n            }\n        }\n    }\n\n    // Label all internal corners with \"needsNeighbor\" = false\n    // Label all external corners with \"needsNeighbor\" = true,\n    // except if in a given dimension the pattern size is reached\n    for (int i = min_row; i <= max_row; ++i)\n    {\n        for (int j = min_column; j <= max_column; ++j)\n        {\n            // A flag that indicates, whether a row/column combination is\n            // executed multiple times\n            bool flag = false;\n\n            // Remember corner and quad\n            int cornerID;\n            int quadID;\n\n            for (int k = 0; k < (int)quadGroup.size(); ++k)\n            {\n                ChessboardQuadPtr& q = quadGroup.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    if ((q->corners[l]->row == i) && (q->corners[l]->column == j))\n                    {\n                        if (flag)\n                        {\n                            // Passed at least twice through here\n                            q->corners[l]->needsNeighbor = false;\n                            quadGroup[quadID]->corners[cornerID]->needsNeighbor = false;\n                        }\n                        else\n                        {\n                            // Mark with needs a neighbor, but note the\n                            // address\n                            q->corners[l]->needsNeighbor = true;\n                            cornerID = l;\n                            quadID = k;\n                        }\n\n                        // set the flag to true\n                        flag = true;\n                    }\n                }\n            }\n        }\n    }\n\n    // Complete Linking:\n    // sometimes not all corners were properly linked in \"findQuadNeighbors\",\n    // but after labeling each corner with its respective row and column, it is\n    // possible to match them anyway.\n    for (int i = min_row; i <= max_row; ++i)\n    {\n        for (int j = min_column; j <= max_column; ++j)\n        {\n            // the following \"number\" indicates the number of corners which\n            // correspond to the given (i,j) value\n            // 1    is a border corner or a conrer which still needs a neighbor\n            // 2    is a fully connected internal corner\n            // >2    something went wrong during labeling, report a warning\n            int number = 1;\n\n            // remember corner and quad\n            int cornerID;\n            int quadID;\n\n            for (int k = 0; k < (int)quadGroup.size(); ++k)\n            {\n                ChessboardQuadPtr& q = quadGroup.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    if ((q->corners[l]->row == i) && (q->corners[l]->column == j))\n                    {\n\n                        if (number == 1)\n                        {\n                            // First corner, note its ID\n                            cornerID = l;\n                            quadID = k;\n                        }\n\n                        else if (number == 2)\n                        {\n                            // Second corner, check wheter this and the\n                            // first one have equal coordinates, else\n                            // interpolate\n                            cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt;\n\n                            if (delta.x != 0.0f || delta.y != 0.0f)\n                            {\n                                // Interpolate\n                                q->corners[l]->pt -= delta * 0.5f;\n\n                                quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f;\n                            }\n                        }\n                        else if (number > 2)\n                        {\n                            // Something went wrong during row/column labeling\n                            // Report a Warning\n                            // ->Implemented in the function \"mrWriteCorners\"\n                        }\n\n                        // increase the number by one\n                        ++number;\n                    }\n                }\n            }\n        }\n    }\n\n\n    // Bordercorners don't need any neighbors, if the pattern size in the\n    // respective direction is reached\n    // The only time we can make shure that the target pattern size is reached in a given\n    // dimension, is when the larger side has reached the target size in the maximal\n    // direction, or if the larger side is larger than the smaller target size and the\n    // smaller side equals the smaller target size\n    int largerDimPattern = std::max(patternSize.height,patternSize.width);\n    int smallerDimPattern = std::min(patternSize.height,patternSize.width);\n    bool flagSmallerDim1 = false;\n    bool flagSmallerDim2 = false;\n\n    if ((largerDimPattern + 1) == max_column - min_column)\n    {\n        flagSmallerDim1 = true;\n        // We found out that in the column direction the target pattern size is reached\n        // Therefore border column corners do not need a neighbor anymore\n        // Go through all corners\n        for (int k = 0; k < (int)quadGroup.size(); ++k)\n        {\n            ChessboardQuadPtr& q = quadGroup.at(k);\n\n            for (int l = 0; l < 4; ++l)\n            {\n                ChessboardCornerPtr& c = q->corners[l];\n\n                if (c->column == min_column || c->column == max_column)\n                {\n                    // Needs no neighbor anymore\n                    c->needsNeighbor = false;\n                }\n            }\n        }\n    }\n\n    if ((largerDimPattern + 1) == max_row - min_row)\n    {\n        flagSmallerDim2 = true;\n        // We found out that in the column direction the target pattern size is reached\n        // Therefore border column corners do not need a neighbor anymore\n        // Go through all corners\n        for (int k = 0; k < (int)quadGroup.size(); ++k)\n        {\n            ChessboardQuadPtr& q = quadGroup.at(k);\n\n            for (int l = 0; l < 4; ++l)\n            {\n                ChessboardCornerPtr& c = q->corners[l];\n\n                if (c->row == min_row || c->row == max_row)\n                {\n                    // Needs no neighbor anymore\n                    c->needsNeighbor = false;\n                }\n            }\n        }\n    }\n\n\n    // Check the two flags:\n    //    -    If one is true and the other false, then the pattern target\n    //        size was reached in in one direction -> We can check, whether the target\n    //        pattern size is also reached in the other direction\n    //  -    If both are set to true, then we deal with a square board -> do nothing\n    //  -    If both are set to false -> There is a possibility that the larger side is\n    //        larger than the smaller target size -> Check and if true, then check whether\n    //        the other side has the same size as the smaller target size\n    if ((flagSmallerDim1 == false && flagSmallerDim2 == true))\n    {\n        // Larger target pattern size is in row direction, check wheter smaller target\n        // pattern size is reached in column direction\n        if ((smallerDimPattern + 1) == max_column - min_column)\n        {\n            for (int k = 0; k < (int)quadGroup.size(); ++k)\n            {\n                ChessboardQuadPtr& q = quadGroup.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    ChessboardCornerPtr& c = q->corners[l];\n\n                    if (c->column == min_column || c->column == max_column)\n                    {\n                        // Needs no neighbor anymore\n                        c->needsNeighbor = false;\n                    }\n                }\n            }\n        }\n    }\n\n    if ((flagSmallerDim1 == true && flagSmallerDim2 == false))\n    {\n        // Larger target pattern size is in column direction, check wheter smaller target\n        // pattern size is reached in row direction\n        if ((smallerDimPattern + 1) == max_row - min_row)\n        {\n            for (int k = 0; k < (int)quadGroup.size(); ++k)\n            {\n                ChessboardQuadPtr& q = quadGroup.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    ChessboardCornerPtr& c = q->corners[l];\n\n                    if (c->row == min_row || c->row == max_row)\n                    {\n                        // Needs no neighbor anymore\n                        c->needsNeighbor = false;\n                    }\n                }\n            }\n        }\n    }\n\n    if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column)\n    {\n        // Larger target pattern size is in column direction, check wheter smaller target\n        // pattern size is reached in row direction\n        if ((smallerDimPattern + 1) == max_row - min_row)\n        {\n            for (int k = 0; k < (int)quadGroup.size(); ++k)\n            {\n                ChessboardQuadPtr& q = quadGroup.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    ChessboardCornerPtr& c = q->corners[l];\n\n                    if (c->row == min_row || c->row == max_row)\n                    {\n                        // Needs no neighbor anymore\n                        c->needsNeighbor = false;\n                    }\n                }\n            }\n        }\n    }\n\n    if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row)\n    {\n        // Larger target pattern size is in row direction, check wheter smaller target\n        // pattern size is reached in column direction\n        if ((smallerDimPattern + 1) == max_column - min_column)\n        {\n            for (int k = 0; k < (int)quadGroup.size(); ++k)\n            {\n                ChessboardQuadPtr& q = quadGroup.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    ChessboardCornerPtr& c = q->corners[l];\n\n                    if (c->column == min_column || c->column == max_column)\n                    {\n                        // Needs no neighbor anymore\n                        c->needsNeighbor = false;\n                    }\n                }\n            }\n        }\n    }\n}\n\n//===========================================================================\n// GIVE A GROUP IDX\n//===========================================================================\nvoid\nChessboard::findQuadNeighbors(std::vector<ChessboardQuadPtr>& quads, int dilation)\n{\n    // Thresh dilation is used to counter the effect of dilation on the\n    // distance between 2 neighboring corners. Since the distance below is\n    // computed as its square, we do here the same. Additionally, we take the\n    // conservative assumption that dilation was performed using the 3x3 CROSS\n    // kernel, which coresponds to the 4-neighborhood.\n    const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2;    // the \"*2\" is for the x and y component\n                                                                            // the \"3\" is for initial corner mismatch\n\n    // Find quad neighbors\n    for (size_t idx = 0; idx < quads.size(); ++idx)\n    {\n        ChessboardQuadPtr& curQuad = quads.at(idx);\n\n        // Go through all quadrangles and label them in groups\n        // For each corner of this quadrangle\n        for (int i = 0; i < 4; ++i)\n        {\n            float minDist = FLT_MAX;\n            int closestCornerIdx = -1;\n            ChessboardQuadPtr closestQuad;\n\n            if (curQuad->neighbors[i])\n            {\n                continue;\n            }\n\n            cv::Point2f pt = curQuad->corners[i]->pt;\n\n            // Find the closest corner in all other quadrangles\n            for (size_t k = 0; k < quads.size(); ++k)\n            {\n                if (k == idx)\n                {\n                    continue;\n                }\n\n                ChessboardQuadPtr& quad = quads.at(k);\n\n                for (int j = 0; j < 4; ++j)\n                {\n                    // If it already has a neighbor\n                    if (quad->neighbors[j])\n                    {\n                        continue;\n                    }\n\n                    cv::Point2f dp = pt - quad->corners[j]->pt;\n                    float dist = dp.dot(dp);\n\n                    // The following \"if\" checks, whether \"dist\" is the\n                    // shortest so far and smaller than the smallest\n                    // edge length of the current and target quads\n                    if (dist < minDist &&\n                        dist <= (curQuad->edge_len + thresh_dilation) &&\n                        dist <= (quad->edge_len + thresh_dilation)   )\n                    {\n                        // Check whether conditions are fulfilled\n                        if (matchCorners(curQuad, i, quad, j))\n                        {\n                            closestCornerIdx = j;\n                            closestQuad = quad;\n                            minDist = dist;\n                        }\n                    }\n                }\n            }\n\n            // Have we found a matching corner point?\n            if (closestCornerIdx >= 0 && minDist < FLT_MAX)\n            {\n                ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];\n\n                // Make sure that the closest quad does not have the current\n                // quad as neighbor already\n                bool valid = true;\n                for (int j = 0; j < 4; ++j)\n                {\n                    if (closestQuad->neighbors[j] == curQuad)\n                    {\n                        valid = false;\n                        break;\n                    }\n                }\n                if (!valid)\n                {\n                    continue;\n                }\n\n                // We've found one more corner - remember it\n                closestCorner->pt = (pt + closestCorner->pt) * 0.5f;\n\n                curQuad->count++;\n                curQuad->neighbors[i] = closestQuad;\n                curQuad->corners[i] = closestCorner;\n\n                closestQuad->count++;\n                closestQuad->neighbors[closestCornerIdx] = curQuad;\n                closestQuad->corners[closestCornerIdx] = closestCorner;\n            }\n        }\n    }\n}\n\n\n\n//===========================================================================\n// AUGMENT PATTERN WITH ADDITIONAL QUADS\n//===========================================================================\n// The first part of the function is basically a copy of\n// \"findQuadNeighbors\"\n// The comparisons between two points and two lines could be computed in their\n// own function\nint\nChessboard::augmentBestRun(std::vector<ChessboardQuadPtr>& candidateQuads, int candidateDilation,\n                           std::vector<ChessboardQuadPtr>& existingQuads, int existingDilation)\n{\n    // thresh dilation is used to counter the effect of dilation on the\n    // distance between 2 neighboring corners. Since the distance below is\n    // computed as its square, we do here the same. Additionally, we take the\n    // conservative assumption that dilation was performed using the 3x3 CROSS\n    // kernel, which coresponds to the 4-neighborhood.\n    const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2;    // the \"*2\" is for the x and y component\n\n    // Search all old quads which have a neighbor that needs to be linked\n    for (size_t idx = 0; idx < existingQuads.size(); ++idx)\n    {\n        ChessboardQuadPtr& curQuad = existingQuads.at(idx);\n\n        // For each corner of this quadrangle\n        for (int i = 0; i < 4; ++i)\n        {\n            float minDist = FLT_MAX;\n            int closestCornerIdx = -1;\n            ChessboardQuadPtr closestQuad;\n\n            // If curQuad corner[i] is already linked, continue\n            if (!curQuad->corners[i]->needsNeighbor)\n            {\n                continue;\n            }\n\n            cv::Point2f pt = curQuad->corners[i]->pt;\n\n            // Look for a match in all candidateQuads' corners\n            for (size_t k = 0; k < candidateQuads.size(); ++k)\n            {\n                ChessboardQuadPtr& candidateQuad = candidateQuads.at(k);\n\n                // Only look at unlabeled new quads\n                if (candidateQuad->labeled)\n                {\n                    continue;\n                }\n\n                for (int j = 0; j < 4; ++j)\n                {\n                    // Only proceed if they are less than dist away from each\n                    // other\n                    cv::Point2f dp = pt - candidateQuad->corners[j]->pt;\n                    float dist = dp.dot(dp);\n\n                    if ((dist < minDist) &&\n                        dist <= (curQuad->edge_len + thresh_dilation) &&\n                        dist <= (candidateQuad->edge_len + thresh_dilation))\n                    {\n                        if (matchCorners(curQuad, i, candidateQuad, j))\n                        {\n                            closestCornerIdx = j;\n                            closestQuad = candidateQuad;\n                            minDist = dist;\n                        }\n                    }\n                }\n            }\n\n            // Have we found a matching corner point?\n            if (closestCornerIdx >= 0 && minDist < FLT_MAX)\n            {\n                ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];\n                closestCorner->pt = (pt + closestCorner->pt) * 0.5f;\n\n                // We've found one more corner - remember it\n                // ATTENTION: write the corner x and y coordinates separately,\n                // else the crucial row/column entries will be overwritten !!!\n                curQuad->corners[i]->pt = closestCorner->pt;\n                curQuad->neighbors[i] = closestQuad;\n                closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt;\n\n                // Label closest quad as labeled. In this way we exclude it\n                // being considered again during the next loop iteration\n                closestQuad->labeled = true;\n\n                // We have a new member of the final pattern, copy it over\n                ChessboardQuadPtr newQuad(new ChessboardQuad);\n                newQuad->count        = 1;\n                newQuad->edge_len    = closestQuad->edge_len;\n                newQuad->group_idx    = curQuad->group_idx;    //the same as the current quad\n                newQuad->labeled    = false;                //do it right afterwards\n\n                curQuad->neighbors[i] = newQuad;\n\n                // We only know one neighbor for sure\n                newQuad->neighbors[closestCornerIdx] = curQuad;\n\n                for (int j = 0; j < 4; j++)\n                {\n                    newQuad->corners[j].reset(new ChessboardCorner);\n                    newQuad->corners[j]->pt = closestQuad->corners[j]->pt;\n                }\n\n                existingQuads.push_back(newQuad);\n\n                // Start the function again\n                return -1;\n            }\n        }\n    }\n\n    // Finished, don't start the function again\n    return 1;\n}\n\n\n\n//===========================================================================\n// GENERATE QUADRANGLES\n//===========================================================================\nvoid\nChessboard::generateQuads(std::vector<ChessboardQuadPtr>& quads,\n                          cv::Mat& image, int flags,\n                          int dilation, bool firstRun)\n{\n    // Empirical lower bound for the size of allowable quadrangles.\n    // MARTIN, modified: Added \"*0.1\" in order to find smaller quads.\n    int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1);\n\n    std::vector< std::vector<cv::Point> > contours;\n    std::vector<cv::Vec4i> hierarchy;\n\n    // Initialize contour retrieving routine\n    cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);\n\n    std::vector< std::vector<cv::Point> > quadContours;\n\n    for (size_t i = 0; i < contours.size(); ++i)\n    {\n        // Reject contours with a too small perimeter and contours which are\n        // completely surrounded by another contour\n        // MARTIN: If this function is called during PART 1, then the parameter \"first run\"\n        // is set to \"true\". This guarantees, that only \"nice\" squares are detected.\n        // During PART 2, we allow the polygonal approximation function below to\n        // approximate more freely, which can result in recognized \"squares\" that are in\n        // reality multiple blurred and sticked together squares.\n        std::vector<cv::Point>& contour = contours.at(i);\n\n        if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize)\n        {\n            continue;\n        }\n\n        int min_approx_level = 1, max_approx_level;\n        if (firstRun)\n        {\n            max_approx_level = 3;\n        }\n        else\n        {\n            max_approx_level = MAX_CONTOUR_APPROX;\n        }\n\n        std::vector<cv::Point> approxContour;\n        for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++)\n        {\n            cv::approxPolyDP(contour, approxContour, approx_level, true);\n\n            if (approxContour.size() == 4)\n            {\n                break;\n            }\n        }\n\n        // Reject non-quadrangles\n        if (approxContour.size() == 4 && cv::isContourConvex(approxContour))\n        {\n            double p = cv::arcLength(approxContour, true);\n            double area = cv::contourArea(approxContour);\n\n            cv::Point pt[4];\n            for (int i = 0; i < 4; i++)\n            {\n                pt[i] = approxContour[i];\n            }\n\n            cv::Point dp = pt[0] - pt[2];\n            double d1 = sqrt(dp.dot(dp));\n\n            dp = pt[1] - pt[3];\n            double d2 = sqrt(dp.dot(dp));\n\n            // PHILIPG: Only accept those quadrangles which are more\n            // square than rectangular and which are big enough\n            dp = pt[0] - pt[1];\n            double d3 = sqrt(dp.dot(dp));\n            dp = pt[1] - pt[2];\n            double d4 = sqrt(dp.dot(dp));\n\n            if (!(flags & CV_CALIB_CB_FILTER_QUADS) ||\n                (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize &&\n                d1 >= 0.15 * p && d2 >= 0.15 * p))\n            {\n                quadContours.push_back(approxContour);\n            }\n        }\n    }\n\n    // Allocate quad & corner buffers\n    quads.resize(quadContours.size());\n\n    // Create array of quads structures\n    for (size_t idx = 0; idx < quadContours.size(); ++idx)\n    {\n        ChessboardQuadPtr& q = quads.at(idx);\n        std::vector<cv::Point>& contour = quadContours.at(idx);\n\n        // Reset group ID\n        q.reset(new ChessboardQuad);\n        assert(contour.size() == 4);\n\n        for (int i = 0; i < 4; ++i)\n        {\n            cv::Point2f pt = contour.at(i);\n            ChessboardCornerPtr corner(new ChessboardCorner);\n            corner->pt = pt;\n            q->corners[i] = corner;\n        }\n\n        for (int i = 0; i < 4; ++i)\n        {\n            cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt;\n            float d = dp.dot(dp);\n            if (q->edge_len > d)\n            {\n                q->edge_len = d;\n            }\n        }\n    }\n}\n\nbool\nChessboard::checkQuadGroup(std::vector<ChessboardQuadPtr>& quads,\n                           std::vector<ChessboardCornerPtr>& corners,\n                           cv::Size patternSize)\n{\n    // Initialize\n    bool flagRow = false;\n    bool flagColumn = false;\n    int height = -1;\n    int width = -1;\n\n    // Compute the minimum and maximum row / column ID\n    // (it is unlikely that more than 8bit checkers are used per dimension)\n    int min_row    =  127;\n    int max_row    = -127;\n    int min_col    =  127;\n    int max_col    = -127;\n\n    for (size_t i = 0; i < quads.size(); ++i)\n    {\n        ChessboardQuadPtr& q = quads.at(i);\n\n        for (int j = 0; j < 4; ++j)\n        {\n            ChessboardCornerPtr& c = q->corners[j];\n\n            if (c->row > max_row)\n            {\n                max_row = c->row;\n            }\n            if (c->row < min_row)\n            {\n                min_row = c->row;\n            }\n            if (c->column > max_col)\n            {\n                max_col = c->column;\n            }\n            if (c->column < min_col)\n            {\n                min_col = c->column;\n            }\n        }\n    }\n\n    // If in a given direction the target pattern size is reached, we know exactly how\n    // the checkerboard is oriented.\n    // Else we need to prepare enough \"dummy\" corners for the worst case.\n    for (size_t i = 0; i < quads.size(); ++i)\n    {\n        ChessboardQuadPtr& q = quads.at(i);\n\n        for (int j = 0; j < 4; ++j)\n        {\n            ChessboardCornerPtr& c = q->corners[j];\n\n            if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor)\n            {\n                flagColumn = true;\n            }\n            if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor)\n            {\n                flagRow = true;\n            }\n        }\n    }\n\n    if (flagColumn)\n    {\n        if (max_col - min_col == patternSize.width + 1)\n        {\n            width = patternSize.width;\n            height = patternSize.height;\n        }\n        else\n        {\n            width = patternSize.height;\n            height = patternSize.width;\n        }\n    }\n    else if (flagRow)\n    {\n        if (max_row - min_row == patternSize.width + 1)\n        {\n            height = patternSize.width;\n            width = patternSize.height;\n        }\n        else\n        {\n            height = patternSize.height;\n            width = patternSize.width;\n        }\n    }\n    else\n    {\n        // If target pattern size is not reached in at least one of the two\n        // directions,  then we do not know where the remaining corners are\n        // located. Account for this.\n        width = std::max(patternSize.width, patternSize.height);\n        height = std::max(patternSize.width, patternSize.height);\n    }\n\n    ++min_row;\n    ++min_col;\n    max_row = min_row + height - 1;\n    max_col = min_col + width - 1;\n\n    corners.clear();\n\n    int linkedBorderCorners = 0;\n\n    // Write the corners in increasing order to the output file\n    for (int i = min_row; i <= max_row; ++i)\n    {\n        for (int j = min_col; j <= max_col; ++j)\n        {\n            // Reset the iterator\n            int iter = 1;\n\n            for (int k = 0; k < (int)quads.size(); ++k)\n            {\n                ChessboardQuadPtr& quad = quads.at(k);\n\n                for (int l = 0; l < 4; ++l)\n                {\n                    ChessboardCornerPtr& c = quad->corners[l];\n\n                    if (c->row == i && c->column == j)\n                    {\n                        bool boardEdge = false;\n                        if (i == min_row || i == max_row ||\n                            j == min_col || j == max_col)\n                        {\n                            boardEdge = true;\n                        }\n\n                        if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge))\n                        {\n                            // The respective row and column have been found\n                            corners.push_back(quad->corners[l]);\n                        }\n\n                        if (iter == 2 && boardEdge)\n                        {\n                            ++linkedBorderCorners;\n                        }\n\n                        // If the iterator is larger than two, this means that more than\n                        // two corners have the same row / column entries. Then some\n                        // linking errors must have occured and we should not use the found\n                        // pattern\n                        if (iter > 2)\n                        {\n                            return false;\n                        }\n\n                        iter++;\n                    }\n                }\n            }\n        }\n    }\n\n    if ((int)corners.size() != patternSize.width * patternSize.height ||\n        linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f)\n    {\n        return false;\n    }\n\n    // check that no corners lie at image boundary\n    float border = 5.0f;\n    for (int i = 0; i < (int)corners.size(); ++i)\n    {\n        ChessboardCornerPtr& c = corners.at(i);\n\n        if (c->pt.x < border || c->pt.x > mImage.cols - border ||\n            c->pt.y < border || c->pt.y > mImage.rows - border)\n        {\n            return false;\n        }\n    }\n\n    // check if we need to transpose the board\n    if (width != patternSize.width)\n    {\n        std::swap(width, height);\n\n        std::vector<ChessboardCornerPtr> outputCorners;\n        outputCorners.resize(corners.size());\n\n        for (int i = 0; i < height; ++i)\n        {\n            for (int j = 0; j < width; ++j)\n            {\n                outputCorners.at(i * width + j) = corners.at(j * height + i);\n            }\n        }\n\n        corners = outputCorners;\n    }\n\n    // check if we need to revert the order in each row\n    cv::Point2f p0 = corners.at(0)->pt;\n    cv::Point2f p1 = corners.at(width-1)->pt;\n    cv::Point2f p2 = corners.at(width)->pt;\n\n    if ((p1 - p0).cross(p2 - p0) < 0.0f)\n    {\n        for (int i = 0; i < height; ++i)\n        {\n            for (int j = 0; j < width / 2; ++j)\n            {\n                std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1));\n            }\n        }\n    }\n\n    p0 = corners.at(0)->pt;\n    p2 = corners.at(width)->pt;\n\n    // check if we need to rotate the board\n    if (p2.y < p0.y)\n    {\n        std::vector<ChessboardCornerPtr> outputCorners;\n        outputCorners.resize(corners.size());\n\n        for (int i = 0; i < height; ++i)\n        {\n            for (int j = 0; j < width; ++j)\n            {\n                outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1);\n            }\n        }\n\n        corners = outputCorners;\n    }\n\n    return true;\n}\n\nvoid\nChessboard::getQuadrangleHypotheses(const std::vector< std::vector<cv::Point> >& contours,\n                                    std::vector< std::pair<float, int> >& quads,\n                                    int classId) const\n{\n    const float minAspectRatio = 0.2f;\n    const float maxAspectRatio = 5.0f;\n    const float minBoxSize = 10.0f;\n\n    for (size_t i = 0; i < contours.size(); ++i)\n    {\n        cv::RotatedRect box = cv::minAreaRect(contours.at(i));\n        float boxSize = std::max(box.size.width, box.size.height);\n        if (boxSize < minBoxSize)\n        {\n            continue;\n        }\n\n        float aspectRatio = box.size.width / std::max(box.size.height, 1.0f);\n\n        if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio)\n        {\n            continue;\n        }\n\n        quads.push_back(std::pair<float, int>(boxSize, classId));\n    }\n}\n\nbool less_pred(const std::pair<float, int>& p1, const std::pair<float, int>& p2)\n{\n    return p1.first < p2.first;\n}\n\nvoid countClasses(const std::vector<std::pair<float, int> >& pairs, size_t idx1, size_t idx2, std::vector<int>& counts)\n{\n    counts.assign(2, 0);\n    for (size_t i = idx1; i != idx2; ++i)\n    {\n        counts[pairs[i].second]++;\n    }\n}\n\nbool\nChessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const\n{\n    const int erosionCount = 1;\n    const float blackLevel = 20.f;\n    const float whiteLevel = 130.f;\n    const float blackWhiteGap = 70.f;\n\n    cv::Mat white = image.clone();\n\n    cv::Mat black = image.clone();\n\n    cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount);\n    cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount);\n\n    cv::Mat thresh(image.rows, image.cols, CV_8UC1);\n\n    bool result = false;\n    for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f)\n    {\n        cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY);\n\n        std::vector< std::vector<cv::Point> > contours;\n        std::vector<cv::Vec4i> hierarchy;\n\n        // Initialize contour retrieving routine\n        cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);\n\n        std::vector<std::pair<float, int> > quads;\n        getQuadrangleHypotheses(contours, quads, 1);\n\n        cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV);\n\n        cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);\n        getQuadrangleHypotheses(contours, quads, 0);\n\n        const size_t min_quads_count = patternSize.width * patternSize.height / 2;\n        std::sort(quads.begin(), quads.end(), less_pred);\n\n        // now check if there are many hypotheses with similar sizes\n        // do this by floodfill-style algorithm\n        const float sizeRelDev = 0.4f;\n\n        for (size_t i = 0; i < quads.size(); ++i)\n        {\n            size_t j = i + 1;\n            for (; j < quads.size(); ++j)\n            {\n                if (quads[j].first / quads[i].first > 1.0f + sizeRelDev)\n                {\n                    break;\n                }\n            }\n\n            if (j + 1 > min_quads_count + i)\n            {\n                // check the number of black and white squares\n                std::vector<int> counts;\n                countClasses(quads, i, j, counts);\n                const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f));\n                const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f));\n                if (counts[0] < blackCount * 0.75f ||\n                    counts[1] < whiteCount * 0.75f)\n                {\n                    continue;\n                }\n                result = true;\n                break;\n            }\n        }\n    }\n\n    return result;\n}\n\nbool\nChessboard::checkBoardMonotony(std::vector<ChessboardCornerPtr>& corners,\n                               cv::Size patternSize)\n{\n    const float threshFactor = 0.2f;\n\n    Spline splineXY, splineYX;\n    splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC);\n    splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC);\n    splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC);\n    splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC);\n\n    // check if each row of corners approximates a cubic spline\n    for (int i = 0; i < patternSize.height; ++i)\n    {\n        splineXY.clear();\n        splineYX.clear();\n\n        cv::Point2f p[3];\n        p[0] = corners.at(i * patternSize.width)->pt;\n        p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt;\n        p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt;\n\n        for (int j = 0; j < 3; ++j)\n        {\n            splineXY.addPoint(p[j].x, p[j].y);\n            splineYX.addPoint(p[j].y, p[j].x);\n        }\n\n        for (int j = 1; j < patternSize.width - 1; ++j)\n        {\n            cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt;\n\n            float thresh = std::numeric_limits<float>::max();\n\n            // up-neighbor\n            if (i > 0)\n            {\n                cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_j));\n            }\n            // down-neighbor\n            if (i < patternSize.height - 1)\n            {\n                cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_j));\n            }\n            // left-neighbor\n            {\n                cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_j));\n            }\n            // right-neighbor\n            {\n                cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_j));\n            }\n\n            thresh *= threshFactor;\n\n            if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh)\n            {\n                return false;\n            }\n        }\n    }\n\n    // check if each column of corners approximates a cubic spline\n    for (int j = 0; j < patternSize.width; ++j)\n    {\n        splineXY.clear();\n        splineYX.clear();\n\n        cv::Point2f p[3];\n        p[0] = corners.at(j)->pt;\n        p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt;\n        p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt;\n\n        for (int i = 0; i < 3; ++i)\n        {\n            splineXY.addPoint(p[i].x, p[i].y);\n            splineYX.addPoint(p[i].y, p[i].x);\n        }\n\n        for (int i = 1; i < patternSize.height - 1; ++i)\n        {\n            cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt;\n\n            float thresh = std::numeric_limits<float>::max();\n\n            // up-neighbor\n            {\n                cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_i));\n            }\n            // down-neighbor\n            {\n                cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_i));\n            }\n            // left-neighbor\n            if (j > 0)\n            {\n                cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_i));\n            }\n            // right-neighbor\n            if (j < patternSize.width - 1)\n            {\n                cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;\n                thresh = fminf(thresh, cv::norm(neighbor - p_i));\n            }\n\n            thresh *= threshFactor;\n\n            if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh)\n            {\n                return false;\n            }\n        }\n    }\n\n    return true;\n}\n\nbool\nChessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1,\n                         ChessboardQuadPtr& quad2, int corner2) const\n{\n    // First Check everything from the viewpoint of the\n    // current quad compute midpoints of \"parallel\" quad\n    // sides 1\n    float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2;\n    float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2;\n    float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;\n    float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;\n    // compute midpoints of \"parallel\" quad sides 2\n    float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;\n    float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;\n    float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2;\n    float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2;\n\n    // MARTIN: Heuristic\n    // For corner2 of quad2 to be considered,\n    // it needs to be on the same side of the two lines as\n    // corner1. This is given, if the cross product has\n    // the same sign for both computations below:\n    float a1 = x1 - x2;\n    float b1 = y1 - y2;\n    // the current corner\n    float c11 = quad1->corners[corner1]->pt.x - x2;\n    float d11 = quad1->corners[corner1]->pt.y - y2;\n    // the candidate corner\n    float c12 = quad2->corners[corner2]->pt.x - x2;\n    float d12 = quad2->corners[corner2]->pt.y - y2;\n    float sign11 = a1*d11 - c11*b1;\n    float sign12 = a1*d12 - c12*b1;\n\n    float a2 = x3 - x4;\n    float b2 = y3 - y4;\n    // the current corner\n    float c21 = quad1->corners[corner1]->pt.x - x4;\n    float d21 = quad1->corners[corner1]->pt.y - y4;\n    // the candidate corner\n    float c22 = quad2->corners[corner2]->pt.x - x4;\n    float d22 = quad2->corners[corner2]->pt.y - y4;\n    float sign21 = a2*d21 - c21*b2;\n    float sign22 = a2*d22 - c22*b2;\n\n    // Also make shure that two border quads of the same row or\n    // column don't link. Check from the current corner's view,\n    // whether the corner diagonal from the candidate corner\n    // is also on the same side of the two lines as the current\n    // corner and the candidate corner.\n    float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2;\n    float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2;\n    float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4;\n    float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4;\n    float sign13 = a1*d13 - c13*b1;\n    float sign23 = a2*d23 - c23*b2;\n\n\n    // Second: Then check everything from the viewpoint of\n    // the candidate quad. Compute midpoints of \"parallel\"\n    // quad sides 1\n    float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2;\n    float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2;\n    float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;\n    float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;\n    // compute midpoints of \"parallel\" quad sides 2\n    float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;\n    float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;\n    float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2;\n    float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2;\n\n    // MARTIN: Heuristic\n    // For corner2 of quad2 to be considered,\n    // it needs to be on the same side of the two lines as\n    // corner1. This is given, if the cross product has\n    // the same sign for both computations below:\n    float a3 = u1 - u2;\n    float b3 = v1 - v2;\n    // the current corner\n    float c31 = quad1->corners[corner1]->pt.x - u2;\n    float d31 = quad1->corners[corner1]->pt.y - v2;\n    // the candidate corner\n    float c32 = quad2->corners[corner2]->pt.x - u2;\n    float d32 = quad2->corners[corner2]->pt.y - v2;\n    float sign31 = a3*d31-c31*b3;\n    float sign32 = a3*d32-c32*b3;\n\n    float a4 = u3 - u4;\n    float b4 = v3 - v4;\n    // the current corner\n    float c41 = quad1->corners[corner1]->pt.x - u4;\n    float d41 = quad1->corners[corner1]->pt.y - v4;\n    // the candidate corner\n    float c42 = quad2->corners[corner2]->pt.x - u4;\n    float d42 = quad2->corners[corner2]->pt.y - v4;\n    float sign41 = a4*d41-c41*b4;\n    float sign42 = a4*d42-c42*b4;\n\n    // Also make sure that two border quads of the same row or\n    // column don't link. Check from the candidate corner's view,\n    // whether the corner diagonal from the current corner\n    // is also on the same side of the two lines as the current\n    // corner and the candidate corner.\n    float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2;\n    float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2;\n    float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4;\n    float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4;\n    float sign33 = a3*d33-c33*b3;\n    float sign43 = a4*d43-c43*b4;\n\n\n    // This time we also need to make shure, that no quad\n    // is linked to a quad of another dilation run which\n    // may lie INSIDE it!!!\n    // Third: Therefore check everything from the viewpoint\n    // of the current quad compute midpoints of \"parallel\"\n    // quad sides 1\n    float x5 = quad1->corners[corner1]->pt.x;\n    float y5 = quad1->corners[corner1]->pt.y;\n    float x6 = quad1->corners[(corner1+1)%4]->pt.x;\n    float y6 = quad1->corners[(corner1+1)%4]->pt.y;\n    // compute midpoints of \"parallel\" quad sides 2\n    float x7 = x5;\n    float y7 = y5;\n    float x8 = quad1->corners[(corner1+3)%4]->pt.x;\n    float y8 = quad1->corners[(corner1+3)%4]->pt.y;\n\n    // MARTIN: Heuristic\n    // For corner2 of quad2 to be considered,\n    // it needs to be on the other side of the two lines than\n    // corner1. This is given, if the cross product has\n    // a different sign for both computations below:\n    float a5 = x6 - x5;\n    float b5 = y6 - y5;\n    // the current corner\n    float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5;\n    float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5;\n    // the candidate corner\n    float c52 = quad2->corners[corner2]->pt.x - x5;\n    float d52 = quad2->corners[corner2]->pt.y - y5;\n    float sign51 = a5*d51 - c51*b5;\n    float sign52 = a5*d52 - c52*b5;\n\n    float a6 = x8 - x7;\n    float b6 = y8 - y7;\n    // the current corner\n    float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7;\n    float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7;\n    // the candidate corner\n    float c62 = quad2->corners[corner2]->pt.x - x7;\n    float d62 = quad2->corners[corner2]->pt.y - y7;\n    float sign61 = a6*d61 - c61*b6;\n    float sign62 = a6*d62 - c62*b6;\n\n\n    // Fourth: Then check everything from the viewpoint of\n    // the candidate quad compute midpoints of \"parallel\"\n    // quad sides 1\n    float u5 = quad2->corners[corner2]->pt.x;\n    float v5 = quad2->corners[corner2]->pt.y;\n    float u6 = quad2->corners[(corner2+1)%4]->pt.x;\n    float v6 = quad2->corners[(corner2+1)%4]->pt.y;\n    // compute midpoints of \"parallel\" quad sides 2\n    float u7 = u5;\n    float v7 = v5;\n    float u8 = quad2->corners[(corner2+3)%4]->pt.x;\n    float v8 = quad2->corners[(corner2+3)%4]->pt.y;\n\n    // MARTIN: Heuristic\n    // For corner2 of quad2 to be considered,\n    // it needs to be on the other side of the two lines than\n    // corner1. This is given, if the cross product has\n    // a different sign for both computations below:\n    float a7 = u6 - u5;\n    float b7 = v6 - v5;\n    // the current corner\n    float c71 = quad1->corners[corner1]->pt.x - u5;\n    float d71 = quad1->corners[corner1]->pt.y - v5;\n    // the candidate corner\n    float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5;\n    float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5;\n    float sign71 = a7*d71-c71*b7;\n    float sign72 = a7*d72-c72*b7;\n\n    float a8 = u8 - u7;\n    float b8 = v8 - v7;\n    // the current corner\n    float c81 = quad1->corners[corner1]->pt.x - u7;\n    float d81 = quad1->corners[corner1]->pt.y - v7;\n    // the candidate corner\n    float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7;\n    float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7;\n    float sign81 = a8*d81-c81*b8;\n    float sign82 = a8*d82-c82*b8;\n\n    // Check whether conditions are fulfilled\n    if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0))  &&\n        ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0))  &&\n        ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0))  &&\n        ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0))  &&\n        ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0))  &&\n        ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0))  &&\n        ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0))  &&\n        ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0))  &&\n        ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0))  &&\n        ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0))  &&\n        ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0))  &&\n        ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0)))\n    {\n        return true;\n    }\n    else\n    {\n        return false;\n    }\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/gpl/EigenQuaternionParameterization.cc",
    "content": "#include \"camodocal/gpl/EigenQuaternionParameterization.h\"\n\n#include <cmath>\n\nnamespace camodocal\n{\n\nbool\nEigenQuaternionParameterization::Plus(const double* x,\n                                      const double* delta,\n                                      double* x_plus_delta) const\n{\n    const double norm_delta =\n        sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);\n    if (norm_delta > 0.0)\n    {\n        const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);\n        double q_delta[4];\n        q_delta[0] = sin_delta_by_delta * delta[0];\n        q_delta[1] = sin_delta_by_delta * delta[1];\n        q_delta[2] = sin_delta_by_delta * delta[2];\n        q_delta[3] = cos(norm_delta);\n        EigenQuaternionProduct(q_delta, x, x_plus_delta);\n    }\n    else\n    {\n        for (int i = 0; i < 4; ++i)\n        {\n            x_plus_delta[i] = x[i];\n        }\n    }\n    return true;\n}\n\nbool\nEigenQuaternionParameterization::ComputeJacobian(const double* x,\n                                                 double* jacobian) const\n{\n    jacobian[0] =  x[3]; jacobian[1]  =  x[2]; jacobian[2]  = -x[1];  // NOLINT\n    jacobian[3] = -x[2]; jacobian[4]  =  x[3]; jacobian[5]  =  x[0];  // NOLINT\n    jacobian[6] =  x[1]; jacobian[7] = -x[0]; jacobian[8] =  x[3];  // NOLINT\n    jacobian[9] = -x[0]; jacobian[10]  = -x[1]; jacobian[11]  = -x[2];  // NOLINT\n    return true;\n}\n\n}\n"
  },
  {
    "path": "camera_model/src/gpl/gpl.cc",
    "content": "#include \"camodocal/gpl/gpl.h\"\r\n\r\n#include <set>\r\n#ifdef _WIN32\r\n#include <winsock.h>\r\n#else\r\n#include <time.h>\r\n#endif\r\n\r\n\r\n// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x\r\n#ifdef __APPLE__\r\n#include <mach/mach_time.h>\r\n#define ORWL_NANO (+1.0E-9)\r\n#define ORWL_GIGA UINT64_C(1000000000)\r\n\r\nstatic double orwl_timebase = 0.0;\r\nstatic uint64_t orwl_timestart = 0;\r\n\r\nstruct timespec orwl_gettime(void) {\r\n  // be more careful in a multithreaded environement\r\n  if (!orwl_timestart) {\r\n    mach_timebase_info_data_t tb = { 0 };\r\n    mach_timebase_info(&tb);\r\n    orwl_timebase = tb.numer;\r\n    orwl_timebase /= tb.denom;\r\n    orwl_timestart = mach_absolute_time();\r\n  }\r\n  struct timespec t;\r\n  double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase;\r\n  t.tv_sec = diff * ORWL_NANO;\r\n  t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA);\r\n  return t;\r\n}\r\n#endif // __APPLE__\r\n\r\n\r\nconst double WGS84_A = 6378137.0;\r\nconst double WGS84_ECCSQ = 0.00669437999013;\r\n\r\n// Windows lacks fminf\r\n#ifndef fminf\r\n#define fminf(x, y) (((x) < (y)) ? (x) : (y))\r\n#endif\r\n\r\nnamespace camodocal\r\n{\r\n\r\ndouble hypot3(double x, double y, double z)\r\n{\r\n\treturn sqrt(square(x) + square(y) + square(z));\r\n}\r\n\r\nfloat hypot3f(float x, float y, float z)\r\n{\r\n\treturn sqrtf(square(x) + square(y) + square(z));\r\n}\r\n\r\ndouble d2r(double deg)\r\n{\r\n\treturn deg / 180.0 * M_PI;\r\n}\r\n\r\nfloat d2r(float deg)\r\n{\r\n\treturn deg / 180.0f * M_PI;\r\n}\r\n\r\ndouble r2d(double rad)\r\n{\r\n\treturn rad / M_PI * 180.0;\r\n}\r\n\r\nfloat r2d(float rad)\r\n{\r\n\treturn rad / M_PI * 180.0f;\r\n}\r\n\r\ndouble sinc(double theta)\r\n{\r\n    return sin(theta) / theta;\r\n}\r\n\r\n#ifdef _WIN32\r\n#include <sys/timeb.h>\r\n#include <sys/types.h>\r\n#include <winsock.h>\r\nLARGE_INTEGER\r\ngetFILETIMEoffset()\r\n{\r\n    SYSTEMTIME s;\r\n    FILETIME f;\r\n    LARGE_INTEGER t;\r\n\r\n    s.wYear = 1970;\r\n    s.wMonth = 1;\r\n    s.wDay = 1;\r\n    s.wHour = 0;\r\n    s.wMinute = 0;\r\n    s.wSecond = 0;\r\n    s.wMilliseconds = 0;\r\n    SystemTimeToFileTime(&s, &f);\r\n    t.QuadPart = f.dwHighDateTime;\r\n    t.QuadPart <<= 32;\r\n    t.QuadPart |= f.dwLowDateTime;\r\n    return (t);\r\n}\r\n\r\nint\r\nclock_gettime(int X, struct timespec *tp)\r\n{\r\n    LARGE_INTEGER           t;\r\n    FILETIME            f;\r\n    double                  microseconds;\r\n    static LARGE_INTEGER    offset;\r\n    static double           frequencyToMicroseconds;\r\n    static int              initialized = 0;\r\n    static BOOL             usePerformanceCounter = 0;\r\n\r\n    if (!initialized) {\r\n        LARGE_INTEGER performanceFrequency;\r\n        initialized = 1;\r\n        usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency);\r\n        if (usePerformanceCounter) {\r\n            QueryPerformanceCounter(&offset);\r\n            frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.;\r\n        } else {\r\n            offset = getFILETIMEoffset();\r\n            frequencyToMicroseconds = 10.;\r\n        }\r\n    }\r\n    if (usePerformanceCounter) QueryPerformanceCounter(&t);\r\n    else {\r\n        GetSystemTimeAsFileTime(&f);\r\n        t.QuadPart = f.dwHighDateTime;\r\n        t.QuadPart <<= 32;\r\n        t.QuadPart |= f.dwLowDateTime;\r\n    }\r\n\r\n    t.QuadPart -= offset.QuadPart;\r\n    microseconds = (double)t.QuadPart / frequencyToMicroseconds;\r\n    t.QuadPart = microseconds;\r\n    tp->tv_sec = t.QuadPart / 1000000;\r\n    tp->tv_nsec = (t.QuadPart % 1000000) * 1000;\r\n    return (0);\r\n}\r\n#endif\r\n\r\nunsigned long long timeInMicroseconds(void)\r\n{\r\n\t struct timespec tp;\r\n#ifdef __APPLE__\r\n     tp = orwl_gettime();\r\n#else\r\n\t clock_gettime(CLOCK_REALTIME, &tp);\r\n#endif\r\n\r\n\t return tp.tv_sec * 1000000 + tp.tv_nsec / 1000;\r\n}\r\n\r\ndouble timeInSeconds(void)\r\n{\r\n    struct timespec tp;\r\n#ifdef __APPLE__\r\n     tp = orwl_gettime();\r\n#else\r\n\t clock_gettime(CLOCK_REALTIME, &tp);\r\n#endif\r\n\r\n\t return static_cast<double>(tp.tv_sec) +\r\n\t\t\t static_cast<double>(tp.tv_nsec) / 1000000000.0;\r\n}\r\n\r\nfloat colormapAutumn[128][3] =\r\n{\r\n\t\t{1.0f,0.f,0.f},\r\n\t\t{1.0f,0.007874f,0.f},\r\n\t\t{1.0f,0.015748f,0.f},\r\n\t\t{1.0f,0.023622f,0.f},\r\n\t\t{1.0f,0.031496f,0.f},\r\n\t\t{1.0f,0.03937f,0.f},\r\n\t\t{1.0f,0.047244f,0.f},\r\n\t\t{1.0f,0.055118f,0.f},\r\n\t\t{1.0f,0.062992f,0.f},\r\n\t\t{1.0f,0.070866f,0.f},\r\n\t\t{1.0f,0.07874f,0.f},\r\n\t\t{1.0f,0.086614f,0.f},\r\n\t\t{1.0f,0.094488f,0.f},\r\n\t\t{1.0f,0.10236f,0.f},\r\n\t\t{1.0f,0.11024f,0.f},\r\n\t\t{1.0f,0.11811f,0.f},\r\n\t\t{1.0f,0.12598f,0.f},\r\n\t\t{1.0f,0.13386f,0.f},\r\n\t\t{1.0f,0.14173f,0.f},\r\n\t\t{1.0f,0.14961f,0.f},\r\n\t\t{1.0f,0.15748f,0.f},\r\n\t\t{1.0f,0.16535f,0.f},\r\n\t\t{1.0f,0.17323f,0.f},\r\n\t\t{1.0f,0.1811f,0.f},\r\n\t\t{1.0f,0.18898f,0.f},\r\n\t\t{1.0f,0.19685f,0.f},\r\n\t\t{1.0f,0.20472f,0.f},\r\n\t\t{1.0f,0.2126f,0.f},\r\n\t\t{1.0f,0.22047f,0.f},\r\n\t\t{1.0f,0.22835f,0.f},\r\n\t\t{1.0f,0.23622f,0.f},\r\n\t\t{1.0f,0.24409f,0.f},\r\n\t\t{1.0f,0.25197f,0.f},\r\n\t\t{1.0f,0.25984f,0.f},\r\n\t\t{1.0f,0.26772f,0.f},\r\n\t\t{1.0f,0.27559f,0.f},\r\n\t\t{1.0f,0.28346f,0.f},\r\n\t\t{1.0f,0.29134f,0.f},\r\n\t\t{1.0f,0.29921f,0.f},\r\n\t\t{1.0f,0.30709f,0.f},\r\n\t\t{1.0f,0.31496f,0.f},\r\n\t\t{1.0f,0.32283f,0.f},\r\n\t\t{1.0f,0.33071f,0.f},\r\n\t\t{1.0f,0.33858f,0.f},\r\n\t\t{1.0f,0.34646f,0.f},\r\n\t\t{1.0f,0.35433f,0.f},\r\n\t\t{1.0f,0.3622f,0.f},\r\n\t\t{1.0f,0.37008f,0.f},\r\n\t\t{1.0f,0.37795f,0.f},\r\n\t\t{1.0f,0.38583f,0.f},\r\n\t\t{1.0f,0.3937f,0.f},\r\n\t\t{1.0f,0.40157f,0.f},\r\n\t\t{1.0f,0.40945f,0.f},\r\n\t\t{1.0f,0.41732f,0.f},\r\n\t\t{1.0f,0.4252f,0.f},\r\n\t\t{1.0f,0.43307f,0.f},\r\n\t\t{1.0f,0.44094f,0.f},\r\n\t\t{1.0f,0.44882f,0.f},\r\n\t\t{1.0f,0.45669f,0.f},\r\n\t\t{1.0f,0.46457f,0.f},\r\n\t\t{1.0f,0.47244f,0.f},\r\n\t\t{1.0f,0.48031f,0.f},\r\n\t\t{1.0f,0.48819f,0.f},\r\n\t\t{1.0f,0.49606f,0.f},\r\n\t\t{1.0f,0.50394f,0.f},\r\n\t\t{1.0f,0.51181f,0.f},\r\n\t\t{1.0f,0.51969f,0.f},\r\n\t\t{1.0f,0.52756f,0.f},\r\n\t\t{1.0f,0.53543f,0.f},\r\n\t\t{1.0f,0.54331f,0.f},\r\n\t\t{1.0f,0.55118f,0.f},\r\n\t\t{1.0f,0.55906f,0.f},\r\n\t\t{1.0f,0.56693f,0.f},\r\n\t\t{1.0f,0.5748f,0.f},\r\n\t\t{1.0f,0.58268f,0.f},\r\n\t\t{1.0f,0.59055f,0.f},\r\n\t\t{1.0f,0.59843f,0.f},\r\n\t\t{1.0f,0.6063f,0.f},\r\n\t\t{1.0f,0.61417f,0.f},\r\n\t\t{1.0f,0.62205f,0.f},\r\n\t\t{1.0f,0.62992f,0.f},\r\n\t\t{1.0f,0.6378f,0.f},\r\n\t\t{1.0f,0.64567f,0.f},\r\n\t\t{1.0f,0.65354f,0.f},\r\n\t\t{1.0f,0.66142f,0.f},\r\n\t\t{1.0f,0.66929f,0.f},\r\n\t\t{1.0f,0.67717f,0.f},\r\n\t\t{1.0f,0.68504f,0.f},\r\n\t\t{1.0f,0.69291f,0.f},\r\n\t\t{1.0f,0.70079f,0.f},\r\n\t\t{1.0f,0.70866f,0.f},\r\n\t\t{1.0f,0.71654f,0.f},\r\n\t\t{1.0f,0.72441f,0.f},\r\n\t\t{1.0f,0.73228f,0.f},\r\n\t\t{1.0f,0.74016f,0.f},\r\n\t\t{1.0f,0.74803f,0.f},\r\n\t\t{1.0f,0.75591f,0.f},\r\n\t\t{1.0f,0.76378f,0.f},\r\n\t\t{1.0f,0.77165f,0.f},\r\n\t\t{1.0f,0.77953f,0.f},\r\n\t\t{1.0f,0.7874f,0.f},\r\n\t\t{1.0f,0.79528f,0.f},\r\n\t\t{1.0f,0.80315f,0.f},\r\n\t\t{1.0f,0.81102f,0.f},\r\n\t\t{1.0f,0.8189f,0.f},\r\n\t\t{1.0f,0.82677f,0.f},\r\n\t\t{1.0f,0.83465f,0.f},\r\n\t\t{1.0f,0.84252f,0.f},\r\n\t\t{1.0f,0.85039f,0.f},\r\n\t\t{1.0f,0.85827f,0.f},\r\n\t\t{1.0f,0.86614f,0.f},\r\n\t\t{1.0f,0.87402f,0.f},\r\n\t\t{1.0f,0.88189f,0.f},\r\n\t\t{1.0f,0.88976f,0.f},\r\n\t\t{1.0f,0.89764f,0.f},\r\n\t\t{1.0f,0.90551f,0.f},\r\n\t\t{1.0f,0.91339f,0.f},\r\n\t\t{1.0f,0.92126f,0.f},\r\n\t\t{1.0f,0.92913f,0.f},\r\n\t\t{1.0f,0.93701f,0.f},\r\n\t\t{1.0f,0.94488f,0.f},\r\n\t\t{1.0f,0.95276f,0.f},\r\n\t\t{1.0f,0.96063f,0.f},\r\n\t\t{1.0f,0.9685f,0.f},\r\n\t\t{1.0f,0.97638f,0.f},\r\n\t\t{1.0f,0.98425f,0.f},\r\n\t\t{1.0f,0.99213f,0.f},\r\n\t\t{1.0f,1.0f,0.0f}\r\n};\r\n\r\n\r\nfloat colormapJet[128][3] =\r\n{\r\n\t\t{0.0f,0.0f,0.53125f},\r\n\t\t{0.0f,0.0f,0.5625f},\r\n\t\t{0.0f,0.0f,0.59375f},\r\n\t\t{0.0f,0.0f,0.625f},\r\n\t\t{0.0f,0.0f,0.65625f},\r\n\t\t{0.0f,0.0f,0.6875f},\r\n\t\t{0.0f,0.0f,0.71875f},\r\n\t\t{0.0f,0.0f,0.75f},\r\n\t\t{0.0f,0.0f,0.78125f},\r\n\t\t{0.0f,0.0f,0.8125f},\r\n\t\t{0.0f,0.0f,0.84375f},\r\n\t\t{0.0f,0.0f,0.875f},\r\n\t\t{0.0f,0.0f,0.90625f},\r\n\t\t{0.0f,0.0f,0.9375f},\r\n\t\t{0.0f,0.0f,0.96875f},\r\n\t\t{0.0f,0.0f,1.0f},\r\n\t\t{0.0f,0.03125f,1.0f},\r\n\t\t{0.0f,0.0625f,1.0f},\r\n\t\t{0.0f,0.09375f,1.0f},\r\n\t\t{0.0f,0.125f,1.0f},\r\n\t\t{0.0f,0.15625f,1.0f},\r\n\t\t{0.0f,0.1875f,1.0f},\r\n\t\t{0.0f,0.21875f,1.0f},\r\n\t\t{0.0f,0.25f,1.0f},\r\n\t\t{0.0f,0.28125f,1.0f},\r\n\t\t{0.0f,0.3125f,1.0f},\r\n\t\t{0.0f,0.34375f,1.0f},\r\n\t\t{0.0f,0.375f,1.0f},\r\n\t\t{0.0f,0.40625f,1.0f},\r\n\t\t{0.0f,0.4375f,1.0f},\r\n\t\t{0.0f,0.46875f,1.0f},\r\n\t\t{0.0f,0.5f,1.0f},\r\n\t\t{0.0f,0.53125f,1.0f},\r\n\t\t{0.0f,0.5625f,1.0f},\r\n\t\t{0.0f,0.59375f,1.0f},\r\n\t\t{0.0f,0.625f,1.0f},\r\n\t\t{0.0f,0.65625f,1.0f},\r\n\t\t{0.0f,0.6875f,1.0f},\r\n\t\t{0.0f,0.71875f,1.0f},\r\n\t\t{0.0f,0.75f,1.0f},\r\n\t\t{0.0f,0.78125f,1.0f},\r\n\t\t{0.0f,0.8125f,1.0f},\r\n\t\t{0.0f,0.84375f,1.0f},\r\n\t\t{0.0f,0.875f,1.0f},\r\n\t\t{0.0f,0.90625f,1.0f},\r\n\t\t{0.0f,0.9375f,1.0f},\r\n\t\t{0.0f,0.96875f,1.0f},\r\n\t\t{0.0f,1.0f,1.0f},\r\n\t\t{0.03125f,1.0f,0.96875f},\r\n\t\t{0.0625f,1.0f,0.9375f},\r\n\t\t{0.09375f,1.0f,0.90625f},\r\n\t\t{0.125f,1.0f,0.875f},\r\n\t\t{0.15625f,1.0f,0.84375f},\r\n\t\t{0.1875f,1.0f,0.8125f},\r\n\t\t{0.21875f,1.0f,0.78125f},\r\n\t\t{0.25f,1.0f,0.75f},\r\n\t\t{0.28125f,1.0f,0.71875f},\r\n\t\t{0.3125f,1.0f,0.6875f},\r\n\t\t{0.34375f,1.0f,0.65625f},\r\n\t\t{0.375f,1.0f,0.625f},\r\n\t\t{0.40625f,1.0f,0.59375f},\r\n\t\t{0.4375f,1.0f,0.5625f},\r\n\t\t{0.46875f,1.0f,0.53125f},\r\n\t\t{0.5f,1.0f,0.5f},\r\n\t\t{0.53125f,1.0f,0.46875f},\r\n\t\t{0.5625f,1.0f,0.4375f},\r\n\t\t{0.59375f,1.0f,0.40625f},\r\n\t\t{0.625f,1.0f,0.375f},\r\n\t\t{0.65625f,1.0f,0.34375f},\r\n\t\t{0.6875f,1.0f,0.3125f},\r\n\t\t{0.71875f,1.0f,0.28125f},\r\n\t\t{0.75f,1.0f,0.25f},\r\n\t\t{0.78125f,1.0f,0.21875f},\r\n\t\t{0.8125f,1.0f,0.1875f},\r\n\t\t{0.84375f,1.0f,0.15625f},\r\n\t\t{0.875f,1.0f,0.125f},\r\n\t\t{0.90625f,1.0f,0.09375f},\r\n\t\t{0.9375f,1.0f,0.0625f},\r\n\t\t{0.96875f,1.0f,0.03125f},\r\n\t\t{1.0f,1.0f,0.0f},\r\n\t\t{1.0f,0.96875f,0.0f},\r\n\t\t{1.0f,0.9375f,0.0f},\r\n\t\t{1.0f,0.90625f,0.0f},\r\n\t\t{1.0f,0.875f,0.0f},\r\n\t\t{1.0f,0.84375f,0.0f},\r\n\t\t{1.0f,0.8125f,0.0f},\r\n\t\t{1.0f,0.78125f,0.0f},\r\n\t\t{1.0f,0.75f,0.0f},\r\n\t\t{1.0f,0.71875f,0.0f},\r\n\t\t{1.0f,0.6875f,0.0f},\r\n\t\t{1.0f,0.65625f,0.0f},\r\n\t\t{1.0f,0.625f,0.0f},\r\n\t\t{1.0f,0.59375f,0.0f},\r\n\t\t{1.0f,0.5625f,0.0f},\r\n\t\t{1.0f,0.53125f,0.0f},\r\n\t\t{1.0f,0.5f,0.0f},\r\n\t\t{1.0f,0.46875f,0.0f},\r\n\t\t{1.0f,0.4375f,0.0f},\r\n\t\t{1.0f,0.40625f,0.0f},\r\n\t\t{1.0f,0.375f,0.0f},\r\n\t\t{1.0f,0.34375f,0.0f},\r\n\t\t{1.0f,0.3125f,0.0f},\r\n\t\t{1.0f,0.28125f,0.0f},\r\n\t\t{1.0f,0.25f,0.0f},\r\n\t\t{1.0f,0.21875f,0.0f},\r\n\t\t{1.0f,0.1875f,0.0f},\r\n\t\t{1.0f,0.15625f,0.0f},\r\n\t\t{1.0f,0.125f,0.0f},\r\n\t\t{1.0f,0.09375f,0.0f},\r\n\t\t{1.0f,0.0625f,0.0f},\r\n\t\t{1.0f,0.03125f,0.0f},\r\n\t\t{1.0f,0.0f,0.0f},\r\n\t\t{0.96875f,0.0f,0.0f},\r\n\t\t{0.9375f,0.0f,0.0f},\r\n\t\t{0.90625f,0.0f,0.0f},\r\n\t\t{0.875f,0.0f,0.0f},\r\n\t\t{0.84375f,0.0f,0.0f},\r\n\t\t{0.8125f,0.0f,0.0f},\r\n\t\t{0.78125f,0.0f,0.0f},\r\n\t\t{0.75f,0.0f,0.0f},\r\n\t\t{0.71875f,0.0f,0.0f},\r\n\t\t{0.6875f,0.0f,0.0f},\r\n\t\t{0.65625f,0.0f,0.0f},\r\n\t\t{0.625f,0.0f,0.0f},\r\n\t\t{0.59375f,0.0f,0.0f},\r\n\t\t{0.5625f,0.0f,0.0f},\r\n\t\t{0.53125f,0.0f,0.0f},\r\n\t\t{0.5f,0.0f,0.0f}\r\n};\r\n\r\nvoid colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth,\r\n\t\t\t\t\t float minRange, float maxRange)\r\n{\r\n\timgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3);\r\n\r\n\tfor (int i = 0; i < imgColoredDepth.rows; ++i)\r\n\t{\r\n\t\tconst float* depth = imgDepth.ptr<float>(i);\r\n\t\tunsigned char* pixel = imgColoredDepth.ptr<unsigned char>(i);\r\n\t\tfor (int j = 0; j < imgColoredDepth.cols; ++j)\r\n\t\t{\r\n\t\t\tif (depth[j] != 0)\r\n\t\t\t{\r\n\t\t\t\tint idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f;\r\n\t\t\t\tidx = 127 - idx;\r\n\r\n\t\t\t\tpixel[0] = colormapJet[idx][2] * 255.0f;\r\n\t\t\t\tpixel[1] = colormapJet[idx][1] * 255.0f;\r\n\t\t\t\tpixel[2] = colormapJet[idx][0] * 255.0f;\r\n\t\t\t}\r\n\r\n\t\t\tpixel += 3;\r\n\t\t}\r\n\t}\r\n}\r\n\r\nbool colormap(const std::string& name, unsigned char idx,\r\n\t\t\t  float& r, float& g, float& b)\r\n{\r\n\tif (name.compare(\"jet\") == 0)\r\n\t{\r\n\t\tfloat* color = colormapJet[idx];\r\n\r\n\t\tr = color[0];\r\n\t\tg = color[1];\r\n\t\tb = color[2];\r\n\r\n\t\treturn true;\r\n\t}\r\n\telse if (name.compare(\"autumn\") == 0)\r\n\t{\r\n\t\tfloat* color = colormapAutumn[idx];\r\n\r\n\t\tr = color[0];\r\n\t\tg = color[1];\r\n\t\tb = color[2];\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\treturn false;\r\n}\r\n\r\nstd::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1)\r\n{\r\n\t// Bresenham's line algorithm\r\n\t// Find cells intersected by line between (x0,y0) and (x1,y1)\r\n\r\n\tstd::vector<cv::Point2i> cells;\r\n\r\n\tint dx = std::abs(x1 - x0);\r\n\tint dy = std::abs(y1 - y0);\r\n\r\n\tint sx = (x0 < x1) ? 1 : -1;\r\n\tint sy = (y0 < y1) ? 1 : -1;\r\n\r\n\tint err = dx - dy;\r\n\r\n\twhile (1)\r\n\t{\r\n\t\tcells.push_back(cv::Point2i(x0, y0));\r\n\r\n\t\tif (x0 == x1 && y0 == y1)\r\n\t\t{\r\n\t\t\tbreak;\r\n\t\t}\r\n\r\n\t\tint e2 = 2 * err;\r\n\t\tif (e2 > -dy)\r\n\t\t{\r\n\t\t\terr -= dy;\r\n\t\t\tx0 += sx;\r\n\t\t}\r\n\t\tif (e2 < dx)\r\n\t\t{\r\n\t\t\terr += dx;\r\n\t\t\ty0 += sy;\r\n\t\t}\r\n\t}\r\n\r\n\treturn cells;\r\n}\r\n\r\nstd::vector<cv::Point2i> bresCircle(int x0, int y0, int r)\r\n{\r\n\t// Bresenham's circle algorithm\r\n\t// Find cells intersected by circle with center (x0,y0) and radius r\r\n\r\n\tstd::vector< std::vector<bool> > mask(2 * r + 1);\r\n\t\r\n\tfor (int i = 0; i < 2 * r + 1; ++i)\r\n\t{\r\n\t\tmask[i].resize(2 * r + 1);\r\n\t\tfor (int j = 0; j < 2 * r + 1; ++j)\r\n\t\t{\r\n\t\t\tmask[i][j] = false;\r\n\t\t}\r\n\t}\r\n\r\n\tint f = 1 - r;\r\n\tint ddF_x = 1;\r\n\tint ddF_y = -2 * r;\r\n\tint x = 0;\r\n\tint y = r;\r\n\r\n\tstd::vector<cv::Point2i> line;\r\n\r\n\tline = bresLine(x0, y0 - r, x0, y0 + r);\r\n\tfor (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)\r\n\t{\r\n\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t}\r\n\r\n\tline = bresLine(x0 - r, y0, x0 + r, y0);\r\n\tfor (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)\r\n\t{\r\n\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t}\r\n\r\n\twhile (x < y)\r\n\t{\r\n\t\tif (f >= 0)\r\n\t\t{\r\n\t\t\ty--;\r\n\t\t\tddF_y += 2;\r\n\t\t\tf += ddF_y;\r\n\t\t}\r\n\r\n\t\tx++;\r\n\t\tddF_x += 2;\r\n\t\tf += ddF_x;\r\n\r\n\t\tline = bresLine(x0 - x, y0 + y, x0 + x, y0 + y);\r\n\t\tfor (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - x, y0 - y, x0 + x, y0 - y);\r\n\t\tfor (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - y, y0 + x, x0 + y, y0 + x);\r\n\t\tfor (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\r\n\t\tline = bresLine(x0 - y, y0 - x, x0 + y, y0 - x);\r\n\t\tfor (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)\r\n\t\t{\r\n\t\t\tmask[it->x - x0 + r][it->y - y0 + r] = true;\r\n\t\t}\r\n\t}\r\n\r\n\tstd::vector<cv::Point2i> cells;\r\n\tfor (int i = 0; i < 2 * r + 1; ++i)\r\n\t{\r\n\t\tfor (int j = 0; j < 2 * r + 1; ++j)\r\n\t\t{\r\n\t\t\tif (mask[i][j])\r\n\t\t\t{\r\n\t\t\t\tcells.push_back(cv::Point2i(i - r + x0, j - r + y0));\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn cells;\r\n}\r\n\r\nvoid\r\nfitCircle(const std::vector<cv::Point2d>& points,\r\n          double& centerX, double& centerY, double& radius)\r\n{\r\n    // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,\r\n    // IEEE Transactions on Instrumentation and Measurement, 2000\r\n    // We use the modified least squares method.\r\n    double sum_x = 0.0;\r\n    double sum_y = 0.0;\r\n    double sum_xx = 0.0;\r\n    double sum_xy = 0.0;\r\n    double sum_yy = 0.0;\r\n    double sum_xxx = 0.0;\r\n    double sum_xxy = 0.0;\r\n    double sum_xyy = 0.0;\r\n    double sum_yyy = 0.0;\r\n\r\n    int n = points.size();\r\n    for (int i = 0; i < n; ++i)\r\n    {\r\n        double x = points.at(i).x;\r\n        double y = points.at(i).y;\r\n\r\n        sum_x += x;\r\n        sum_y += y;\r\n        sum_xx += x * x;\r\n        sum_xy += x * y;\r\n        sum_yy += y * y;\r\n        sum_xxx += x * x * x;\r\n        sum_xxy += x * x * y;\r\n        sum_xyy += x * y * y;\r\n        sum_yyy += y * y * y;\r\n    }\r\n\r\n    double A = n * sum_xx - square(sum_x);\r\n    double B = n * sum_xy - sum_x * sum_y;\r\n    double C = n * sum_yy - square(sum_y);\r\n    double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);\r\n    double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);\r\n\r\n    centerX = (D * C - B * E) / (A * C - square(B));\r\n    centerY = (A * E - B * D) / (A * C - square(B));\r\n\r\n    double sum_r = 0.0;\r\n    for (int i = 0; i < n; ++i)\r\n    {\r\n        double x = points.at(i).x;\r\n        double y = points.at(i).y;\r\n\r\n        sum_r += hypot(x - centerX, y - centerY);\r\n    }\r\n\r\n    radius = sum_r / n;\r\n}\r\n\r\nstd::vector<cv::Point2d>\r\nintersectCircles(double x1, double y1, double r1,\r\n                 double x2, double y2, double r2)\r\n{\r\n    std::vector<cv::Point2d> ipts;\r\n\r\n    double d = hypot(x1 - x2, y1 - y2);\r\n    if (d > r1 + r2)\r\n    {\r\n        // circles are separate\r\n        return ipts;\r\n    }\r\n    if (d < fabs(r1 - r2))\r\n    {\r\n        // one circle is contained within the other\r\n        return ipts;\r\n    }\r\n\r\n    double a = (square(r1) - square(r2) + square(d)) / (2.0 * d);\r\n    double h = sqrt(square(r1) - square(a));\r\n\r\n    double x3 = x1 + a * (x2 - x1) / d;\r\n    double y3 = y1 + a * (y2 - y1) / d;\r\n\r\n    if (h < 1e-10)\r\n    {\r\n        // two circles touch at one point\r\n        ipts.push_back(cv::Point2d(x3, y3));\r\n        return ipts;\r\n    }\r\n\r\n    ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d,\r\n                               y3 - h * (x2 - x1) / d));\r\n    ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d,\r\n                               y3 + h * (x2 - x1) / d));\r\n    return ipts;\r\n}\r\n\r\nchar\r\nUTMLetterDesignator(double latitude)\r\n{\r\n    // This routine determines the correct UTM letter designator for the given latitude\r\n    // returns 'Z' if latitude is outside the UTM limits of 84N to 80S\r\n    // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n    char letterDesignator;\r\n\r\n    if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X';\r\n    else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W';\r\n    else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V';\r\n    else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U';\r\n    else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T';\r\n    else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S';\r\n    else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R';\r\n    else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q';\r\n    else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P';\r\n    else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N';\r\n    else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M';\r\n    else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L';\r\n    else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K';\r\n    else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J';\r\n    else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H';\r\n    else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G';\r\n    else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F';\r\n    else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E';\r\n    else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D';\r\n    else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C';\r\n    else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits\r\n\r\n    return letterDesignator;\r\n}\r\n\r\nvoid\r\nLLtoUTM(double latitude, double longitude,\r\n        double& utmNorthing, double& utmEasting, std::string& utmZone)\r\n{\r\n    // converts lat/long to UTM coords.  Equations from USGS Bulletin 1532\r\n    // East Longitudes are positive, West longitudes are negative.\r\n    // North latitudes are positive, South latitudes are negative\r\n    // Lat and Long are in decimal degrees\r\n    // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n\r\n    double k0 = 0.9996;\r\n\r\n    double LongOrigin;\r\n    double eccPrimeSquared;\r\n    double N, T, C, A, M;\r\n\r\n    double LatRad = latitude * M_PI / 180.0;\r\n    double LongRad = longitude * M_PI / 180.0;\r\n    double LongOriginRad;\r\n\r\n    int ZoneNumber = static_cast<int>((longitude + 180.0) / 6.0) + 1;\r\n\r\n    if (latitude >= 56.0 && latitude < 64.0 &&\r\n            longitude >= 3.0 && longitude < 12.0) {\r\n        ZoneNumber = 32;\r\n    }\r\n\r\n    // Special zones for Svalbard\r\n    if (latitude >= 72.0 && latitude < 84.0) {\r\n        if (     longitude >= 0.0  && longitude <  9.0) ZoneNumber = 31;\r\n        else if (longitude >= 9.0  && longitude < 21.0) ZoneNumber = 33;\r\n        else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35;\r\n        else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37;\r\n    }\r\n    LongOrigin = static_cast<double>((ZoneNumber - 1) * 6 - 180 + 3);  //+3 puts origin in middle of zone\r\n    LongOriginRad = LongOrigin * M_PI / 180.0;\r\n\r\n    // compute the UTM Zone from the latitude and longitude\r\n    std::ostringstream oss;\r\n    oss << ZoneNumber << UTMLetterDesignator(latitude);\r\n    utmZone = oss.str();\r\n\r\n    eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);\r\n\r\n    N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad));\r\n    T = tan(LatRad) * tan(LatRad);\r\n    C = eccPrimeSquared * cos(LatRad) * cos(LatRad);\r\n    A = cos(LatRad) * (LongRad - LongOriginRad);\r\n\r\n    M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0\r\n                    - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0\r\n                    - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)\r\n                   * LatRad\r\n                   - (3.0 * WGS84_ECCSQ / 8.0\r\n                      + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0\r\n                      + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)\r\n                   * sin(2.0 * LatRad)\r\n                   + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0\r\n                      + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)\r\n                   * sin(4.0 * LatRad)\r\n                   - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0)\r\n                   * sin(6.0 * LatRad));\r\n\r\n    utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0\r\n                           + (5.0 - 18.0 * T + T * T + 72.0 * C\r\n                              - 58.0 * eccPrimeSquared)\r\n                           * A * A * A * A * A / 120.0)\r\n                 + 500000.0;\r\n\r\n    utmNorthing = k0 * (M + N * tan(LatRad) *\r\n                        (A * A / 2.0 +\r\n                         (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0\r\n                         + (61.0 - 58.0 * T + T * T + 600.0 * C\r\n                            - 330.0 * eccPrimeSquared)\r\n                         * A * A * A * A * A * A / 720.0));\r\n    if (latitude < 0.0) {\r\n        utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere\r\n    }\r\n}\r\n\r\nvoid\r\nUTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone,\r\n        double& latitude, double& longitude)\r\n{\r\n    // converts UTM coords to lat/long.  Equations from USGS Bulletin 1532\r\n    // East Longitudes are positive, West longitudes are negative.\r\n    // North latitudes are positive, South latitudes are negative\r\n    // Lat and Long are in decimal degrees.\r\n    // Written by Chuck Gantz- chuck.gantz@globalstar.com\r\n\r\n    double k0 = 0.9996;\r\n    double eccPrimeSquared;\r\n    double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ));\r\n    double N1, T1, C1, R1, D, M;\r\n    double LongOrigin;\r\n    double mu, phi1, phi1Rad;\r\n    double x, y;\r\n    int ZoneNumber;\r\n    char ZoneLetter;\r\n    bool NorthernHemisphere;\r\n\r\n    x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude\r\n    y = utmNorthing;\r\n\r\n    std::istringstream iss(utmZone);\r\n    iss >> ZoneNumber >> ZoneLetter;\r\n    if ((static_cast<int>(ZoneLetter) - static_cast<int>('N')) >= 0) {\r\n        NorthernHemisphere = true;//point is in northern hemisphere\r\n    } else {\r\n        NorthernHemisphere = false;//point is in southern hemisphere\r\n        y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere\r\n    }\r\n\r\n    LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0;  //+3 puts origin in middle of zone\r\n\r\n    eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);\r\n\r\n    M = y / k0;\r\n    mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0\r\n                         - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0\r\n                         - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0));\r\n\r\n    phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu)\r\n              + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0)\r\n              * sin(4.0 * mu)\r\n              + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu);\r\n    phi1 = phi1Rad / M_PI * 180.0;\r\n\r\n    N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad));\r\n    T1 = tan(phi1Rad) * tan(phi1Rad);\r\n    C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);\r\n    R1 = WGS84_A * (1.0 - WGS84_ECCSQ) /\r\n         pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5);\r\n    D = x / (N1 * k0);\r\n\r\n    latitude = phi1Rad - (N1 * tan(phi1Rad) / R1)\r\n               * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1\r\n                                 - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0\r\n                  + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1\r\n                     - 252.0 * eccPrimeSquared - 3.0 * C1 * C1)\r\n                  * D * D * D * D * D * D / 720.0);\r\n    latitude *= 180.0 / M_PI;\r\n\r\n    longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0\r\n                 + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1\r\n                    + 8.0 * eccPrimeSquared + 24.0 * T1 * T1)\r\n                 * D * D * D * D * D / 120.0) / cos(phi1Rad);\r\n    longitude = LongOrigin + longitude / M_PI * 180.0;\r\n}\r\n\r\nlong int\r\ntimestampDiff(uint64_t t1, uint64_t t2)\r\n{\r\n    if (t2 > t1)\r\n    {\r\n        uint64_t d = t2 - t1;\r\n\r\n        if (d > std::numeric_limits<long int>::max())\r\n        {\r\n            return std::numeric_limits<long int>::max();\r\n        }\r\n        else\r\n        {\r\n            return d;\r\n        }\r\n    }\r\n    else\r\n    {\r\n        uint64_t d = t1 - t2;\r\n\r\n        if (d > std::numeric_limits<long int>::max())\r\n        {\r\n            return std::numeric_limits<long int>::min();\r\n        }\r\n        else\r\n        {\r\n            return - static_cast<long int>(d);\r\n        }\r\n    }\r\n}\r\n\r\n}\r\n"
  },
  {
    "path": "camera_model/src/intrinsic_calib.cc",
    "content": "#include <boost/algorithm/string.hpp>\n#include <boost/filesystem.hpp>\n#include <boost/program_options.hpp>\n#include <iomanip>\n#include <iostream>\n#include <algorithm>\n#include <opencv2/core/core.hpp>\n#include <opencv2/imgproc/imgproc.hpp>\n#include <opencv2/highgui/highgui.hpp>\n\n#include \"camodocal/chessboard/Chessboard.h\"\n#include \"camodocal/calib/CameraCalibration.h\"\n#include \"camodocal/gpl/gpl.h\"\n\nint main(int argc, char** argv)\n{\n    cv::Size boardSize;\n    float squareSize;\n    std::string inputDir;\n    std::string cameraModel;\n    std::string cameraName;\n    std::string prefix;\n    std::string fileExtension;\n    bool useOpenCV;\n    bool viewResults;\n    bool verbose;\n\n    //========= Handling Program options =========\n    boost::program_options::options_description desc(\"Allowed options\");\n    desc.add_options()\n        (\"help\", \"produce help message\")\n        (\"width,w\", boost::program_options::value<int>(&boardSize.width)->default_value(8), \"Number of inner corners on the chessboard pattern in x direction\")\n        (\"height,h\", boost::program_options::value<int>(&boardSize.height)->default_value(12), \"Number of inner corners on the chessboard pattern in y direction\")\n        (\"size,s\", boost::program_options::value<float>(&squareSize)->default_value(7.f), \"Size of one square in mm\")\n        (\"input,i\", boost::program_options::value<std::string>(&inputDir)->default_value(\"calibrationdata\"), \"Input directory containing chessboard images\")\n        (\"prefix,p\", boost::program_options::value<std::string>(&prefix)->default_value(\"left-\"), \"Prefix of images\")\n        (\"file-extension,e\", boost::program_options::value<std::string>(&fileExtension)->default_value(\".png\"), \"File extension of images\")\n        (\"camera-model\", boost::program_options::value<std::string>(&cameraModel)->default_value(\"mei\"), \"Camera model: kannala-brandt | mei | pinhole\")\n        (\"camera-name\", boost::program_options::value<std::string>(&cameraName)->default_value(\"camera\"), \"Name of camera\")\n        (\"opencv\", boost::program_options::bool_switch(&useOpenCV)->default_value(true), \"Use OpenCV to detect corners\")\n        (\"view-results\", boost::program_options::bool_switch(&viewResults)->default_value(false), \"View results\")\n        (\"verbose,v\", boost::program_options::bool_switch(&verbose)->default_value(true), \"Verbose output\")\n        ;\n\n    boost::program_options::positional_options_description pdesc;\n    pdesc.add(\"input\", 1);\n\n    boost::program_options::variables_map vm;\n    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);\n    boost::program_options::notify(vm);\n\n    if (vm.count(\"help\"))\n    {\n        std::cout << desc << std::endl;\n        return 1;\n    }\n\n    if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir))\n    {\n        std::cerr << \"# ERROR: Cannot find input directory \" << inputDir << \".\" << std::endl;\n        return 1;\n    }\n\n    camodocal::Camera::ModelType modelType;\n    if (boost::iequals(cameraModel, \"kannala-brandt\"))\n    {\n        modelType = camodocal::Camera::KANNALA_BRANDT;\n    }\n    else if (boost::iequals(cameraModel, \"mei\"))\n    {\n        modelType = camodocal::Camera::MEI;\n    }\n    else if (boost::iequals(cameraModel, \"pinhole\"))\n    {\n        modelType = camodocal::Camera::PINHOLE;\n    }\n    else if (boost::iequals(cameraModel, \"scaramuzza\"))\n    {\n        modelType = camodocal::Camera::SCARAMUZZA;\n    }\n    else\n    {\n        std::cerr << \"# ERROR: Unknown camera model: \" << cameraModel << std::endl;\n        return 1;\n    }\n\n    switch (modelType)\n    {\n    case camodocal::Camera::KANNALA_BRANDT:\n        std::cout << \"# INFO: Camera model: Kannala-Brandt\" << std::endl;\n        break;\n    case camodocal::Camera::MEI:\n        std::cout << \"# INFO: Camera model: Mei\" << std::endl;\n        break;\n    case camodocal::Camera::PINHOLE:\n        std::cout << \"# INFO: Camera model: Pinhole\" << std::endl;\n        break;\n    case camodocal::Camera::SCARAMUZZA:\n        std::cout << \"# INFO: Camera model: Scaramuzza-Omnidirect\" << std::endl;\n        break;\n    }\n\n    // look for images in input directory\n    std::vector<std::string> imageFilenames;\n    boost::filesystem::directory_iterator itr;\n    for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr)\n    {\n        if (!boost::filesystem::is_regular_file(itr->status()))\n        {\n            continue;\n        }\n\n        std::string filename = itr->path().filename().string();\n\n        // check if prefix matches\n        if (!prefix.empty())\n        {\n            if (filename.compare(0, prefix.length(), prefix) != 0)\n            {\n                continue;\n            }\n        }\n\n        // check if file extension matches\n        if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0)\n        {\n            continue;\n        }\n\n        imageFilenames.push_back(itr->path().string());\n\n        if (verbose)\n        {\n            std::cerr << \"# INFO: Adding \" << imageFilenames.back() << std::endl;\n        }\n    }\n\n    if (imageFilenames.empty())\n    {\n        std::cerr << \"# ERROR: No chessboard images found.\" << std::endl;\n        return 1;\n    }\n\n    if (verbose)\n    {\n        std::cerr << \"# INFO: # images: \" << imageFilenames.size() << std::endl;\n    }\n\n    std::sort(imageFilenames.begin(), imageFilenames.end());\n\n    cv::Mat image = cv::imread(imageFilenames.front(), -1);\n    const cv::Size frameSize = image.size();\n\n    camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize);\n    calibration.setVerbose(verbose);\n\n    std::vector<bool> chessboardFound(imageFilenames.size(), false);\n    for (size_t i = 0; i < imageFilenames.size(); ++i)\n    {\n        image = cv::imread(imageFilenames.at(i), -1);\n\n        camodocal::Chessboard chessboard(boardSize, image);\n\n        chessboard.findCorners(useOpenCV);\n        if (chessboard.cornersFound())\n        {\n            if (verbose)\n            {\n                std::cerr << \"# INFO: Detected chessboard in image \" << i + 1 << \", \" << imageFilenames.at(i) << std::endl;\n            }\n\n            calibration.addChessboardData(chessboard.getCorners());\n\n            cv::Mat sketch;\n            chessboard.getSketch().copyTo(sketch);\n\n            cv::imshow(\"Image\", sketch);\n            cv::waitKey(50);\n        }\n        else if (verbose)\n        {\n            std::cerr << \"# INFO: Did not detect chessboard in image \" << i + 1 << std::endl;\n        }\n        chessboardFound.at(i) = chessboard.cornersFound();\n    }\n    cv::destroyWindow(\"Image\");\n\n    if (calibration.sampleCount() < 10)\n    {\n        std::cerr << \"# ERROR: Insufficient number of detected chessboards.\" << std::endl;\n        return 1;\n    }\n\n    if (verbose)\n    {\n        std::cerr << \"# INFO: Calibrating...\" << std::endl;\n    }\n\n    double startTime = camodocal::timeInSeconds();\n\n    calibration.calibrate();\n    calibration.writeParams(cameraName + \"_camera_calib.yaml\");\n    calibration.writeChessboardData(cameraName + \"_chessboard_data.dat\");\n\n    if (verbose)\n    {\n        std::cout << \"# INFO: Calibration took a total time of \"\n                  << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime\n                  << \" sec.\\n\";\n    }\n\n    if (verbose)\n    {\n        std::cerr << \"# INFO: Wrote calibration file to \" << cameraName + \"_camera_calib.yaml\" << std::endl;\n    }\n\n    if (viewResults)\n    {\n        std::vector<cv::Mat> cbImages;\n        std::vector<std::string> cbImageFilenames;\n\n        for (size_t i = 0; i < imageFilenames.size(); ++i)\n        {\n            if (!chessboardFound.at(i))\n            {\n                continue;\n            }\n\n            cbImages.push_back(cv::imread(imageFilenames.at(i), -1));\n            cbImageFilenames.push_back(imageFilenames.at(i));\n        }\n\n        // visualize observed and reprojected points\n        calibration.drawResults(cbImages);\n\n        for (size_t i = 0; i < cbImages.size(); ++i)\n        {\n            cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20),\n                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),\n                        1, CV_AA);\n            cv::imshow(\"Image\", cbImages.at(i));\n            cv::waitKey(0);\n        }\n    }\n\n    return 0;\n}\n"
  },
  {
    "path": "camera_model/src/sparse_graph/Transform.cc",
    "content": "#include <camodocal/sparse_graph/Transform.h>\n\nnamespace camodocal\n{\n\nTransform::Transform()\n{\n    m_q.setIdentity();\n    m_t.setZero();\n}\n\nTransform::Transform(const Eigen::Matrix4d& H)\n{\n   m_q = Eigen::Quaterniond(H.block<3,3>(0,0));\n   m_t = H.block<3,1>(0,3);\n}\n\nEigen::Quaterniond&\nTransform::rotation(void)\n{\n    return m_q;\n}\n\nconst Eigen::Quaterniond&\nTransform::rotation(void) const\n{\n    return m_q;\n}\n\ndouble*\nTransform::rotationData(void)\n{\n    return m_q.coeffs().data();\n}\n\nconst double* const\nTransform::rotationData(void) const\n{\n    return m_q.coeffs().data();\n}\n\nEigen::Vector3d&\nTransform::translation(void)\n{\n    return m_t;\n}\n\nconst Eigen::Vector3d&\nTransform::translation(void) const\n{\n    return m_t;\n}\n\ndouble*\nTransform::translationData(void)\n{\n    return m_t.data();\n}\n\nconst double* const\nTransform::translationData(void) const\n{\n    return m_t.data();\n}\n\nEigen::Matrix4d\nTransform::toMatrix(void) const\n{\n    Eigen::Matrix4d H;\n    H.setIdentity();\n    H.block<3,3>(0,0) = m_q.toRotationMatrix();\n    H.block<3,1>(0,3) = m_t;\n\n    return H;\n}\n\n}\n"
  },
  {
    "path": "config/3dm/3dm_config.yaml",
    "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/imu_3dm_gx4/imu\"\nimage_topic: \"/mv_25001498/image_raw\"\noutput_path: \"/config/3dm/vins_result.csv\"         # vins outputs will be wrttento vins_folder_path + output_path\n\n# MEI is better than PINHOLE for large FOV camera\nmodel_type: MEI\ncamera_name: camera\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n   xi: 2.057e+00\ndistortion_parameters:\n   k1: 7.145e-02\n   k2: 5.059e-01\n   p1: 4.727e-05\n   p2: -5.492e-04\nprojection_parameters:\n   gamma1: 1.115e+03\n   gamma2: 1.114e+03\n   u0: 3.672e+02\n   v0: 2.385e+02\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. \nex_calib_result_path: \"/config/euroc/ex_calib_result.yaml\"  # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.                        \n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n   rows: 3\n   cols: 3\n   dt: d\n   data: [ 0, 0, -1, \n           -1, 0, 0, \n           0, 1, 0]\n\n#Translation from camera frame to imu frame, imu^T_cam\nextrinsicTranslation: !!opencv-matrix\n   rows: 3\n   cols: 1\n   dt: d\n   data: [ 0, 0, 0.02]\n\n#feature traker paprameters\n\nmax_cnt: 150            # max feature number in feature tracking\nmin_dist: 20            # min distance between two features \nfreq: 20                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image \nF_threshold: 1.0        # ransac threshold (pixel)\nshow_track: 1           # publish tracking image as topic\nequalize: 0             # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n#optimization parameters\n\nmax_solver_time: 0.035  # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 10   # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters       The more accurate parameters you provide, the better performance\nacc_n: 0.2          # accelerometer measurement noise standard deviation.\ngyr_n: 0.05         # gyroscope measurement noise standard deviation.\nacc_w: 0.002        # accelerometer bias random work noise standard deviation.\ngyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.\ng_norm: 9.805       #\n\n\n#loop closure parameters\nloop_closure: 0   #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;\n                     #also give the camera calibration file same as feature_tracker node\npattern_file: \"/support_files/brief_pattern.yml\"\nvoc_file: \"/support_files/brief_k10L6.bin\"\nmin_loop_num: 20\n"
  },
  {
    "path": "config/A3/A3_config.yaml",
    "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/djiros/imu\"\nimage_topic: \"/djiros/image\"\noutput_path: \"/config/A3/vins_result.csv\"         # vins outputs will be wrttento vins_folder_path + output_path\n\n#camera calibration \nmodel_type: MEI\ncamera_name: camera\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n   xi: 2.5826025650890592e+00\ndistortion_parameters:\n   k1: 3.9389417936108251e-01\n   k2: 2.2073276594959599e+00\n   p1: -1.9253757033376501e-03\n   p2: -6.1503758445210599e-04\nprojection_parameters:\n   gamma1: 1.3068921176079280e+03\n   gamma2: 1.3065892394640514e+03\n   u0: 3.6959543908262725e+02\n   v0: 2.1891731132428231e+02\n\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. \nex_calib_result_path: \"/config/A3/ex_calib_result.yaml\"  # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.                        \n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n   rows: 3\n   cols: 3\n   dt: d\n   data: [ 0, -1, 0,\n           1, 0, 0, \n           0, 0, 1]\n\n#Translation from camera frame to imu frame, imu^T_cam\nextrinsicTranslation: !!opencv-matrix\n   rows: 3\n   cols: 1\n   dt: d\n   data: [ 0, 0.05, 0]\n\n\n#feature traker paprameters\n\nmax_cnt: 250            # max feature number in feature tracking\nmin_dist: 30            # min distance between two features \nfreq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image \nF_threshold: 1.0        # ransac threshold (pixel)\nshow_track: 1           # publish tracking image as topic\nequalize: 1             # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04   # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8   # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters       The more accurate parameters you provide, the better performance\nacc_n: 0.2          # accelerometer measurement noise standard deviation.\ngyr_n: 0.05         # gyroscope measurement noise standard deviation.\nacc_w: 0.002        # accelerometer bias random work noise standard deviation.\ngyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.\ng_norm: 9.805         #\n\n\n#loop closure parameters\nloop_closure: 1   #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;\n                     #also give the camera calibration file same as feature_tracker node\npattern_file: \"/support_files/brief_pattern.yml\"\nvoc_file: \"/support_files/brief_k10L6.bin\"\nmin_loop_num: 30\n"
  },
  {
    "path": "config/AR_demo.rviz",
    "content": "Panels:\n  - Class: rviz/Displays\n    Help Height: 0\n    Name: Displays\n    Property Tree Widget:\n      Expanded:\n        - /Global Options1\n        - /Status1\n        - /AR_image1/Status1\n        - /raw image1/Status1\n        - /Path2/Status1\n      Splitter Ratio: 0.465116\n    Tree Height: 729\n  - Class: rviz/Selection\n    Name: Selection\n  - Class: rviz/Tool Properties\n    Expanded:\n      - /2D Pose Estimate1\n      - /2D Nav Goal1\n      - /Publish Point1\n    Name: Tool Properties\n    Splitter Ratio: 0.588679\n  - Class: rviz/Views\n    Expanded:\n      - /Current View1\n    Name: Views\n    Splitter Ratio: 0.5\n  - Class: rviz/Time\n    Experimental: false\n    Name: Time\n    SyncMode: 0\n    SyncSource: PointCloud\nVisualization Manager:\n  Class: \"\"\n  Displays:\n    - Alpha: 0.5\n      Cell Size: 1\n      Class: rviz/Grid\n      Color: 130; 130; 130\n      Enabled: true\n      Line Style:\n        Line Width: 0.03\n        Value: Lines\n      Name: Grid\n      Normal Cell Count: 0\n      Offset:\n        X: 0\n        Y: 0\n        Z: -1.2\n      Plane: XY\n      Plane Cell Count: 10\n      Reference Frame: <Fixed Frame>\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 0; 170; 0\n      Enabled: true\n      Head Diameter: 0.3\n      Head Length: 0.2\n      Length: 0.3\n      Line Style: Lines\n      Line Width: 0.01\n      Name: Path\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Style: None\n      Radius: 0.03\n      Shaft Diameter: 0.1\n      Shaft Length: 0.1\n      Topic: /vins_estimator/path\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Autocompute Intensity Bounds: false\n      Autocompute Value Bounds:\n        Max Value: 10\n        Min Value: -10\n        Value: true\n      Axis: Z\n      Channel Name: intensity\n      Class: rviz/PointCloud\n      Color: 255; 255; 255\n      Color Transformer: FlatColor\n      Decay Time: 0\n      Enabled: true\n      Invert Rainbow: true\n      Max Color: 0; 0; 0\n      Max Intensity: 4096\n      Min Color: 0; 0; 0\n      Min Intensity: 0\n      Name: PointCloud\n      Position Transformer: XYZ\n      Queue Size: 10\n      Selectable: true\n      Size (Pixels): 1\n      Size (m): 0.5\n      Style: Points\n      Topic: /vins_estimator/point_cloud\n      Unreliable: false\n      Use Fixed Frame: true\n      Use rainbow: false\n      Value: true\n    - Class: rviz/Image\n      Enabled: true\n      Image Topic: /ar_demo_node/AR_image\n      Max Value: 1\n      Median window: 5\n      Min Value: 0\n      Name: AR_image\n      Normalize Range: true\n      Queue Size: 2\n      Transport Hint: raw\n      Unreliable: false\n      Value: true\n    - Class: rviz/Image\n      Enabled: true\n      Image Topic: /mv_25001498/image_raw\n      Max Value: 1\n      Median window: 5\n      Min Value: 0\n      Name: raw image\n      Normalize Range: true\n      Queue Size: 2\n      Transport Hint: raw\n      Unreliable: false\n      Value: true\n    - Class: rviz/MarkerArray\n      Enabled: true\n      Marker Topic: /vins_estimator/camera_pose_visual\n      Name: MarkerArray\n      Namespaces:\n        CameraPoseVisualization: true\n      Queue Size: 100\n      Value: true\n    - Class: rviz/MarkerArray\n      Enabled: true\n      Marker Topic: /ar_demo_node/AR_object\n      Name: MarkerArray\n      Namespaces:\n        basic_shapes: true\n      Queue Size: 100\n      Value: true\n    - Class: rviz/Axes\n      Enabled: true\n      Length: 1\n      Name: Axes\n      Radius: 0.1\n      Reference Frame: <Fixed Frame>\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 255; 85; 255\n      Enabled: true\n      Head Diameter: 0.3\n      Head Length: 0.2\n      Length: 0.3\n      Line Style: Billboards\n      Line Width: 0.3\n      Name: Path\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Style: None\n      Radius: 0.03\n      Shaft Diameter: 0.1\n      Shaft Length: 0.1\n      Topic: /odom_translation/vins\n      Unreliable: false\n      Value: true\n  Enabled: true\n  Global Options:\n    Background Color: 0; 0; 0\n    Fixed Frame: world\n    Frame Rate: 30\n  Name: root\n  Tools:\n    - Class: rviz/MoveCamera\n    - Class: rviz/Select\n    - Class: rviz/FocusCamera\n    - Class: rviz/Measure\n    - Class: rviz/SetInitialPose\n      Topic: /initialpose\n    - Class: rviz/SetGoal\n      Topic: /move_base_simple/goal\n    - Class: rviz/PublishPoint\n      Single click: true\n      Topic: /clicked_point\n  Value: true\n  Views:\n    Current:\n      Class: rviz/Orbit\n      Distance: 33.293\n      Enable Stereo Rendering:\n        Stereo Eye Separation: 0.06\n        Stereo Focal Distance: 1\n        Swap Stereo Eyes: false\n        Value: false\n      Focal Point:\n        X: -3.42082\n        Y: -4.08465\n        Z: -8.071\n      Name: Current View\n      Near Clip Distance: 0.01\n      Pitch: 1.5698\n      Target Frame: <Fixed Frame>\n      Value: Orbit (rviz)\n      Yaw: 0.0571614\n    Saved: ~\nWindow Geometry:\n  AR_image:\n    collapsed: false\n  Displays:\n    collapsed: true\n  Height: 959\n  Hide Left Dock: false\n  Hide Right Dock: true\n  QMainWindow State: 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\n  Selection:\n    collapsed: false\n  Time:\n    collapsed: false\n  Tool Properties:\n    collapsed: false\n  Views:\n    collapsed: true\n  Width: 1572\n  X: 286\n  Y: 43\n  raw image:\n    collapsed: false\n"
  },
  {
    "path": "config/euroc/euroc_config.yaml",
    "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/imu0\"\nimage_topic: \"/cam0/image_raw\"\noutput_path: \"/src/config/euroc/vins_result.csv\"         # vins outputs will be wrttento vins_folder_path + output_path\n\n#camera calibration \nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n   k1: -2.917e-01\n   k2: 8.228e-02\n   p1: 5.333e-05\n   p2: -1.578e-04\nprojection_parameters:\n   fx: 4.616e+02\n   fy: 4.603e+02\n   cx: 3.630e+02\n   cy: 2.481e+02\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. \nex_calib_result_path: \"/src/config/euroc/ex_calib_result.yaml\"  # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.                        \n#If you choose 0 or 1, you should write down the following matrix.\n#Rotation from camera frame to imu frame, imu^R_cam\nextrinsicRotation: !!opencv-matrix\n   rows: 3\n   cols: 3\n   dt: d\n   data: [0, -1, 0, \n           1, 0, 0, \n           0, 0, 1]\n#Translation from camera frame to imu frame, imu^T_cam\nextrinsicTranslation: !!opencv-matrix\n   rows: 3\n   cols: 1\n   dt: d\n   data: [-0.02,-0.06, 0.01]\n\n#feature traker paprameters\nmax_cnt: 150            # max feature number in feature tracking\nmin_dist: 30            # min distance between two features \nfreq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image \nF_threshold: 1.0        # ransac threshold (pixel)\nshow_track: 0           # publish tracking image as topic\nequalize: 1             # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04  # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8   # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters       The more accurate parameters you provide, the better performance\nacc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2\ngyr_n: 0.02         # gyroscope measurement noise standard deviation.     #0.05\nacc_w: 0.0002         # accelerometer bias random work noise standard deviation.  #0.02\ngyr_w: 2.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5\ng_norm: 9.81007     # gravity magnitude\n\n\n#loop closure parameters\nloop_closure: 1   #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;\n                     #also give the camera calibration file same as feature_tracker node\npattern_file: \"/src/support_files/brief_pattern.yml\"\nvoc_file: \"/src/support_files/brief_k10L6.bin\"\nmin_loop_num: 25\n\nvisualLookAtX: 1\nvisualLookAtY: 0\nvisualLookAtZ: 1\n"
  },
  {
    "path": "config/euroc/euroc_config_no_extrinsic.yaml",
    "content": "%YAML:1.0\n\n#common parameters\nimu_topic: \"/imu0\"\nimage_topic: \"/cam0/image_raw\"\noutput_path: \"/config/euroc/vins_result.csv\"         # vins outputs will be wrttento vins_folder_path + output_path\n\n#camera calibration \nmodel_type: PINHOLE\ncamera_name: camera\nimage_width: 752\nimage_height: 480\ndistortion_parameters:\n   k1: -2.917e-01\n   k2: 8.228e-02\n   p1: 5.333e-05\n   p2: -1.578e-04\nprojection_parameters:\n   fx: 4.616e+02\n   fy: 4.603e+02\n   cx: 3.630e+02\n   cy: 2.481e+02\n\n# Extrinsic parameter between IMU and Camera.\nestimate_extrinsic: 2   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.\n                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.\n                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. \nex_calib_result_path: \"/config/euroc/ex_calib_result.yaml\"  # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.                        \n#If you choose 0 or 1, you should write down the following matrix.\n\n#feature traker paprameters\nmax_cnt: 150            # max feature number in feature tracking\nmin_dist: 30            # min distance between two features \nfreq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image \nF_threshold: 1.0        # ransac threshold (pixel)\nshow_track: 1           # publish tracking image as topic\nequalize: 1             # if image is too dark or light, trun on equalize to find enough features\nfisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points\n\n#optimization parameters\nmax_solver_time: 0.04  # max solver itration time (ms), to guarantee real time\nmax_num_iterations: 8   # max solver itrations, to guarantee real time\nkeyframe_parallax: 10.0 # keyframe selection threshold (pixel)\n\n#imu parameters       The more accurate parameters you provide, the better performance\nacc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2\ngyr_n: 0.02         # gyroscope measurement noise standard deviation.     #0.05\nacc_w: 0.0002         # accelerometer bias random work noise standard deviation.  #0.02\ngyr_w: 2.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5\ng_norm: 9.81007     # gravity magnitude\n\n\n#loop closure parameters\nloop_closure: 1   #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;\n                     #also give the camera calibration file same as feature_tracker node\npattern_file: \"/support_files/brief_pattern.yml\"\nvoc_file: \"/support_files/brief_k10L6.bin\"\nmin_loop_num: 25\n\n\n\n"
  },
  {
    "path": "config/euroc/ex_calib_result.yaml",
    "content": "%YAML:1.0\nextrinsicRotation: !!opencv-matrix\n   rows: 3\n   cols: 3\n   dt: d\n   data: [ 1.5942328773486114e-02, -9.9987084186029263e-01,\n       2.0351292016634716e-03, 9.9974794303804226e-01,\n       1.5972482669662336e-02, 1.5777521623200924e-02,\n       -1.5807989893762975e-02, 1.7830857962318550e-03,\n       9.9987345602359201e-01 ]\nextrinsicTranslation: !!opencv-matrix\n   rows: 3\n   cols: 1\n   dt: d\n   data: [ -1.3988442922766306e-02, -6.9759508958340238e-02,\n       -5.3892141087174272e-03 ]\n"
  },
  {
    "path": "config/euroc/vins_result.csv",
    "content": "1403636580863555328,-0.00682,0.01361,-0.10559,-0.03014,0.83605,0.03655,0.54660,0.08347,-0.00007,0.78361,\n1403636580963555584,0.00063,0.01544,-0.02783,-0.02147,0.82770,0.03682,0.55955,0.06403,0.02669,0.74688,\n1403636581063555584,0.00615,0.01809,0.04191,-0.02227,0.81983,0.03778,0.57093,0.05168,0.01699,0.65374,\n1403636581163555328,0.01111,0.01832,0.10225,-0.02462,0.81161,0.03404,0.58269,0.03790,-0.00546,0.54246,\n1403636581263555584,0.01280,0.01693,0.15032,-0.02759,0.80171,0.02989,0.59633,-0.00011,-0.01448,0.39584,\n1403636581363555328,0.01139,0.01555,0.17670,-0.02755,0.79400,0.03025,0.60654,-0.02140,-0.01516,0.08846,\n1403636581463555584,0.00908,0.01307,0.17128,-0.02164,0.79456,0.03262,0.60593,-0.02207,-0.02356,-0.26511,\n1403636581563555584,0.00549,0.00999,0.13216,-0.01908,0.80127,0.03309,0.59708,-0.00963,-0.01141,-0.53333,\n1403636581663555328,0.00425,0.00813,0.07301,-0.02201,0.80807,0.03159,0.58783,-0.00746,-0.02882,-0.63866,\n1403636581763555584,0.00224,0.00486,0.00733,-0.02349,0.81096,0.03284,0.58371,-0.02661,-0.02463,-0.66817,\n1403636581863555328,-0.00138,0.00367,-0.06040,-0.02713,0.81300,0.03475,0.58059,-0.05044,-0.00807,-0.67626,\n1403636581963555584,-0.00818,0.00224,-0.12536,-0.02876,0.81965,0.03211,0.57125,-0.05573,0.00336,-0.61788,\n1403636582063555584,-0.01213,0.00277,-0.18242,-0.02620,0.82924,0.02980,0.55748,-0.04760,0.01219,-0.48735,\n1403636582163555328,-0.01622,0.00515,-0.22037,-0.02718,0.83795,0.03162,0.54414,-0.04203,0.02805,-0.23760,\n1403636582263555584,-0.01772,0.00836,-0.22837,-0.02825,0.84310,0.03385,0.53594,0.00988,0.02488,0.10118,\n1403636582363555328,-0.01587,0.01170,-0.20023,-0.03057,0.84240,0.03443,0.53689,0.02482,0.03975,0.43828,\n1403636582463555584,-0.01297,0.01503,-0.14738,-0.03151,0.83480,0.03368,0.54862,0.04004,0.02633,0.59434,\n1403636582563555584,-0.00857,0.01659,-0.08374,-0.02784,0.82851,0.03327,0.55829,0.04105,0.00830,0.65474,\n1403636582663555328,-0.00481,0.01614,-0.01704,-0.02201,0.82558,0.03171,0.56297,0.02795,-0.00798,0.66133,\n1403636582763555584,-0.00073,0.01393,0.04884,-0.02107,0.82246,0.02766,0.56777,0.04748,-0.03126,0.64921,\n1403636582863555328,0.00401,0.00921,0.11337,-0.01812,0.81602,0.01894,0.57742,0.03672,-0.04482,0.62942,\n1403636582963555584,0.00621,0.00464,0.16994,-0.01667,0.80631,0.01348,0.59110,0.00175,-0.03304,0.48289,\n1403636583063555584,0.00546,0.00173,0.20403,-0.01539,0.79817,0.01292,0.60210,-0.01204,-0.01868,0.19227,\n1403636583163555328,0.00458,-0.00081,0.20675,-0.01137,0.79670,0.01378,0.60411,-0.01422,-0.01915,-0.15242,\n1403636583263555584,0.00508,-0.00164,0.17817,-0.01010,0.79854,0.01637,0.60164,0.00180,-0.00825,-0.42100,\n1403636583363555328,0.00693,-0.00265,0.12937,-0.01123,0.80119,0.01817,0.59803,0.00130,-0.02156,-0.59314,\n1403636583463555584,0.00645,-0.00530,0.06614,-0.01431,0.80447,0.02210,0.59342,-0.01723,-0.01765,-0.67679,\n1403636583563555584,0.00484,-0.00848,-0.00345,-0.01442,0.80886,0.02554,0.58727,-0.01692,-0.02377,-0.70251,\n1403636583663555328,0.00311,-0.00947,-0.07431,-0.01476,0.81695,0.02628,0.57591,-0.04006,0.00836,-0.68957,\n1403636583763555584,-0.00078,-0.00980,-0.13802,-0.02199,0.82965,0.02314,0.55737,-0.04540,-0.00183,-0.58660,\n1403636583863555328,-0.00448,-0.00961,-0.19031,-0.02375,0.83956,0.02172,0.54232,-0.04907,-0.00471,-0.46805,\n1403636583963555584,-0.00885,-0.00900,-0.22914,-0.02092,0.84388,0.02786,0.53540,-0.05500,0.01272,-0.28167,\n1403636584063555584,-0.01361,-0.00641,-0.24448,-0.02514,0.84441,0.03415,0.53401,-0.02981,0.02875,-0.02367,\n1403636584163555328,-0.01307,-0.00382,-0.23146,-0.02957,0.84435,0.03741,0.53366,0.02839,0.01821,0.27895,\n1403636584263555584,-0.00937,-0.00188,-0.19180,-0.02999,0.84341,0.03856,0.53505,0.04755,0.01772,0.49891,\n1403636584363555328,-0.00327,-0.00054,-0.13544,-0.02825,0.84128,0.03930,0.53842,0.08527,-0.00339,0.63773,\n1403636584463555584,0.00570,-0.00134,-0.06794,-0.01914,0.83835,0.03999,0.54332,0.09626,-0.00778,0.70873,\n1403636584563555584,0.01455,-0.00120,0.00492,-0.01673,0.83187,0.03785,0.55343,0.09200,0.00124,0.74241,\n1403636584663555328,0.02252,-0.00084,0.07846,-0.01370,0.82133,0.03199,0.56940,0.06049,0.00450,0.71021,\n1403636584763555584,0.02661,-0.00030,0.14443,-0.00954,0.80942,0.02684,0.58654,0.02434,0.00644,0.60608,\n1403636584863555328,0.02660,0.00043,0.19703,-0.00966,0.80061,0.02284,0.59868,-0.01410,0.01794,0.43076,\n1403636584963555584,0.02523,0.00151,0.22624,-0.01281,0.79647,0.01868,0.60425,-0.00257,0.00904,0.14903,\n1403636585063555584,0.02616,0.00122,0.22568,-0.01164,0.79779,0.01475,0.60264,0.01033,-0.00187,-0.14730,\n1403636585163555328,0.02695,0.00066,0.20140,-0.01014,0.80085,0.01266,0.59865,0.01872,-0.01485,-0.37597,\n1403636585263555584,0.02864,-0.00284,0.15758,-0.00327,0.80455,0.01256,0.59374,0.02415,-0.03724,-0.49994,\n1403636585363555328,0.02988,-0.00751,0.10519,0.00403,0.80779,0.01678,0.58921,0.00758,-0.04032,-0.54607,\n1403636585463555584,0.02981,-0.01061,0.05146,0.00427,0.81249,0.02212,0.58254,-0.03808,-0.02216,-0.53485,\n1403636585563555584,0.02496,-0.01199,0.00102,-0.00124,0.81883,0.02935,0.57328,-0.10143,-0.01583,-0.45602,\n1403636585663555328,0.01371,-0.01370,-0.03851,-0.01312,0.82613,0.03561,0.56221,-0.12840,-0.00625,-0.30761,\n1403636585763555584,0.00059,-0.01433,-0.06270,-0.02456,0.83456,0.03742,0.54910,-0.11288,-0.00365,-0.15807,\n1403636585863555328,-0.01119,-0.01407,-0.07323,-0.03499,0.84394,0.03648,0.53406,-0.10343,0.00023,-0.03933,\n1403636585963555584,-0.02096,-0.01383,-0.07419,-0.03917,0.85465,0.03402,0.51660,-0.07020,-0.00846,0.00663,\n1403636586063555584,-0.02792,-0.01432,-0.07235,-0.03164,0.86754,0.02674,0.49563,-0.05282,-0.00646,0.03034,\n1403636586163555328,-0.03457,-0.01190,-0.06826,-0.02351,0.87785,0.01733,0.47805,-0.06373,0.03524,0.05472,\n1403636586263555584,-0.04310,-0.00816,-0.06367,-0.02241,0.88658,0.01297,0.46186,-0.08928,0.02216,0.03913,\n1403636586363555328,-0.05323,-0.00721,-0.06067,-0.02313,0.89605,0.00884,0.44326,-0.10886,-0.00328,0.02318,\n1403636586463555584,-0.06517,-0.00872,-0.05883,-0.02388,0.90491,0.00698,0.42487,-0.11631,-0.03065,0.02336,\n1403636586563555584,-0.07493,-0.01369,-0.05573,-0.02413,0.91132,0.00988,0.41088,-0.07317,-0.06445,0.04026,\n1403636586663555328,-0.07811,-0.02196,-0.05045,-0.02568,0.91324,0.01098,0.40646,-0.00491,-0.09759,0.05111,\n1403636586763555584,-0.07581,-0.03102,-0.04673,-0.02967,0.91068,0.01224,0.41186,0.03783,-0.08282,0.02462,\n1403636586863555328,-0.07068,-0.03886,-0.04569,-0.03244,0.90736,0.01465,0.41883,0.04341,-0.04604,-0.00889,\n1403636586963555584,-0.06558,-0.04120,-0.04663,-0.03432,0.90516,0.01977,0.42321,0.05356,-0.01656,-0.00643,\n1403636587063555584,-0.05950,-0.04229,-0.04677,-0.03563,0.90496,0.02578,0.42322,0.05596,-0.01597,0.00264,\n1403636587163555328,-0.05341,-0.04581,-0.04670,-0.03508,0.90704,0.03001,0.41851,0.06335,-0.06018,-0.00411,\n1403636587263555584,-0.04627,-0.05463,-0.04767,-0.03593,0.90814,0.03038,0.41601,0.07638,-0.12152,-0.00936,\n1403636587363555328,-0.03736,-0.07088,-0.04863,-0.03581,0.90890,0.02802,0.41453,0.09566,-0.19419,-0.00491,\n1403636587463555584,-0.02808,-0.09346,-0.04964,-0.03621,0.90985,0.02434,0.41264,0.08926,-0.26001,-0.00517,\n1403636587563555584,-0.01936,-0.12289,-0.05033,-0.03725,0.91064,0.02028,0.41101,0.08643,-0.32509,-0.00006,\n1403636587663555328,-0.00990,-0.15754,-0.04991,-0.03641,0.90967,0.01470,0.41346,0.09966,-0.36416,0.01376,\n1403636587763555584,0.00005,-0.19409,-0.04795,-0.03462,0.90688,0.00668,0.41991,0.09540,-0.37206,0.01780,\n1403636587863555328,0.00942,-0.23144,-0.04776,-0.03139,0.90547,-0.00087,0.42325,0.07767,-0.36062,-0.01786,\n1403636587963555584,0.01604,-0.26618,-0.05105,-0.02592,0.90765,-0.00704,0.41887,0.05261,-0.33472,-0.04167,\n1403636588063555584,0.02054,-0.29684,-0.05572,-0.01923,0.91195,-0.01385,0.40962,0.04259,-0.26643,-0.05225,\n1403636588163555328,0.02460,-0.32010,-0.06119,-0.01817,0.91412,-0.01539,0.40474,0.03465,-0.20117,-0.04792,\n1403636588263555584,0.02810,-0.33634,-0.06610,-0.01968,0.91389,-0.01481,0.40522,0.02442,-0.11077,-0.03777,\n1403636588363555328,0.02974,-0.34342,-0.06920,-0.02331,0.91240,-0.01432,0.40839,0.00077,-0.02755,-0.02192,\n1403636588463555584,0.02875,-0.34269,-0.07078,-0.02514,0.91154,-0.01558,0.41015,-0.02461,0.04398,-0.00377,\n1403636588563555584,0.02509,-0.33341,-0.06879,-0.02194,0.91176,-0.01567,0.40983,-0.04221,0.12909,0.03743,\n1403636588663555328,0.01995,-0.31644,-0.06463,-0.02282,0.91146,-0.01479,0.41050,-0.06892,0.20966,0.03432,\n1403636588763555584,0.01177,-0.29202,-0.06275,-0.02619,0.91225,-0.01150,0.40864,-0.08583,0.26853,0.00885,\n1403636588863555328,0.00363,-0.26182,-0.06224,-0.02952,0.91326,-0.00814,0.40623,-0.08470,0.34110,0.00966,\n1403636588963555584,-0.00566,-0.22510,-0.05993,-0.03354,0.91286,-0.00406,0.40688,-0.09694,0.38578,0.03623,\n1403636589063555584,-0.01669,-0.18430,-0.05505,-0.03735,0.91090,0.00140,0.41093,-0.11290,0.41538,0.05490,\n1403636589163555328,-0.02731,-0.14102,-0.04881,-0.04018,0.90859,0.00696,0.41569,-0.10618,0.44169,0.06434,\n1403636589263555584,-0.03758,-0.09523,-0.04298,-0.04280,0.90801,0.01107,0.41660,-0.10848,0.46164,0.04558,\n1403636589363555328,-0.04830,-0.04811,-0.04024,-0.04517,0.90932,0.01696,0.41329,-0.10462,0.47017,0.01421,\n1403636589463555584,-0.05835,-0.00121,-0.03905,-0.04548,0.91011,0.02256,0.41125,-0.10205,0.46503,0.00706,\n1403636589563555584,-0.06890,0.04647,-0.03888,-0.04755,0.90905,0.02509,0.41320,-0.10730,0.46799,-0.00518,\n1403636589663555328,-0.07917,0.09256,-0.03928,-0.04874,0.90778,0.02848,0.41563,-0.10641,0.45580,-0.00837,\n1403636589763555584,-0.08919,0.13692,-0.04085,-0.05218,0.90743,0.03226,0.41569,-0.09978,0.43413,-0.03160,\n1403636589863555328,-0.09836,0.17811,-0.04453,-0.05713,0.90927,0.03858,0.41045,-0.08170,0.36937,-0.04216,\n1403636589963555584,-0.10596,0.21260,-0.04879,-0.06392,0.91086,0.04442,0.40530,-0.06691,0.31370,-0.04101,\n1403636590063555584,-0.11237,0.24052,-0.05032,-0.07020,0.90912,0.04926,0.40760,-0.04657,0.22390,0.00185,\n1403636590163555328,-0.11681,0.25720,-0.04898,-0.07547,0.90579,0.05271,0.41360,-0.02865,0.10738,0.01516,\n1403636590263555584,-0.11960,0.26160,-0.04673,-0.07605,0.90444,0.05478,0.41617,-0.01896,-0.02379,0.01853,\n1403636590363555328,-0.12084,0.25273,-0.04436,-0.07261,0.90548,0.05460,0.41455,-0.00460,-0.14818,0.02077,\n1403636590463555584,-0.12057,0.23120,-0.04168,-0.06706,0.90606,0.05186,0.41457,0.01344,-0.26474,0.03520,\n1403636590563555584,-0.11870,0.20039,-0.03788,-0.06078,0.90529,0.04906,0.41755,0.03013,-0.34299,0.04058,\n1403636590663555328,-0.11484,0.16275,-0.03487,-0.05607,0.90460,0.04567,0.42007,0.05054,-0.39332,0.02048,\n1403636590763555584,-0.10920,0.12076,-0.03437,-0.05265,0.90566,0.04106,0.41871,0.07106,-0.42412,-0.00980,\n1403636590863555328,-0.10128,0.07649,-0.03679,-0.05307,0.90829,0.03661,0.41335,0.08818,-0.45671,-0.02931,\n1403636590963555584,-0.09170,0.02951,-0.03991,-0.05163,0.91005,0.03377,0.40988,0.09851,-0.47209,-0.03359,\n1403636591063555584,-0.08079,-0.01612,-0.04334,-0.05303,0.90964,0.03330,0.41066,0.11031,-0.44694,-0.02970,\n1403636591163555328,-0.06959,-0.05904,-0.04587,-0.05516,0.90936,0.03160,0.41113,0.10976,-0.41387,-0.02422,\n1403636591263555584,-0.06016,-0.09751,-0.05005,-0.05455,0.91031,0.02786,0.40936,0.09728,-0.36481,-0.04707,\n1403636591363555328,-0.05107,-0.13047,-0.05581,-0.05219,0.91167,0.02443,0.40686,0.07705,-0.28123,-0.06557,\n1403636591463555584,-0.04337,-0.15327,-0.06396,-0.05118,0.91207,0.02024,0.40632,0.08267,-0.19965,-0.08545,\n1403636591563555584,-0.03458,-0.17116,-0.07260,-0.04504,0.91042,0.01468,0.41095,0.09424,-0.16068,-0.08309,\n1403636591663555328,-0.02464,-0.18362,-0.08006,-0.03458,0.90426,0.00435,0.42555,0.08755,-0.09190,-0.06213,\n1403636591763555584,-0.01655,-0.18966,-0.08360,-0.02540,0.89522,-0.00128,0.44490,0.08067,-0.05856,-0.01115,\n1403636591863555328,-0.00882,-0.19432,-0.08322,-0.01754,0.88636,-0.00813,0.46260,0.07733,-0.05141,0.00730,\n1403636591963555584,-0.00112,-0.20004,-0.08272,-0.01367,0.88367,-0.01692,0.46761,0.09628,-0.06433,-0.00289,\n1403636592063555584,0.01176,-0.20863,-0.08402,-0.00999,0.88689,-0.02811,0.46102,0.17390,-0.10482,-0.01744,\n1403636592163555328,0.03066,-0.21988,-0.08641,-0.00601,0.89020,-0.03977,0.45378,0.21921,-0.11231,-0.02903,\n1403636592263555584,0.05468,-0.23358,-0.08749,-0.00387,0.89433,-0.04459,0.44517,0.25114,-0.07821,-0.02095,\n1403636592363555328,0.08135,-0.23837,-0.08893,-0.00685,0.89773,-0.04606,0.43809,0.29023,-0.00289,-0.01497,\n1403636592463555584,0.11191,-0.23401,-0.08872,-0.01430,0.89929,-0.04039,0.43524,0.32042,0.09797,0.01015,\n1403636592563555584,0.14448,-0.21963,-0.08712,-0.02315,0.89638,-0.02754,0.44182,0.31710,0.20038,0.00951,\n1403636592663555328,0.17553,-0.19576,-0.08724,-0.03127,0.89232,-0.00982,0.45021,0.28819,0.28902,-0.00837,\n1403636592763555584,0.20117,-0.16260,-0.08868,-0.04233,0.89044,0.00973,0.45303,0.21309,0.39459,-0.01027,\n1403636592863555328,0.21691,-0.11848,-0.09042,-0.06127,0.89028,0.03128,0.45018,0.08598,0.49931,-0.03187,\n1403636592963555584,0.21840,-0.06605,-0.09281,-0.08025,0.89101,0.05353,0.44361,-0.06741,0.56246,-0.02102,\n1403636593063555584,0.20346,-0.00686,-0.09368,-0.09851,0.89089,0.07624,0.43680,-0.22059,0.58798,-0.00098,\n1403636593163555328,0.17410,0.05109,-0.09266,-0.11511,0.88908,0.09737,0.43222,-0.36521,0.56406,0.01991,\n1403636593263555584,0.13310,0.10445,-0.08925,-0.13437,0.88674,0.10941,0.42856,-0.46725,0.47655,0.02912,\n1403636593363555328,0.08549,0.14451,-0.08664,-0.14344,0.88688,0.11757,0.42314,-0.50600,0.30737,0.00811,\n1403636593463555584,0.03384,0.16626,-0.08670,-0.14366,0.88805,0.12070,0.41972,-0.51433,0.10594,-0.02011,\n1403636593563555584,-0.01733,0.17084,-0.09002,-0.13887,0.89099,0.11426,0.41689,-0.48751,-0.05172,-0.04922,\n1403636593663555328,-0.06447,0.15750,-0.09594,-0.13356,0.89660,0.09928,0.41037,-0.44477,-0.21654,-0.06680,\n1403636593663555328,-0.10084,0.13182,-0.10591,-0.11997,0.90340,0.07379,0.40501,-0.38163,-0.36947,-0.05984,\n1403636593763555584,-0.09691,0.13180,-0.10656,-0.12129,0.90321,0.07623,0.40459,-0.37775,-0.37030,-0.06075,\n1403636593863555328,-0.13101,0.08869,-0.11131,-0.10241,0.90644,0.04423,0.40734,-0.28909,-0.48335,-0.02787,\n1403636593963555584,-0.15554,0.03619,-0.11254,-0.07956,0.90490,0.00884,0.41803,-0.18973,-0.55366,0.00534,\n1403636594063555584,-0.16791,-0.02167,-0.11166,-0.05410,0.90197,-0.02448,0.42770,-0.05762,-0.58409,0.01743,\n1403636594163555328,-0.17237,-0.07899,-0.11226,-0.03070,0.90037,-0.05876,0.43004,0.10699,-0.58349,-0.00752,\n1403636594263555584,-0.15388,-0.13613,-0.11437,-0.01244,0.90018,-0.07933,0.42805,0.27780,-0.56836,-0.02762,\n1403636594363555328,-0.12050,-0.19160,-0.11628,0.00313,0.89954,-0.08958,0.42753,0.42417,-0.53528,-0.02272,\n1403636594463555584,-0.07348,-0.23924,-0.11759,0.01538,0.89627,-0.09151,0.43370,0.52489,-0.45553,-0.01436,\n1403636594563555584,-0.01923,-0.27626,-0.11916,0.01912,0.89112,-0.09056,0.44423,0.55799,-0.28508,-0.01597,\n1403636594663555328,0.03281,-0.29241,-0.11996,0.01639,0.88728,-0.09066,0.45193,0.50463,-0.04699,-0.01367,\n1403636594763555584,0.07783,-0.28270,-0.12110,0.01040,0.88577,-0.08323,0.45649,0.42284,0.21243,-0.03246,\n1403636594863555328,0.11384,-0.23325,-0.12318,-0.00362,0.88525,-0.06025,0.46118,0.32476,0.47793,-0.03677,\n1403636594963555584,0.13853,-0.17429,-0.12664,-0.01871,0.88421,-0.03049,0.46572,0.16838,0.68734,-0.02763,\n1403636595063555584,0.14685,-0.09948,-0.12921,-0.04245,0.88350,0.00488,0.46648,0.00450,0.79116,-0.03132,\n1403636595163555328,0.14116,-0.02101,-0.13271,-0.06667,0.88402,0.03805,0.46111,-0.12211,0.76155,-0.03158,\n1403636595263555584,0.12255,0.04960,-0.13515,-0.09068,0.88393,0.06540,0.45406,-0.23555,0.61849,-0.02903,\n1403636595363555328,0.09439,0.10089,-0.13676,-0.10722,0.88332,0.08690,0.44799,-0.31234,0.38474,-0.01481,\n1403636595463555584,0.06176,0.12731,-0.13754,-0.11544,0.88305,0.09746,0.44429,-0.33081,0.10386,-0.00994,\n1403636595563555584,0.02777,0.12579,-0.13807,-0.11771,0.88447,0.10154,0.43995,-0.31892,-0.16939,-0.01660,\n1403636595663555328,-0.00425,0.09766,-0.13981,-0.11315,0.88701,0.09985,0.43639,-0.29158,-0.40027,-0.03223,\n1403636595763555584,-0.03153,0.04891,-0.14405,-0.09935,0.88982,0.08920,0.43634,-0.25290,-0.56772,-0.05189,\n1403636595863555328,-0.05514,-0.01154,-0.14960,-0.07675,0.89245,0.06958,0.43910,-0.23192,-0.62305,-0.06147,\n1403636595963555584,-0.07817,-0.07212,-0.15516,-0.05575,0.89547,0.04444,0.43938,-0.25214,-0.57696,-0.04308,\n1403636596063555584,-0.10623,-0.12215,-0.15892,-0.03570,0.89905,0.01974,0.43593,-0.31119,-0.41437,-0.02581,\n1403636596163555328,-0.13836,-0.14907,-0.16015,-0.02328,0.90118,-0.00088,0.43281,-0.33905,-0.14649,0.01762,\n1403636596263555584,-0.17220,-0.14545,-0.15614,-0.02041,0.90066,-0.01229,0.43388,-0.36979,0.14056,0.07938,\n1403636596363555328,-0.21138,-0.12113,-0.14539,-0.02067,0.89878,-0.01747,0.43756,-0.43060,0.30312,0.13165,\n1403636596463555584,-0.25505,-0.08811,-0.13133,-0.01911,0.89628,-0.01497,0.44283,-0.42792,0.34467,0.12715,\n1403636596563555584,-0.29315,-0.05483,-0.11992,-0.01818,0.89404,-0.00822,0.44754,-0.33924,0.31222,0.09396,\n1403636596663555328,-0.32116,-0.02725,-0.11265,-0.01715,0.89458,-0.00573,0.44655,-0.20314,0.23203,0.04550,\n1403636596763555584,-0.33260,-0.01011,-0.11030,-0.01701,0.89511,-0.00154,0.44552,-0.00101,0.10693,0.00212,\n1403636596863555328,-0.32174,-0.00514,-0.11148,-0.01427,0.89257,0.00294,0.45068,0.23547,-0.00582,-0.01745,\n1403636596963555584,-0.28725,-0.00949,-0.11471,-0.00964,0.89116,0.00639,0.45354,0.44816,-0.07730,-0.03251,\n1403636597063555584,-0.23449,-0.01816,-0.11946,-0.00729,0.89467,0.00715,0.44661,0.58653,-0.10878,-0.05415,\n1403636597163555328,-0.17164,-0.02825,-0.12327,-0.01221,0.89947,0.01292,0.43662,0.66796,-0.10274,-0.01583,\n1403636597263555584,-0.10189,-0.03732,-0.12177,-0.01988,0.90076,0.02480,0.43315,0.70939,-0.07312,0.04366,\n1403636597363555328,-0.03042,-0.04170,-0.11744,-0.02865,0.89858,0.03919,0.43612,0.70974,-0.01665,0.04586,\n1403636597463555584,0.03663,-0.04001,-0.11460,-0.03659,0.89575,0.05610,0.43948,0.61233,0.04991,0.02286,\n1403636597563555584,0.08910,-0.02970,-0.11447,-0.04893,0.89193,0.07461,0.44329,0.41936,0.14268,-0.00854,\n1403636597663555328,0.12499,-0.01749,-0.11297,-0.06809,0.88611,0.09050,0.44943,0.15918,0.22296,-0.03031,\n1403636597763555584,0.12760,0.00643,-0.11508,-0.08370,0.87993,0.10434,0.45588,-0.08903,0.24017,-0.02098,\n1403636597863555328,0.10847,0.02767,-0.11633,-0.09089,0.87499,0.11561,0.46127,-0.27328,0.17322,-0.01735,\n1403636597963555584,0.07316,0.04087,-0.11765,-0.09212,0.87295,0.12640,0.46206,-0.39496,0.09130,-0.02519,\n1403636598063555584,0.02918,0.04652,-0.12085,-0.09115,0.87516,0.13134,0.45666,-0.45577,0.02105,-0.05687,\n1403636598163555328,-0.01662,0.04488,-0.12749,-0.08947,0.88161,0.13092,0.44453,-0.43147,-0.05868,-0.08993,\n1403636598263555584,-0.05501,0.03676,-0.13854,-0.08238,0.89050,0.13067,0.42796,-0.31238,-0.10177,-0.13634,\n1403636598363555328,-0.07703,0.02535,-0.15214,-0.07379,0.89625,0.12917,0.41786,-0.11699,-0.12213,-0.14492,\n1403636598463555584,-0.07724,0.01801,-0.16950,-0.06689,0.89751,0.12802,0.41666,0.09088,-0.11015,-0.15172,\n1403636598463555584,-0.07943,0.01229,-0.16647,-0.06703,0.89747,0.12832,0.41664,0.09096,-0.11009,-0.15172,\n1403636598563555584,-0.05796,0.00904,-0.18325,-0.06850,0.89387,0.12938,0.42375,0.27184,-0.07597,-0.13226,\n1403636598663555328,-0.02589,0.00302,-0.19627,-0.07659,0.88633,0.13274,0.43697,0.36279,-0.05058,-0.13910,\n1403636598763555584,0.01057,-0.00050,-0.21270,-0.08912,0.87555,0.13824,0.45426,0.36234,-0.02832,-0.18901,\n1403636598863555328,0.04489,-0.00370,-0.23361,-0.10044,0.86344,0.14036,0.47401,0.32182,-0.03103,-0.22984,\n1403636598963555584,0.07535,-0.00696,-0.25737,-0.10394,0.85361,0.13910,0.49111,0.26675,-0.02738,-0.23978,\n1403636599063555584,0.09903,-0.00870,-0.27910,-0.10852,0.84501,0.13887,0.50487,0.19871,-0.00439,-0.18365,\n1403636599163555328,0.11632,-0.00878,-0.29384,-0.12039,0.83266,0.13810,0.52260,0.15549,-0.00118,-0.10707,\n1403636599263555584,0.13168,-0.00814,-0.30286,-0.12534,0.81537,0.13425,0.54904,0.14063,0.00806,-0.08170,\n1403636599363555328,0.14429,-0.00742,-0.31123,-0.12829,0.80276,0.12895,0.56788,0.11734,0.00343,-0.09217,\n1403636599463555584,0.15403,-0.00704,-0.32091,-0.12200,0.79873,0.12706,0.57533,0.08643,0.00203,-0.09467,\n1403636599563555584,0.16073,-0.00675,-0.33028,-0.11690,0.80251,0.11999,0.57264,0.04537,-0.00300,-0.08892,\n1403636599663555328,0.16393,-0.00738,-0.33941,-0.11326,0.80647,0.11714,0.56839,0.01502,-0.01273,-0.08602,\n1403636599763555584,0.16446,-0.00933,-0.34275,-0.11516,0.80023,0.11649,0.57688,0.00150,-0.02237,0.00447,\n1403636599863555328,0.16420,-0.01105,-0.34293,-0.11714,0.79197,0.11404,0.58826,-0.02030,-0.01295,-0.00920,\n1403636599963555584,0.16379,-0.01239,-0.34381,-0.11882,0.79154,0.11128,0.58903,0.00374,-0.00949,-0.01245,\n1403636600063555584,0.16388,-0.01390,-0.34536,-0.12106,0.79207,0.10856,0.58837,0.00759,-0.01925,-0.02878,\n1403636600163555328,0.16363,-0.01529,-0.34536,-0.12227,0.78984,0.10785,0.59125,0.00293,0.00475,0.00234,\n1403636600263555584,0.16354,-0.01486,-0.34545,-0.12369,0.79005,0.10703,0.59082,-0.00320,-0.00091,0.00274,\n1403636600363555328,0.16326,-0.01475,-0.34543,-0.12431,0.79008,0.10630,0.59078,-0.00131,0.00038,0.00419,\n1403636600463555584,0.16318,-0.01464,-0.34543,-0.12474,0.79012,0.10600,0.59069,-0.00084,-0.00171,0.00266,\n1403636600563555584,0.16327,-0.01483,-0.34536,-0.12411,0.79007,0.10637,0.59082,0.00412,-0.00229,0.00420,\n1403636600663555328,0.16324,-0.01479,-0.34537,-0.12431,0.79017,0.10599,0.59071,0.00388,-0.00761,0.00229,\n1403636600763555584,0.16324,-0.01474,-0.34536,-0.12449,0.79022,0.10583,0.59065,0.00188,-0.00598,0.00291,\n1403636600863555328,0.16320,-0.01477,-0.34535,-0.12434,0.79029,0.10579,0.59058,0.00242,-0.00209,0.00157,\n1403636600963555584,0.16320,-0.01478,-0.34534,-0.12427,0.79033,0.10570,0.59056,0.00045,-0.00357,0.00296,\n1403636601063555584,0.16320,-0.01480,-0.34533,-0.12417,0.79037,0.10568,0.59053,0.00089,-0.00034,0.00159,\n1403636601163555328,0.16319,-0.01485,-0.34532,-0.12404,0.79041,0.10567,0.59051,-0.00044,-0.00086,0.00131,\n1403636601263555584,0.16320,-0.01485,-0.34532,-0.12402,0.79045,0.10562,0.59047,-0.00017,-0.00125,0.00126,\n1403636601363555328,0.16319,-0.01489,-0.34532,-0.12383,0.79046,0.10564,0.59049,0.00034,0.00135,0.00115,\n1403636601463555584,0.16321,-0.01490,-0.34534,-0.12380,0.79049,0.10558,0.59047,0.00006,-0.00043,0.00143,\n1403636601563555584,0.16320,-0.01496,-0.34535,-0.12368,0.79048,0.10549,0.59053,0.00097,0.00185,0.00118,\n1403636601663555328,0.16322,-0.01498,-0.34536,-0.12355,0.79046,0.10554,0.59056,-0.00040,0.00033,0.00154,\n1403636601763555584,0.16322,-0.01500,-0.34537,-0.12351,0.79049,0.10554,0.59054,-0.00010,0.00056,0.00009,\n1403636601863555328,0.16323,-0.01501,-0.34538,-0.12350,0.79051,0.10551,0.59051,0.00032,-0.00137,0.00114,\n1403636601963555584,0.16320,-0.01503,-0.34536,-0.12337,0.79056,0.10553,0.59048,-0.00079,0.00013,0.00122,\n1403636602063555584,0.16318,-0.01508,-0.34536,-0.12325,0.79059,0.10557,0.59045,0.00115,0.00056,0.00041,\n1403636602163555328,0.16318,-0.01508,-0.34535,-0.12316,0.79060,0.10557,0.59045,0.00113,0.00021,0.00133,\n1403636602263555584,0.16317,-0.01509,-0.34534,-0.12304,0.79062,0.10558,0.59046,0.00102,0.00001,0.00145,\n1403636602363555328,0.16318,-0.01510,-0.34534,-0.12309,0.79065,0.10551,0.59042,0.00197,-0.00066,0.00115,\n1403636602463555584,0.16318,-0.01510,-0.34536,-0.12311,0.79064,0.10543,0.59044,0.00137,-0.00144,0.00125,\n1403636602563555584,0.16316,-0.01510,-0.34533,-0.12306,0.79068,0.10540,0.59040,0.00166,-0.00128,0.00087,\n1403636602663555328,0.16316,-0.01512,-0.34533,-0.12297,0.79070,0.10541,0.59039,0.00169,-0.00043,0.00137,\n1403636602763555584,0.16315,-0.01512,-0.34533,-0.12293,0.79071,0.10535,0.59040,0.00074,-0.00128,0.00129,\n1403636602863555328,0.16313,-0.01514,-0.34533,-0.12291,0.79072,0.10534,0.59039,0.00021,-0.00138,0.00104,\n1403636602963555584,0.16313,-0.01515,-0.34533,-0.12286,0.79073,0.10532,0.59039,0.00105,-0.00076,0.00100,\n1403636603063555584,0.16313,-0.01515,-0.34533,-0.12284,0.79074,0.10532,0.59038,-0.00041,-0.00082,0.00094,\n1403636603063555584,0.16619,-0.01844,-0.34372,-0.12685,0.79001,0.11076,0.58951,-0.00109,-0.00065,0.00236,\n1403636603163555328,0.16623,-0.01836,-0.34389,-0.12683,0.79002,0.11078,0.58950,-0.00080,0.00028,0.00068,\n1403636603263555584,0.16626,-0.01839,-0.34390,-0.12684,0.79003,0.11079,0.58949,0.00039,-0.00089,0.00034,\n1403636603363555328,0.16623,-0.01840,-0.34391,-0.12677,0.79002,0.11077,0.58951,0.00019,-0.00117,0.00087,\n1403636603463555584,0.16623,-0.01840,-0.34389,-0.12670,0.79004,0.11078,0.58950,-0.00051,0.00013,0.00094,\n1403636603563555584,0.16624,-0.01842,-0.34389,-0.12666,0.79008,0.11077,0.58946,-0.00022,0.00076,0.00095,\n1403636603663555328,0.16623,-0.01843,-0.34388,-0.12660,0.79009,0.11075,0.58946,-0.00068,-0.00030,0.00092,\n1403636603763555584,0.16619,-0.01845,-0.34388,-0.12659,0.79013,0.11070,0.58942,0.00060,0.00004,-0.00033,\n1403636603863555328,0.16625,-0.01846,-0.34389,-0.12655,0.79009,0.11068,0.58949,0.00089,0.00033,0.00185,\n1403636603963555584,0.16623,-0.01849,-0.34388,-0.12642,0.79011,0.11072,0.58948,-0.00255,-0.00020,0.00076,\n1403636604063555584,0.16623,-0.01850,-0.34389,-0.12646,0.79014,0.11069,0.58944,-0.00125,-0.00118,-0.00046,\n1403636604163555328,0.16621,-0.01851,-0.34389,-0.12641,0.79014,0.11070,0.58944,-0.00001,-0.00172,0.00101,\n1403636604263555584,0.16621,-0.01851,-0.34388,-0.12642,0.79017,0.11064,0.58942,-0.00004,-0.00150,0.00077,\n1403636604363555328,0.16620,-0.01851,-0.34386,-0.12635,0.79020,0.11066,0.58939,0.00040,-0.00102,0.00118,\n1403636604463555584,0.16620,-0.01853,-0.34387,-0.12632,0.79021,0.11065,0.58938,0.00044,-0.00124,-0.00006,\n1403636604563555584,0.16621,-0.01852,-0.34387,-0.12631,0.79022,0.11060,0.58938,0.00085,-0.00140,0.00097,\n1403636604663555328,0.16620,-0.01853,-0.34387,-0.12629,0.79024,0.11061,0.58936,-0.00028,-0.00094,0.00033,\n1403636604763555584,0.16620,-0.01854,-0.34386,-0.12624,0.79025,0.11061,0.58935,-0.00022,-0.00092,0.00140,\n1403636604863555328,0.16619,-0.01854,-0.34385,-0.12619,0.79027,0.11061,0.58933,-0.00004,0.00012,0.00118,\n1403636604963555584,0.16619,-0.01856,-0.34384,-0.12618,0.79029,0.11060,0.58931,-0.00087,-0.00132,0.00092,\n1403636605063555584,0.16618,-0.01857,-0.34384,-0.12615,0.79032,0.11056,0.58928,0.00128,-0.00176,0.00056,\n1403636605163555328,0.16619,-0.01857,-0.34385,-0.12612,0.79032,0.11053,0.58930,0.00115,-0.00019,0.00161,\n1403636605263555584,0.16618,-0.01857,-0.34384,-0.12610,0.79033,0.11052,0.58929,-0.00046,-0.00114,0.00110,\n1403636605363555328,0.16618,-0.01858,-0.34384,-0.12609,0.79034,0.11050,0.58928,-0.00071,0.00009,0.00139,\n1403636605463555584,0.16618,-0.01859,-0.34383,-0.12605,0.79037,0.11050,0.58925,-0.00033,-0.00027,0.00103,\n1403636605563555584,0.16616,-0.01860,-0.34382,-0.12600,0.79039,0.11048,0.58924,-0.00090,0.00011,0.00121,\n1403636605663555328,0.16617,-0.01862,-0.34383,-0.12598,0.79039,0.11050,0.58923,0.00003,-0.00051,0.00049,\n1403636605763555584,0.16612,-0.01861,-0.34380,-0.12590,0.79042,0.11048,0.58921,-0.00087,-0.00076,0.00157,\n1403636605863555328,0.16611,-0.01862,-0.34379,-0.12586,0.79042,0.11048,0.58924,-0.00031,0.00049,0.00098,\n1403636605963555584,0.16612,-0.01863,-0.34380,-0.12585,0.79044,0.11048,0.58921,-0.00023,0.00002,0.00030,\n1403636606063555584,0.16612,-0.01864,-0.34380,-0.12581,0.79044,0.11048,0.58922,-0.00044,-0.00002,0.00137,\n1403636606163555328,0.16611,-0.01865,-0.34379,-0.12576,0.79045,0.11046,0.58921,-0.00088,-0.00017,0.00059,\n1403636606263555584,0.16612,-0.01865,-0.34379,-0.12572,0.79045,0.11047,0.58922,0.00076,-0.00047,0.00118,\n1403636606363555328,0.16611,-0.01866,-0.34379,-0.12570,0.79047,0.11043,0.58921,-0.00100,0.00049,0.00087,\n1403636606463555584,0.16611,-0.01867,-0.34379,-0.12570,0.79048,0.11043,0.58919,0.00035,-0.00035,0.00179,\n1403636606563555584,0.16611,-0.01868,-0.34379,-0.12568,0.79050,0.11040,0.58918,0.00031,-0.00104,0.00045,\n1403636606663555328,0.16611,-0.01868,-0.34379,-0.12563,0.79049,0.11043,0.58920,-0.00013,0.00046,0.00076,\n1403636606763555584,0.16611,-0.01869,-0.34378,-0.12560,0.79051,0.11041,0.58918,-0.00090,0.00005,0.00060,\n1403636606863555328,0.16611,-0.01871,-0.34378,-0.12561,0.79052,0.11041,0.58915,-0.00013,-0.00082,0.00076,\n1403636606963555584,0.16609,-0.01871,-0.34376,-0.12554,0.79055,0.11040,0.58914,-0.00016,0.00016,0.00138,\n1403636607063555584,0.16610,-0.01872,-0.34377,-0.12552,0.79056,0.11039,0.58913,-0.00101,-0.00052,0.00123,\n1403636607163555328,0.16609,-0.01873,-0.34376,-0.12548,0.79057,0.11037,0.58912,-0.00044,-0.00085,0.00143,\n1403636607263555584,0.16608,-0.01874,-0.34375,-0.12542,0.79060,0.11036,0.58911,-0.00046,0.00001,0.00074,\n1403636607363555328,0.16608,-0.01875,-0.34374,-0.12538,0.79062,0.11036,0.58909,-0.00103,-0.00026,0.00104,\n1403636607463555584,0.16608,-0.01877,-0.34375,-0.12537,0.79062,0.11033,0.58909,0.00008,-0.00083,0.00115,\n1403636607563555584,0.16608,-0.01877,-0.34374,-0.12531,0.79063,0.11033,0.58909,-0.00076,-0.00106,0.00082,\n1403636607663555328,0.16606,-0.01877,-0.34373,-0.12524,0.79066,0.11031,0.58908,-0.00114,-0.00005,0.00119,\n1403636607763555584,0.16606,-0.01878,-0.34372,-0.12525,0.79068,0.11029,0.58905,0.00010,-0.00087,0.00064,\n1403636607863555328,0.16607,-0.01879,-0.34373,-0.12525,0.79067,0.11027,0.58906,-0.00032,-0.00033,0.00091,\n1403636607963555584,0.16606,-0.01879,-0.34373,-0.12518,0.79068,0.11027,0.58906,-0.00060,-0.00064,0.00093,\n1403636608063555584,0.16606,-0.01880,-0.34372,-0.12518,0.79070,0.11026,0.58904,-0.00012,-0.00139,0.00076,\n1403636608163555328,0.16606,-0.01881,-0.34373,-0.12519,0.79070,0.11023,0.58904,-0.00019,-0.00054,0.00062,\n1403636608263555584,0.16607,-0.01881,-0.34373,-0.12520,0.79071,0.11023,0.58903,0.00004,-0.00100,0.00066,\n1403636608363555328,0.16607,-0.01882,-0.34373,-0.12516,0.79072,0.11024,0.58902,0.00011,-0.00054,0.00064,\n1403636608463555584,0.16607,-0.01882,-0.34373,-0.12516,0.79072,0.11022,0.58902,-0.00039,-0.00038,0.00081,\n1403636608563555584,0.16607,-0.01882,-0.34373,-0.12513,0.79072,0.11021,0.58904,-0.00003,-0.00140,0.00119,\n1403636608663555328,0.16606,-0.01883,-0.34373,-0.12507,0.79073,0.11021,0.58902,-0.00078,-0.00050,0.00102,\n1403636608763555584,0.16606,-0.01883,-0.34372,-0.12505,0.79075,0.11021,0.58901,-0.00021,-0.00048,0.00056,\n1403636608863555328,0.16606,-0.01884,-0.34371,-0.12504,0.79076,0.11020,0.58899,-0.00097,-0.00081,0.00066,\n1403636608963555584,0.16606,-0.01885,-0.34372,-0.12505,0.79076,0.11017,0.58900,-0.00062,-0.00129,0.00082,\n1403636609063555584,0.16606,-0.01885,-0.34372,-0.12502,0.79077,0.11020,0.58899,-0.00023,0.00057,0.00122,\n1403636609163555328,0.16606,-0.01885,-0.34373,-0.12504,0.79076,0.11018,0.58901,-0.00072,-0.00040,0.00103,\n1403636609263555584,0.16607,-0.01886,-0.34373,-0.12503,0.79076,0.11017,0.58900,-0.00033,-0.00032,0.00058,\n1403636609363555328,0.16605,-0.01886,-0.34372,-0.12500,0.79080,0.11018,0.58896,-0.00017,0.00041,0.00079,\n1403636609463555584,0.16605,-0.01886,-0.34371,-0.12497,0.79079,0.11017,0.58898,-0.00105,-0.00006,0.00100,\n1403636609563555584,0.16605,-0.01887,-0.34372,-0.12497,0.79079,0.11015,0.58899,0.00022,-0.00077,0.00089,\n1403636609663555328,0.16603,-0.01887,-0.34373,-0.12499,0.79083,0.11012,0.58894,0.00015,0.00041,-0.00026,\n1403636609763555584,0.16606,-0.01889,-0.34373,-0.12500,0.79079,0.11009,0.58899,0.00034,0.00036,0.00112,\n1403636609863555328,0.16606,-0.01890,-0.34371,-0.12493,0.79081,0.11012,0.58898,-0.00295,-0.00084,-0.00116,\n1403636609963555584,0.16604,-0.01890,-0.34372,-0.12495,0.79084,0.11009,0.58893,-0.00140,-0.00113,0.00042,\n1403636610063555584,0.16634,-0.01815,-0.34402,-0.12780,0.78946,0.11089,0.59002,-0.00137,-0.00217,0.00029,\n1403636610163555328,0.16634,-0.01797,-0.34406,-0.12848,0.78933,0.11119,0.58998,-0.00097,-0.00413,0.00034,\n1403636610263555584,0.16633,-0.01792,-0.34406,-0.12867,0.78934,0.11134,0.58991,-0.00143,-0.00351,0.00007,\n1403636610363555328,0.16631,-0.01787,-0.34408,-0.12887,0.78931,0.11133,0.58990,0.00044,-0.00308,-0.00074,\n1403636610463555584,0.16634,-0.01785,-0.34407,-0.12892,0.78930,0.11135,0.58991,0.00025,-0.00330,0.00185,\n1403636610563555584,0.16633,-0.01784,-0.34408,-0.12894,0.78930,0.11137,0.58990,-0.00093,-0.00226,0.00028,\n1403636610663555328,0.16632,-0.01784,-0.34407,-0.12894,0.78930,0.11139,0.58989,-0.00165,-0.00309,0.00070,\n1403636610763555584,0.16633,-0.01782,-0.34405,-0.12889,0.78938,0.11146,0.58978,-0.00237,-0.00393,-0.00233,\n1403636610863555328,0.16629,-0.01783,-0.34407,-0.12900,0.78940,0.11152,0.58971,-0.00232,-0.00365,-0.00039,\n1403636610963555584,0.16628,-0.01784,-0.34404,-0.12895,0.78943,0.11157,0.58968,-0.00013,0.00045,0.00205,\n1403636611063555584,0.16629,-0.01784,-0.34404,-0.12885,0.78941,0.11161,0.58972,-0.00001,-0.00181,0.00072,\n1403636611163555328,0.16635,-0.01785,-0.34405,-0.12888,0.78940,0.11157,0.58974,0.00087,-0.00284,-0.00044,\n1403636611263555584,0.16636,-0.01783,-0.34404,-0.12892,0.78941,0.11150,0.58973,0.00133,-0.00013,0.00062,\n1403636611363555328,0.16624,-0.01755,-0.34407,-0.12963,0.78950,0.11133,0.58949,-0.01074,0.01855,-0.00108,\n1403636611463555584,0.16638,-0.01805,-0.34436,-0.12972,0.78956,0.11094,0.58946,0.00006,-0.00252,-0.01418,\n1403636611563555584,0.16647,-0.01831,-0.34437,-0.12902,0.78950,0.11119,0.58964,-0.00235,0.00498,-0.00579,\n1403636611663555328,0.16644,-0.01822,-0.34432,-0.12924,0.78946,0.11106,0.58967,-0.00506,0.00134,-0.00397,\n1403636611763555584,0.16648,-0.01822,-0.34430,-0.12921,0.78937,0.11113,0.58979,-0.00572,0.01146,-0.00130,\n1403636611863555328,0.16655,-0.01847,-0.34428,-0.12864,0.78920,0.11120,0.59013,-0.00141,-0.00732,0.00143,\n1403636611963555584,0.16642,-0.01819,-0.34429,-0.12934,0.78938,0.11115,0.58975,-0.01268,0.01012,-0.00739,\n1403636612063555584,0.16653,-0.01839,-0.34431,-0.12877,0.78938,0.11146,0.58981,-0.00715,0.00898,-0.00347,\n1403636612163555328,0.16656,-0.01797,-0.34425,-0.12992,0.78925,0.11094,0.58983,-0.00364,0.00228,0.00042,\n1403636612263555584,0.16673,-0.01878,-0.34426,-0.12739,0.78905,0.11249,0.59035,-0.00673,0.00351,0.00067,\n1403636612363555328,0.16664,-0.01871,-0.34425,-0.12783,0.78920,0.11230,0.59009,-0.00426,-0.00819,-0.00185,\n1403636612463555584,0.16660,-0.01866,-0.34431,-0.12805,0.78915,0.11204,0.59016,0.00388,-0.00716,-0.00060,\n1403636612563555584,0.16659,-0.01857,-0.34429,-0.12840,0.78918,0.11178,0.59010,0.00157,-0.01287,0.00458,\n1403636612663555328,0.16657,-0.01855,-0.34428,-0.12850,0.78923,0.11173,0.59001,0.00298,-0.00662,0.00343,\n1403636612763555584,0.16656,-0.01854,-0.34430,-0.12853,0.78925,0.11162,0.59001,0.00344,-0.00973,0.00218,\n1403636612863555328,0.16652,-0.01844,-0.34426,-0.12889,0.78924,0.11140,0.58998,0.00343,-0.01298,0.00315,\n1403636612963555584,0.16649,-0.01843,-0.34427,-0.12904,0.78924,0.11124,0.58998,0.00254,-0.00783,0.00163,\n1403636613063555584,0.16648,-0.01838,-0.34432,-0.12928,0.78919,0.11109,0.59002,0.00157,-0.00630,0.00056,\n1403636613163555328,0.16649,-0.01834,-0.34430,-0.12933,0.78917,0.11100,0.59005,-0.00137,-0.00549,0.00117,\n1403636613263555584,0.16647,-0.01839,-0.34434,-0.12923,0.78923,0.11110,0.58998,-0.00062,-0.00374,0.00083,\n1403636613363555328,0.16650,-0.01837,-0.34431,-0.12925,0.78922,0.11109,0.58998,0.00017,-0.00403,0.00031,\n1403636613463555584,0.16648,-0.01839,-0.34432,-0.12916,0.78919,0.11111,0.59004,-0.00267,-0.00293,-0.00054,\n1403636613563555584,0.16649,-0.01840,-0.34431,-0.12913,0.78920,0.11111,0.59004,-0.00228,-0.00241,0.00049,\n1403636613663555328,0.16649,-0.01841,-0.34432,-0.12909,0.78920,0.11113,0.59004,-0.00150,-0.00042,0.00024,\n1403636613763555584,0.16650,-0.01844,-0.34432,-0.12894,0.78915,0.11119,0.59013,-0.00192,-0.00077,-0.00099,\n1403636613863555328,0.16649,-0.01844,-0.34431,-0.12895,0.78918,0.11121,0.59009,-0.00188,-0.00141,0.00082,\n1403636613963555584,0.16650,-0.01845,-0.34431,-0.12891,0.78918,0.11122,0.59009,-0.00142,-0.00115,0.00017,\n1403636614063555584,0.16650,-0.01845,-0.34431,-0.12888,0.78915,0.11126,0.59013,-0.00185,-0.00161,0.00034,\n1403636614163555328,0.16649,-0.01845,-0.34429,-0.12886,0.78916,0.11129,0.59011,-0.00287,-0.00155,0.00092,\n1403636614263555584,0.16650,-0.01846,-0.34430,-0.12879,0.78919,0.11130,0.59009,-0.00200,-0.00187,0.00034,\n1403636614363555328,0.16652,-0.01847,-0.34427,-0.12877,0.78915,0.11130,0.59015,-0.00074,-0.00155,0.00072,\n1403636614463555584,0.16650,-0.01848,-0.34429,-0.12874,0.78917,0.11135,0.59012,-0.00293,-0.00345,-0.00159,\n1403636614563555584,0.16650,-0.01847,-0.34430,-0.12880,0.78919,0.11133,0.59008,-0.00067,-0.00417,0.00173,\n1403636614663555328,0.16649,-0.01847,-0.34429,-0.12881,0.78919,0.11132,0.59009,-0.00044,-0.00194,0.00138,\n1403636614763555584,0.16649,-0.01846,-0.34429,-0.12883,0.78919,0.11131,0.59008,-0.00136,-0.00465,0.00131,\n1403636614863555328,0.16649,-0.01845,-0.34429,-0.12883,0.78921,0.11130,0.59006,0.00083,-0.00289,0.00055,\n1403636614963555584,0.16648,-0.01844,-0.34429,-0.12883,0.78918,0.11128,0.59009,-0.00065,-0.00261,0.00051,\n1403636615063555584,0.16649,-0.01844,-0.34428,-0.12890,0.78920,0.11127,0.59006,-0.00020,-0.00391,0.00080,\n1403636615163555328,0.16649,-0.01844,-0.34430,-0.12888,0.78921,0.11129,0.59005,-0.00027,-0.00294,0.00043,\n1403636615263555584,0.16650,-0.01843,-0.34429,-0.12889,0.78917,0.11124,0.59010,-0.00020,-0.00314,0.00034,\n1403636615363555328,0.16649,-0.01842,-0.34429,-0.12890,0.78920,0.11124,0.59007,-0.00187,-0.00366,0.00092,\n1403636615463555584,0.16650,-0.01844,-0.34429,-0.12887,0.78919,0.11126,0.59008,-0.00046,-0.00291,-0.00040,\n1403636615563555584,0.16650,-0.01843,-0.34429,-0.12889,0.78919,0.11125,0.59007,-0.00135,-0.00298,0.00081,\n1403636615663555328,0.16649,-0.01843,-0.34428,-0.12890,0.78920,0.11125,0.59006,-0.00210,-0.00321,0.00058,\n1403636615763555584,0.16649,-0.01844,-0.34429,-0.12889,0.78920,0.11125,0.59006,0.00001,-0.00456,0.00083,\n1403636615863555328,0.16648,-0.01842,-0.34429,-0.12891,0.78920,0.11123,0.59006,-0.00081,-0.00222,0.00071,\n1403636615963555584,0.16649,-0.01843,-0.34428,-0.12888,0.78919,0.11127,0.59007,-0.00092,-0.00258,0.00036,\n1403636616063555584,0.16649,-0.01845,-0.34430,-0.12889,0.78921,0.11125,0.59005,-0.00065,-0.00322,0.00075,\n1403636616163555328,0.16650,-0.01844,-0.34429,-0.12889,0.78919,0.11126,0.59007,-0.00070,-0.00254,-0.00007,\n1403636616263555584,0.16649,-0.01843,-0.34429,-0.12884,0.78920,0.11128,0.59007,-0.00135,-0.00302,0.00067,\n1403636616363555328,0.16650,-0.01844,-0.34429,-0.12890,0.78919,0.11124,0.59007,-0.00086,-0.00260,0.00039,\n1403636616463555584,0.16649,-0.01844,-0.34429,-0.12888,0.78920,0.11126,0.59006,-0.00082,-0.00182,0.00104,\n1403636616563555584,0.16648,-0.01843,-0.34428,-0.12886,0.78920,0.11125,0.59008,-0.00123,-0.00228,0.00162,\n1403636616663555328,0.16649,-0.01844,-0.34429,-0.12889,0.78919,0.11127,0.59007,-0.00093,-0.00282,0.00005,\n1403636616763555584,0.16649,-0.01845,-0.34429,-0.12889,0.78919,0.11125,0.59007,-0.00076,-0.00225,0.00093,\n1403636616863555328,0.16649,-0.01844,-0.34429,-0.12887,0.78919,0.11127,0.59008,-0.00125,-0.00341,0.00003,\n1403636616963555584,0.16649,-0.01844,-0.34430,-0.12888,0.78920,0.11125,0.59007,-0.00054,-0.00284,-0.00004,\n1403636617063555584,0.16649,-0.01844,-0.34429,-0.12886,0.78918,0.11128,0.59009,-0.00082,-0.00302,0.00009,\n1403636617163555328,0.16648,-0.01845,-0.34428,-0.12886,0.78920,0.11128,0.59006,-0.00148,-0.00262,0.00044,\n1403636617263555584,0.16648,-0.01845,-0.34429,-0.12886,0.78919,0.11127,0.59008,-0.00104,-0.00380,0.00080,\n1403636617363555328,0.16649,-0.01845,-0.34429,-0.12887,0.78919,0.11126,0.59007,-0.00055,-0.00284,0.00138,\n1403636617463555584,0.16648,-0.01844,-0.34429,-0.12887,0.78919,0.11126,0.59008,-0.00133,-0.00323,0.00112,\n1403636617563555584,0.16648,-0.01844,-0.34428,-0.12889,0.78919,0.11124,0.59007,-0.00081,-0.00276,0.00129,\n1403636617663555328,0.16649,-0.01844,-0.34429,-0.12890,0.78919,0.11125,0.59007,-0.00014,-0.00250,0.00001,\n1403636617763555584,0.16649,-0.01844,-0.34429,-0.12891,0.78918,0.11123,0.59008,-0.00104,-0.00334,-0.00016,\n1403636617863555328,0.16648,-0.01843,-0.34428,-0.12887,0.78919,0.11126,0.59007,-0.00056,-0.00255,0.00080,\n1403636617963555584,0.16649,-0.01844,-0.34429,-0.12888,0.78920,0.11126,0.59006,-0.00085,-0.00241,0.00055,\n1403636618063555584,0.16649,-0.01843,-0.34428,-0.12888,0.78919,0.11125,0.59008,-0.00059,-0.00367,0.00078,\n1403636618163555328,0.16650,-0.01844,-0.34429,-0.12889,0.78918,0.11126,0.59009,-0.00039,-0.00333,0.00080,\n1403636618263555584,0.16649,-0.01844,-0.34429,-0.12888,0.78920,0.11127,0.59007,-0.00077,-0.00322,0.00109,\n1403636618363555328,0.16649,-0.01844,-0.34429,-0.12890,0.78919,0.11124,0.59007,-0.00146,-0.00319,0.00024,\n1403636618463555584,0.16650,-0.01844,-0.34429,-0.12887,0.78918,0.11127,0.59008,-0.00124,-0.00278,0.00046,\n1403636618563555584,0.16652,-0.01839,-0.34434,-0.12896,0.78913,0.11146,0.59010,0.00043,-0.00736,-0.00208,\n1403636618663555328,0.16653,-0.01840,-0.34436,-0.12889,0.78914,0.11137,0.59013,-0.00027,-0.00864,-0.00131,\n1403636618763555584,0.16652,-0.01844,-0.34436,-0.12885,0.78915,0.11124,0.59015,-0.00222,0.00131,-0.00077,\n1403636618863555328,0.16652,-0.01842,-0.34435,-0.12892,0.78915,0.11126,0.59012,-0.00220,0.00045,0.00094,\n1403636618963555584,0.16650,-0.01836,-0.34431,-0.12909,0.78911,0.11146,0.59010,-0.00248,-0.00189,0.00034,\n1403636619063555584,0.16651,-0.01838,-0.34433,-0.12904,0.78917,0.11139,0.59005,-0.00119,-0.00376,-0.00044,\n1403636619163555328,0.16651,-0.01843,-0.34433,-0.12886,0.78920,0.11116,0.59008,0.00061,-0.01186,-0.00190,\n1403636619263555584,0.16652,-0.01845,-0.34431,-0.12883,0.78918,0.11112,0.59012,-0.00061,-0.00620,0.00032,\n1403636619363555328,0.16653,-0.01838,-0.34431,-0.12907,0.78915,0.11148,0.59005,-0.00329,0.00892,0.00111,\n1403636619463555584,0.16651,-0.01836,-0.34431,-0.12902,0.78914,0.11144,0.59007,-0.00130,0.00056,-0.00047,\n1403636619563555584,0.16654,-0.01842,-0.34433,-0.12893,0.78920,0.11128,0.59005,-0.00226,-0.00754,-0.00156,\n1403636619663555328,0.16657,-0.01843,-0.34434,-0.12894,0.78912,0.11132,0.59014,-0.00110,-0.00624,-0.00171,\n1403636619763555584,0.16656,-0.01845,-0.34434,-0.12887,0.78917,0.11124,0.59011,0.00085,-0.00235,-0.00046,\n1403636619863555328,0.16657,-0.01845,-0.34433,-0.12889,0.78918,0.11124,0.59009,-0.00300,-0.00095,-0.00040,\n1403636619963555584,0.16656,-0.01845,-0.34433,-0.12888,0.78916,0.11133,0.59011,-0.00318,0.00245,-0.00125,\n1403636620063555584,0.16656,-0.01846,-0.34433,-0.12877,0.78918,0.11133,0.59010,-0.00350,0.00048,-0.00011,\n1403636620163555328,0.16657,-0.01846,-0.34433,-0.12876,0.78915,0.11143,0.59012,-0.00204,-0.00058,0.00119,\n1403636620263555584,0.16656,-0.01846,-0.34433,-0.12879,0.78918,0.11141,0.59009,-0.00138,-0.00240,-0.00024,\n1403636620363555328,0.16656,-0.01846,-0.34433,-0.12876,0.78916,0.11139,0.59012,-0.00106,0.00015,-0.00037,\n1403636620463555584,0.16656,-0.01848,-0.34432,-0.12875,0.78915,0.11140,0.59013,-0.00147,-0.00329,0.00068,\n1403636620563555584,0.16655,-0.01847,-0.34432,-0.12878,0.78916,0.11133,0.59012,-0.00150,-0.00249,-0.00121,\n1403636620663555328,0.16656,-0.01848,-0.34433,-0.12874,0.78917,0.11138,0.59011,-0.00066,-0.00294,0.00102,\n1403636620763555584,0.16656,-0.01847,-0.34432,-0.12877,0.78917,0.11135,0.59012,-0.00063,-0.00186,0.00089,\n1403636620863555328,0.16657,-0.01847,-0.34433,-0.12881,0.78915,0.11134,0.59013,-0.00152,-0.00257,-0.00038,\n1403636620963555584,0.16656,-0.01847,-0.34434,-0.12879,0.78917,0.11131,0.59011,-0.00048,-0.00187,-0.00066,\n1403636621063555584,0.16664,-0.01858,-0.34428,-0.12831,0.78910,0.11159,0.59025,-0.00284,0.00630,0.00268,\n1403636621163555328,0.16662,-0.01856,-0.34431,-0.12849,0.78911,0.11159,0.59020,-0.00013,0.00273,-0.00141,\n1403636621263555584,0.16661,-0.01854,-0.34430,-0.12850,0.78914,0.11152,0.59018,0.00154,0.00019,0.00072,\n1403636621363555328,0.16662,-0.01854,-0.34430,-0.12857,0.78912,0.11147,0.59019,-0.00201,-0.00303,-0.00156,\n1403636621463555584,0.16660,-0.01852,-0.34430,-0.12858,0.78914,0.11145,0.59017,-0.00092,-0.00431,-0.00092,\n1403636621563555584,0.16658,-0.01853,-0.34429,-0.12856,0.78916,0.11147,0.59015,-0.00094,-0.00321,0.00093,\n1403636621663555328,0.16659,-0.01852,-0.34430,-0.12858,0.78914,0.11144,0.59017,0.00055,-0.00381,-0.00051,\n1403636621763555584,0.16659,-0.01852,-0.34431,-0.12867,0.78916,0.11143,0.59013,-0.00103,-0.00373,0.00065,\n1403636621863555328,0.16658,-0.01851,-0.34430,-0.12866,0.78917,0.11142,0.59012,-0.00149,-0.00335,-0.00032,\n1403636621963555584,0.16659,-0.01849,-0.34431,-0.12874,0.78913,0.11141,0.59016,-0.00120,-0.00142,-0.00131,\n1403636622063555584,0.16661,-0.01849,-0.34430,-0.12871,0.78912,0.11143,0.59017,-0.00091,-0.00360,-0.00076,\n1403636622163555328,0.16661,-0.01850,-0.34429,-0.12869,0.78915,0.11145,0.59014,-0.00025,-0.00269,-0.00002,\n1403636622263555584,0.16660,-0.01850,-0.34429,-0.12868,0.78914,0.11145,0.59015,-0.00078,-0.00335,-0.00038,\n1403636622363555328,0.16660,-0.01851,-0.34430,-0.12861,0.78915,0.11144,0.59015,-0.00252,-0.00293,0.00021,\n1403636622463555584,0.16660,-0.01851,-0.34430,-0.12866,0.78915,0.11143,0.59014,0.00094,-0.00368,-0.00174,\n1403636622563555584,0.16659,-0.01851,-0.34431,-0.12866,0.78916,0.11143,0.59013,-0.00004,-0.00309,0.00111,\n1403636622663555328,0.16660,-0.01852,-0.34429,-0.12867,0.78915,0.11144,0.59014,-0.00018,-0.00404,-0.00073,\n1403636622763555584,0.16662,-0.01853,-0.34428,-0.12861,0.78914,0.11141,0.59018,-0.00090,-0.00654,0.00098,\n1403636622863555328,0.16664,-0.01856,-0.34426,-0.12860,0.78912,0.11128,0.59022,0.00153,-0.00640,-0.00148,\n1403636622963555584,0.16666,-0.01860,-0.34420,-0.12870,0.78906,0.11109,0.59032,-0.00066,-0.00318,-0.00436,\n1403636623063555584,0.16668,-0.01865,-0.34417,-0.12858,0.78898,0.11113,0.59044,-0.00072,-0.00414,0.00239,\n1403636623163555328,0.16670,-0.01868,-0.34413,-0.12857,0.78897,0.11105,0.59047,-0.00290,-0.00203,-0.00336,\n1403636623263555584,0.16669,-0.01867,-0.34415,-0.12865,0.78900,0.11100,0.59043,-0.00013,-0.00322,0.00032,\n1403636623363555328,0.16673,-0.01871,-0.34416,-0.12864,0.78898,0.11089,0.59048,-0.00404,-0.00385,-0.00246,\n1403636623463555584,0.16675,-0.01873,-0.34414,-0.12852,0.78895,0.11095,0.59054,-0.01029,-0.00661,0.00027,\n1403636623563555584,0.16678,-0.01874,-0.34412,-0.12851,0.78904,0.11085,0.59043,-0.00170,-0.00272,-0.00392,\n1403636623663555328,0.16679,-0.01875,-0.34412,-0.12841,0.78915,0.11080,0.59032,0.00984,0.00043,0.00330,\n1403636623763555584,0.16682,-0.01883,-0.34409,-0.12823,0.78915,0.11051,0.59041,0.00159,-0.00138,-0.00348,\n1403636623863555328,0.16679,-0.01891,-0.34406,-0.12802,0.78934,0.11007,0.59028,-0.00092,-0.00293,-0.00521,\n1403636623963555584,0.16680,-0.01895,-0.34404,-0.12751,0.78967,0.10950,0.59006,-0.00463,-0.00276,0.00145,\n1403636624063555584,0.16681,-0.01903,-0.34402,-0.12738,0.78962,0.10949,0.59016,0.00225,0.00077,0.00143,\n1403636624163555328,0.16668,-0.01902,-0.34396,-0.12700,0.79007,0.10873,0.58977,-0.00441,-0.00803,0.00104,\n1403636624263555584,0.16660,-0.01920,-0.34389,-0.12696,0.79037,0.10885,0.58937,0.01162,-0.00114,0.00591,\n1403636624363555328,0.16560,-0.02101,-0.34412,-0.12379,0.79490,0.10907,0.58389,-0.02577,-0.04051,-0.00510,\n1403636624463555584,0.16297,-0.02618,-0.34431,-0.12054,0.80022,0.10783,0.57748,-0.03899,-0.06216,-0.01601,\n1403636624563555584,0.15915,-0.03391,-0.34421,-0.11683,0.80532,0.10644,0.57139,-0.03506,-0.08328,0.01085,\n1403636624663555328,0.15548,-0.04408,-0.34368,-0.11208,0.80472,0.10765,0.57295,-0.03227,-0.11556,-0.01194,\n1403636624763555584,0.15018,-0.05747,-0.34455,-0.10607,0.80282,0.11147,0.57602,-0.05936,-0.16018,-0.00382,\n1403636624863555328,0.14148,-0.07454,-0.34250,-0.09385,0.80616,0.12093,0.57156,-0.09230,-0.17451,0.04803,\n1403636624963555584,0.12964,-0.09309,-0.33278,-0.07969,0.81252,0.13194,0.56218,-0.12714,-0.17678,0.13790,\n1403636625063555584,0.11685,-0.10884,-0.31424,-0.07188,0.81868,0.13820,0.55272,-0.12154,-0.12788,0.22121,\n1403636625163555328,0.10719,-0.11827,-0.28930,-0.07508,0.82317,0.13705,0.54586,-0.08622,-0.06774,0.27786,\n1403636625263555584,0.10271,-0.12230,-0.26030,-0.08258,0.82066,0.13279,0.54960,-0.02879,-0.02640,0.26770,\n1403636625363555328,0.10123,-0.12403,-0.24190,-0.08795,0.81732,0.13097,0.55416,-0.00247,-0.00849,0.06222,\n1403636625463555584,0.10116,-0.12450,-0.24330,-0.08840,0.81967,0.13227,0.55031,0.01822,0.00519,-0.07692,\n1403636625563555584,0.10369,-0.12305,-0.25200,-0.08889,0.82103,0.13335,0.54792,0.05126,0.02997,-0.06478,\n1403636625663555328,0.11020,-0.11918,-0.25117,-0.08852,0.81993,0.13515,0.54919,0.08057,0.05951,0.07939,\n1403636625763555584,0.12138,-0.11168,-0.23292,-0.09049,0.81521,0.13515,0.55586,0.12264,0.08331,0.26662,\n1403636625863555328,0.13505,-0.10224,-0.19805,-0.09403,0.80891,0.13372,0.56475,0.14279,0.10497,0.40907,\n1403636625963555584,0.14934,-0.09147,-0.15140,-0.09697,0.80525,0.13240,0.56977,0.13291,0.10910,0.49509,\n1403636626063555584,0.16198,-0.08093,-0.10318,-0.10191,0.80343,0.12955,0.57214,0.11112,0.10434,0.46302,\n1403636626163555328,0.17355,-0.07162,-0.05833,-0.11081,0.80323,0.12378,0.57203,0.10497,0.08738,0.42731,\n1403636626263555584,0.19213,-0.05668,-0.02915,-0.11572,0.80336,0.11800,0.57210,0.09292,0.04708,0.30122,\n1403636626363555328,0.20203,-0.05522,-0.00665,-0.11675,0.80319,0.11765,0.57220,0.08904,0.00180,0.15255,\n1403636626463555584,0.21232,-0.05605,0.00185,-0.11301,0.80193,0.12060,0.57410,0.09130,-0.03075,0.07086,\n1403636626563555584,0.22136,-0.06118,0.00593,-0.10821,0.79727,0.12507,0.58053,0.07418,-0.06254,0.03176,\n1403636626663555328,0.22723,-0.06892,0.00709,-0.10470,0.79297,0.12994,0.58597,0.03428,-0.08609,-0.01476,\n1403636626763555584,0.22884,-0.07847,0.00292,-0.10456,0.79419,0.13278,0.58370,-0.00848,-0.09840,-0.05978,\n1403636626863555328,0.22638,-0.08900,-0.00555,-0.10443,0.79776,0.13492,0.57834,-0.04141,-0.10702,-0.10221,\n1403636626963555584,0.22117,-0.10045,-0.01707,-0.10285,0.80275,0.13694,0.57120,-0.05818,-0.11439,-0.12406,\n1403636627063555584,0.21477,-0.11202,-0.02975,-0.10123,0.80749,0.13820,0.56446,-0.06522,-0.11700,-0.11405,\n1403636627163555328,0.20926,-0.12260,-0.03465,-0.10128,0.80957,0.13877,0.56132,-0.04661,-0.10196,0.03076,\n1403636627263555584,0.20526,-0.13251,-0.02739,-0.10174,0.80569,0.13879,0.56678,-0.03907,-0.09393,0.14107,\n1403636627363555328,0.20140,-0.14196,-0.01115,-0.10439,0.80308,0.13750,0.57032,-0.05226,-0.10058,0.20638,\n1403636627463555584,0.19667,-0.15183,0.01341,-0.10633,0.80247,0.13564,0.57126,-0.02940,-0.10251,0.28090,\n1403636627563555584,0.18351,-0.16842,0.04094,-0.10457,0.80461,0.12977,0.56994,-0.05352,-0.13010,0.14272,\n1403636627663555328,0.17698,-0.18159,0.04759,-0.09940,0.80820,0.12354,0.56715,-0.06641,-0.13009,-0.03012,\n1403636627763555584,0.17081,-0.19680,0.04004,-0.09143,0.81559,0.11663,0.55933,-0.04980,-0.13892,-0.11567,\n1403636627863555328,0.16773,-0.21254,0.02735,-0.08153,0.82305,0.10792,0.55163,-0.01239,-0.14825,-0.12669,\n1403636627963555584,0.16859,-0.22691,0.01846,-0.07297,0.82818,0.10153,0.54633,0.04246,-0.12588,-0.06638,\n1403636628063555584,0.18107,-0.23920,0.01937,-0.06780,0.82814,0.09660,0.54795,0.13058,-0.08707,0.08725,\n1403636628163555328,0.19404,-0.24796,0.03365,-0.06747,0.82007,0.09024,0.56106,0.15076,-0.07853,0.17398,\n1403636628263555584,0.20928,-0.25555,0.05457,-0.07140,0.81237,0.08319,0.57275,0.15174,-0.07443,0.22238,\n1403636628363555328,0.22390,-0.26402,0.07853,-0.07689,0.80939,0.07753,0.57703,0.14406,-0.09853,0.24397,\n1403636628463555584,0.23771,-0.27486,0.10104,-0.07977,0.80930,0.07505,0.57709,0.13351,-0.12504,0.18622,\n1403636628563555584,0.25078,-0.28920,0.11774,-0.08222,0.81007,0.07418,0.57578,0.12490,-0.15659,0.13322,\n1403636628663555328,0.26249,-0.30646,0.12709,-0.08413,0.81034,0.07469,0.57506,0.11820,-0.19835,0.07159,\n1403636628763555584,0.27361,-0.32820,0.12989,-0.07773,0.81187,0.07560,0.57368,0.10777,-0.24461,-0.00503,\n1403636628863555328,0.28450,-0.35358,0.12690,-0.06733,0.81672,0.07402,0.56829,0.12420,-0.25914,-0.02362,\n1403636628963555584,0.28499,-0.38244,0.13105,-0.05885,0.82170,0.07014,0.56252,0.07773,-0.25980,0.06512,\n1403636629063555584,0.29170,-0.40924,0.14291,-0.05454,0.82465,0.06637,0.55908,0.05246,-0.27372,0.16563,\n1403636629163555328,0.30093,-0.43647,0.15974,-0.04960,0.82565,0.06153,0.55861,0.08458,-0.28194,0.14507,\n1403636629263555584,0.31517,-0.46365,0.17199,-0.04400,0.82485,0.05268,0.56117,0.12583,-0.27289,0.10880,\n1403636629363555328,0.33523,-0.48910,0.17851,-0.04248,0.82288,0.04351,0.56496,0.16169,-0.25747,0.05791,\n1403636629463555584,0.35826,-0.51442,0.17869,-0.04446,0.81978,0.03208,0.57005,0.18393,-0.28442,-0.00949,\n1403636629563555584,0.38422,-0.54074,0.17043,-0.04215,0.81965,0.01827,0.57101,0.19890,-0.30838,-0.09430,\n1403636629663555328,0.41245,-0.57073,0.15341,-0.03113,0.82479,0.00586,0.56456,0.22498,-0.33872,-0.16801,\n1403636629763555584,0.44187,-0.58276,0.13523,-0.01880,0.83041,-0.00495,0.55681,0.24916,-0.35913,-0.22004,\n1403636629863555328,0.47391,-0.61505,0.10955,-0.00409,0.83359,-0.02205,0.55193,0.30110,-0.35512,-0.25622,\n1403636629963555584,0.50985,-0.64749,0.08560,0.00835,0.83458,-0.03721,0.54956,0.31634,-0.34803,-0.19415,\n1403636630063555584,0.54564,-0.67931,0.06899,0.01651,0.83313,-0.04805,0.55073,0.35422,-0.33709,-0.11728,\n1403636630163555328,0.58816,-0.71086,0.06234,0.02229,0.82623,-0.05537,0.56016,0.39762,-0.33162,-0.03481,\n1403636630263555584,0.63569,-0.74041,0.06352,0.02623,0.81868,-0.05990,0.57051,0.43109,-0.32780,0.03716,\n1403636630363555328,0.68392,-0.76984,0.06966,0.02788,0.81695,-0.06138,0.57275,0.43795,-0.31785,0.07079,\n1403636630463555584,0.75626,-0.79466,0.06669,0.03147,0.81923,-0.06406,0.56900,0.44860,-0.32380,0.13071,\n1403636630563555584,0.79998,-0.82875,0.08224,0.03381,0.82110,-0.06038,0.56657,0.46395,-0.31458,0.17655,\n1403636630663555328,0.84893,-0.86252,0.09900,0.03554,0.82237,-0.05497,0.56518,0.48133,-0.30001,0.13357,\n1403636630663555328,0.87137,-0.91561,0.12440,0.03370,0.82299,-0.05244,0.56462,0.50257,-0.26839,0.07297,\n1403636630763555584,0.89365,-0.89326,0.11028,0.03425,0.82284,-0.05296,0.56476,0.49523,-0.27196,0.07421,\n1403636630863555328,0.93835,-0.92267,0.11648,0.03025,0.82323,-0.05315,0.56441,0.50066,-0.25887,0.00949,\n1403636630963555584,0.98248,-0.95573,0.11768,0.02645,0.82280,-0.05399,0.56515,0.50072,-0.26510,-0.04086,\n1403636631063555584,1.02814,-0.98371,0.11341,0.02523,0.82200,-0.05419,0.56635,0.51233,-0.25562,-0.07435,\n1403636631163555328,1.07255,-1.01295,0.10743,0.02529,0.81999,-0.05434,0.56923,0.50600,-0.25088,-0.09452,\n1403636631263555584,1.11398,-1.04619,0.10041,0.02568,0.81775,-0.05382,0.57248,0.49506,-0.26504,-0.09717,\n1403636631363555328,1.15830,-1.07584,0.09160,0.02568,0.81759,-0.05341,0.57275,0.49243,-0.26460,-0.14015,\n1403636631463555584,1.19556,-1.10791,0.07969,0.02682,0.81748,-0.05274,0.57291,0.47096,-0.26389,-0.19208,\n1403636631563555584,1.23901,-1.13671,0.06232,0.02868,0.81745,-0.05061,0.57305,0.46152,-0.25355,-0.20801,\n1403636631663555328,1.28179,-1.16590,0.04678,0.02969,0.81808,-0.04838,0.57230,0.46718,-0.24843,-0.18026,\n1403636631763555584,1.32414,-1.19368,0.03627,0.03036,0.81856,-0.04683,0.57170,0.43814,-0.23494,-0.09760,\n1403636631863555328,1.36057,-1.22122,0.03714,0.03181,0.81868,-0.04538,0.57157,0.48029,-0.23648,-0.01616,\n1403636631963555584,1.40146,-1.24342,0.03649,0.03320,0.81941,-0.04449,0.57051,0.48188,-0.22278,0.01737,\n1403636632063555584,1.44121,-1.26398,0.03793,0.03430,0.81830,-0.04501,0.57200,0.44821,-0.18867,0.01298,\n1403636632163555328,1.47975,-1.28234,0.04036,0.03909,0.81643,-0.04296,0.57452,0.43922,-0.17353,0.02175,\n1403636632263555584,1.51430,-1.30185,0.04555,0.04032,0.81609,-0.04376,0.57485,0.42895,-0.14470,-0.02336,\n1403636632363555328,1.55255,-1.31513,0.03987,0.03957,0.81570,-0.04574,0.57530,0.40229,-0.11127,-0.10306,\n1403636632463555584,1.59007,-1.32587,0.02785,0.03755,0.81791,-0.04872,0.57205,0.39109,-0.08048,-0.16560,\n1403636632563555584,1.62617,-1.33669,0.01261,0.02729,0.82310,-0.05130,0.56492,0.38649,-0.06503,-0.20889,\n1403636632663555328,1.66742,-1.34438,-0.00851,0.01375,0.82830,-0.04872,0.55799,0.41251,-0.06902,-0.21871,\n1403636632763555584,1.70812,-1.35555,-0.02500,0.00009,0.83060,-0.03715,0.55563,0.43901,-0.08377,-0.14930,\n1403636632863555328,1.75387,-1.36863,-0.03483,-0.00881,0.82876,-0.01456,0.55935,0.46278,-0.10814,-0.07071,\n1403636632963555584,1.80303,-1.38111,-0.03734,-0.01458,0.82450,0.01021,0.56558,0.49072,-0.12120,-0.00513,\n1403636633063555584,1.85407,-1.39353,-0.03520,-0.01941,0.82207,0.02910,0.56832,0.49187,-0.12261,0.02145,\n1403636633163555328,1.90285,-1.40337,-0.03459,-0.02476,0.82091,0.04151,0.56902,0.49294,-0.10714,-0.03577,\n1403636633263555584,1.95476,-1.41205,-0.04239,-0.03041,0.82112,0.04782,0.56794,0.49403,-0.09133,-0.10275,\n1403636633363555328,2.00651,-1.41983,-0.05449,-0.03679,0.82273,0.04917,0.56510,0.49515,-0.08787,-0.15634,\n1403636633463555584,2.05859,-1.42845,-0.06968,-0.04138,0.82506,0.04758,0.56151,0.51403,-0.07782,-0.19441,\n1403636633563555584,2.11481,-1.43559,-0.08742,-0.04260,0.82705,0.04537,0.55867,0.54374,-0.08087,-0.21370,\n1403636633663555328,2.17304,-1.44495,-0.10437,-0.04119,0.82916,0.04342,0.55580,0.59502,-0.09437,-0.13743,\n1403636633763555584,2.23320,-1.45291,-0.11311,-0.04013,0.82880,0.04035,0.55665,0.62571,-0.09816,-0.07226,\n1403636633863555328,2.29651,-1.46309,-0.11601,-0.04014,0.82828,0.03652,0.55768,0.65503,-0.10104,0.00208,\n1403636633963555584,2.36192,-1.47258,-0.11313,-0.04512,0.82701,0.02927,0.55960,0.67182,-0.10656,0.00508,\n1403636634063555584,2.43010,-1.48497,-0.11606,-0.04779,0.82600,0.02353,0.56115,0.69002,-0.13917,-0.02458,\n1403636634163555328,2.50033,-1.49933,-0.11516,-0.04723,0.82697,0.02461,0.55972,0.71859,-0.17025,0.04633,\n1403636634263555584,2.57375,-1.51794,-0.10899,-0.04207,0.82745,0.03008,0.55915,0.75607,-0.19568,0.10924,\n1403636634363555328,2.65147,-1.53820,-0.09487,-0.03807,0.82307,0.03527,0.56557,0.77593,-0.20129,0.12445,\n1403636634463555584,2.72944,-1.55836,-0.08794,-0.03521,0.81857,0.03943,0.57197,0.77251,-0.20303,0.05411,\n1403636634563555584,2.80558,-1.57950,-0.08627,-0.03499,0.81516,0.04246,0.57661,0.75894,-0.20015,-0.01256,\n1403636634663555328,2.87984,-1.59799,-0.09093,-0.03639,0.81382,0.04857,0.57794,0.72798,-0.18161,-0.07166,\n1403636634763555584,2.95467,-1.61589,-0.09690,-0.03918,0.81416,0.05698,0.57652,0.70389,-0.16236,-0.04837,\n1403636634863555328,3.02328,-1.63213,-0.09872,-0.04358,0.81615,0.06257,0.57278,0.67164,-0.14911,0.00644,\n1403636634963555584,3.09383,-1.64620,-0.09472,-0.04711,0.81802,0.06864,0.56914,0.68467,-0.13024,0.07692,\n1403636635063555584,3.16056,-1.65876,-0.08604,-0.04866,0.81867,0.07439,0.56735,0.67552,-0.11371,0.08138,\n1403636635163555328,3.22783,-1.67026,-0.08346,-0.05053,0.81964,0.07777,0.56532,0.66835,-0.10399,0.00887,\n1403636635263555584,3.29476,-1.68063,-0.08530,-0.05410,0.82097,0.07877,0.56292,0.67375,-0.08976,-0.04955,\n1403636635363555328,3.36239,-1.68975,-0.09367,-0.06014,0.82288,0.07646,0.55983,0.68467,-0.08171,-0.04882,\n1403636635363555328,3.32491,-1.71140,-0.07289,-0.06062,0.82281,0.07718,0.55977,0.68481,-0.08051,-0.04882,\n1403636635463555584,3.39625,-1.72005,-0.07640,-0.06375,0.82331,0.07593,0.55887,0.72623,-0.08687,-0.00258,\n1403636635563555584,3.47266,-1.72965,-0.07435,-0.06698,0.82375,0.07353,0.55817,0.69226,-0.10120,0.07645,\n1403636635663555328,3.54404,-1.74037,-0.06584,-0.06808,0.82293,0.07167,0.55948,0.72009,-0.11740,0.10152,\n1403636635763555584,3.62456,-1.75257,-0.05291,-0.06987,0.82048,0.06897,0.56318,0.77085,-0.12703,0.18463,\n1403636635863555328,3.70156,-1.76588,-0.02792,-0.07579,0.81712,0.06291,0.56799,0.77930,-0.16292,0.24886,\n1403636635963555584,3.78447,-1.78394,-0.00075,-0.07854,0.82005,0.05948,0.56375,0.79980,-0.20566,0.27207,\n1403636636063555584,3.86666,-1.80616,0.02573,-0.07185,0.82521,0.06315,0.55667,0.81700,-0.23808,0.23475,\n1403636636163555328,3.95033,-1.83011,0.05013,-0.06391,0.82553,0.06849,0.55653,0.84513,-0.25061,0.27293,\n1403636636263555584,4.03263,-1.85612,0.08194,-0.05815,0.81889,0.07274,0.56635,0.83854,-0.25506,0.33572,\n1403636636363555328,4.11508,-1.87983,0.12003,-0.05604,0.81171,0.07411,0.57662,0.83303,-0.24284,0.37483,\n1403636636463555584,4.19185,-1.90360,0.15368,-0.05600,0.80241,0.07470,0.58942,0.77835,-0.24427,0.29423,\n1403636636563555584,4.26531,-1.92674,0.17781,-0.05065,0.79963,0.07846,0.59319,0.71717,-0.23247,0.21170,\n1403636636663555328,4.33096,-1.94769,0.19442,-0.04298,0.79912,0.08380,0.59375,0.65096,-0.21242,0.12015,\n1403636636763555584,4.39706,-1.96644,0.20525,-0.03619,0.79913,0.08849,0.59351,0.58991,-0.16346,0.03575,\n1403636636863555328,4.45998,-1.97943,0.20634,-0.02791,0.79964,0.09467,0.59232,0.52794,-0.10583,-0.04524,\n1403636636963555584,4.51656,-1.98900,0.20262,-0.03579,0.80308,0.09077,0.58783,0.48073,-0.05033,-0.10914,\n1403636637063555584,4.56926,-1.99178,0.19234,-0.04798,0.80629,0.08410,0.58354,0.44323,-0.01850,-0.14885,\n1403636637163555328,4.61884,-1.99293,0.17853,-0.05481,0.81025,0.08065,0.57791,0.41916,-0.01093,-0.17843,\n1403636637263555584,4.66860,-1.99492,0.16503,-0.05648,0.81460,0.08051,0.57162,0.41909,-0.00088,-0.14719,\n1403636637363555328,4.71191,-1.99312,0.15593,-0.05626,0.81706,0.08167,0.56796,0.48717,0.02009,-0.04523,\n1403636637463555584,4.76182,-1.98968,0.15526,-0.05735,0.81850,0.08306,0.56556,0.49715,0.03514,0.06835,\n1403636637563555584,4.80325,-1.98545,0.15994,-0.05896,0.81957,0.08345,0.56380,0.46325,0.03651,0.09863,\n1403636637663555328,4.84610,-1.98166,0.17033,-0.06123,0.82012,0.08321,0.56278,0.47382,0.03499,0.17663,\n1403636637763555584,4.89394,-1.97622,0.19004,-0.06170,0.81884,0.08413,0.56446,0.52982,0.04879,0.22717,\n1403636637863555328,4.93782,-1.96983,0.21648,-0.06208,0.81646,0.08480,0.56775,0.53290,0.06003,0.31408,\n1403636637963555584,4.98440,-1.96152,0.24375,-0.06308,0.81372,0.08500,0.57154,0.52972,0.06300,0.23792,\n1403636638063555584,5.03165,-1.95172,0.25532,-0.06404,0.81157,0.08505,0.57447,0.51176,0.07172,0.05644,\n1403636638163555328,5.07866,-1.94366,0.25263,-0.06530,0.80940,0.08482,0.57742,0.49188,0.07614,-0.05616,\n1403636638263555584,5.12114,-1.93538,0.24139,-0.06542,0.80868,0.08541,0.57833,0.45378,0.06615,-0.11374,\n1403636638363555328,5.16141,-1.92885,0.22509,-0.07064,0.81333,0.08251,0.57157,0.41852,0.04937,-0.19991,\n1403636638463555584,5.19924,-1.92434,0.20255,-0.07753,0.82513,0.07795,0.55415,0.43076,0.04567,-0.23016,\n1403636638563555584,5.23893,-1.92306,0.18246,-0.08186,0.83271,0.07508,0.54245,0.43083,-0.00296,-0.17098,\n1403636638663555328,5.28798,-1.92429,0.17337,-0.07583,0.83258,0.07987,0.54285,0.51062,-0.02574,-0.02499,\n1403636638763555584,5.34872,-1.92601,0.17411,-0.07074,0.82812,0.08370,0.54974,0.56606,-0.03288,0.06607,\n1403636638863555328,5.40954,-1.92908,0.18232,-0.07048,0.82268,0.08405,0.55783,0.58810,-0.03652,0.11656,\n1403636638963555584,5.47689,-1.93026,0.19294,-0.07474,0.81951,0.08136,0.56231,0.62410,-0.04347,0.16237,\n1403636639063555584,5.54470,-1.93350,0.20558,-0.08429,0.81923,0.07457,0.56231,0.66265,-0.05702,0.12435,\n1403636639163555328,5.61514,-1.93931,0.20909,-0.09173,0.81960,0.06887,0.56134,0.68487,-0.09668,-0.01745,\n1403636639263555584,5.68553,-1.95008,0.19752,-0.09519,0.82041,0.06580,0.55994,0.71686,-0.14582,-0.17178,\n1403636639363555328,5.75684,-1.96757,0.18004,-0.08584,0.82041,0.07224,0.56066,0.73620,-0.19513,-0.21984,\n1403636639463555584,5.83496,-1.98579,0.15611,-0.07471,0.82029,0.07982,0.56140,0.75262,-0.21609,-0.25650,\n1403636639563555584,5.91271,-2.00667,0.12904,-0.06289,0.81775,0.08811,0.56530,0.75161,-0.21493,-0.28793,\n1403636639663555328,5.98903,-2.02650,0.09986,-0.05747,0.81431,0.09284,0.57006,0.75537,-0.17925,-0.26368,\n1403636639763555584,6.06854,-2.04335,0.07487,-0.06209,0.80987,0.09013,0.57631,0.70270,-0.16794,-0.21780,\n1403636639863555328,6.14256,-2.05992,0.05821,-0.06671,0.80812,0.08735,0.57867,0.68382,-0.16239,-0.13639,\n1403636639963555584,6.21100,-2.07617,0.05063,-0.06966,0.80760,0.08598,0.57926,0.65087,-0.17155,-0.06729,\n1403636640063555584,6.27597,-2.09219,0.04860,-0.07104,0.80675,0.08891,0.57983,0.65269,-0.14997,0.04437,\n1403636640163555328,6.33935,-2.10721,0.05776,-0.07351,0.80556,0.09768,0.57977,0.62871,-0.14847,0.12540,\n1403636640263555584,6.40101,-2.12232,0.06893,-0.07747,0.80528,0.10554,0.57826,0.58107,-0.15335,0.16919,\n1403636640363555328,6.46084,-2.13982,0.07825,-0.08117,0.80487,0.11092,0.57731,0.55623,-0.15537,0.10042,\n1403636640463555584,6.51823,-2.15623,0.08111,-0.08307,0.80421,0.11561,0.57704,0.53055,-0.16135,0.03783,\n1403636640563555584,6.57852,-2.17314,0.07521,-0.08479,0.80447,0.11879,0.57579,0.51363,-0.16275,-0.02281,\n1403636640663555328,6.62916,-2.18754,0.06901,-0.08616,0.80393,0.12043,0.57600,0.49031,-0.15331,-0.07516,\n1403636640763555584,6.67765,-2.20074,0.05867,-0.08722,0.80516,0.12146,0.57390,0.48199,-0.14518,-0.06794,\n1403636640863555328,6.72240,-2.21298,0.05116,-0.08677,0.80662,0.12309,0.57156,0.45760,-0.13925,-0.02372,\n1403636640963555584,6.77185,-2.22528,0.04571,-0.08639,0.80711,0.12361,0.57082,0.42907,-0.13117,0.04166,\n1403636641063555584,6.81308,-2.23581,0.04868,-0.08658,0.80617,0.12272,0.57230,0.41057,-0.11670,0.02192,\n1403636641163555328,6.85462,-2.24492,0.04547,-0.08539,0.80604,0.12245,0.57272,0.39580,-0.10893,-0.04930,\n1403636641263555584,6.89435,-2.25278,0.03607,-0.08626,0.80512,0.12072,0.57426,0.37281,-0.09528,-0.11705,\n1403636641363555328,6.93293,-2.26000,0.02110,-0.09202,0.80618,0.11522,0.57300,0.35335,-0.09503,-0.16692,\n1403636641463555584,6.97019,-2.26900,-0.00222,-0.09538,0.80635,0.11195,0.57287,0.34804,-0.11982,-0.17979,\n1403636641563555584,7.00607,-2.27910,-0.01772,-0.09207,0.80619,0.11348,0.57332,0.29025,-0.13873,-0.17003,\n1403636641663555328,7.04060,-2.29023,-0.02755,-0.08802,0.80628,0.11515,0.57350,0.28357,-0.13787,-0.09088,\n1403636641763555584,7.07381,-2.30186,-0.02843,-0.08573,0.80611,0.11576,0.57397,0.26395,-0.14209,0.00727,\n1403636641863555328,7.10591,-2.31287,-0.02253,-0.08508,0.80520,0.11572,0.57535,0.30230,-0.12138,0.06301,\n1403636641963555584,7.13769,-2.32377,-0.01294,-0.08325,0.80533,0.11165,0.57624,0.29048,-0.12690,0.12459,\n1403636642063555584,7.16343,-2.33759,0.00273,-0.07851,0.80512,0.10385,0.57865,0.28613,-0.13459,0.15281,\n1403636642163555328,7.18682,-2.35028,0.01722,-0.07275,0.80511,0.09745,0.58053,0.26446,-0.12709,0.10133,\n1403636642263555584,7.20672,-2.36305,0.02507,-0.06969,0.80497,0.09152,0.58206,0.23660,-0.12166,0.03861,\n1403636642363555328,7.22699,-2.37727,0.02560,-0.06537,0.80524,0.08344,0.58340,0.20416,-0.13421,-0.03600,\n1403636642463555584,7.24516,-2.38882,0.02034,-0.06087,0.80553,0.07568,0.58454,0.18309,-0.13378,-0.10353,\n1403636642563555584,7.25825,-2.40234,0.00931,-0.05679,0.80860,0.07064,0.58133,0.14643,-0.13389,-0.15625,\n1403636642663555328,7.27379,-2.41291,-0.00710,-0.05471,0.81383,0.06674,0.57466,0.13939,-0.12657,-0.19993,\n1403636642763555584,7.28911,-2.42445,-0.02428,-0.05292,0.81848,0.06483,0.56840,0.16465,-0.11513,-0.16223,\n1403636642863555328,7.30256,-2.43617,-0.03139,-0.05299,0.82133,0.06316,0.56446,0.15838,-0.11042,-0.05880,\n1403636642963555584,7.31576,-2.44572,-0.03603,-0.05332,0.82273,0.06243,0.56247,0.14468,-0.11487,0.01664,\n1403636643063555584,7.33142,-2.45782,-0.02786,-0.05413,0.82036,0.06189,0.56590,0.20458,-0.12469,0.07838,\n1403636643163555328,7.35123,-2.47134,-0.02392,-0.05598,0.81603,0.06109,0.57204,0.20164,-0.13576,0.01002,\n1403636643263555584,7.36835,-2.48531,-0.02709,-0.05708,0.81388,0.06089,0.57500,0.19016,-0.14815,-0.05597,\n1403636643363555328,7.38959,-2.49837,-0.02968,-0.05655,0.81534,0.06177,0.57289,0.20135,-0.15342,-0.05816,\n1403636643463555584,7.41110,-2.51310,-0.03134,-0.05787,0.81881,0.06182,0.56778,0.23370,-0.15622,0.02193,\n1403636643563555584,7.43596,-2.52855,-0.02678,-0.05798,0.82284,0.06324,0.56177,0.30356,-0.16458,0.08490,\n1403636643663555328,7.46256,-2.54402,-0.01459,-0.05720,0.82657,0.06546,0.55608,0.31031,-0.16707,0.14281,\n1403636643763555584,7.48978,-2.55868,0.00636,-0.05643,0.82893,0.06737,0.55240,0.33506,-0.17163,0.19021,\n1403636643863555328,7.52482,-2.57352,0.02497,-0.05512,0.83203,0.06915,0.54763,0.39112,-0.16898,0.14658,\n1403636643963555584,7.56437,-2.58775,0.03827,-0.05392,0.83408,0.07053,0.54446,0.44975,-0.16099,0.09359,\n1403636644063555584,7.60882,-2.60292,0.04599,-0.05337,0.83487,0.07097,0.54324,0.49625,-0.14576,0.04713,\n1403636644163555328,7.66225,-2.61389,0.04809,-0.05284,0.83465,0.07118,0.54360,0.55657,-0.12757,0.00012,\n1403636644263555584,7.72016,-2.62418,0.04643,-0.06024,0.82804,0.06546,0.55356,0.61124,-0.11015,-0.03452,\n1403636644363555328,7.77998,-2.63658,0.03853,-0.07136,0.81941,0.05683,0.56590,0.62561,-0.14032,-0.09756,\n1403636644463555584,7.83988,-2.65148,0.02319,-0.07743,0.81314,0.05187,0.57455,0.62509,-0.18855,-0.18335,\n1403636644563555584,7.89903,-2.66941,-0.00093,-0.08556,0.80890,0.04610,0.57986,0.61127,-0.24069,-0.25227,\n1403636644663555328,7.95810,-2.69405,-0.03112,-0.08277,0.80727,0.04742,0.58243,0.58777,-0.31030,-0.32236,\n1403636644763555584,8.01610,-2.72547,-0.06623,-0.06869,0.80689,0.05712,0.58391,0.55878,-0.35485,-0.38179,\n1403636644863555328,8.07364,-2.75778,-0.10747,-0.05491,0.80752,0.06509,0.58367,0.52849,-0.36266,-0.42806,\n1403636644963555584,8.12873,-2.79017,-0.14715,-0.05170,0.81095,0.06556,0.57912,0.49631,-0.35070,-0.36005,\n1403636645063555584,8.17531,-2.82226,-0.17972,-0.05113,0.82198,0.06448,0.56353,0.47829,-0.33953,-0.28802,\n1403636645163555328,8.22030,-2.85498,-0.20355,-0.04952,0.83432,0.06436,0.54526,0.47498,-0.34237,-0.21460,\n1403636645263555584,8.28021,-2.88769,-0.22001,-0.04906,0.84195,0.06375,0.53353,0.59033,-0.32672,-0.08701,\n1403636645363555328,8.34738,-2.91710,-0.22988,-0.04884,0.83925,0.06342,0.53783,0.67492,-0.31071,-0.05097,\n1403636645463555584,8.42638,-2.94514,-0.24360,-0.04825,0.83425,0.06257,0.54570,0.75101,-0.29143,-0.09411,\n1403636645563555584,8.50597,-2.97205,-0.25602,-0.04752,0.83173,0.06254,0.54959,0.81133,-0.27805,-0.15916,\n1403636645663555328,8.59321,-2.99545,-0.27368,-0.04641,0.82978,0.06261,0.55263,0.86145,-0.25753,-0.17659,\n1403636645763555584,8.68154,-3.01822,-0.28565,-0.04694,0.82857,0.06232,0.55442,0.91933,-0.22838,-0.10091,\n1403636645863555328,8.77709,-3.03650,-0.29088,-0.04788,0.82892,0.06100,0.55396,0.93787,-0.22408,0.03212,\n1403636645963555584,8.87385,-3.05843,-0.28379,-0.04964,0.82680,0.05809,0.55729,0.98303,-0.22640,0.14354,\n1403636646063555584,8.97226,-3.07798,-0.26984,-0.04998,0.82302,0.04738,0.56382,0.95638,-0.24280,0.19892,\n1403636646163555328,9.07238,-3.10198,-0.25433,-0.04482,0.82004,0.02992,0.56976,0.99757,-0.27338,0.16761,\n1403636646263555584,9.17407,-3.12886,-0.24181,-0.03661,0.81694,0.00651,0.57553,0.98092,-0.31631,0.10894,\n1403636646363555328,9.27240,-3.16007,-0.23376,-0.02534,0.81212,-0.02025,0.58259,0.94500,-0.35846,0.04655,\n1403636646463555584,9.36320,-3.19496,-0.23129,-0.00584,0.80670,-0.04101,0.58950,0.90289,-0.38802,0.01887,\n1403636646563555584,9.45148,-3.23312,-0.22921,0.01304,0.80154,-0.05394,0.59536,0.82125,-0.41354,0.02082,\n1403636646663555328,9.53629,-3.27323,-0.22248,0.02634,0.80219,-0.06294,0.59315,0.77500,-0.41394,0.10823,\n1403636646763555584,9.61781,-3.31347,-0.20649,0.03360,0.80331,-0.07041,0.59042,0.74153,-0.40514,0.19447,\n1403636646863555328,9.69466,-3.35311,-0.18089,0.03866,0.80263,-0.07507,0.59046,0.68654,-0.40536,0.26489,\n1403636646963555584,9.76526,-3.39265,-0.15337,0.04017,0.80380,-0.07773,0.58843,0.64499,-0.40550,0.23146,\n1403636647063555584,9.83070,-3.43181,-0.13036,0.04149,0.80610,-0.07815,0.58513,0.61480,-0.39635,0.17153,\n1403636647163555328,9.89036,-3.47073,-0.11455,0.04132,0.81028,-0.07816,0.57934,0.59825,-0.39693,0.11953,\n1403636647263555584,9.95133,-3.50975,-0.09940,0.04080,0.81153,-0.07753,0.57771,0.58881,-0.39739,0.08686,\n1403636647363555328,10.01019,-3.54927,-0.09181,0.03761,0.81002,-0.07749,0.58004,0.57824,-0.40858,0.04454,\n1403636647463555584,10.06755,-3.58976,-0.08814,0.03439,0.80789,-0.07717,0.58325,0.54757,-0.41271,-0.00347,\n1403636647563555584,10.12119,-3.63076,-0.08686,0.02987,0.80353,-0.07706,0.58949,0.50591,-0.42680,0.00514,\n1403636647663555328,10.17327,-3.67445,-0.07912,0.02723,0.79982,-0.07933,0.59435,0.46823,-0.45956,0.04120,\n1403636647763555584,10.21818,-3.71978,-0.06735,0.03085,0.80194,-0.07943,0.59129,0.44804,-0.48777,0.10073,\n1403636647863555328,10.25739,-3.76723,-0.05957,0.03630,0.80350,-0.08170,0.58856,0.39372,-0.50458,0.05261,\n1403636647963555584,10.28703,-3.81343,-0.05386,0.04421,0.80293,-0.08978,0.58761,0.33547,-0.51299,-0.02918,\n1403636648063555584,10.30929,-3.86315,-0.05968,0.05582,0.80207,-0.10418,0.58542,0.27132,-0.52592,-0.13074,\n1403636648163555328,10.33127,-3.91614,-0.07251,0.07005,0.79974,-0.12429,0.58315,0.24644,-0.54026,-0.17342,\n1403636648263555584,10.35143,-3.97000,-0.08940,0.08530,0.79600,-0.14594,0.58121,0.18681,-0.54622,-0.21592,\n1403636648363555328,10.36575,-4.02416,-0.11124,0.10145,0.79146,-0.16849,0.57871,0.13906,-0.54159,-0.24887,\n1403636648463555584,10.37552,-4.07885,-0.13725,0.11808,0.78680,-0.19175,0.57466,0.08620,-0.53844,-0.28218,\n1403636648563555584,10.38265,-4.12974,-0.16346,0.13286,0.78139,-0.21358,0.57110,0.04771,-0.52044,-0.24058,\n1403636648663555328,10.38681,-4.18092,-0.18340,0.14483,0.77821,-0.22889,0.56659,-0.00716,-0.49282,-0.18062,\n1403636648763555584,10.38894,-4.23176,-0.19807,0.15338,0.77896,-0.23901,0.55908,-0.00273,-0.48999,-0.12713,\n1403636648863555328,10.39465,-4.28336,-0.20514,0.15477,0.78537,-0.24695,0.54612,0.00797,-0.49807,-0.03699,\n1403636648963555584,10.40270,-4.33599,-0.20129,0.15579,0.79122,-0.25021,0.53582,0.04982,-0.52312,0.06709,\n1403636649063555584,10.41342,-4.38801,-0.18610,0.15863,0.79000,-0.24980,0.53696,0.11303,-0.54662,0.17118,\n1403636649163555328,10.42569,-4.43959,-0.16256,0.16225,0.78193,-0.24665,0.54903,0.12600,-0.54124,0.23093,\n1403636649263555584,10.43625,-4.49064,-0.13429,0.16543,0.77560,-0.24251,0.55881,0.12152,-0.52982,0.29789,\n1403636649363555328,10.44343,-4.53804,-0.10123,0.16505,0.77587,-0.24001,0.55963,0.08434,-0.49560,0.32780,\n1403636649463555584,10.44845,-4.58347,-0.06320,0.16247,0.78008,-0.23817,0.55529,0.05897,-0.47003,0.41172,\n1403636649563555584,10.45387,-4.62763,-0.01688,0.15984,0.77922,-0.23533,0.55847,0.05821,-0.46253,0.48815,\n1403636649663555328,10.45697,-4.67118,0.03269,0.15823,0.77452,-0.23727,0.56462,0.01219,-0.44680,0.46548,\n1403636649763555584,10.45336,-4.71011,0.07795,0.16241,0.77020,-0.24392,0.56649,-0.03406,-0.43084,0.39453,\n1403636649863555328,10.45007,-4.74969,0.11664,0.16896,0.76728,-0.24885,0.56640,-0.06023,-0.41170,0.32473,\n1403636649963555584,10.44414,-4.78541,0.14767,0.17471,0.76545,-0.25275,0.56541,-0.07429,-0.39306,0.23094,\n1403636650063555584,10.43484,-4.82046,0.16772,0.17871,0.76394,-0.25649,0.56451,-0.11429,-0.36444,0.11807,\n1403636650163555328,10.41975,-4.85544,0.17776,0.18450,0.76079,-0.26476,0.56308,-0.17745,-0.34384,0.03832,\n1403636650263555584,10.40117,-4.88767,0.17796,0.19591,0.75630,-0.27946,0.55815,-0.22066,-0.31892,-0.03868,\n1403636650363555328,10.37772,-4.92139,0.17622,0.20796,0.75043,-0.29991,0.55105,-0.23999,-0.31913,-0.07447,\n1403636650463555584,10.35117,-4.95297,0.17069,0.22223,0.74488,-0.32165,0.54066,-0.28850,-0.28057,-0.11631,\n1403636650563555584,10.32299,-4.98009,0.16014,0.23344,0.74026,-0.34255,0.52933,-0.28837,-0.25333,-0.14769,\n1403636650663555328,10.29437,-5.00427,0.14425,0.24165,0.73621,-0.35726,0.52151,-0.28421,-0.23720,-0.18006,\n1403636650763555584,10.26651,-5.02850,0.12874,0.24839,0.73262,-0.36736,0.51636,-0.28048,-0.22213,-0.13093,\n1403636650863555328,10.24048,-5.05076,0.12119,0.25446,0.72801,-0.37263,0.51614,-0.26379,-0.22331,-0.02776,\n1403636650963555584,10.21672,-5.07114,0.12144,0.26023,0.72216,-0.37366,0.52072,-0.26064,-0.20622,0.03702,\n1403636651063555584,10.19266,-5.09035,0.12742,0.26374,0.71914,-0.37297,0.52362,-0.25517,-0.18173,0.10322,\n1403636651163555328,10.16801,-5.10882,0.13943,0.26396,0.71867,-0.37218,0.52471,-0.26891,-0.15767,0.15162,\n1403636651263555584,10.13850,-5.12180,0.15514,0.26152,0.72167,-0.37189,0.52202,-0.28709,-0.12634,0.20029,\n1403636651363555328,10.10957,-5.13347,0.17449,0.25766,0.72584,-0.37154,0.51839,-0.28438,-0.11384,0.19245,\n1403636651463555584,10.07899,-5.14285,0.18907,0.25498,0.72840,-0.36978,0.51737,-0.29212,-0.10319,0.11515,\n1403636651563555584,10.04796,-5.15090,0.19681,0.25404,0.72773,-0.36676,0.52093,-0.28942,-0.09476,0.05872,\n1403636651663555328,10.01667,-5.15674,0.19885,0.25476,0.72463,-0.36321,0.52735,-0.30403,-0.06884,0.00257,\n1403636651763555584,9.98373,-5.16091,0.19540,0.25255,0.72106,-0.36502,0.53202,-0.34122,-0.05647,-0.04248,\n1403636651863555328,9.94579,-5.16188,0.18882,0.25160,0.71376,-0.37443,0.53577,-0.39940,-0.02600,-0.09129,\n1403636651963555584,9.90412,-5.16361,0.17842,0.25684,0.70503,-0.38842,0.53488,-0.45186,-0.02217,-0.13907,\n1403636652063555584,9.85377,-5.16286,0.16371,0.27025,0.69583,-0.40522,0.52781,-0.52418,-0.02042,-0.19961,\n1403636652163555328,9.79680,-5.16161,0.14507,0.29000,0.68768,-0.42511,0.51213,-0.58080,0.00169,-0.21163,\n1403636652263555584,9.73524,-5.15725,0.12282,0.30847,0.67706,-0.45099,0.49299,-0.62172,0.07828,-0.28232,\n1403636652363555328,9.66639,-5.14957,0.09470,0.32212,0.66640,-0.47937,0.47154,-0.63730,0.08624,-0.32037,\n1403636652463555584,9.59950,-5.14410,0.06782,0.33269,0.65527,-0.50148,0.45658,-0.61331,0.06855,-0.28282,\n1403636652563555584,9.53438,-5.14262,0.04676,0.33791,0.64740,-0.51855,0.44475,-0.59884,0.03578,-0.20938,\n1403636652663555328,9.47202,-5.14888,0.03536,0.33990,0.64504,-0.52999,0.43302,-0.59070,-0.03999,-0.10891,\n1403636652763555584,9.41434,-5.16031,0.03436,0.34262,0.64367,-0.53512,0.42655,-0.55455,-0.09934,0.00003,\n1403636652863555328,9.36023,-5.17838,0.04271,0.34476,0.64196,-0.53667,0.42547,-0.52823,-0.17557,0.09374,\n1403636652963555584,9.30855,-5.20250,0.05884,0.34820,0.64078,-0.53420,0.42753,-0.51379,-0.23374,0.14859,\n1403636653063555584,9.25986,-5.22717,0.06939,0.35170,0.64017,-0.53038,0.43034,-0.48651,-0.26838,0.02613,\n1403636653163555328,9.21161,-5.26125,0.06895,0.35472,0.63879,-0.52518,0.43625,-0.46256,-0.32516,-0.08434,\n1403636653263555584,9.16455,-5.29269,0.06045,0.35981,0.63585,-0.51784,0.44506,-0.45214,-0.32264,-0.07093,\n1403636653363555328,9.11803,-5.32702,0.05929,0.36273,0.63552,-0.51117,0.45083,-0.43031,-0.31509,0.00072,\n1403636653463555584,9.07265,-5.35722,0.06557,0.36161,0.63753,-0.50764,0.45287,-0.41880,-0.29159,0.08512,\n1403636653563555584,9.02937,-5.38373,0.07735,0.35910,0.64015,-0.50516,0.45394,-0.40885,-0.27197,0.16205,\n1403636653663555328,8.98730,-5.40509,0.09700,0.35937,0.63957,-0.50209,0.45794,-0.40435,-0.24453,0.18370,\n1403636653763555584,8.94738,-5.42601,0.11125,0.36100,0.63823,-0.49895,0.46194,-0.39338,-0.21003,0.11377,\n1403636653863555328,8.90745,-5.44220,0.11831,0.36279,0.63789,-0.49645,0.46370,-0.39497,-0.16894,0.04419,\n1403636653963555584,8.86491,-5.45124,0.11839,0.36107,0.63804,-0.49732,0.46390,-0.38985,-0.11970,-0.02778,\n1403636654063555584,8.82626,-5.45830,0.11042,0.35647,0.63792,-0.50057,0.46413,-0.38850,-0.08347,-0.09025,\n1403636654163555328,8.78719,-5.46107,0.09996,0.35725,0.63432,-0.50396,0.46479,-0.40132,-0.05848,-0.06125,\n1403636654263555584,8.74647,-5.46076,0.09419,0.36289,0.62962,-0.50941,0.46086,-0.42666,-0.02738,-0.00120,\n1403636654363555328,8.70281,-5.45522,0.09403,0.36937,0.62396,-0.51883,0.45283,-0.43760,0.00306,0.07803,\n1403636654463555584,8.65970,-5.44782,0.10245,0.37675,0.61631,-0.53124,0.44272,-0.43707,0.03546,0.15232,\n1403636654563555584,8.61703,-5.43898,0.12024,0.38684,0.60624,-0.54430,0.43194,-0.42702,0.06210,0.20197,\n1403636654663555328,8.57730,-5.42930,0.13888,0.38959,0.59706,-0.55913,0.42322,-0.40515,0.07139,0.16706,\n1403636654763555584,8.53858,-5.41923,0.15181,0.39260,0.59069,-0.56935,0.41568,-0.39014,0.07167,0.10099,\n1403636654863555328,8.50035,-5.40958,0.15709,0.39669,0.58592,-0.57583,0.40957,-0.37887,0.06619,0.02826,\n1403636654963555584,8.46401,-5.39987,0.15497,0.40350,0.58047,-0.57757,0.40823,-0.36648,0.06194,-0.02741,\n1403636655063555584,8.42955,-5.38889,0.14810,-0.41098,-0.57331,0.57625,-0.41273,-0.34557,0.07650,-0.07313,\n1403636655163555328,8.39504,-5.37504,0.13848,-0.41587,-0.56856,0.57506,-0.41605,-0.33682,0.11025,-0.10477,\n1403636655263555584,8.35963,-5.35817,0.12757,-0.41794,-0.56381,0.57573,-0.41950,-0.34930,0.15925,-0.12251,\n1403636655363555328,8.32458,-5.33949,0.11472,-0.42095,-0.55739,0.57974,-0.41954,-0.35170,0.21044,-0.12251,\n1403636655463555584,8.28863,-5.31091,0.10624,-0.42482,-0.55047,0.58484,-0.41770,-0.36325,0.27733,-0.05182,\n1403636655563555584,8.25066,-5.27614,0.10482,-0.42666,-0.54448,0.59439,-0.41012,-0.36988,0.36791,-0.02138,\n1403636655663555328,8.21098,-5.23597,0.10209,-0.42334,-0.53924,0.60962,-0.39795,-0.38000,0.40940,-0.06664,\n1403636655763555584,8.17082,-5.19550,0.08977,-0.42121,-0.53406,0.62210,-0.38775,-0.39847,0.40637,-0.18606,\n1403636655863555328,8.12722,-5.15511,0.06395,-0.42307,-0.52449,0.63406,-0.37933,-0.42532,0.40706,-0.30973,\n1403636655963555584,8.08020,-5.11573,0.03079,-0.42841,-0.51094,0.64627,-0.37112,-0.44706,0.39215,-0.30724,\n1403636656063555584,8.02990,-5.07744,0.00280,-0.43489,-0.49900,0.65474,-0.36494,-0.48515,0.40806,-0.22569,\n1403636656163555328,7.97565,-5.04029,-0.01593,-0.44042,-0.49283,0.66001,-0.35712,-0.51258,0.38573,-0.12597,\n1403636656263555584,7.92199,-5.00366,-0.02231,-0.44380,-0.49066,0.66344,-0.34949,-0.52100,0.38028,-0.01491,\n1403636656363555328,7.86708,-4.97101,-0.01917,-0.44406,-0.48927,0.66617,-0.34588,-0.53672,0.33447,0.08620,\n1403636656463555584,7.81232,-4.94533,-0.00491,-0.44400,-0.48938,0.66686,-0.34448,-0.55730,0.28757,0.16447,\n1403636656563555584,7.75790,-4.92651,0.01515,-0.44519,-0.49123,0.66514,-0.34364,-0.56536,0.23644,0.18088,\n1403636656663555328,7.70427,-4.90760,0.03429,-0.44942,-0.49078,0.66085,-0.34703,-0.56658,0.20310,0.15748,\n1403636656763555584,7.64788,-4.89291,0.04769,-0.45802,-0.48688,0.65264,-0.35668,-0.57470,0.20299,0.09121,\n1403636656863555328,7.59049,-4.87612,0.05260,-0.46460,-0.48711,0.64601,-0.35993,-0.57151,0.21494,0.01320,\n1403636656963555584,7.53532,-4.85527,0.05016,-0.46710,-0.48715,0.64214,-0.36354,-0.54227,0.24596,-0.06162,\n1403636657063555584,7.48198,-4.82922,0.04028,-0.46638,-0.48596,0.64092,-0.36819,-0.53074,0.28820,-0.13514,\n1403636657163555328,7.42977,-4.79762,0.02559,-0.46391,-0.48450,0.64119,-0.37272,-0.51624,0.34406,-0.13940,\n1403636657263555584,7.37998,-4.76196,0.01576,-0.45897,-0.48471,0.64383,-0.37401,-0.51596,0.37995,-0.08492,\n1403636657363555328,7.32768,-4.72050,0.00979,-0.45391,-0.48616,0.64739,-0.37215,-0.52377,0.41842,-0.04269,\n1403636657463555584,7.27829,-4.67603,0.00519,-0.44510,-0.48794,0.65398,-0.36892,-0.53762,0.43806,-0.07349,\n1403636657563555584,7.22685,-4.62954,-0.00331,-0.43911,-0.48970,0.65857,-0.36558,-0.56737,0.43973,-0.10902,\n1403636657663555328,7.17273,-4.58234,-0.01564,-0.43814,-0.49031,0.65953,-0.36422,-0.59000,0.44539,-0.14753,\n1403636657763555584,7.11333,-4.53554,-0.03180,-0.43899,-0.48790,0.66050,-0.36466,-0.62421,0.43957,-0.18769,\n1403636657863555328,7.05054,-4.49053,-0.05329,-0.43614,-0.48362,0.66512,-0.36539,-0.65172,0.44304,-0.23139,\n1403636657963555584,6.98430,-4.44221,-0.07285,-0.43060,-0.48048,0.67105,-0.36527,-0.70566,0.43561,-0.17684,\n1403636658063555584,6.91165,-4.39488,-0.08309,-0.43159,-0.48155,0.67217,-0.36059,-0.76048,0.45892,-0.06714,\n1403636658163555328,6.83406,-4.34673,-0.08434,-0.43845,-0.48542,0.66919,-0.35258,-0.80070,0.44321,-0.00689,\n1403636658263555584,6.75414,-4.30264,-0.08317,-0.44461,-0.48881,0.66641,-0.34539,-0.81173,0.42728,-0.03249,\n1403636658363555328,6.67493,-4.26291,-0.08689,-0.44743,-0.49073,0.66559,-0.34056,-0.80524,0.39696,-0.08656,\n1403636658463555584,6.59774,-4.22689,-0.09423,-0.44947,-0.49283,0.66507,-0.33584,-0.79662,0.36328,-0.10994,\n1403636658563555584,6.52029,-4.19531,-0.10410,-0.45369,-0.49612,0.66268,-0.32998,-0.77847,0.32099,-0.12267,\n1403636658663555328,6.44345,-4.16771,-0.11707,-0.45696,-0.49825,0.66058,-0.32644,-0.74318,0.28266,-0.14874,\n1403636658763555584,6.37007,-4.14467,-0.13260,-0.46143,-0.49899,0.65761,-0.32503,-0.70429,0.24100,-0.17253,\n1403636658863555328,6.29934,-4.12357,-0.15011,-0.46874,-0.49908,0.65253,-0.32467,-0.65290,0.20710,-0.19375,\n1403636658963555584,6.23360,-4.10614,-0.17128,-0.47032,-0.49385,0.65153,-0.33230,-0.59964,0.18674,-0.23275,\n1403636659063555584,6.17208,-4.09068,-0.19626,-0.47144,-0.48850,0.65056,-0.34041,-0.56957,0.18629,-0.27219,\n1403636659163555328,6.11620,-4.07422,-0.22503,-0.47273,-0.48560,0.64938,-0.34500,-0.53527,0.19532,-0.30793,\n1403636659263555584,6.06379,-4.05237,-0.25507,-0.47284,-0.48453,0.64901,-0.34704,-0.49361,0.21587,-0.28812,\n1403636659363555328,6.01504,-4.03155,-0.28031,-0.46881,-0.48688,0.65160,-0.34436,-0.46594,0.22057,-0.23536,\n1403636659463555584,5.96999,-4.00755,-0.30133,-0.46320,-0.49136,0.65527,-0.33858,-0.44558,0.21135,-0.15594,\n1403636659563555584,5.92701,-3.98440,-0.31289,-0.46172,-0.49189,0.65651,-0.33741,-0.42100,0.21181,-0.11435,\n1403636659663555328,5.88721,-3.96138,-0.32529,-0.46515,-0.49075,0.65480,-0.33769,-0.39288,0.20488,-0.15683,\n1403636659763555584,5.85270,-3.94140,-0.34452,-0.46844,-0.48867,0.65361,-0.33847,-0.35173,0.19793,-0.22315,\n1403636659863555328,5.82102,-3.91847,-0.36832,-0.46922,-0.48727,0.65413,-0.33840,-0.30894,0.21174,-0.27881,\n1403636659963555584,5.79283,-3.89617,-0.39882,-0.46645,-0.48853,0.65733,-0.33418,-0.27747,0.21693,-0.33163,\n1403636660063555584,5.76655,-3.87061,-0.43198,-0.46087,-0.49055,0.66221,-0.32930,-0.24854,0.21183,-0.36133,\n1403636660163555328,5.74241,-3.84800,-0.46756,-0.45684,-0.49273,0.66569,-0.32462,-0.22549,0.18413,-0.38040,\n1403636660263555584,5.72167,-3.82960,-0.50534,-0.45414,-0.49377,0.66803,-0.32201,-0.20905,0.14560,-0.38784,\n1403636660363555328,5.70300,-3.81593,-0.54025,-0.44749,-0.49746,0.67302,-0.31516,-0.19416,0.09584,-0.32863,\n1403636660463555584,5.68716,-3.81065,-0.56729,-0.44090,-0.49992,0.67798,-0.30988,-0.17399,0.01662,-0.22641,\n1403636660563555584,5.67260,-3.81448,-0.58897,-0.44427,-0.49915,0.67588,-0.31090,-0.16080,-0.07805,-0.21521,\n1403636660663555328,5.65822,-3.82649,-0.61337,-0.45618,-0.49539,0.66757,-0.31754,-0.13956,-0.13873,-0.26462,\n1403636660763555584,5.64571,-3.84332,-0.64306,-0.46968,-0.49335,0.65760,-0.32182,-0.10226,-0.16762,-0.31580,\n1403636660863555328,5.63813,-3.86284,-0.67793,-0.48029,-0.49475,0.64939,-0.32068,-0.04378,-0.17128,-0.37639,\n1403636660963555584,5.63624,-3.88241,-0.71804,-0.48351,-0.49399,0.64670,-0.32245,0.02349,-0.17929,-0.42568,\n1403636661063555584,5.64152,-3.90001,-0.76313,-0.48135,-0.48998,0.64818,-0.32876,0.08280,-0.17327,-0.45669,\n1403636661163555328,5.65249,-3.91720,-0.81068,-0.47897,-0.48625,0.64997,-0.33419,0.13239,-0.16442,-0.46775,\n1403636661263555584,5.66787,-3.93393,-0.85423,-0.47735,-0.48375,0.65117,-0.33778,0.17926,-0.12610,-0.41717,\n1403636661363555328,5.68747,-3.94603,-0.89064,-0.47711,-0.48246,0.65129,-0.33972,0.20763,-0.09707,-0.33199,\n1403636661463555584,5.70919,-3.95695,-0.91935,-0.47663,-0.48229,0.65147,-0.34031,0.22813,-0.08685,-0.23954,\n1403636661563555584,5.73416,-3.96348,-0.94197,-0.47362,-0.48234,0.65320,-0.34113,0.26313,-0.04963,-0.17435,\n1403636661663555328,5.76191,-3.96921,-0.95777,-0.47085,-0.48151,0.65481,-0.34304,0.28439,-0.02914,-0.11411,\n1403636661763555584,5.79100,-3.97230,-0.96541,-0.46998,-0.47835,0.65514,-0.34797,0.29282,-0.01325,-0.03026,\n1403636661863555328,5.82096,-3.97440,-0.96724,-0.46685,-0.47429,0.65652,-0.35508,0.27755,0.01276,0.03749,\n1403636661963555584,5.84929,-3.97208,-0.96116,-0.46619,-0.47322,0.65641,-0.35757,0.26771,0.04910,0.10770,\n1403636662063555584,5.87794,-3.96765,-0.94959,-0.46772,-0.47332,0.65540,-0.35730,0.25809,0.08395,0.17629,\n1403636662163555328,5.90459,-3.95686,-0.93588,-0.46922,-0.47576,0.65435,-0.35400,0.24663,0.12662,0.14303,\n1403636662263555584,5.93264,-3.94429,-0.92818,-0.47287,-0.48127,0.65245,-0.34510,0.25785,0.16118,0.07242,\n1403636662363555328,5.95958,-3.92767,-0.92440,-0.47404,-0.48527,0.65234,-0.33800,0.27401,0.17243,0.02149,\n1403636662463555584,5.98962,-3.90990,-0.92694,-0.47015,-0.48852,0.65605,-0.33151,0.30737,0.17652,-0.06073,\n1403636662563555584,6.02515,-3.89175,-0.93549,-0.46389,-0.48972,0.66141,-0.32790,0.33864,0.15986,-0.09667,\n1403636662663555328,6.05926,-3.87560,-0.94596,-0.45489,-0.48280,0.66867,-0.33595,0.33710,0.14020,-0.12011,\n1403636662763555584,6.09460,-3.86132,-0.96074,-0.45007,-0.47421,0.67258,-0.34669,0.30708,0.12554,-0.16096,\n1403636662863555328,6.12642,-3.84801,-0.97985,-0.44715,-0.47037,0.67529,-0.35042,0.26736,0.12325,-0.20432,\n1403636662963555584,6.15287,-3.83581,-1.00362,-0.44369,-0.47283,0.67850,-0.34526,0.22615,0.12087,-0.24842,\n1403636663063555584,6.17231,-3.82422,-1.02592,-0.43556,-0.47105,0.68439,-0.34641,0.16693,0.10517,-0.18920,\n1403636663163555328,6.18703,-3.81411,-1.04199,-0.42980,-0.46945,0.68869,-0.34727,0.08194,0.08110,-0.11505,\n1403636663263555584,6.19153,-3.80710,-1.05347,-0.43757,-0.47685,0.68438,-0.33581,0.00769,0.05160,-0.12066,\n1403636663363555328,6.19090,-3.80461,-1.06967,-0.44477,-0.48704,0.67965,-0.32101,-0.02262,0.01446,-0.19580,\n1403636663463555584,6.18953,-3.80689,-1.09388,-0.44595,-0.49531,0.67834,-0.30928,-0.01874,-0.04876,-0.25924,\n1403636663563555584,6.18756,-3.81666,-1.12240,-0.44532,-0.50025,0.67864,-0.30149,0.00317,-0.12504,-0.30225,\n1403636663663555328,6.18750,-3.83499,-1.15326,-0.44407,-0.50217,0.67917,-0.29894,0.00266,-0.22259,-0.31314,\n1403636663763555584,6.18919,-3.86297,-1.18149,-0.44956,-0.50667,0.67447,-0.29371,0.02638,-0.31345,-0.25824,\n1403636663863555328,6.19359,-3.90292,-1.20238,-0.46073,-0.51112,0.66507,-0.29010,0.08248,-0.43502,-0.17709,\n1403636663963555584,6.20494,-3.95204,-1.21668,-0.47147,-0.51250,0.65580,-0.29153,0.16320,-0.52654,-0.09931,\n1403636664063555584,6.22528,-4.00980,-1.22342,-0.48028,-0.51177,0.64797,-0.29589,0.27154,-0.57510,-0.04059,\n1403636664163555328,6.25574,-4.07255,-1.22525,-0.48081,-0.50700,0.64618,-0.30696,0.36287,-0.62212,0.01066,\n1403636664263555584,6.29583,-4.13861,-1.22409,-0.48037,-0.50115,0.64460,-0.32030,0.43573,-0.63980,-0.01663,\n1403636664363555328,6.34166,-4.20355,-1.23566,-0.47878,-0.49803,0.64353,-0.32954,0.48933,-0.61942,-0.18923,\n1403636664463555584,6.39128,-4.26768,-1.26233,-0.47676,-0.49742,0.64237,-0.33560,0.53524,-0.59464,-0.30856,\n1403636664563555584,6.44406,-4.33027,-1.29470,-0.47463,-0.49703,0.64080,-0.34215,0.57350,-0.58553,-0.31384,\n1403636664663555328,6.50186,-4.38968,-1.32333,-0.46919,-0.49462,0.64140,-0.35187,0.59892,-0.54247,-0.25445,\n1403636664763555584,6.56156,-4.44356,-1.34387,-0.46483,-0.49745,0.64180,-0.35294,0.60422,-0.50067,-0.16324,\n1403636664863555328,6.62295,-4.49380,-1.35695,-0.46385,-0.50266,0.63967,-0.35071,0.62573,-0.48760,-0.10133,\n1403636664963555584,6.68590,-4.54332,-1.36469,-0.45969,-0.50527,0.64033,-0.35123,0.64102,-0.47806,-0.04024,\n1403636665063555584,6.75185,-4.59134,-1.36600,-0.45170,-0.50908,0.64425,-0.34893,0.65450,-0.43455,0.03130,\n1403636665163555328,6.81783,-4.63624,-1.35887,-0.44551,-0.51221,0.64724,-0.34675,0.66100,-0.45343,0.10024,\n1403636665263555584,6.88368,-4.68298,-1.34702,-0.44080,-0.50721,0.64956,-0.35567,0.64476,-0.45818,0.13247,\n1403636665363555328,6.94707,-4.73435,-1.33482,-0.44207,-0.50319,0.64812,-0.36236,0.61666,-0.47123,0.12566,\n1403636665463555584,7.00834,-4.78551,-1.32431,-0.44627,-0.50618,0.64571,-0.35733,0.58844,-0.45632,0.11145,\n1403636665563555584,7.06832,-4.83255,-1.31367,-0.45243,-0.51154,0.64265,-0.34732,0.60053,-0.43756,0.12424,\n1403636665663555328,7.12932,-4.87696,-1.29938,-0.45507,-0.51470,0.64186,-0.34062,0.62283,-0.43792,0.13438,\n1403636665763555584,7.19406,-4.92601,-1.28534,-0.46036,-0.51299,0.63974,-0.34005,0.64512,-0.45432,0.14549,\n1403636665863555328,7.26024,-4.97220,-1.26873,-0.46078,-0.50532,0.64097,-0.34856,0.67910,-0.46372,0.15623,\n1403636665963555584,7.33051,-5.01827,-1.25094,-0.45479,-0.49511,0.64666,-0.36038,0.68623,-0.44003,0.12929,\n1403636666063555584,7.39867,-5.06268,-1.23886,-0.45015,-0.49029,0.65090,-0.36511,0.65955,-0.41268,0.07760,\n1403636666163555328,7.46344,-5.10438,-1.23135,-0.45470,-0.49196,0.64857,-0.36136,0.62605,-0.39384,0.03406,\n1403636666263555584,7.52676,-5.14383,-1.22885,-0.45730,-0.49253,0.64673,-0.36061,0.61415,-0.36917,-0.02497,\n1403636666363555328,7.59051,-5.17902,-1.23357,-0.45679,-0.48976,0.64667,-0.36509,0.60794,-0.33634,-0.06278,\n1403636666463555584,7.65238,-5.21021,-1.23780,-0.45156,-0.48861,0.64955,-0.36800,0.59191,-0.28855,-0.03002,\n1403636666563555584,7.71131,-5.23656,-1.23576,-0.44862,-0.49203,0.65063,-0.36512,0.54743,-0.22820,0.06942,\n1403636666663555328,7.76466,-5.25946,-1.22528,-0.44749,-0.49940,0.65021,-0.35719,0.53289,-0.19385,0.12274,\n1403636666763555584,7.81774,-5.28084,-1.21123,-0.44365,-0.50685,0.65155,-0.34894,0.52645,-0.20494,0.15482,\n1403636666863555328,7.87367,-5.30300,-1.19714,-0.43493,-0.50956,0.65657,-0.34656,0.52401,-0.22324,0.11290,\n1403636666963555584,7.92562,-5.32729,-1.19007,-0.41708,-0.50549,0.66787,-0.35281,0.48332,-0.25311,0.05587,\n1403636667063555584,7.97224,-5.35595,-1.18736,-0.41451,-0.50285,0.66912,-0.35722,0.40909,-0.30184,0.04123,\n1403636667163555328,8.01137,-5.38828,-1.18651,-0.43280,-0.50363,0.65647,-0.35790,0.36211,-0.33132,0.00043,\n1403636667263555584,8.04849,-5.42075,-1.18934,-0.44943,-0.50569,0.64447,-0.35630,0.34963,-0.33405,-0.04344,\n1403636667363555328,8.08740,-5.45113,-1.20057,-0.45681,-0.50720,0.63871,-0.35514,0.36725,-0.31108,-0.10769,\n1403636667463555584,8.12821,-5.47939,-1.21569,-0.45527,-0.50982,0.63946,-0.35199,0.39615,-0.28538,-0.15035,\n1403636667563555584,8.17055,-5.50711,-1.23041,-0.44644,-0.51221,0.64525,-0.34927,0.41110,-0.29288,-0.14559,\n1403636667663555328,8.21405,-5.53504,-1.24048,-0.43550,-0.51138,0.65275,-0.35035,0.41270,-0.29405,-0.05306,\n1403636667763555584,8.25902,-5.56474,-1.24239,-0.43187,-0.50895,0.65548,-0.35328,0.39376,-0.31484,0.02579,\n1403636667863555328,8.30075,-5.59587,-1.23904,-0.43231,-0.50670,0.65594,-0.35511,0.37337,-0.31408,0.05417,\n1403636667963555584,8.33513,-5.62706,-1.23690,-0.43166,-0.50588,0.65700,-0.35511,0.34894,-0.31748,0.01604,\n1403636668063555584,8.37016,-5.66008,-1.24051,-0.43478,-0.50559,0.65627,-0.35306,0.31928,-0.34166,-0.06525,\n1403636668163555328,8.40069,-5.69471,-1.25212,-0.43950,-0.50683,0.65466,-0.34839,0.30143,-0.35896,-0.13408,\n1403636668263555584,8.43098,-5.73032,-1.26889,-0.44150,-0.50869,0.65512,-0.34223,0.29321,-0.37600,-0.18429,\n1403636668363555328,8.46292,-5.76829,-1.28927,-0.44363,-0.50816,0.65591,-0.33874,0.30913,-0.40145,-0.20652,\n1403636668463555584,8.49616,-5.80944,-1.31188,-0.44746,-0.50595,0.65527,-0.33824,0.31393,-0.42588,-0.23757,\n1403636668563555584,8.52973,-5.85284,-1.33650,-0.44933,-0.50425,0.65574,-0.33741,0.32981,-0.43900,-0.25280,\n1403636668663555328,8.56746,-5.89676,-1.36309,-0.45087,-0.50321,0.65644,-0.33552,0.33879,-0.45492,-0.24548,\n1403636668763555584,8.60309,-5.94277,-1.38766,-0.45807,-0.50552,0.65245,-0.33005,0.36016,-0.47502,-0.24131,\n1403636668863555328,8.64358,-5.99082,-1.40989,-0.46799,-0.50764,0.64605,-0.32543,0.41901,-0.49145,-0.18923,\n1403636668963555584,8.69065,-6.04055,-1.42526,-0.47432,-0.50600,0.64196,-0.32692,0.49574,-0.50128,-0.11004,\n1403636669063555584,8.74546,-6.08676,-1.43212,-0.47219,-0.49921,0.64398,-0.33633,0.56344,-0.46615,-0.04056,\n1403636669163555328,8.80139,-6.13321,-1.43220,-0.46988,-0.49413,0.64624,-0.34267,0.59482,-0.43600,-0.03983,\n1403636669263555584,8.86085,-6.17371,-1.43418,-0.47026,-0.49186,0.64659,-0.34474,0.61894,-0.40722,-0.08759,\n1403636669363555328,8.92256,-6.21058,-1.44208,-0.46995,-0.49162,0.64748,-0.34386,0.63863,-0.38463,-0.14404,\n1403636669463555584,8.98809,-6.24955,-1.45594,-0.47187,-0.48954,0.64614,-0.34668,0.66211,-0.36434,-0.17163,\n1403636669563555584,9.05473,-6.28371,-1.47229,-0.47604,-0.48774,0.64259,-0.35010,0.67809,-0.33134,-0.19920,\n1403636669663555328,9.12464,-6.31290,-1.49077,-0.48303,-0.48466,0.63666,-0.35562,0.71269,-0.27878,-0.16020,\n1403636669763555584,9.19575,-6.33536,-1.50210,-0.48712,-0.48154,0.63293,-0.36088,0.74268,-0.23485,-0.10168,\n1403636669863555328,9.26964,-6.34957,-1.50911,-0.47339,-0.47152,0.64108,-0.37761,0.76012,-0.13647,-0.02043,\n1403636669963555584,9.34002,-6.35148,-1.50951,-0.45297,-0.46386,0.65401,-0.38976,0.70103,-0.04457,0.05201,\n1403636670063555584,9.40680,-6.34596,-1.50300,-0.44125,-0.46211,0.66141,-0.39278,0.62227,0.04645,0.09739,\n1403636670163555328,9.45851,-6.33279,-1.48843,-0.43391,-0.46082,0.66308,-0.39961,0.51297,0.11477,0.16204,\n1403636670263555584,9.50210,-6.31253,-1.47310,-0.42833,-0.45387,0.66077,-0.41704,0.39618,0.20306,0.12825,\n1403636670363555328,9.53509,-6.28224,-1.46409,-0.42113,-0.45149,0.65914,-0.42935,0.26291,0.30348,0.04511,\n1403636670463555584,9.55161,-6.24111,-1.45996,-0.40982,-0.45645,0.65727,-0.43783,0.09933,0.40162,0.04615,\n1403636670563555584,9.55165,-6.19096,-1.45145,-0.39816,-0.46606,0.65152,-0.44696,-0.08093,0.50627,0.10909,\n1403636670663555328,9.53532,-6.13294,-1.43636,-0.38822,-0.47971,0.64183,-0.45521,-0.28293,0.58768,0.20787,\n1403636670763555584,9.50003,-6.06759,-1.41450,-0.37899,-0.49454,0.62921,-0.46463,-0.46968,0.65584,0.23208,\n1403636670863555328,9.44789,-5.99657,-1.39342,-0.36433,-0.50680,0.61680,-0.47956,-0.63581,0.72683,0.19401,\n1403636670963555584,9.37739,-5.91955,-1.37368,-0.34672,-0.51693,0.60222,-0.49990,-0.83807,0.79917,0.21715,\n1403636671063555584,9.28369,-5.83421,-1.34583,-0.33329,-0.53328,0.58118,-0.51650,-1.09179,0.87406,0.32766,\n1403636671163555328,9.16361,-5.74125,-1.30952,0.32508,0.55783,-0.55727,0.52211,-1.33165,0.93602,0.39898,\n1403636671263555584,9.01936,-5.64292,-1.26743,0.31755,0.58341,-0.53759,0.51942,-1.55432,0.99405,0.43198,\n1403636671363555328,8.85543,-5.54101,-1.22478,0.32004,0.61566,-0.51816,0.50004,-1.70623,1.00579,0.43926,\n1403636671463555584,8.68190,-5.43950,-1.18449,0.32309,0.64828,-0.49696,0.47788,-1.74204,0.99960,0.38370,\n1403636671563555584,8.50734,-5.33913,-1.15026,0.31480,0.67200,-0.48406,0.46368,-1.72771,0.96939,0.31204,\n1403636671663555328,8.33468,-5.24274,-1.12223,0.30392,0.68847,-0.47828,0.45264,-1.70335,0.90575,0.25955,\n1403636671763555584,8.16530,-5.15428,-1.09979,0.29568,0.69828,-0.47589,0.44554,-1.66133,0.82410,0.20038,\n1403636671863555328,8.00264,-5.07538,-1.08062,0.29110,0.70599,-0.47516,0.43711,-1.59138,0.71479,0.19034,\n1403636671963555584,7.84838,-5.00996,-1.05820,0.28730,0.71326,-0.47768,0.42491,-1.50379,0.57166,0.28345,\n1403636672063555584,7.70681,-4.95976,-1.02699,0.29086,0.71998,-0.47658,0.41219,-1.38921,0.41418,0.36342,\n1403636672163555328,7.58162,-4.92732,-0.98679,0.30384,0.72280,-0.46976,0.40568,-1.23979,0.28328,0.44667,\n1403636672263555584,7.47557,-4.90780,-0.94218,0.31220,0.72278,-0.46707,0.40245,-1.07374,0.15002,0.43400,\n1403636672363555328,7.38969,-4.90061,-0.89973,0.32487,0.72169,-0.46054,0.40191,-0.90570,0.04832,0.37675,\n1403636672463555584,7.31891,-4.90028,-0.86738,0.34193,0.71690,-0.44942,0.40885,-0.73109,-0.01315,0.25969,\n1403636672563555584,7.26209,-4.90379,-0.84803,0.34858,0.70000,-0.44646,0.43493,-0.58244,-0.02843,0.08705,\n1403636672663555328,7.21453,-4.90871,-0.84798,0.34653,0.68465,-0.44878,0.45801,-0.49164,-0.02440,-0.10969,\n1403636672763555584,7.17091,-4.91026,-0.86945,0.33605,0.68130,-0.45658,0.46307,-0.43797,0.00518,-0.30185,\n1403636672863555328,7.13356,-4.91063,-0.90029,0.32554,0.68648,-0.45835,0.46118,-0.37635,0.00894,-0.26972,\n1403636672963555584,7.09927,-4.90963,-0.92353,0.31886,0.69371,-0.44960,0.46362,-0.31844,0.01421,-0.19778,\n1403636673063555584,7.07584,-4.91191,-0.93803,0.31247,0.70365,-0.43652,0.46550,-0.24966,-0.00584,-0.12276,\n1403636673163555328,7.05668,-4.91500,-0.94388,0.30215,0.71504,-0.42745,0.46337,-0.19025,-0.02931,-0.03338,\n1403636673263555584,7.04320,-4.91951,-0.94192,0.29029,0.72869,-0.42284,0.45382,-0.11783,-0.06197,0.04333,\n1403636673363555328,7.03759,-4.92620,-0.93335,0.28154,0.73743,-0.42041,0.44742,-0.02159,-0.12104,0.10302,\n1403636673463555584,7.04042,-4.94178,-0.92736,0.27852,0.73972,-0.41652,0.44918,0.06414,-0.18471,0.06269,\n1403636673563555584,7.05500,-4.96207,-0.92661,0.27926,0.73900,-0.41241,0.45367,0.15475,-0.23879,0.02099,\n1403636673663555328,7.07786,-4.98800,-0.92769,0.28512,0.73349,-0.40587,0.46472,0.24028,-0.27513,-0.01864,\n1403636673763555584,7.10951,-5.01536,-0.93277,0.29209,0.72660,-0.39923,0.47682,0.29722,-0.25365,-0.02315,\n1403636673863555328,7.14285,-5.04128,-0.93610,0.29536,0.72221,-0.39520,0.48476,0.34245,-0.23816,-0.01355,\n1403636673963555584,7.18134,-5.06693,-0.93511,0.29643,0.72348,-0.39383,0.48332,0.40443,-0.22983,0.03198,\n1403636674063555584,7.22584,-5.09160,-0.93519,0.29459,0.72380,-0.39530,0.48277,0.46439,-0.22741,-0.02630,\n1403636674163555328,7.27307,-5.11374,-0.94021,0.29281,0.72261,-0.39657,0.48458,0.49565,-0.19928,-0.08120,\n1403636674263555584,7.32508,-5.13429,-0.95120,0.29033,0.71845,-0.39892,0.49031,0.53050,-0.17328,-0.15734,\n1403636674363555328,7.38000,-5.15241,-0.96921,0.28016,0.71383,-0.40552,0.49750,0.55031,-0.15293,-0.22515,\n1403636674463555584,7.43652,-5.16751,-0.99485,0.26722,0.71183,-0.40684,0.50633,0.54890,-0.14962,-0.28748,\n1403636674563555584,7.49184,-5.18476,-1.02525,0.25751,0.71351,-0.40103,0.51358,0.51589,-0.15626,-0.33299,\n1403636674663555328,7.54346,-5.20291,-1.05989,0.25398,0.71677,-0.39221,0.51760,0.48570,-0.16929,-0.37083,\n1403636674763555584,7.59330,-5.22271,-1.09828,0.24631,0.72280,-0.38860,0.51564,0.44592,-0.17744,-0.42823,\n1403636674863555328,7.63820,-5.24278,-1.13574,0.23584,0.73292,-0.38927,0.50564,0.41486,-0.20182,-0.36196,\n1403636674963555584,7.68017,-5.26817,-1.16663,0.22848,0.74137,-0.39001,0.49605,0.40474,-0.27631,-0.27287,\n1403636675063555584,7.72335,-5.30217,-1.18747,0.22433,0.74771,-0.39053,0.48794,0.41295,-0.36046,-0.14404,\n1403636675163555328,7.77027,-5.34593,-1.19949,0.23110,0.75388,-0.38523,0.47944,0.45227,-0.45971,-0.04562,\n1403636675263555584,7.82226,-5.39812,-1.20394,0.24404,0.75774,-0.37608,0.47417,0.54125,-0.56308,0.01381,\n1403636675363555328,7.88219,-5.45458,-1.20075,0.25437,0.75700,-0.36871,0.47571,0.62186,-0.58822,0.09040,\n1403636675463555584,7.95299,-5.51506,-1.19780,0.26036,0.75188,-0.35838,0.48832,0.71344,-0.59731,0.04760,\n1403636675563555584,8.02940,-5.57359,-1.19925,0.26456,0.75022,-0.34418,0.49872,0.77053,-0.57438,-0.02257,\n1403636675663555328,8.11001,-5.62847,-1.20732,0.26531,0.75012,-0.33148,0.50700,0.81596,-0.52695,-0.08775,\n1403636675763555584,8.19457,-5.67693,-1.22092,0.25816,0.74928,-0.32034,0.51896,0.85530,-0.45393,-0.14721,\n1403636675863555328,8.28193,-5.71906,-1.24084,0.23468,0.74652,-0.31679,0.53599,0.86187,-0.39411,-0.22378,\n1403636675963555584,8.36861,-5.75683,-1.26732,0.20491,0.74930,-0.31440,0.54563,0.83186,-0.36044,-0.27872,\n1403636676063555584,8.45131,-5.79364,-1.29712,0.18538,0.75762,-0.30310,0.54752,0.78021,-0.36991,-0.31297,\n1403636676163555328,8.52974,-5.83132,-1.32760,0.18129,0.76978,-0.28048,0.54398,0.75091,-0.38283,-0.32476,\n1403636676263555584,8.60529,-5.86823,-1.35851,0.18370,0.78250,-0.25116,0.53932,0.74353,-0.37332,-0.34933,\n1403636676363555328,8.68101,-5.90183,-1.39018,0.18223,0.79343,-0.22188,0.53668,0.77737,-0.34156,-0.30086,\n1403636676463555584,8.76132,-5.93052,-1.41541,0.17488,0.80167,-0.19701,0.53659,0.80679,-0.25917,-0.21974,\n1403636676563555584,8.84353,-5.95200,-1.43296,0.15493,0.80590,-0.18270,0.54143,0.86430,-0.21015,-0.14922,\n1403636676663555328,8.93197,-5.97112,-1.44182,0.13573,0.80686,-0.17382,0.54803,0.89831,-0.19275,-0.05659,\n1403636676763555584,9.02262,-5.98768,-1.44476,0.12076,0.80571,-0.17007,0.55437,0.90537,-0.17484,0.00452,\n1403636676863555328,9.11339,-6.00330,-1.44145,0.10644,0.80602,-0.16837,0.55736,0.88517,-0.16557,0.04994,\n1403636676963555584,9.19948,-6.01821,-1.43796,0.09907,0.80538,-0.16601,0.56035,0.85479,-0.17722,-0.01257,\n1403636677063555584,9.28327,-6.03473,-1.44714,0.10326,0.80573,-0.15995,0.56085,0.84087,-0.19153,-0.15962,\n1403636677163555328,9.36443,-6.05022,-1.46968,0.11192,0.80649,-0.15347,0.55991,0.81735,-0.17333,-0.29888,\n1403636677263555584,9.44920,-6.06752,-1.49967,0.11778,0.80426,-0.14859,0.56322,0.80551,-0.14067,-0.30281,\n1403636677363555328,9.53336,-6.07948,-1.52447,0.11794,0.79979,-0.14080,0.57149,0.81554,-0.09283,-0.23591,\n1403636677463555584,9.61717,-6.08860,-1.54017,0.11338,0.79381,-0.13032,0.58312,0.78707,-0.04598,-0.14108,\n1403636677563555584,9.69756,-6.09193,-1.54764,0.10186,0.79068,-0.11987,0.59167,0.72411,0.01681,-0.06910,\n1403636677663555328,9.77054,-6.09056,-1.54908,0.08489,0.79235,-0.10953,0.59413,0.66899,0.05413,-0.01946,\n1403636677763555584,9.83849,-6.08636,-1.54612,0.06465,0.79691,-0.09774,0.59263,0.59886,0.07193,0.04351,\n1403636677863555328,9.89506,-6.07914,-1.54061,0.03762,0.80537,-0.08678,0.58518,0.52357,0.06839,0.05661,\n1403636677963555584,9.94663,-6.07439,-1.53212,0.01340,0.81287,-0.07143,0.57790,0.49292,0.02790,0.09497,\n1403636678063555584,9.99480,-6.07403,-1.52262,-0.00131,0.81715,-0.05062,0.57419,0.47117,-0.03791,0.06832,\n1403636678163555328,10.04530,-6.07867,-1.51841,-0.00382,0.81754,-0.02840,0.57515,0.46178,-0.08277,0.00754,\n1403636678263555584,10.09340,-6.08536,-1.52000,-0.00297,0.81805,-0.00883,0.57507,0.44312,-0.10139,-0.02060,\n1403636678363555328,10.13687,-6.09197,-1.51898,-0.01124,0.81743,0.00587,0.57589,0.43010,-0.08712,0.02736,\n1403636678463555584,10.17777,-6.09824,-1.51280,-0.02165,0.81818,0.02038,0.57420,0.42857,-0.08393,0.09612,\n1403636678563555584,10.21794,-6.10443,-1.50018,-0.03235,0.81812,0.03624,0.57299,0.41636,-0.07260,0.16224,\n1403636678663555328,10.25645,-6.11058,-1.48406,-0.04266,0.81625,0.05769,0.57323,0.39079,-0.07474,0.14898,\n1403636678763555584,10.29382,-6.11637,-1.47174,-0.04572,0.81339,0.08730,0.57331,0.37571,-0.05100,0.10079,\n1403636678863555328,10.33111,-6.11937,-1.46661,-0.04612,0.81051,0.11787,0.57188,0.35966,-0.01425,0.01585,\n1403636678963555584,10.36737,-6.11631,-1.46617,-0.04882,0.80746,0.14258,0.57035,0.32930,0.05697,-0.04269,\n1403636679063555584,10.40025,-6.10566,-1.47126,-0.06282,0.80335,0.15911,0.57040,0.29586,0.13901,-0.08195,\n1403636679163555328,10.43045,-6.08839,-1.48177,-0.08492,0.79820,0.17093,0.57135,0.25861,0.20421,-0.13359,\n1403636679263555584,10.45561,-6.06558,-1.49385,-0.11459,0.79149,0.18020,0.57266,0.21474,0.23437,-0.11881,\n1403636679363555328,10.47779,-6.04103,-1.50101,-0.13950,0.78553,0.19403,0.57082,0.18402,0.23373,-0.04540,\n1403636679463555584,10.49576,-6.01756,-1.50057,-0.15817,0.78054,0.21180,0.56646,0.15241,0.21517,0.04632,\n1403636679563555584,10.51052,-5.99717,-1.49401,-0.17128,0.77673,0.22949,0.56097,0.13448,0.18807,0.09612,\n1403636679663555328,10.52245,-5.97881,-1.48238,-0.18077,0.77508,0.24198,0.55499,0.09458,0.14302,0.15443,\n1403636679763555584,10.53067,-5.96522,-1.46415,-0.18405,0.77301,0.25241,0.55214,0.06848,0.10260,0.23239,\n1403636679863555328,10.53811,-5.95310,-1.43635,-0.18607,0.76676,0.25797,0.55760,0.07653,0.10689,0.32218,\n1403636679963555584,10.54589,-5.94024,-1.40154,-0.18166,0.76108,0.26414,0.56390,0.05285,0.10073,0.38105,\n1403636680063555584,10.54734,-5.93003,-1.36115,-0.16918,0.75750,0.27237,0.56867,-0.03456,0.08449,0.42081,\n1403636680163555328,10.54053,-5.91991,-1.32123,-0.15766,0.75450,0.27861,0.57293,-0.11583,0.10629,0.36356,\n1403636680263555584,10.52505,-5.90717,-1.28757,-0.15862,0.75325,0.27646,0.57534,-0.19969,0.13712,0.31459,\n1403636680363555328,10.50141,-5.89174,-1.25921,-0.14869,0.75310,0.28004,0.57646,-0.28482,0.16408,0.23198,\n1403636680463555584,10.46908,-5.87146,-1.23801,-0.13904,0.75616,0.28296,0.57343,-0.37280,0.22078,0.18147,\n1403636680563555584,10.42727,-5.84560,-1.22200,-0.13567,0.75961,0.28166,0.57031,-0.46295,0.29592,0.12460,\n1403636680663555328,10.37824,-5.81192,-1.20947,-0.13896,0.76190,0.27645,0.56901,-0.53364,0.36383,0.08249,\n1403636680763555584,10.32204,-5.77267,-1.20235,-0.14830,0.76736,0.26782,0.56342,-0.59896,0.42216,0.03826,\n1403636680863555328,10.26235,-5.72818,-1.19757,-0.16449,0.77391,0.25504,0.55585,-0.62932,0.44565,0.00234,\n1403636680963555584,10.20031,-5.68324,-1.19958,-0.17698,0.78404,0.24516,0.54209,-0.62725,0.43930,-0.06399,\n1403636681063555584,10.14020,-5.63828,-1.20868,-0.17526,0.79342,0.24536,0.52875,-0.58762,0.43922,-0.11934,\n1403636681163555328,10.08563,-5.59221,-1.21908,-0.17032,0.79636,0.24841,0.52450,-0.51804,0.45121,-0.11037,\n1403636681263555584,10.03791,-5.54328,-1.22806,-0.16526,0.78715,0.25276,0.53778,-0.47112,0.48460,-0.07896,\n1403636681363555328,9.99298,-5.49189,-1.23351,-0.16609,0.77910,0.25274,0.54913,-0.46428,0.50307,-0.01686,\n1403636681463555584,9.94611,-5.44095,-1.23447,-0.17124,0.77462,0.25038,0.55494,-0.47950,0.49617,0.03692,\n1403636681563555584,9.89838,-5.38944,-1.22939,-0.17364,0.77315,0.25044,0.55620,-0.49288,0.49568,0.05483,\n1403636681663555328,9.84886,-5.33852,-1.22553,-0.17114,0.77221,0.25365,0.55684,-0.50894,0.49195,0.00910,\n1403636681763555584,9.79801,-5.28709,-1.22553,-0.17019,0.77101,0.25597,0.55772,-0.52623,0.50216,-0.02758,\n1403636681863555328,9.74577,-5.23598,-1.22729,-0.17128,0.76996,0.25718,0.55829,-0.54764,0.50545,-0.01125,\n1403636681963555584,9.69136,-5.18208,-1.22363,-0.17372,0.76928,0.25728,0.55842,-0.56561,0.51804,0.05774,\n1403636682063555584,9.63516,-5.13133,-1.21423,-0.17726,0.76966,0.25666,0.55707,-0.59373,0.51025,0.14385,\n1403636682163555328,9.57572,-5.08041,-1.19712,-0.17972,0.76988,0.25618,0.55619,-0.61748,0.49097,0.18070,\n1403636682263555584,9.51388,-5.03146,-1.18242,-0.18082,0.77409,0.25579,0.55014,-0.62406,0.47631,0.11818,\n1403636682363555328,9.45425,-4.98257,-1.17453,-0.17911,0.78110,0.25650,0.54038,-0.60975,0.47711,0.05759,\n1403636682463555584,9.39755,-4.93336,-1.16979,-0.17859,0.78225,0.25564,0.53929,-0.57118,0.47698,0.03638,\n1403636682563555584,9.34208,-4.88363,-1.16854,-0.17592,0.77741,0.25513,0.54734,-0.55772,0.47956,-0.00322,\n1403636682663555328,9.28527,-4.83422,-1.17030,-0.17402,0.77320,0.25443,0.55421,-0.57049,0.48145,-0.03832,\n1403636682763555584,9.22786,-4.78305,-1.17109,-0.17602,0.77142,0.25078,0.55771,-0.57093,0.48198,0.02131,\n1403636682863555328,9.16996,-4.73561,-1.16652,-0.17467,0.76936,0.24968,0.56145,-0.58809,0.47108,0.06695,\n1403636682963555584,9.10874,-4.68720,-1.15666,-0.16896,0.76893,0.25128,0.56307,-0.62765,0.47041,0.13015,\n1403636683063555584,9.04483,-4.64048,-1.14367,-0.16601,0.76802,0.25282,0.56451,-0.64791,0.47356,0.10690,\n1403636683163555328,8.97879,-4.59251,-1.13662,-0.16797,0.76928,0.25209,0.56253,-0.67926,0.48370,0.03058,\n1403636683263555584,8.91004,-4.54491,-1.13582,-0.17105,0.77098,0.25117,0.55969,-0.69961,0.48488,-0.01600,\n1403636683363555328,8.83903,-4.49575,-1.14058,-0.17438,0.77168,0.25073,0.55789,-0.71306,0.47628,-0.06754,\n1403636683463555584,8.76870,-4.44825,-1.14962,-0.17698,0.77211,0.25109,0.55630,-0.71852,0.47223,-0.10587,\n1403636683563555584,8.69799,-4.40057,-1.16177,-0.17786,0.77190,0.25190,0.55596,-0.71920,0.45926,-0.13236,\n1403636683663555328,8.62653,-4.35564,-1.17688,-0.17839,0.77393,0.25299,0.55245,-0.72209,0.44786,-0.15857,\n1403636683763555584,8.55549,-4.31296,-1.19241,-0.17956,0.77695,0.25352,0.54757,-0.71419,0.44272,-0.11849,\n1403636683863555328,8.48398,-4.27010,-1.19931,-0.18109,0.78010,0.25436,0.54218,-0.69924,0.43612,-0.02752,\n1403636683963555584,8.41480,-4.22786,-1.19928,-0.18088,0.77972,0.25625,0.54190,-0.66472,0.44249,0.04571,\n1403636684063555584,8.34656,-4.18293,-1.18998,-0.18299,0.77716,0.25681,0.54460,-0.63348,0.44717,0.14252,\n1403636684163555328,8.27823,-4.14019,-1.17686,-0.18445,0.77505,0.25828,0.54642,-0.63645,0.43561,0.20023,\n1403636684263555584,8.20653,-4.09697,-1.15581,-0.18724,0.77299,0.25864,0.54821,-0.64892,0.41897,0.23949,\n1403636684363555328,8.13579,-4.05462,-1.13518,-0.18851,0.77018,0.25959,0.55127,-0.66330,0.40088,0.18873,\n1403636684463555584,8.06599,-4.01532,-1.12110,-0.19059,0.76897,0.25959,0.55224,-0.67244,0.38349,0.11431,\n1403636684563555584,7.99659,-3.97973,-1.11428,-0.19544,0.76825,0.25757,0.55250,-0.67343,0.35123,0.05798,\n1403636684663555328,7.92995,-3.94659,-1.11194,-0.19840,0.76782,0.25652,0.55253,-0.67185,0.31821,0.00238,\n1403636684763555584,7.86190,-3.91729,-1.11757,-0.19895,0.76686,0.25661,0.55362,-0.67572,0.28071,-0.06516,\n1403636684863555328,7.79725,-3.89209,-1.12661,-0.19812,0.76688,0.25740,0.55353,-0.68209,0.23536,-0.12732,\n1403636684963555584,7.72989,-3.86920,-1.14181,-0.19018,0.76583,0.26260,0.55532,-0.69274,0.19752,-0.10883,\n1403636685063555584,7.66206,-3.84758,-1.15142,-0.18393,0.76634,0.26559,0.55529,-0.68693,0.20436,-0.04209,\n1403636685163555328,7.59313,-3.82818,-1.15420,-0.18209,0.76924,0.26508,0.55212,-0.70979,0.18807,0.01932,\n1403636685263555584,7.52269,-3.80925,-1.15118,-0.17532,0.77168,0.26682,0.55008,-0.72292,0.19120,0.07072,\n1403636685363555328,7.44946,-3.78668,-1.14103,-0.17039,0.77296,0.26719,0.54965,-0.72530,0.21385,0.11328,\n1403636685463555584,7.37518,-3.76365,-1.13418,-0.17093,0.77412,0.26401,0.54938,-0.73464,0.24066,0.06640,\n1403636685563555584,7.30167,-3.73973,-1.13128,-0.17249,0.77545,0.26092,0.54849,-0.73772,0.25492,-0.00648,\n1403636685663555328,7.22807,-3.71456,-1.13413,-0.17315,0.77543,0.25892,0.54926,-0.73556,0.25750,-0.05715,\n1403636685763555584,7.15424,-3.68551,-1.14229,-0.17151,0.77445,0.25938,0.55094,-0.73793,0.27768,-0.10149,\n1403636685863555328,7.07980,-3.65971,-1.15423,-0.16810,0.77223,0.26150,0.55409,-0.74386,0.29382,-0.14483,\n1403636685963555584,7.00104,-3.63293,-1.17201,-0.17160,0.77353,0.25950,0.55214,-0.76586,0.29410,-0.17833,\n1403636686063555584,6.92182,-3.60791,-1.18831,-0.17628,0.77679,0.25699,0.54725,-0.77786,0.29610,-0.11209,\n1403636686163555328,6.84466,-3.58083,-1.19694,-0.17853,0.77796,0.25537,0.54562,-0.75189,0.29617,-0.02621,\n1403636686263555584,6.76840,-3.55193,-1.19886,-0.17978,0.77958,0.25395,0.54354,-0.72957,0.29676,0.02883,\n1403636686363555328,6.69495,-3.52458,-1.19462,-0.18078,0.78102,0.25300,0.54159,-0.70341,0.29487,0.09303,\n1403636686463555584,6.62113,-3.49444,-1.18831,-0.18015,0.78252,0.25307,0.53960,-0.68486,0.29031,0.05643,\n1403636686563555584,6.55363,-3.46719,-1.18717,-0.18020,0.78332,0.25322,0.53834,-0.65455,0.28831,0.00193,\n1403636686663555328,6.48471,-3.43888,-1.19407,-0.17887,0.78295,0.25505,0.53846,-0.63326,0.28783,-0.07010,\n1403636686763555584,6.42382,-3.41155,-1.20149,-0.18016,0.78172,0.25498,0.53986,-0.60435,0.30001,-0.06682,\n1403636686863555328,6.36152,-3.38108,-1.20562,-0.18184,0.77992,0.25449,0.54212,-0.58686,0.29909,-0.00567,\n1403636686963555584,6.30568,-3.35162,-1.20205,-0.18403,0.77873,0.25369,0.54347,-0.56792,0.29268,0.07312,\n1403636687063555584,6.24865,-3.32147,-1.19290,-0.18568,0.77806,0.25277,0.54429,-0.53839,0.28875,0.14051,\n1403636687163555328,6.19456,-3.29371,-1.18012,-0.18620,0.77769,0.25322,0.54443,-0.53474,0.27252,0.15079,\n1403636687263555584,6.13943,-3.26594,-1.17118,-0.18608,0.77642,0.25354,0.54614,-0.52671,0.26688,0.08907,\n1403636687363555328,6.08946,-3.24352,-1.16668,-0.18596,0.77467,0.25425,0.54833,-0.51251,0.24469,0.03104,\n1403636687463555584,6.03858,-3.21845,-1.17018,-0.18560,0.77312,0.25510,0.55023,-0.50525,0.24212,-0.03381,\n1403636687563555584,5.99124,-3.19878,-1.17753,-0.18736,0.77297,0.25457,0.55010,-0.50299,0.22390,-0.10278,\n1403636687663555328,5.94347,-3.17865,-1.19585,-0.18967,0.77225,0.25385,0.55065,-0.50481,0.19816,-0.18708,\n1403636687763555584,5.89504,-3.16469,-1.21777,-0.19209,0.77165,0.25328,0.55092,-0.50011,0.17880,-0.24389,\n1403636687863555328,5.84812,-3.14992,-1.24722,-0.19151,0.77169,0.25415,0.55066,-0.48928,0.15179,-0.29411,\n1403636687963555584,5.80007,-3.13726,-1.27903,-0.18832,0.77137,0.25686,0.55096,-0.48904,0.13404,-0.34401,\n1403636688063555584,5.75392,-3.12523,-1.31458,-0.18362,0.77028,0.26050,0.55236,-0.49132,0.13003,-0.34366,\n1403636688163555328,5.70622,-3.11459,-1.34660,-0.18064,0.76998,0.26271,0.55271,-0.50231,0.13018,-0.27930,\n1403636688263555584,5.65920,-3.10507,-1.37024,-0.18208,0.76981,0.26213,0.55275,-0.51197,0.13815,-0.22001,\n1403636688363555328,5.60938,-3.09211,-1.39111,-0.18620,0.77124,0.25958,0.55058,-0.52210,0.13810,-0.18974,\n1403636688463555584,5.55839,-3.07994,-1.41239,-0.18491,0.77175,0.26050,0.54986,-0.51928,0.13334,-0.23233,\n1403636688563555584,5.50790,-3.06768,-1.43799,-0.17888,0.77118,0.26492,0.55054,-0.51334,0.13075,-0.26348,\n1403636688663555328,5.45812,-3.05411,-1.46703,-0.17270,0.77025,0.26989,0.55141,-0.52140,0.15396,-0.30740,\n1403636688763555584,5.40599,-3.03762,-1.50046,-0.17130,0.76972,0.27260,0.55125,-0.53946,0.17730,-0.31983,\n1403636688863555328,5.35138,-3.01886,-1.52924,-0.17421,0.77000,0.27440,0.54905,-0.55305,0.22142,-0.26134,\n1403636688963555584,5.29570,-2.99547,-1.55308,-0.18117,0.77074,0.27731,0.54428,-0.56336,0.24255,-0.17585,\n1403636689063555584,5.23987,-2.97038,-1.56841,-0.18870,0.77130,0.27972,0.53967,-0.55340,0.26833,-0.09297,\n1403636689163555328,5.18529,-2.94465,-1.57468,-0.19629,0.77223,0.28201,0.53442,-0.53801,0.28502,-0.01912,\n1403636689263555584,5.13105,-2.91875,-1.57327,-0.20089,0.77237,0.28524,0.53078,-0.52132,0.27528,0.06289,\n1403636689363555328,5.08151,-2.88992,-1.56580,-0.20401,0.77197,0.28812,0.52860,-0.47017,0.28886,0.14749,\n1403636689463555584,5.03634,-2.85900,-1.55036,-0.20634,0.77081,0.29025,0.52823,-0.44670,0.28960,0.22248,\n1403636689563555584,4.99474,-2.82869,-1.52724,-0.20808,0.77181,0.29146,0.52541,-0.43616,0.27733,0.27187,\n1403636689663555328,4.95498,-2.79884,-1.49976,-0.21033,0.77527,0.29071,0.51981,-0.40298,0.28340,0.27958,\n1403636689763555584,4.92077,-2.76701,-1.47668,-0.21517,0.78147,0.28651,0.51079,-0.33837,0.29780,0.22066,\n1403636689863555328,4.89426,-2.73421,-1.45718,-0.22035,0.78414,0.28150,0.50728,-0.24942,0.31239,0.18847,\n1403636689963555584,4.87626,-2.70041,-1.44173,-0.22849,0.78403,0.27355,0.50819,-0.15548,0.30153,0.14950,\n1403636690063555584,4.86807,-2.66852,-1.43083,-0.22686,0.78317,0.27266,0.51072,-0.06336,0.27542,0.09697,\n1403636690163555328,4.86763,-2.63908,-1.42386,-0.21610,0.78317,0.27850,0.51223,0.02182,0.26941,0.08193,\n1403636690263555584,4.87242,-2.61111,-1.41170,-0.20932,0.78256,0.28197,0.51408,0.08480,0.27874,0.15409,\n1403636690363555328,4.88372,-2.58138,-1.39290,-0.20521,0.78147,0.28384,0.51636,0.14779,0.29670,0.20290,\n1403636690463555584,4.90070,-2.54904,-1.37258,-0.20547,0.78088,0.28274,0.51774,0.19137,0.31396,0.20340,\n1403636690563555584,4.92116,-2.51715,-1.35464,-0.21221,0.78031,0.27774,0.51859,0.23462,0.31553,0.14440,\n1403636690663555328,4.94764,-2.48347,-1.34394,-0.22189,0.78163,0.26982,0.51674,0.29219,0.30282,0.09267,\n1403636690763555584,4.97986,-2.45186,-1.34307,-0.23842,0.78087,0.25709,0.51700,0.34142,0.27028,-0.06486,\n1403636690863555328,5.01793,-2.42667,-1.35690,-0.25081,0.77770,0.24763,0.52053,0.40691,0.18954,-0.18568,\n1403636690963555584,5.06189,-2.41057,-1.37479,-0.26102,0.76983,0.24003,0.53068,0.48071,0.08153,-0.15528,\n1403636691063555584,5.11238,-2.40888,-1.38600,-0.26864,0.75899,0.23499,0.54457,0.53103,-0.06763,-0.07776,\n1403636691163555328,5.16531,-2.42528,-1.38853,-0.26762,0.75030,0.23732,0.55597,0.54676,-0.24968,0.03228,\n1403636691263555584,5.21802,-2.45916,-1.38162,-0.25309,0.74185,0.24943,0.56868,0.49910,-0.42894,0.09886,\n1403636691363555328,5.26408,-2.50860,-1.36883,-0.23718,0.73902,0.26286,0.57315,0.43898,-0.54216,0.16133,\n1403636691463555584,5.30782,-2.56397,-1.35171,-0.22896,0.74727,0.27239,0.56122,0.40023,-0.60282,0.21791,\n1403636691563555584,5.34945,-2.62457,-1.32581,-0.22722,0.75459,0.27707,0.54972,0.38629,-0.63099,0.32548,\n1403636691663555328,5.39097,-2.68872,-1.29247,-0.22027,0.75753,0.28459,0.54464,0.39330,-0.66818,0.37384,\n1403636691763555584,5.43258,-2.75506,-1.25960,-0.21005,0.75770,0.29344,0.54375,0.37460,-0.66759,0.32340,\n1403636691863555328,5.46962,-2.82209,-1.23326,-0.21001,0.75882,0.29455,0.54160,0.34426,-0.66808,0.23520,\n1403636691963555584,5.50459,-2.88754,-1.21377,-0.21590,0.75712,0.29080,0.54368,0.33236,-0.65994,0.20235,\n1403636692063555584,5.53638,-2.95324,-1.19715,-0.22054,0.75513,0.28734,0.54643,0.30813,-0.67657,0.17709,\n1403636692163555328,5.56594,-3.01916,-1.17854,-0.21375,0.75488,0.29111,0.54747,0.30426,-0.66611,0.22127,\n1403636692263555584,5.59271,-3.08455,-1.15335,-0.20891,0.75614,0.29359,0.54627,0.26028,-0.65901,0.29566,\n1403636692363555328,5.61346,-3.15066,-1.11939,-0.21087,0.75793,0.29147,0.54417,0.20704,-0.67408,0.36840,\n1403636692463555584,5.63365,-3.21632,-1.07923,-0.20727,0.75914,0.29291,0.54309,0.19438,-0.66347,0.44175,\n1403636692563555584,5.60555,-3.29325,-1.01180,-0.20488,0.75744,0.29802,0.54360,0.17521,-0.63459,0.49436,\n1403636692663555328,5.62090,-3.35830,-0.95913,-0.20601,0.75835,0.29607,0.54296,0.15525,-0.62695,0.54158,\n1403636692763555584,5.63449,-3.42120,-0.90734,-0.19534,0.76021,0.30187,0.54111,0.13934,-0.62098,0.48754,\n1403636692863555328,5.64584,-3.47770,-0.86032,-0.17978,0.76123,0.31097,0.53991,0.10947,-0.55165,0.43399,\n1403636692963555584,5.65269,-3.52895,-0.81598,-0.17463,0.76233,0.31412,0.53823,0.06977,-0.45868,0.39750,\n1403636693063555584,5.65726,-3.57235,-0.77478,-0.18104,0.76395,0.30993,0.53623,0.04704,-0.37979,0.36159,\n1403636693163555328,5.66109,-3.60936,-0.74172,-0.18883,0.76317,0.30564,0.53712,0.02590,-0.32498,0.28699,\n1403636693263555584,5.66334,-3.64201,-0.71561,-0.19242,0.76666,0.30418,0.53168,0.01606,-0.28153,0.21857,\n1403636693363555328,5.66397,-3.67159,-0.69273,-0.19674,0.77237,0.30225,0.52286,0.02080,-0.24055,0.17513,\n1403636693463555584,5.66821,-3.69679,-0.67602,-0.19985,0.77726,0.30154,0.51477,0.04793,-0.20539,0.13815,\n1403636693563555584,5.67506,-3.71463,-0.66312,-0.19959,0.78459,0.30168,0.50355,0.10016,-0.16127,0.10381,\n1403636693663555328,5.68899,-3.72995,-0.65302,-0.19838,0.78820,0.30330,0.49739,0.16946,-0.10295,0.09426,\n1403636693763555584,5.70742,-3.73792,-0.64178,-0.20065,0.78430,0.30296,0.50282,0.23739,-0.04536,0.09306,\n1403636693863555328,5.73297,-3.74007,-0.63462,-0.20353,0.77723,0.30164,0.51333,0.28455,0.00589,0.06267,\n1403636693963555584,5.76244,-3.73895,-0.62912,-0.20848,0.76818,0.29896,0.52637,0.31352,0.04085,0.03690,\n1403636694063555584,5.79178,-3.73481,-0.62486,-0.21325,0.76014,0.29647,0.53741,0.31216,0.03967,-0.00645,\n1403636694163555328,5.82220,-3.73244,-0.62698,-0.21438,0.75703,0.29502,0.54214,0.29593,0.02959,0.02501,\n1403636694263555584,5.84873,-3.73063,-0.61999,-0.21410,0.75605,0.29587,0.54315,0.27157,0.02722,0.06859,\n1403636694363555328,5.87543,-3.73027,-0.61001,-0.21429,0.75870,0.29616,0.53921,0.25152,0.00595,0.11189,\n1403636694463555584,5.90028,-3.73122,-0.59672,-0.21255,0.76163,0.29771,0.53490,0.25408,0.00806,0.09832,\n1403636694563555584,5.92539,-3.73024,-0.58871,-0.21078,0.76296,0.29866,0.53317,0.25240,0.01801,0.04938,\n1403636694663555328,5.94980,-3.72889,-0.58660,-0.20934,0.76356,0.29873,0.53283,0.25278,0.02492,-0.01244,\n1403636694763555584,5.97235,-3.72725,-0.58937,-0.21101,0.76209,0.29657,0.53547,0.24162,0.02497,-0.05844,\n1403636694863555328,5.99513,-3.72582,-0.59838,-0.21612,0.76261,0.29175,0.53535,0.22530,0.02398,-0.13152,\n1403636694963555584,6.01833,-3.72595,-0.61034,-0.22308,0.76501,0.28538,0.53250,0.23374,-0.00232,-0.15115,\n1403636695063555584,6.04332,-3.72866,-0.62298,-0.22206,0.76662,0.28481,0.53090,0.25617,-0.03103,-0.11311,\n1403636695163555328,6.07010,-3.73435,-0.62999,-0.21712,0.76629,0.28642,0.53256,0.26799,-0.05430,-0.05329,\n1403636695263555584,6.09792,-3.74008,-0.63126,-0.21122,0.76502,0.28846,0.53565,0.27099,-0.06322,0.03785,\n1403636695363555328,6.12320,-3.74644,-0.62600,-0.20741,0.76415,0.28896,0.53811,0.25372,-0.07774,0.10015,\n1403636695463555584,6.14924,-3.75308,-0.61535,-0.20551,0.76360,0.28890,0.53965,0.24831,-0.07847,0.12198,\n1403636695563555584,6.17354,-3.75838,-0.60561,-0.20196,0.76229,0.29005,0.54221,0.22858,-0.06523,0.08689,\n1403636695663555328,6.19677,-3.76310,-0.59991,-0.19727,0.76110,0.29182,0.54466,0.20494,-0.05269,0.02878,\n1403636695763555584,6.21807,-3.76659,-0.60043,-0.19880,0.76203,0.28957,0.54400,0.17540,-0.04506,-0.02957,\n1403636695863555328,6.23675,-3.77114,-0.60420,-0.19946,0.76273,0.28845,0.54337,0.15563,-0.04693,-0.06998,\n1403636695963555584,6.25481,-3.77438,-0.61135,-0.19945,0.76501,0.28757,0.54063,0.14095,-0.03744,-0.11606,\n1403636696063555584,6.27246,-3.77699,-0.62356,-0.19879,0.76890,0.28688,0.53570,0.13916,-0.02920,-0.14483,\n1403636696163555328,6.28824,-3.77838,-0.63619,-0.19224,0.77133,0.29007,0.53287,0.14227,-0.00968,-0.15806,\n1403636696263555584,6.30692,-3.77697,-0.65015,-0.18877,0.77025,0.29179,0.53474,0.13516,0.01922,-0.16542,\n1403636696363555328,6.32230,-3.77268,-0.66495,-0.19278,0.76419,0.28851,0.54370,0.12734,0.04893,-0.14247,\n1403636696463555584,6.33712,-3.76795,-0.67666,-0.20166,0.75798,0.28175,0.55265,0.10424,0.04637,-0.10004,\n1403636696463555584,6.35059,-3.76842,-0.67611,-0.20096,0.75921,0.27724,0.55349,0.03946,0.01197,-0.05797,\n1403636696563555584,6.35059,-3.76842,-0.67611,-0.20096,0.75921,0.27724,0.55349,0.03946,0.01197,-0.05797,\n1403636696663555328,6.35535,-3.76762,-0.67946,-0.20167,0.76112,0.27632,0.55107,0.00736,-0.01134,0.00143,\n1403636696763555584,6.35800,-3.76858,-0.67915,-0.20160,0.76264,0.27625,0.54902,-0.00971,-0.02768,-0.02140,\n1403636696863555328,6.35915,-3.77057,-0.68278,-0.19942,0.76325,0.27752,0.54832,-0.03011,-0.04263,-0.07565,\n1403636696963555584,6.35847,-3.77268,-0.69060,-0.19232,0.76188,0.28233,0.55030,-0.04400,-0.03488,-0.10270,\n1403636697063555584,6.35414,-3.77441,-0.70006,-0.18105,0.75932,0.28982,0.55377,-0.07459,-0.00963,-0.08558,\n1403636697163555328,6.34457,-3.77402,-0.70436,-0.16990,0.75710,0.29675,0.55666,-0.13863,0.02795,-0.01580,\n1403636697263555584,6.32999,-3.76715,-0.70201,-0.16633,0.75557,0.29859,0.55883,-0.21412,0.08861,0.06643,\n1403636697363555328,6.30615,-3.75537,-0.69108,-0.17279,0.76024,0.29377,0.55306,-0.26943,0.15065,0.12282,\n1403636697463555584,6.27804,-3.73741,-0.67433,-0.18188,0.76589,0.28771,0.54549,-0.30997,0.19131,0.21395,\n1403636697563555584,6.24592,-3.71698,-0.64724,-0.18457,0.76995,0.28263,0.54151,-0.31475,0.21258,0.29104,\n1403636697663555328,6.21524,-3.69620,-0.61925,-0.17795,0.77235,0.27527,0.54409,-0.31035,0.21973,0.25101,\n1403636697763555584,6.18306,-3.67456,-0.59866,-0.16665,0.77836,0.26055,0.54636,-0.31988,0.22312,0.18638,\n1403636697863555328,6.14960,-3.65080,-0.58350,-0.15488,0.78348,0.24306,0.55054,-0.33453,0.25392,0.12188,\n1403636697963555584,6.11429,-3.62398,-0.57359,-0.14634,0.78817,0.22690,0.55307,-0.34651,0.28240,0.06467,\n1403636698063555584,6.07820,-3.59549,-0.56933,-0.13989,0.79105,0.21534,0.55525,-0.35717,0.30422,0.02456,\n1403636698163555328,6.04226,-3.56409,-0.57157,-0.13644,0.79353,0.20632,0.55599,-0.35771,0.33018,-0.03815,\n1403636698263555584,6.00627,-3.53072,-0.57763,-0.13218,0.79536,0.19766,0.55754,-0.35423,0.32376,-0.08383,\n1403636698363555328,5.97152,-3.49786,-0.58699,-0.12286,0.79777,0.18655,0.56006,-0.34599,0.33381,-0.11491,\n1403636698463555584,5.93651,-3.46477,-0.59908,-0.11168,0.80218,0.17157,0.56089,-0.34434,0.33561,-0.15054,\n1403636698563555584,5.90266,-3.43039,-0.61555,-0.09810,0.80610,0.15419,0.56285,-0.33488,0.34628,-0.17540,\n1403636698663555328,5.87052,-3.39456,-0.63567,-0.08515,0.81041,0.13266,0.56426,-0.32583,0.35152,-0.21330,\n1403636698763555584,5.83763,-3.35677,-0.65960,-0.07089,0.81435,0.10896,0.56563,-0.31245,0.36659,-0.25226,\n1403636698863555328,5.80529,-3.31790,-0.68595,-0.05655,0.81791,0.08293,0.56652,-0.30559,0.38391,-0.26963,\n1403636698963555584,5.77469,-3.27823,-0.71033,-0.04147,0.82022,0.05726,0.56766,-0.29600,0.39074,-0.21199,\n1403636699063555584,5.74330,-3.23813,-0.72876,-0.02805,0.82140,0.03171,0.56878,-0.27679,0.39972,-0.14855,\n1403636699163555328,5.71371,-3.19862,-0.73882,-0.01728,0.82123,0.00321,0.57033,-0.28175,0.38132,-0.07668,\n1403636699263555584,5.68457,-3.16198,-0.74169,-0.00483,0.82145,-0.02593,0.56967,-0.26718,0.34618,-0.01381,\n1403636699363555328,5.65912,-3.12883,-0.73852,0.00742,0.82172,-0.05515,0.56717,-0.23528,0.30966,0.04272,\n1403636699463555584,5.63719,-3.09997,-0.73123,0.01931,0.82098,-0.08253,0.56464,-0.20749,0.26460,0.06992,\n1403636699563555584,5.61737,-3.07551,-0.72555,0.03269,0.81892,-0.10690,0.56291,-0.19137,0.21518,0.02303,\n1403636699663555328,5.59933,-3.05612,-0.72574,0.04479,0.81574,-0.13118,0.56157,-0.17115,0.14853,-0.02624,\n1403636699763555584,5.58044,-3.04530,-0.73163,0.06174,0.81177,-0.14965,0.56110,-0.17392,0.08796,-0.06961,\n1403636699863555328,5.56183,-3.03985,-0.74061,0.07933,0.80749,-0.15954,0.56234,-0.17523,0.05001,-0.08631,\n1403636699963555584,5.54498,-3.03653,-0.75043,0.08774,0.80326,-0.16732,0.56487,-0.17156,0.02765,-0.12152,\n1403636700063555584,5.52860,-3.03643,-0.76463,0.09080,0.80167,-0.17248,0.56509,-0.18803,0.01259,-0.16442,\n1403636700163555328,5.51144,-3.03932,-0.78206,0.09161,0.80048,-0.17501,0.56588,-0.18894,-0.03471,-0.19973,\n1403636700263555584,5.49459,-3.04450,-0.80487,0.09160,0.80030,-0.17637,0.56571,-0.19852,-0.06939,-0.25187,\n1403636700363555328,5.47734,-3.05518,-0.83420,0.09141,0.80107,-0.18294,0.56256,-0.20504,-0.10974,-0.32264,\n1403636700463555584,5.45784,-3.06896,-0.86899,0.09647,0.79892,-0.19133,0.56197,-0.20540,-0.16324,-0.36647,\n1403636700563555584,5.44282,-3.08888,-0.90851,0.10558,0.79467,-0.19585,0.56480,-0.20699,-0.19883,-0.39147,\n1403636700663555328,5.42619,-3.11086,-0.94853,0.11871,0.79008,-0.20181,0.56653,-0.22603,-0.23239,-0.41435,\n1403636700763555584,5.40917,-3.13282,-0.98884,0.12941,0.78635,-0.20949,0.56659,-0.22686,-0.23145,-0.37001,\n1403636700863555328,5.39052,-3.15438,-1.02250,0.13565,0.78658,-0.21552,0.56253,-0.23433,-0.22572,-0.29113,\n1403636700963555584,5.37120,-3.17608,-1.04811,0.13861,0.78919,-0.21967,0.55652,-0.23025,-0.21811,-0.22480,\n1403636701063555584,5.35223,-3.19770,-1.06622,0.13844,0.79338,-0.22212,0.54959,-0.21626,-0.22453,-0.13287,\n1403636701163555328,5.33386,-3.22081,-1.07546,0.13779,0.79766,-0.22186,0.54363,-0.18593,-0.24240,-0.04462,\n1403636701263555584,5.31670,-3.24671,-1.07484,0.13645,0.80009,-0.22002,0.54114,-0.14587,-0.26546,0.05014,\n1403636701363555328,5.30269,-3.27353,-1.06747,0.13499,0.80105,-0.21778,0.54099,-0.11878,-0.28175,0.09467,\n1403636701463555584,5.29003,-3.30363,-1.05849,0.13504,0.79936,-0.21339,0.54520,-0.08557,-0.29919,0.10914,\n1403636701563555584,5.28217,-3.33424,-1.04889,0.13570,0.79526,-0.20871,0.55279,-0.07170,-0.30326,0.09355,\n1403636701663555328,5.27501,-3.36500,-1.04093,0.13598,0.79342,-0.20388,0.55716,-0.06270,-0.30563,0.08543,\n1403636701763555584,5.26713,-3.39504,-1.03382,0.13620,0.79179,-0.20508,0.55898,-0.07023,-0.30536,0.04439,\n1403636701863555328,5.26012,-3.42513,-1.03329,0.14069,0.79045,-0.21284,0.55686,-0.08212,-0.31483,-0.02979,\n1403636701963555584,5.25316,-3.45571,-1.03903,0.14696,0.78743,-0.22626,0.55423,-0.09139,-0.31578,-0.07624,\n1403636702063555584,5.24340,-3.48626,-1.04910,0.15631,0.78272,-0.24303,0.55123,-0.10372,-0.31364,-0.12868,\n1403636702163555328,5.23275,-3.51738,-1.06521,0.16849,0.77563,-0.26226,0.54885,-0.11242,-0.32755,-0.17793,\n1403636702263555584,5.22245,-3.54825,-1.08604,0.17934,0.77093,-0.27852,0.54400,-0.11373,-0.31550,-0.24243,\n1403636702363555328,5.21327,-3.57723,-1.11270,0.18828,0.76977,-0.28970,0.53674,-0.10229,-0.31848,-0.26875,\n1403636702463555584,5.20583,-3.60787,-1.14273,0.19679,0.76984,-0.29629,0.52993,-0.07742,-0.32080,-0.30094,\n1403636702563555584,5.20212,-3.63766,-1.17791,0.20298,0.76965,-0.30006,0.52574,-0.04443,-0.31899,-0.35769,\n1403636702663555328,5.20221,-3.66743,-1.21817,0.20623,0.77123,-0.30286,0.52053,-0.00271,-0.32103,-0.40244,\n1403636702763555584,5.20560,-3.69708,-1.26166,0.20765,0.77200,-0.30421,0.51803,0.04121,-0.31657,-0.45078,\n1403636702863555328,5.21311,-3.72774,-1.30920,0.20848,0.77413,-0.30391,0.51468,0.08963,-0.33097,-0.46625,\n1403636702963555584,5.22344,-3.75988,-1.35334,0.20750,0.77469,-0.30387,0.51425,0.13078,-0.32451,-0.40685,\n1403636703063555584,5.23975,-3.79323,-1.39210,0.20533,0.77189,-0.30452,0.51894,0.17826,-0.34833,-0.32667,\n1403636703163555328,5.25810,-3.82937,-1.42091,0.19989,0.76503,-0.30720,0.52954,0.20514,-0.35237,-0.22342,\n1403636703263555584,5.27722,-3.86703,-1.44260,0.19192,0.75865,-0.31153,0.53904,0.20952,-0.38397,-0.16933,\n1403636703363555328,5.29442,-3.90573,-1.45943,0.18823,0.75433,-0.31313,0.54544,0.15983,-0.39694,-0.10898,\n1403636703463555584,5.30694,-3.94629,-1.46838,0.19599,0.75647,-0.30752,0.54292,0.10731,-0.40804,-0.04019,\n1403636703563555584,5.31526,-3.98810,-1.47000,0.20445,0.76108,-0.30094,0.53701,0.09590,-0.41848,0.02378,\n1403636703663555328,5.32290,-4.02899,-1.46577,0.20542,0.76472,-0.29913,0.53247,0.09594,-0.40599,0.05070,\n1403636703763555584,5.33085,-4.06811,-1.46235,0.20445,0.76499,-0.29835,0.53289,0.09498,-0.39386,0.01097,\n1403636703863555328,5.34125,-4.10736,-1.46411,0.20134,0.76245,-0.29897,0.53735,0.10227,-0.38267,-0.03237,\n1403636703963555584,5.35187,-4.14643,-1.47067,0.19847,0.76071,-0.29905,0.54083,0.09240,-0.37704,-0.07701,\n1403636704063555584,5.36126,-4.18511,-1.48080,0.19447,0.75972,-0.29956,0.54339,0.07369,-0.37500,-0.12780,\n1403636704163555328,5.36896,-4.22385,-1.49537,0.19298,0.76289,-0.29869,0.53994,0.05840,-0.37497,-0.17357,\n1403636704263555584,5.37497,-4.26254,-1.51507,0.19322,0.76784,-0.29702,0.53373,0.05734,-0.38900,-0.19207,\n1403636704363555328,5.38324,-4.30313,-1.53102,0.19515,0.77137,-0.29460,0.52926,0.06986,-0.39958,-0.12222,\n1403636704463555584,5.39348,-4.34512,-1.53989,0.19535,0.77410,-0.29347,0.52581,0.09911,-0.41783,-0.03162,\n1403636704563555584,5.40611,-4.38793,-1.54044,0.19560,0.77717,-0.29283,0.52153,0.13475,-0.42595,0.03138,\n1403636704663555328,5.42299,-4.43255,-1.53934,0.19562,0.77955,-0.29248,0.51816,0.18165,-0.44174,-0.00431,\n1403636704763555584,5.44226,-4.47801,-1.54439,0.19550,0.78405,-0.29148,0.51194,0.22013,-0.45691,-0.06962,\n1403636704863555328,5.46728,-4.52558,-1.55600,0.19230,0.78804,-0.29235,0.50650,0.28503,-0.47840,-0.14031,\n1403636704963555584,5.49859,-4.57490,-1.57270,0.19001,0.79150,-0.29215,0.50207,0.34360,-0.51236,-0.18392,\n1403636705063555584,5.53627,-4.62874,-1.59234,0.19087,0.79350,-0.29008,0.49978,0.42921,-0.55477,-0.19395,\n1403636705163555328,5.58227,-4.68532,-1.61167,0.19498,0.78693,-0.28623,0.51069,0.49961,-0.57886,-0.17846,\n1403636705263555584,5.63426,-4.74254,-1.62643,0.20103,0.77903,-0.28088,0.52327,0.57017,-0.59288,-0.10696,\n1403636705363555328,5.69176,-4.80125,-1.63592,0.20281,0.77512,-0.27895,0.52938,0.59132,-0.56868,-0.06941,\n1403636705463555584,5.75398,-4.85913,-1.64299,0.20098,0.77719,-0.28020,0.52638,0.61734,-0.55407,-0.06792,\n1403636705563555584,5.81908,-4.91439,-1.65299,0.19920,0.78114,-0.28178,0.52033,0.65518,-0.54678,-0.11706,\n1403636705663555328,5.88571,-4.96885,-1.66717,0.19828,0.78243,-0.28320,0.51797,0.69391,-0.55347,-0.17335,\n1403636705763555584,5.95680,-5.02309,-1.68700,0.19855,0.77941,-0.28356,0.52219,0.73617,-0.56304,-0.19319,\n1403636705863555328,6.03172,-5.07879,-1.70372,0.19682,0.77454,-0.28444,0.52957,0.77044,-0.56678,-0.15207,\n1403636705963555584,6.10817,-5.13512,-1.71425,0.19104,0.76859,-0.28795,0.53838,0.76906,-0.56835,-0.06779,\n1403636706063555584,6.18234,-5.19053,-1.71785,0.18012,0.76337,-0.29391,0.54630,0.73273,-0.57051,0.00010,\n1403636706163555328,6.25302,-5.24847,-1.72031,0.17824,0.76044,-0.29370,0.55110,0.69050,-0.59860,-0.01498,\n1403636706263555584,6.31713,-5.30857,-1.72842,0.18699,0.76345,-0.28587,0.54814,0.64142,-0.60367,-0.10462,\n1403636706363555328,6.37819,-5.36759,-1.74419,0.19556,0.76857,-0.27814,0.54194,0.62076,-0.58712,-0.17378,\n1403636706463555584,6.43878,-5.42499,-1.76386,0.20008,0.77303,-0.27372,0.53617,0.62569,-0.57181,-0.18504,\n1403636706563555584,6.50097,-5.48000,-1.77997,0.20234,0.77568,-0.27213,0.53227,0.63155,-0.54582,-0.11922,\n1403636706663555328,6.56166,-5.53146,-1.79109,0.20248,0.77694,-0.27231,0.53030,0.65640,-0.53658,-0.05932,\n1403636706763555584,6.62864,-5.58465,-1.79401,0.20051,0.77931,-0.27458,0.52638,0.69031,-0.52118,0.01786,\n1403636706863555328,6.69646,-5.63512,-1.79193,0.20041,0.78147,-0.27670,0.52209,0.71541,-0.51089,0.05906,\n1403636706963555584,6.77042,-5.68669,-1.78520,0.20103,0.78093,-0.27900,0.52144,0.75936,-0.51608,0.09373,\n1403636707063555584,6.84579,-5.73708,-1.77602,0.20389,0.77727,-0.27972,0.52538,0.77922,-0.50983,0.09320,\n1403636707163555328,6.92650,-5.78838,-1.76764,0.20606,0.77492,-0.28159,0.52702,0.80191,-0.49817,0.08304,\n1403636707263555584,7.00843,-5.83952,-1.75855,0.20818,0.77445,-0.28421,0.52547,0.82862,-0.49334,0.13781,\n1403636707363555328,7.09370,-5.88784,-1.74199,0.21041,0.77486,-0.28593,0.52303,0.86846,-0.47909,0.20987,\n1403636707463555584,7.18064,-5.93559,-1.71888,0.21110,0.77532,-0.28721,0.52137,0.89934,-0.46011,0.30457,\n1403636707563555584,7.27355,-5.98230,-1.68667,0.20932,0.77481,-0.28967,0.52149,0.92668,-0.44686,0.34346,\n1403636707663555328,7.36482,-6.02717,-1.65860,0.20128,0.77436,-0.29578,0.52189,0.94186,-0.45253,0.25762,\n1403636707763555584,7.45984,-6.07324,-1.63879,0.19421,0.77385,-0.30017,0.52282,0.96389,-0.47760,0.14106,\n1403636707863555328,7.55681,-6.12214,-1.63404,0.19225,0.77215,-0.30065,0.52577,0.96135,-0.49334,0.02543,\n1403636707963555584,7.65142,-6.16998,-1.63742,0.19240,0.77076,-0.29826,0.52910,0.96441,-0.51287,-0.07138,\n1403636708063555584,7.74180,-6.21762,-1.65212,0.19487,0.77056,-0.29346,0.53118,0.96409,-0.52613,-0.14670,\n1403636708163555328,7.83124,-6.26635,-1.67206,0.19782,0.77013,-0.28783,0.53377,0.95714,-0.52029,-0.19531,\n1403636708263555584,7.92316,-6.31692,-1.69661,0.19411,0.76809,-0.28793,0.53801,0.94990,-0.51726,-0.24464,\n1403636708363555328,8.01113,-6.36599,-1.72574,0.18300,0.76277,-0.29429,0.54597,0.91464,-0.52179,-0.24800,\n1403636708463555584,8.09541,-6.41780,-1.75053,0.17461,0.75860,-0.29921,0.55182,0.88145,-0.57126,-0.18362,\n1403636708563555584,8.17996,-6.47728,-1.76757,0.17454,0.75676,-0.29907,0.55444,0.83876,-0.62086,-0.10076,\n1403636708663555328,8.25709,-6.53918,-1.78037,0.18516,0.75853,-0.29284,0.55190,0.76776,-0.64563,-0.10565,\n1403636708763555584,8.33057,-6.60059,-1.79612,0.20007,0.76025,-0.28423,0.54883,0.73229,-0.62697,-0.15172,\n1403636708863555328,8.39566,-6.65803,-1.81401,0.21054,0.76057,-0.27955,0.54685,0.70393,-0.58273,-0.19500,\n1403636708963555584,8.46061,-6.71199,-1.83113,0.21736,0.75851,-0.27919,0.54724,0.68944,-0.53112,-0.14491,\n1403636709063555584,8.52208,-6.75878,-1.84105,0.22267,0.75525,-0.27940,0.54949,0.66262,-0.45616,-0.07241,\n1403636709163555328,8.58656,-6.79988,-1.84590,0.22533,0.75430,-0.28169,0.54855,0.64253,-0.39083,-0.01362,\n1403636709263555584,8.64377,-6.83339,-1.84245,0.22510,0.75740,-0.28504,0.54261,0.62148,-0.31855,0.04322,\n1403636709363555328,8.70536,-6.86147,-1.83679,0.22308,0.76072,-0.28785,0.53730,0.62255,-0.26573,0.04380,\n1403636709463555584,8.76138,-6.88375,-1.83386,0.22355,0.76226,-0.28798,0.53484,0.62530,-0.22369,-0.02456,\n1403636709563555584,8.82445,-6.90181,-1.84030,0.22384,0.76376,-0.28779,0.53266,0.63551,-0.16541,-0.10214,\n1403636709663555328,8.88825,-6.91606,-1.85188,0.22315,0.76545,-0.28779,0.53053,0.65203,-0.13195,-0.13831,\n1403636709763555584,8.94957,-6.92601,-1.86399,0.22142,0.76639,-0.28712,0.53027,0.66847,-0.08515,-0.14794,\n1403636709863555328,9.01958,-6.93420,-1.87633,0.22053,0.76650,-0.28570,0.53124,0.68783,-0.04944,-0.09922,\n1403636709963555584,9.09137,-6.93924,-1.88164,0.22087,0.76647,-0.28348,0.53234,0.70916,-0.01104,-0.02216,\n1403636710063555584,9.16126,-6.93942,-1.88063,0.21936,0.76641,-0.28240,0.53361,0.71605,0.03191,0.01387,\n1403636710163555328,9.23509,-6.93547,-1.88163,0.21883,0.76645,-0.28003,0.53503,0.72812,0.06339,-0.04363,\n1403636710263555584,9.31411,-6.93057,-1.89155,0.21616,0.76634,-0.27862,0.53700,0.74942,0.09939,-0.14363,\n1403636710363555328,9.38999,-6.91858,-1.90961,0.20283,0.76379,-0.28340,0.54329,0.75258,0.13716,-0.20171,\n1403636710463555584,9.47023,-6.90594,-1.93138,0.18732,0.76380,-0.28430,0.54835,0.74860,0.14699,-0.25161,\n1403636710563555584,9.54737,-6.89194,-1.95594,0.17305,0.76556,-0.27981,0.55289,0.72393,0.13010,-0.27942,\n1403636710663555328,9.62022,-6.88019,-1.98434,0.16307,0.76826,-0.27064,0.55672,0.69752,0.11069,-0.30507,\n1403636710763555584,9.68673,-6.87104,-2.01416,0.15604,0.77224,-0.25572,0.56026,0.66243,0.08435,-0.27523,\n1403636710863555328,9.74714,-6.86416,-2.03941,0.15432,0.77876,-0.23352,0.56141,0.62020,0.06997,-0.20995,\n1403636710963555584,9.80381,-6.85812,-2.05802,0.15150,0.78497,-0.21222,0.56199,0.60025,0.06672,-0.13570,\n1403636711063555584,9.86267,-6.85030,-2.07154,0.15035,0.79031,-0.19484,0.56111,0.57545,0.08693,-0.09297,\n1403636711163555328,9.91929,-6.83963,-2.07992,0.15020,0.79243,-0.18305,0.56212,0.56362,0.12216,-0.05673,\n1403636711263555584,9.97542,-6.82383,-2.08629,0.14914,0.79312,-0.17696,0.56339,0.54562,0.17409,-0.04415,\n1403636711363555328,10.02911,-6.80278,-2.08958,0.14392,0.79152,-0.17715,0.56692,0.53953,0.21524,0.00044,\n1403636711463555584,10.08138,-6.77778,-2.08685,0.14137,0.78374,-0.17821,0.57794,0.49287,0.26142,0.04947,\n1403636711563555584,10.12895,-6.74852,-2.08081,0.14488,0.77656,-0.17733,0.58696,0.47016,0.29249,0.06927,\n1403636711663555328,10.16937,-6.71478,-2.07327,0.14996,0.77395,-0.17629,0.58944,0.40855,0.35167,0.07210,\n1403636711763555584,10.20176,-6.67572,-2.06527,0.15314,0.77487,-0.17711,0.58716,0.34140,0.41611,0.06908,\n1403636711863555328,10.22962,-6.63123,-2.05717,0.14803,0.77753,-0.18310,0.58311,0.28576,0.47108,0.09086,\n1403636711963555584,10.25016,-6.58270,-2.04521,0.13787,0.78074,-0.19223,0.57835,0.22850,0.50086,0.13134,\n1403636712063555584,10.26482,-6.53286,-2.02901,0.12723,0.78193,-0.20109,0.57616,0.17294,0.49670,0.18851,\n1403636712163555328,10.26827,-6.48582,-2.00556,0.12278,0.78096,-0.20380,0.57749,0.12182,0.46827,0.23188,\n1403636712263555584,10.27188,-6.44146,-1.97923,0.12707,0.78364,-0.19183,0.57704,0.08474,0.46455,0.29247,\n1403636712363555328,10.27718,-6.39462,-1.94849,0.12946,0.78634,-0.17376,0.57855,0.04583,0.47907,0.32269,\n1403636712463555584,10.27478,-6.34816,-1.91397,0.12812,0.78785,-0.15854,0.58115,0.00514,0.50394,0.37395,\n1403636712563555584,10.27088,-6.29829,-1.87597,0.12311,0.78877,-0.14743,0.58391,-0.03647,0.54026,0.38919,\n1403636712663555328,10.26531,-6.24299,-1.83996,0.11512,0.78624,-0.14065,0.59058,-0.08048,0.56681,0.32644,\n1403636712763555584,10.25540,-6.18390,-1.81052,0.10785,0.78447,-0.13623,0.59532,-0.13499,0.59185,0.26343,\n1403636712863555328,10.23954,-6.12308,-1.78723,0.10318,0.78676,-0.13373,0.59370,-0.19760,0.60947,0.21275,\n1403636712963555584,10.21620,-6.06112,-1.77024,0.10136,0.79045,-0.13218,0.58945,-0.26037,0.62711,0.15026,\n1403636713063555584,10.15529,-5.96010,-1.72816,0.09530,0.79282,-0.12289,0.58928,-0.32417,0.64043,0.17998,\n1403636713163555328,10.12099,-5.89519,-1.70640,0.09140,0.79350,-0.12333,0.58890,-0.37705,0.65315,0.24750,\n1403636713263555584,10.08195,-5.82759,-1.67909,0.08896,0.79583,-0.12462,0.58585,-0.41376,0.65842,0.33178,\n1403636713363555328,10.04182,-5.76203,-1.64320,0.08901,0.79790,-0.12509,0.58291,-0.40961,0.65270,0.41927,\n1403636713463555584,10.00058,-5.69605,-1.60474,0.08611,0.79817,-0.12792,0.58237,-0.43553,0.64518,0.44357,\n1403636713563555584,9.95533,-5.63220,-1.55956,0.08249,0.79842,-0.13152,0.58174,-0.46370,0.63554,0.42974,\n1403636713663555328,9.90881,-5.56901,-1.52250,0.07769,0.79993,-0.13703,0.57905,-0.48496,0.61774,0.36563,\n1403636713763555584,9.85981,-5.51004,-1.48787,0.07350,0.80052,-0.14228,0.57751,-0.50131,0.57485,0.30117,\n1403636713863555328,9.80815,-5.45395,-1.46153,0.07265,0.80067,-0.14513,0.57670,-0.52193,0.52446,0.25689,\n1403636713963555584,9.75456,-5.40361,-1.43871,0.07736,0.80137,-0.14421,0.57535,-0.54173,0.48390,0.20020,\n1403636714063555584,9.69847,-5.35611,-1.42171,0.08435,0.80183,-0.14223,0.57422,-0.55203,0.44881,0.17120,\n1403636714163555328,9.64362,-5.31195,-1.40928,0.08808,0.80192,-0.14266,0.57342,-0.56622,0.43188,0.11480,\n1403636714263555584,9.58521,-5.26743,-1.39882,0.08749,0.79990,-0.14562,0.57559,-0.57808,0.41781,0.07483,\n1403636714363555328,9.52816,-5.22637,-1.39459,0.08612,0.79976,-0.14914,0.57509,-0.59027,0.38845,0.03222,\n1403636714463555584,9.46941,-5.18763,-1.39430,0.08541,0.80454,-0.14972,0.56833,-0.59417,0.36951,-0.00847,\n1403636714563555584,9.40960,-5.14982,-1.39619,0.07971,0.81024,-0.14599,0.56199,-0.57152,0.34970,-0.04332,\n1403636714663555328,9.34610,-5.11759,-1.43193,0.07307,0.81520,-0.13614,0.55819,-0.53675,0.31548,-0.02685,\n1403636714763555584,9.29243,-5.08610,-1.43169,0.06363,0.81955,-0.11926,0.55683,-0.50150,0.28221,0.04322,\n1403636714863555328,9.24067,-5.05655,-1.42233,0.05549,0.82227,-0.09509,0.55835,-0.47345,0.25767,0.11306,\n1403636714963555584,9.19758,-5.03103,-1.40602,0.04390,0.82522,-0.07137,0.55856,-0.42858,0.24764,0.19348,\n1403636715063555584,9.15896,-5.00548,-1.38382,0.03359,0.82727,-0.05121,0.55846,-0.38656,0.23279,0.26781,\n1403636715163555328,9.12470,-4.98248,-1.35765,0.02611,0.82900,-0.03611,0.55746,-0.34105,0.22544,0.29786,\n1403636715263555584,9.09472,-4.95880,-1.33157,0.02042,0.82885,-0.02704,0.55845,-0.29331,0.21951,0.25952,\n1403636715363555328,9.07004,-4.93549,-1.31170,0.01487,0.82678,-0.02326,0.56185,-0.24618,0.22102,0.18331,\n1403636715363555328,9.05425,-4.88981,-1.28291,0.00690,0.82260,-0.02490,0.56803,-0.21698,0.20773,0.08859,\n1403636715463555584,9.05425,-4.88981,-1.28291,0.00690,0.82260,-0.02490,0.56803,-0.21698,0.20773,0.08859,\n1403636715563555584,9.03314,-4.86842,-1.27814,-0.00572,0.81890,-0.03267,0.57298,-0.20874,0.17576,0.02743,\n1403636715663555328,9.01231,-4.85176,-1.27824,-0.00728,0.81607,-0.03471,0.57686,-0.20236,0.11673,-0.01375,\n1403636715763555584,8.99178,-4.83934,-1.27927,0.00235,0.80945,-0.03048,0.58639,-0.22241,0.07924,-0.01948,\n1403636715863555328,8.97047,-4.83135,-1.27903,0.00987,0.80411,-0.02781,0.59374,-0.23491,0.07002,0.04943,\n1403636715963555584,8.94449,-4.82255,-1.27138,0.01568,0.80009,-0.02693,0.59907,-0.30359,0.06681,0.11319,\n1403636716063555584,8.91271,-4.81571,-1.26078,0.01766,0.79963,-0.02850,0.59956,-0.34274,0.06365,0.13672,\n1403636716163555328,8.87505,-4.80922,-1.24651,0.01616,0.80331,-0.03296,0.59442,-0.40702,0.06260,0.19444,\n1403636716263555584,8.83295,-4.80317,-1.22830,0.01594,0.80954,-0.03750,0.58566,-0.44959,0.05974,0.18252,\n1403636716363555328,8.78861,-4.79717,-1.21562,0.01717,0.81327,-0.04102,0.58019,-0.45833,0.04783,0.12097,\n1403636716463555584,8.74373,-4.79279,-1.20837,0.01902,0.81418,-0.04325,0.57868,-0.45311,0.03699,0.05261,\n1403636716563555584,8.69991,-4.78919,-1.20789,0.02171,0.81362,-0.04291,0.57940,-0.44745,0.03968,-0.00505,\n1403636716663555328,8.65478,-4.78474,-1.21222,0.02046,0.81363,-0.03783,0.57979,-0.44768,0.05286,-0.07098,\n1403636716763555584,8.61005,-4.77971,-1.22270,0.01480,0.81365,-0.02935,0.58042,-0.46122,0.05925,-0.11325,\n1403636716863555328,8.56448,-4.77427,-1.23555,0.00516,0.81424,-0.01717,0.58026,-0.47183,0.05765,-0.10389,\n1403636716963555584,8.51650,-4.76748,-1.24510,-0.00690,0.81525,0.00023,0.57907,-0.47748,0.07070,-0.03362,\n1403636717063555584,8.46745,-4.76116,-1.24749,-0.01991,0.81524,0.02021,0.57842,-0.48014,0.06953,0.03485,\n1403636717163555328,8.41745,-4.75384,-1.24284,-0.03024,0.81473,0.03856,0.57777,-0.46578,0.05876,0.08751,\n1403636717263555584,8.36587,-4.74811,-1.23586,-0.03529,0.81412,0.05379,0.57713,-0.48109,0.04481,0.10247,\n1403636717363555328,8.31221,-4.74314,-1.22855,-0.03134,0.81303,0.06885,0.57729,-0.49538,0.05239,0.10796,\n1403636717463555584,8.25811,-4.73514,-1.22072,-0.02749,0.81167,0.07911,0.57808,-0.51381,0.08877,0.07014,\n1403636717563555584,8.20451,-4.72299,-1.21707,-0.03047,0.81141,0.08072,0.57807,-0.52323,0.13860,-0.00276,\n1403636717663555328,8.15192,-4.70779,-1.22024,-0.03558,0.81152,0.07832,0.57796,-0.52887,0.17365,-0.07563,\n1403636717763555584,8.09503,-4.68747,-1.22781,-0.03486,0.81144,0.07969,0.57792,-0.54447,0.21336,-0.11293,\n1403636717863555328,8.04046,-4.66408,-1.23800,-0.03572,0.81148,0.08637,0.57686,-0.55627,0.27196,-0.06959,\n1403636717963555584,7.98337,-4.63290,-1.24042,-0.03960,0.81049,0.09747,0.57623,-0.57255,0.33188,-0.00504,\n1403636718063555584,7.92597,-4.59687,-1.23738,-0.04944,0.80911,0.11064,0.57502,-0.57852,0.39833,0.07865,\n1403636718163555328,7.86570,-4.55285,-1.22709,-0.06725,0.80784,0.12208,0.57268,-0.57706,0.44393,0.08359,\n1403636718263555584,7.80638,-4.50687,-1.22204,-0.08242,0.80696,0.13189,0.56976,-0.59958,0.45084,0.00725,\n1403636718363555328,7.74692,-4.46023,-1.22614,-0.09524,0.80608,0.14071,0.56689,-0.60282,0.47369,-0.06999,\n1403636718463555584,7.69019,-4.41189,-1.23781,-0.10907,0.80506,0.15026,0.56338,-0.58936,0.49180,-0.12938,\n1403636718563555584,7.63526,-4.36421,-1.25442,-0.12261,0.80312,0.15992,0.56070,-0.56998,0.48182,-0.17471,\n1403636718663555328,7.58195,-4.31720,-1.27448,-0.13707,0.80271,0.16469,0.55654,-0.53400,0.46237,-0.20432,\n1403636718763555584,7.53102,-4.27340,-1.29917,-0.14697,0.80207,0.16956,0.55347,-0.49332,0.42223,-0.24435,\n1403636718863555328,7.48376,-4.23448,-1.32630,-0.14357,0.80016,0.18027,0.55374,-0.45462,0.37792,-0.22860,\n1403636718963555584,7.43800,-4.19891,-1.34785,-0.13807,0.79725,0.18936,0.55631,-0.43313,0.35809,-0.14814,\n1403636719063555584,7.39401,-4.16532,-1.36187,-0.13585,0.79453,0.19423,0.55905,-0.43326,0.35094,-0.05882,\n1403636719163555328,7.39898,-4.15329,-1.41123,-0.13698,0.79049,0.19640,0.56373,-0.40842,0.36795,0.02750,\n1403636719263555584,7.35244,-4.11746,-1.41388,-0.13850,0.78678,0.19473,0.56910,-0.43896,0.35119,0.00019,\n1403636719363555328,7.30486,-4.08311,-1.42074,-0.14146,0.78452,0.19119,0.57268,-0.46477,0.33484,-0.06967,\n1403636719463555584,7.25091,-4.05088,-1.43564,-0.14304,0.78366,0.18824,0.57444,-0.49245,0.31335,-0.13953,\n1403636719563555584,7.20010,-4.02205,-1.45375,-0.14271,0.78588,0.18613,0.57217,-0.51157,0.28160,-0.18526,\n1403636719663555328,7.14269,-3.99607,-1.47226,-0.14172,0.78999,0.18409,0.56740,-0.52737,0.26136,-0.10798,\n1403636719763555584,7.08835,-3.97228,-1.47941,-0.13948,0.79414,0.18182,0.56287,-0.53667,0.23312,-0.02966,\n1403636719863555328,7.03024,-3.95258,-1.48115,-0.13808,0.79625,0.17899,0.56114,-0.52997,0.20882,0.05322,\n1403636719963555584,6.97618,-3.93371,-1.47737,-0.13702,0.79819,0.17651,0.55943,-0.52200,0.18842,0.04795,\n1403636720063555584,6.91957,-3.91888,-1.48007,-0.13580,0.79887,0.17526,0.55915,-0.50207,0.17651,-0.01230,\n1403636720163555328,6.86982,-3.90045,-1.48579,-0.13444,0.79990,0.17492,0.55811,-0.48587,0.16651,-0.08528,\n1403636720263555584,6.82205,-3.88370,-1.49651,-0.13526,0.79999,0.17425,0.55799,-0.46156,0.16058,-0.12230,\n1403636720363555328,6.77245,-3.87182,-1.51292,-0.13616,0.79974,0.17432,0.55811,-0.44369,0.14362,-0.15968,\n1403636720463555584,6.72801,-3.86003,-1.53097,-0.13789,0.79990,0.17469,0.55735,-0.43393,0.11851,-0.15358,\n1403636720563555584,6.68504,-3.84958,-1.54460,-0.13972,0.80006,0.17500,0.55656,-0.41636,0.09992,-0.09318,\n1403636720663555328,6.64061,-3.84343,-1.54985,-0.14035,0.80010,0.17656,0.55586,-0.40371,0.07687,-0.01180,\n1403636720763555584,6.60136,-3.83658,-1.54808,-0.14002,0.79985,0.17489,0.55683,-0.38495,0.04601,0.05024,\n1403636720863555328,6.56437,-3.83333,-1.54238,-0.13403,0.80141,0.16968,0.55766,-0.34467,0.03225,0.05078,\n1403636720963555584,6.53094,-3.83137,-1.54088,-0.12677,0.80305,0.15927,0.56007,-0.32419,0.01854,-0.01222,\n1403636721063555584,6.49869,-3.83008,-1.54436,-0.12160,0.80356,0.14684,0.56386,-0.30970,0.00772,-0.06971,\n1403636721163555328,6.49512,-3.82450,-1.59015,-0.10913,0.80512,0.12769,0.56883,-0.29418,-0.01460,-0.09764,\n1403636721263555584,6.46591,-3.82929,-1.59544,-0.10121,0.80512,0.11331,0.57333,-0.29639,-0.03695,-0.03787,\n1403636721363555328,6.43548,-3.83531,-1.59474,-0.09294,0.80651,0.10301,0.57471,-0.30147,-0.06701,0.04557,\n1403636721463555584,6.40229,-3.84317,-1.58857,-0.08636,0.80721,0.09588,0.57599,-0.31555,-0.07824,0.09485,\n1403636721563555584,6.36966,-3.85241,-1.58096,-0.07802,0.80917,0.08832,0.57564,-0.32577,-0.09557,0.05862,\n1403636721663555328,6.33483,-3.86242,-1.57911,-0.06931,0.81129,0.08141,0.57478,-0.33408,-0.09926,-0.00361,\n1403636721763555584,6.30058,-3.87294,-1.58441,-0.06261,0.81154,0.07807,0.57566,-0.34212,-0.09680,-0.06821,\n1403636721863555328,6.26433,-3.88260,-1.59324,-0.06094,0.81134,0.07509,0.57652,-0.34826,-0.09821,-0.11554,\n1403636721963555584,6.23152,-3.89239,-1.60642,-0.05780,0.81106,0.07638,0.57707,-0.34408,-0.09443,-0.13188,\n1403636722063555584,6.19574,-3.90117,-1.61562,-0.05264,0.81064,0.08071,0.57756,-0.34531,-0.07135,-0.07265,\n1403636722163555328,6.15982,-3.90710,-1.61733,-0.05430,0.81101,0.08152,0.57677,-0.35685,-0.05306,0.01827,\n1403636722263555584,6.12310,-3.91154,-1.61174,-0.05773,0.81158,0.08124,0.57567,-0.36669,-0.03929,0.06441,\n1403636722363555328,6.08645,-3.91566,-1.60646,-0.06063,0.81169,0.08070,0.57530,-0.36874,-0.03999,0.02171,\n1403636722463555584,6.04989,-3.92041,-1.60633,-0.05823,0.81179,0.07787,0.57580,-0.37115,-0.04455,-0.03305,\n1403636722563555584,6.01228,-3.92407,-1.61122,-0.05585,0.81222,0.07356,0.57599,-0.37443,-0.03451,-0.06674,\n1403636722663555328,5.97466,-3.92693,-1.61911,-0.05195,0.81271,0.07194,0.57587,-0.37515,-0.03071,-0.08771,\n1403636722763555584,5.93916,-3.92901,-1.62745,-0.04866,0.81280,0.07105,0.57615,-0.37194,-0.01888,-0.08686,\n1403636722863555328,5.89980,-3.92925,-1.63308,-0.04612,0.81318,0.07103,0.57582,-0.39646,-0.00337,-0.01262,\n1403636722963555584,5.86001,-3.92770,-1.63167,-0.04756,0.81378,0.06935,0.57506,-0.41492,0.01286,0.03767,\n1403636723063555584,5.82150,-3.92432,-1.62282,-0.05139,0.81320,0.06682,0.57585,-0.39665,0.02616,0.08886,\n1403636723163555328,5.78540,-3.92086,-1.61097,-0.05235,0.81305,0.06595,0.57607,-0.38976,0.02461,0.05635,\n1403636723263555584,5.74968,-3.91778,-1.60328,-0.05138,0.81357,0.06614,0.57540,-0.38763,0.02325,0.01114,\n1403636723363555328,5.71348,-3.91471,-1.60020,-0.04719,0.81338,0.06632,0.57601,-0.38353,0.03028,-0.03895,\n1403636723463555584,5.67824,-3.91011,-1.59895,-0.03818,0.81212,0.06250,0.57888,-0.37283,0.03311,-0.02855,\n1403636723563555584,5.64026,-3.90574,-1.59914,-0.03398,0.80681,0.05122,0.58761,-0.35332,0.05899,0.01985,\n1403636723663555328,5.60115,-3.89836,-1.59048,-0.03144,0.80301,0.04069,0.59374,-0.38740,0.07000,0.07266,\n1403636723763555584,5.55928,-3.89094,-1.57930,-0.02670,0.80817,0.03394,0.58736,-0.43223,0.07908,0.09698,\n1403636723863555328,5.51525,-3.88321,-1.57017,-0.02493,0.81329,0.02755,0.58067,-0.44381,0.08444,0.05982,\n1403636723963555584,5.46880,-3.87552,-1.56589,-0.02369,0.81798,0.02253,0.57432,-0.44750,0.08274,-0.00558,\n1403636724063555584,5.42514,-3.86768,-1.56681,-0.01880,0.82253,0.01615,0.56819,-0.43779,0.06334,-0.05466,\n1403636724163555328,5.38314,-3.86142,-1.57190,-0.01000,0.82488,0.00639,0.56518,-0.41754,0.05652,-0.08572,\n1403636724263555584,5.34514,-3.85713,-1.57981,-0.00055,0.82560,-0.00775,0.56420,-0.38312,0.04417,-0.11674,\n1403636724363555328,5.30573,-3.85257,-1.58931,0.01196,0.82344,-0.02283,0.56682,-0.36061,0.04478,-0.10045,\n1403636724463555584,5.26676,-3.84721,-1.59524,0.02297,0.81998,-0.03733,0.57071,-0.35350,0.05600,-0.03808,\n1403636724563555584,5.23094,-3.84109,-1.59471,0.03171,0.81636,-0.04690,0.57476,-0.35438,0.07303,0.01752,\n1403636724663555328,5.19971,-3.83399,-1.58697,0.03730,0.81519,-0.05357,0.57551,-0.33532,0.09021,0.10448,\n1403636724763555584,5.17060,-3.82402,-1.56999,0.03994,0.81527,-0.05808,0.57477,-0.31625,0.10821,0.19318,\n1403636724863555328,5.14194,-3.81236,-1.54734,0.04018,0.81624,-0.06027,0.57315,-0.29729,0.12408,0.26507,\n1403636724963555584,5.11523,-3.79986,-1.51619,0.03613,0.81717,-0.06238,0.57188,-0.28995,0.13251,0.32861,\n1403636724963555584,5.08474,-3.81679,-1.45191,0.03395,0.81740,-0.05927,0.57201,-0.29095,0.13030,0.32861,\n1403636725063555584,5.05934,-3.80425,-1.41533,0.02858,0.81722,-0.06105,0.57238,-0.27635,0.12757,0.40623,\n1403636725163555328,5.03529,-3.79335,-1.37362,0.02462,0.81804,-0.06193,0.57129,-0.25270,0.10363,0.42519,\n1403636725263555584,5.01204,-3.78497,-1.33310,0.02409,0.81830,-0.06058,0.57108,-0.23730,0.08163,0.38716,\n1403636725363555328,4.98940,-3.77798,-1.29858,0.02679,0.81880,-0.05827,0.57049,-0.22646,0.06215,0.32048,\n1403636725463555584,4.96760,-3.77248,-1.26945,0.02897,0.81862,-0.05649,0.57081,-0.21469,0.05340,0.25586,\n1403636725563555584,4.94668,-3.76749,-1.24647,0.03126,0.81912,-0.05518,0.57011,-0.21329,0.04396,0.18496,\n1403636725663555328,4.92673,-3.76248,-1.22896,0.03286,0.81803,-0.05439,0.57166,-0.19443,0.04502,0.12221,\n1403636725763555584,4.90804,-3.75724,-1.21674,0.03264,0.81805,-0.05479,0.57161,-0.18174,0.05408,0.06225,\n1403636725863555328,4.88879,-3.75226,-1.21154,0.03192,0.81864,-0.05479,0.57080,-0.16662,0.05000,0.02018,\n1403636725963555584,4.85171,-3.68663,-1.20983,0.03152,0.81863,-0.05906,0.57041,-0.15000,0.05096,-0.02290,\n1403636726063555584,4.83469,-3.68138,-1.21190,0.03100,0.81790,-0.05825,0.57156,-0.13178,0.04124,-0.06074,\n1403636726163555328,4.81840,-3.67631,-1.21787,0.03221,0.81666,-0.05622,0.57347,-0.12056,0.04194,-0.10034,\n1403636726263555584,4.80149,-3.67107,-1.22678,0.03263,0.81614,-0.05445,0.57436,-0.11969,0.04616,-0.11631,\n1403636726363555328,4.78559,-3.66577,-1.23295,0.03248,0.81522,-0.05285,0.57581,-0.09560,0.04550,-0.04828,\n1403636726463555584,4.77113,-3.66075,-1.23143,0.03099,0.81449,-0.05176,0.57703,-0.07727,0.05472,0.04548,\n1403636726563555584,4.75655,-3.65538,-1.22386,0.02225,0.81321,-0.05611,0.57884,-0.07101,0.05364,0.11352,\n1403636726663555328,4.74008,-3.65058,-1.20686,0.00789,0.81068,-0.06468,0.58185,-0.07925,0.01559,0.16858,\n1403636726763555584,4.72262,-3.65222,-1.18707,0.00987,0.81016,-0.06268,0.58276,-0.09547,-0.04204,0.23875,\n1403636726863555328,4.70313,-3.65649,-1.15685,0.01821,0.81027,-0.05380,0.58330,-0.13328,-0.06898,0.30715,\n1403636726963555584,4.68349,-3.66331,-1.12483,0.02204,0.81071,-0.04593,0.58322,-0.15579,-0.07248,0.34209,\n1403636727063555584,4.66309,-3.67052,-1.08760,0.02347,0.81053,-0.04144,0.58375,-0.16640,-0.07506,0.36896,\n1403636727163555328,4.63804,-3.67779,-1.05125,0.02644,0.81145,-0.03675,0.58267,-0.20000,-0.07058,0.30857,\n1403636727263555584,4.61691,-3.68337,-1.02441,0.03035,0.81221,-0.03250,0.58167,-0.21152,-0.03936,0.24182,\n1403636727363555328,4.59352,-3.68657,-1.00240,0.03336,0.81358,-0.02981,0.57973,-0.22174,-0.00455,0.20281,\n1403636727463555584,4.56852,-3.68615,-0.98337,0.03506,0.81358,-0.02836,0.57971,-0.23213,0.02550,0.18061,\n1403636727563555584,4.53784,-3.68205,-0.96060,0.03029,0.81174,-0.03167,0.58237,-0.25564,0.05954,0.22029,\n1403636727663555328,4.51185,-3.67604,-0.93612,0.02741,0.81062,-0.03369,0.58396,-0.25959,0.08220,0.27303,\n1403636727763555584,4.48540,-3.66801,-0.90425,0.02872,0.81189,-0.03297,0.58217,-0.25675,0.10088,0.35233,\n1403636727863555328,4.45794,-3.65836,-0.86563,0.03441,0.81277,-0.02915,0.58084,-0.28391,0.11682,0.41052,\n1403636727963555584,4.42931,-3.64501,-0.82686,0.03959,0.81275,-0.02580,0.58070,-0.28922,0.15475,0.35534,\n1403636728063555584,4.39470,-3.62821,-0.79110,0.03701,0.81311,-0.02790,0.58026,-0.29153,0.19352,0.29665,\n1403636728163555328,4.36518,-3.60752,-0.76544,0.03288,0.81151,-0.03143,0.58257,-0.30540,0.21525,0.23489,\n1403636728263555584,4.33424,-3.58486,-0.74543,0.03035,0.81047,-0.03343,0.58404,-0.31305,0.23012,0.17771,\n1403636728363555328,4.30204,-3.56063,-0.73147,0.02956,0.81077,-0.03448,0.58359,-0.32236,0.25167,0.11790,\n1403636728463555584,4.26360,-3.53614,-0.71632,0.02059,0.81077,-0.04085,0.58357,-0.33140,0.25634,0.05522,\n1403636728563555584,4.22867,-3.51143,-0.71260,0.01089,0.81108,-0.04704,0.58294,-0.35210,0.23427,0.01318,\n1403636728663555328,4.19202,-3.49032,-0.70900,0.00918,0.81154,-0.04205,0.58271,-0.36416,0.20063,0.01938,\n1403636728763555584,4.15568,-3.46927,-0.70371,0.01067,0.81203,-0.02707,0.58288,-0.37247,0.19616,0.08672,\n1403636728863555328,4.11463,-3.44644,-0.68887,0.00818,0.81252,-0.01024,0.58279,-0.38873,0.19843,0.16050,\n1403636728963555584,4.08679,-3.40785,-0.70396,0.00200,0.81120,0.00439,0.58475,-0.39759,0.20773,0.14362,\n1403636729063555584,4.04622,-3.38434,-0.69215,-0.00252,0.80933,0.01381,0.58719,-0.41394,0.21671,0.09006,\n1403636729163555328,4.00410,-3.36013,-0.68746,-0.00621,0.80874,0.02015,0.58779,-0.43969,0.22555,0.02624,\n1403636729263555584,3.95903,-3.33527,-0.68858,-0.01101,0.81052,0.02253,0.58517,-0.45431,0.24867,-0.02510,\n1403636729363555328,3.91316,-3.30822,-0.69499,-0.01315,0.81059,0.02408,0.58498,-0.47469,0.26051,-0.04363,\n1403636729463555584,3.86486,-3.28101,-0.69772,-0.01354,0.80999,0.02532,0.58574,-0.51285,0.26267,0.00727,\n1403636729563555584,3.81346,-3.25513,-0.69409,-0.01479,0.81032,0.02551,0.58525,-0.51753,0.26842,0.07694,\n1403636729663555328,3.76185,-3.22795,-0.68377,-0.01516,0.81116,0.02549,0.58407,-0.54666,0.26429,0.12652,\n1403636729663555328,3.69820,-3.24875,-0.63656,-0.01649,0.81213,0.02594,0.58267,-0.54063,0.26632,0.14135,\n1403636729763555584,3.69923,-3.24614,-0.63631,-0.01630,0.81211,0.02572,0.58270,-0.53959,0.26961,0.14198,\n1403636729863555328,3.64505,-3.21806,-0.62295,-0.01558,0.81492,0.02516,0.57881,-0.55475,0.27331,0.08780,\n1403636729963555584,3.59041,-3.18944,-0.61471,-0.01595,0.81700,0.02427,0.57591,-0.54535,0.28028,0.04556,\n1403636730063555584,3.53817,-3.15828,-0.61206,-0.01667,0.81841,0.02350,0.57391,-0.53283,0.29580,0.00460,\n1403636730163555328,3.48583,-3.12916,-0.61399,-0.01795,0.81867,0.02324,0.57351,-0.52407,0.29516,-0.03929,\n1403636730263555584,3.43482,-3.09907,-0.61868,-0.01733,0.81934,0.02411,0.57254,-0.50670,0.28910,-0.07476,\n1403636730363555328,3.38376,-3.07060,-0.62626,-0.01953,0.81933,0.02336,0.57251,-0.50256,0.28470,-0.10916,\n1403636730463555584,3.33473,-3.04272,-0.63362,-0.01965,0.82215,0.02415,0.56842,-0.47312,0.28369,-0.06797,\n1403636730563555584,3.28950,-3.01480,-0.63529,-0.01929,0.82672,0.02474,0.56174,-0.43118,0.28458,0.01535,\n1403636730663555328,3.25007,-2.98727,-0.62957,-0.02024,0.82924,0.02436,0.55800,-0.35261,0.28958,0.11190,\n1403636730763555584,3.21480,-2.95671,-0.61175,-0.02000,0.83050,0.02478,0.55611,-0.27986,0.29336,0.22322,\n1403636730863555328,3.18311,-2.92730,-0.58975,-0.02163,0.83114,0.02384,0.55514,-0.25479,0.28869,0.24589,\n1403636730963555584,3.14836,-2.89419,-0.56830,-0.02877,0.83090,0.01810,0.55538,-0.22061,0.28105,0.20545,\n1403636731063555584,3.12580,-2.86615,-0.55074,-0.03872,0.82712,0.01004,0.56060,-0.17393,0.25718,0.16741,\n1403636731163555328,3.10658,-2.84261,-0.53693,-0.04350,0.82039,0.00535,0.57012,-0.14695,0.20084,0.11400,\n1403636731263555584,3.09268,-2.82555,-0.52998,-0.03817,0.81622,0.00796,0.57643,-0.14152,0.13996,0.06376,\n1403636731363555328,3.08098,-2.81415,-0.52786,-0.02963,0.81477,0.01283,0.57888,-0.13910,0.10906,0.00260,\n1403636731463555584,3.06927,-2.80621,-0.52968,-0.02372,0.81493,0.01665,0.57884,-0.13610,0.09177,-0.02474,\n1403636731563555584,3.05623,-2.80034,-0.53659,-0.02856,0.81581,0.01364,0.57745,-0.14054,0.08028,-0.07568,\n1403636731663555328,3.04312,-2.79501,-0.54657,-0.03328,0.81584,0.01025,0.57723,-0.14169,0.04290,-0.11015,\n1403636731763555584,3.03015,-2.79482,-0.56107,-0.03007,0.81594,0.01233,0.57722,-0.14155,0.00230,-0.14165,\n1403636731863555328,3.01661,-2.79687,-0.57860,-0.02649,0.81600,0.01525,0.57724,-0.14156,-0.02058,-0.18118,\n1403636731963555584,3.00355,-2.80028,-0.59974,-0.02532,0.81531,0.01706,0.57823,-0.14168,-0.02973,-0.21704,\n1403636732063555584,2.99015,-2.80499,-0.62530,-0.02453,0.81420,0.01850,0.57977,-0.14385,-0.04055,-0.25565,\n1403636732163555328,2.97581,-2.81064,-0.65498,-0.02462,0.81401,0.01926,0.58000,-0.15217,-0.04323,-0.28754,\n1403636732263555584,2.95960,-2.81772,-0.68127,-0.02319,0.81256,0.02108,0.58203,-0.17204,-0.05751,-0.23224,\n1403636732363555328,2.94106,-2.82449,-0.70171,-0.02225,0.81228,0.02248,0.58241,-0.20316,-0.06429,-0.16027,\n1403636732463555584,2.92116,-2.83140,-0.71133,-0.01256,0.80964,0.02947,0.58605,-0.23192,-0.05933,-0.04961,\n1403636732563555584,2.89766,-2.83589,-0.71316,-0.00048,0.80564,0.03859,0.59115,-0.28789,-0.02119,0.00450,\n1403636732663555328,2.86986,-2.83608,-0.71147,0.00249,0.80206,0.04172,0.59578,-0.32610,0.04065,0.04748,\n1403636732763555584,2.83499,-2.82963,-0.70526,-0.00570,0.80108,0.03693,0.59738,-0.36871,0.09517,0.10859,\n1403636732863555328,2.79650,-2.81844,-0.69293,-0.01215,0.80073,0.03313,0.59799,-0.42082,0.11283,0.10296,\n1403636732963555584,2.75304,-2.80497,-0.68483,-0.01440,0.80270,0.03226,0.59534,-0.46504,0.13570,0.04234,\n1403636733063555584,2.70835,-2.78903,-0.68330,-0.01500,0.80608,0.03251,0.59073,-0.47704,0.15565,0.04431,\n1403636733163555328,2.65752,-2.77198,-0.67551,-0.01303,0.80846,0.03439,0.58740,-0.51044,0.17197,0.11554,\n1403636733263555584,2.60813,-2.75137,-0.66088,-0.01264,0.80973,0.03569,0.58559,-0.53020,0.20267,0.18503,\n1403636733363555328,2.55337,-2.72888,-0.64070,-0.01617,0.81142,0.03419,0.58324,-0.56185,0.22535,0.21017,\n1403636733463555584,2.49912,-2.70432,-0.61471,-0.02003,0.81404,0.03214,0.57957,-0.53703,0.24023,0.26444,\n1403636733563555584,2.44673,-2.67782,-0.58764,-0.02108,0.81620,0.03161,0.57652,-0.54270,0.25092,0.24064,\n1403636733663555328,2.39267,-2.65206,-0.56495,-0.02193,0.81767,0.03155,0.57441,-0.54276,0.25373,0.18040,\n1403636733763555584,2.33964,-2.62441,-0.54712,-0.02117,0.81847,0.03252,0.57324,-0.53086,0.26103,0.12621,\n1403636733863555328,2.28666,-2.59725,-0.53730,-0.02172,0.82011,0.03248,0.57087,-0.52611,0.27278,0.07523,\n1403636733963555584,2.23477,-2.56896,-0.53087,-0.01773,0.81806,0.03528,0.57378,-0.51081,0.28168,0.04364,\n1403636734063555584,2.18099,-2.53836,-0.52610,-0.01641,0.81472,0.03560,0.57853,-0.51192,0.29589,0.01377,\n1403636734163555328,2.13008,-2.50823,-0.52493,-0.01744,0.81316,0.03375,0.58080,-0.52477,0.30662,0.02953,\n1403636734263555584,2.07837,-2.47668,-0.51747,-0.02050,0.81265,0.03261,0.58147,-0.53931,0.31776,0.09874,\n1403636734363555328,2.02453,-2.44321,-0.50209,-0.02322,0.81424,0.03157,0.57921,-0.52626,0.32213,0.17583,\n1403636734463555584,1.97204,-2.41112,-0.48087,-0.02528,0.81621,0.03137,0.57634,-0.52982,0.32024,0.21170,\n1403636734563555584,1.92057,-2.37884,-0.46012,-0.02641,0.81841,0.03186,0.57315,-0.52010,0.32084,0.17411,\n1403636734663555328,1.86892,-2.34783,-0.44528,-0.02654,0.82055,0.03300,0.57001,-0.50302,0.31280,0.11660,\n1403636734763555584,1.81845,-2.31739,-0.43635,-0.02575,0.82068,0.03531,0.56972,-0.48722,0.31188,0.06277,\n1403636734863555328,1.77059,-2.28744,-0.43411,-0.02691,0.82132,0.03617,0.56868,-0.46118,0.31195,0.00436,\n1403636734963555584,1.72588,-2.25771,-0.43677,-0.02852,0.82157,0.03620,0.56823,-0.43060,0.30527,-0.03695,\n1403636735063555584,1.68287,-2.22888,-0.44251,-0.02861,0.82111,0.03687,0.56887,-0.40999,0.30266,-0.06748,\n1403636735163555328,1.64279,-2.19967,-0.44865,-0.03028,0.82123,0.03550,0.56869,-0.39564,0.29404,-0.03554,\n1403636735263555584,1.60567,-2.17090,-0.44914,-0.03165,0.82258,0.03348,0.56679,-0.34685,0.29198,0.04748,\n1403636735363555328,1.57316,-2.14352,-0.43847,-0.03249,0.82414,0.03097,0.56461,-0.28179,0.28477,0.17937,\n1403636735463555584,1.54080,-2.11509,-0.41671,-0.03169,0.82274,0.02810,0.56684,-0.26979,0.27104,0.28450,\n1403636735563555584,1.51088,-2.08910,-0.38903,-0.02856,0.82136,0.02602,0.56910,-0.23071,0.25564,0.35325,\n1403636735663555328,1.48310,-2.06372,-0.35777,-0.02539,0.82047,0.02418,0.57061,-0.22152,0.24299,0.32805,\n1403636735763555584,1.45754,-2.03993,-0.33208,-0.02340,0.82121,0.02245,0.56970,-0.20229,0.23663,0.26124,\n1403636735863555328,1.43390,-2.01653,-0.31060,-0.02307,0.82073,0.02090,0.57046,-0.18807,0.22728,0.20675,\n1403636735963555584,1.41307,-1.99499,-0.29496,-0.02243,0.82034,0.02090,0.57106,-0.16897,0.21910,0.16781,\n1403636736063555584,1.39422,-1.97398,-0.28016,-0.02395,0.81920,0.02588,0.57242,-0.16769,0.22331,0.19985,\n1403636736163555328,1.37680,-1.95252,-0.25816,-0.03091,0.81645,0.03427,0.57557,-0.17392,0.23264,0.27491,\n1403636736263555584,1.35946,-1.93153,-0.22974,-0.03871,0.81435,0.04460,0.57737,-0.17950,0.22549,0.30264,\n1403636736363555328,1.34356,-1.91024,-0.20481,-0.04336,0.81325,0.05440,0.57775,-0.18259,0.21373,0.24785,\n1403636736463555584,1.32617,-1.88975,-0.18413,-0.04426,0.81244,0.06348,0.57789,-0.18232,0.20715,0.18473,\n1403636736563555584,1.30810,-1.87075,-0.17013,-0.04057,0.81078,0.07304,0.57936,-0.18514,0.21351,0.10639,\n1403636736663555328,1.28928,-1.84885,-0.16169,-0.03898,0.80954,0.07846,0.58050,-0.20728,0.24052,0.06556,\n1403636736763555584,1.26785,-1.82529,-0.15921,-0.03879,0.80927,0.08134,0.58048,-0.22704,0.26234,0.00997,\n1403636736863555328,1.24491,-1.79666,-0.15889,-0.04055,0.80840,0.08253,0.58140,-0.25417,0.29027,-0.01818,\n1403636736963555584,1.22058,-1.76473,-0.16076,-0.04293,0.80896,0.08338,0.58033,-0.26824,0.32490,-0.04912,\n1403636737063555584,1.19549,-1.72936,-0.16797,-0.05040,0.80916,0.08148,0.57973,-0.27986,0.34657,-0.07477,\n1403636737163555328,1.16821,-1.69401,-0.17687,-0.05684,0.80900,0.08043,0.57951,-0.29263,0.35042,-0.11191,\n1403636737263555584,1.13992,-1.65747,-0.19081,-0.06177,0.80950,0.08041,0.57830,-0.30731,0.34611,-0.15460,\n1403636737363555328,1.11001,-1.62219,-0.20379,-0.06344,0.81170,0.08225,0.57476,-0.31847,0.33873,-0.11566,\n1403636737463555584,1.07691,-1.58607,-0.21133,-0.06453,0.81413,0.08473,0.57083,-0.32171,0.33109,-0.04018,\n1403636737563555584,1.04760,-1.55158,-0.20893,-0.06581,0.81563,0.08578,0.56838,-0.29984,0.32827,0.02886,\n1403636737663555328,1.02266,-1.51826,-0.20115,-0.07123,0.81726,0.08336,0.56574,-0.30826,0.30721,0.07220,\n1403636737763555584,0.99741,-1.48751,-0.18573,-0.07591,0.81871,0.08028,0.56348,-0.27499,0.28320,0.13862,\n1403636737863555328,0.98220,-1.45776,-0.16282,-0.07887,0.81943,0.07732,0.56243,-0.25229,0.25296,0.23397,\n1403636737963555584,0.96630,-1.43561,-0.13173,-0.07938,0.81938,0.07617,0.56260,-0.20172,0.21308,0.32628,\n1403636738063555584,0.95695,-1.41722,-0.09331,-0.07745,0.81939,0.07555,0.56293,-0.17349,0.17525,0.38930,\n1403636738163555328,0.94616,-1.40285,-0.05489,-0.07310,0.81782,0.07688,0.56561,-0.14696,0.14782,0.35339,\n1403636738263555584,0.93859,-1.39048,-0.02197,-0.07051,0.81716,0.07657,0.56693,-0.12665,0.12751,0.28931,\n1403636738363555328,0.93814,-1.38005,0.00301,-0.06883,0.81759,0.07550,0.56667,-0.09139,0.11272,0.22775,\n1403636738463555584,0.93294,-1.37070,0.02247,-0.06675,0.81823,0.07370,0.56623,-0.06436,0.08926,0.16658,\n1403636738563555584,0.93222,-1.36416,0.03465,-0.06216,0.82055,0.06621,0.56431,-0.03279,0.06462,0.09375,\n1403636738663555328,0.93512,-1.36093,0.03886,-0.05514,0.82278,0.05324,0.56317,0.00174,0.03203,0.02669,\n1403636738763555584,0.94612,-1.36280,0.03520,-0.04622,0.82195,0.03534,0.56659,0.02098,0.00011,-0.04461,\n1403636738863555328,0.95551,-1.36663,0.02399,-0.03702,0.81937,0.01290,0.57193,0.04069,-0.03655,-0.10380,\n1403636738963555584,1.08532,-1.95887,0.00188,-0.04221,0.81795,0.01072,0.57363,0.04807,-0.06743,-0.16164,\n1403636739063555584,1.09500,-1.96760,-0.01564,-0.03107,0.81730,-0.01637,0.57514,0.06790,-0.12154,-0.20139,\n1403636739163555328,1.10459,-1.98425,-0.03876,-0.01027,0.81705,-0.03801,0.57522,0.06110,-0.17350,-0.25251,\n1403636739263555584,1.11204,-2.00611,-0.06675,0.01392,0.81578,-0.05796,0.57528,0.04834,-0.21356,-0.28011,\n1403636739363555328,1.11830,-2.02960,-0.09726,0.04062,0.81399,-0.07560,0.57451,0.04257,-0.23202,-0.27258,\n1403636739463555584,1.12286,-2.05256,-0.12111,0.06259,0.81208,-0.09405,0.57251,0.03417,-0.22779,-0.18956,\n1403636739563555584,1.12590,-2.07517,-0.13547,0.07571,0.80911,-0.11496,0.57131,0.02780,-0.20101,-0.10602,\n1403636739663555328,1.13200,-2.09571,-0.14256,0.08416,0.80744,-0.13031,0.56919,0.04805,-0.19065,-0.04237,\n1403636739763555584,1.13927,-2.11506,-0.14711,0.08751,0.80543,-0.14117,0.56895,0.07358,-0.18170,-0.06466,\n1403636739863555328,1.14647,-2.13425,-0.15682,0.08902,0.80457,-0.14745,0.56833,0.08271,-0.18681,-0.12018,\n1403636739963555584,1.15340,-2.15504,-0.17150,0.09080,0.80420,-0.14906,0.56815,0.09207,-0.20158,-0.16103,\n1403636740063555584,1.15778,-2.17608,-0.19027,0.09291,0.80511,-0.14712,0.56703,0.08011,-0.19945,-0.20377,\n1403636740163555328,1.15859,-2.19881,-0.21240,0.09780,0.80550,-0.14161,0.56705,0.06254,-0.20467,-0.24005,\n1403636740263555584,1.16184,-2.21902,-0.23823,0.10274,0.80582,-0.13503,0.56733,0.05420,-0.18927,-0.27181,\n1403636740363555328,1.16328,-2.23767,-0.26782,0.09869,0.80460,-0.13430,0.56995,0.05016,-0.15965,-0.31384,\n1403636740463555584,1.16273,-2.25467,-0.30136,0.09218,0.80434,-0.13592,0.57102,0.03786,-0.14406,-0.35453,\n1403636740563555584,1.16406,-2.27026,-0.33502,0.08856,0.80291,-0.13538,0.57373,0.01823,-0.14346,-0.29517,\n1403636740663555328,1.16300,-2.28638,-0.36336,0.08538,0.80350,-0.13477,0.57353,0.01053,-0.15433,-0.23590,\n1403636740763555584,1.15994,-2.30278,-0.38588,0.08352,0.80648,-0.13369,0.56987,-0.01440,-0.15592,-0.18703,\n1403636740863555328,1.15471,-2.32091,-0.40624,0.08187,0.80646,-0.13286,0.57032,-0.03547,-0.17094,-0.22753,\n1403636740963555584,1.15068,-2.33775,-0.43294,0.08794,0.80514,-0.12710,0.57259,-0.04721,-0.17176,-0.28373,\n1403636741063555584,1.14589,-2.35299,-0.46601,0.10042,0.80426,-0.11887,0.57354,-0.05955,-0.15189,-0.35714,\n1403636741163555328,1.14080,-2.36511,-0.50692,0.10568,0.80156,-0.12246,0.57562,-0.09090,-0.11787,-0.44004,\n1403636741263555584,1.13342,-2.37402,-0.55296,0.10268,0.79885,-0.13484,0.57716,-0.09465,-0.07059,-0.49948,\n1403636741363555328,1.12507,-2.38043,-0.60396,0.09763,0.80035,-0.14586,0.57327,-0.12388,-0.04963,-0.50938,\n1403636741463555584,1.11446,-2.38637,-0.65131,0.09628,0.80427,-0.15313,0.56607,-0.12357,-0.04770,-0.43203,\n1403636741563555584,1.10221,-2.39414,-0.69285,0.09260,0.80991,-0.15975,0.55672,-0.11361,-0.06386,-0.36008,\n1403636741663555328,1.09124,-2.40380,-0.72649,0.09550,0.81307,-0.15956,0.55166,-0.08222,-0.08159,-0.28384,\n1403636741763555584,1.08483,-2.41522,-0.75056,0.09773,0.81165,-0.15778,0.55387,-0.03556,-0.10820,-0.20163,\n1403636741863555328,1.08187,-2.42652,-0.77092,0.10174,0.80919,-0.15304,0.55806,-0.02652,-0.11566,-0.22736,\n1403636741963555584,1.07980,-2.43867,-0.79755,0.10761,0.80848,-0.14558,0.55999,-0.00804,-0.11119,-0.30312,\n1403636742063555584,1.07971,-2.44941,-0.83053,0.11017,0.81153,-0.13984,0.55653,0.02145,-0.08533,-0.31642,\n1403636742163555328,1.08231,-2.46001,-0.86294,0.10369,0.81641,-0.14046,0.55045,0.04268,-0.06684,-0.28875,\n1403636742263555584,1.08780,-2.46746,-0.88763,0.09600,0.81758,-0.14217,0.54967,0.08836,-0.06090,-0.19804,\n1403636742363555328,1.09823,-2.47563,-0.90431,0.09197,0.81425,-0.14170,0.55540,0.10431,-0.06179,-0.14122,\n1403636742463555584,1.11122,-2.48309,-0.91072,0.09019,0.81049,-0.14002,0.56157,0.13004,-0.07394,-0.07447,\n1403636742563555584,1.12540,-2.49097,-0.91371,0.08919,0.80903,-0.13828,0.56427,0.15608,-0.08239,-0.05444,\n1403636742663555328,1.14087,-2.49902,-0.91688,0.08943,0.80643,-0.13658,0.56834,0.15733,-0.08226,-0.09164,\n1403636742763555584,1.15423,-2.50776,-0.92943,0.08857,0.80391,-0.13493,0.57243,0.14948,-0.08071,-0.19295,\n1403636742863555328,1.16618,-2.51604,-0.94865,0.08709,0.80245,-0.13424,0.57486,0.12713,-0.07266,-0.26263,\n1403636742963555584,1.17634,-2.52605,-0.97477,0.08597,0.80208,-0.13505,0.57536,0.09790,-0.07770,-0.33055,\n1403636743063555584,1.18500,-2.53539,-1.01260,0.08769,0.80575,-0.14047,0.56864,0.09001,-0.08750,-0.39484,\n1403636743163555328,1.19182,-2.54506,-1.05170,0.08736,0.81594,-0.14921,0.55167,0.09858,-0.09826,-0.36265,\n1403636743263555584,1.20006,-2.55769,-1.08279,0.08927,0.82458,-0.15416,0.53697,0.13871,-0.12340,-0.29718,\n1403636743363555328,1.21355,-2.57249,-1.10471,0.08800,0.82685,-0.15919,0.53220,0.20247,-0.15711,-0.19219,\n1403636743463555584,1.23614,-2.59142,-1.11856,0.08864,0.82441,-0.16122,0.53526,0.28550,-0.20121,-0.11485,\n1403636743563555584,1.26692,-2.61245,-1.13440,0.09141,0.81949,-0.16101,0.54237,0.32438,-0.22395,-0.15615,\n1403636743663555328,1.30294,-2.63637,-1.15551,0.09669,0.81506,-0.15738,0.54915,0.36235,-0.23961,-0.24349,\n1403636743763555584,1.34114,-2.66295,-1.18244,0.10271,0.80916,-0.15547,0.55727,0.39085,-0.25726,-0.29835,\n1403636743863555328,1.38032,-2.68762,-1.21530,0.10470,0.80429,-0.16208,0.56204,0.40163,-0.24364,-0.36188,\n1403636743863555328,1.45637,-2.70972,-1.26419,0.11035,0.80021,-0.16380,0.56626,0.40900,-0.23913,-0.41660,\n1403636743963555584,1.45632,-2.71200,-1.26283,0.11012,0.80031,-0.16364,0.56622,0.41084,-0.24341,-0.41748,\n1403636744063555584,1.49684,-2.73835,-1.30592,0.11508,0.79790,-0.16686,0.56769,0.41716,-0.23438,-0.47760,\n1403636744163555328,1.53665,-2.76154,-1.35647,0.12049,0.79533,-0.17366,0.56813,0.38900,-0.21804,-0.53422,\n1403636744263555584,1.57246,-2.78403,-1.41118,0.12086,0.79174,-0.18155,0.57059,0.36632,-0.19600,-0.58440,\n1403636744363555328,1.60870,-2.80522,-1.47294,0.11980,0.79009,-0.18787,0.57106,0.34672,-0.18890,-0.64633,\n1403636744463555584,1.64249,-2.82362,-1.53960,0.11496,0.78849,-0.19382,0.57227,0.32539,-0.17798,-0.69366,\n1403636744563555584,1.67140,-2.84378,-1.61015,0.10884,0.78626,-0.19757,0.57525,0.29829,-0.20932,-0.73270,\n1403636744663555328,1.69827,-2.86471,-1.68554,0.10564,0.78405,-0.19884,0.57843,0.25804,-0.22906,-0.76029,\n1403636744763555584,1.72062,-2.88881,-1.76348,0.11266,0.78339,-0.19362,0.57976,0.20098,-0.24282,-0.77261,\n1403636744863555328,1.73601,-2.91389,-1.83742,0.11888,0.78559,-0.18906,0.57704,0.13884,-0.23596,-0.68799,\n1403636744963555584,1.74961,-2.93709,-1.90307,0.12141,0.79034,-0.18614,0.57093,0.09386,-0.21579,-0.59968,\n1403636745063555584,1.75980,-2.96028,-1.96035,0.11955,0.79552,-0.18441,0.56467,0.10695,-0.20769,-0.51725,\n1403636745163555328,1.77014,-2.98274,-2.00929,0.11422,0.79963,-0.18369,0.56018,0.11108,-0.21771,-0.45172,\n1403636745263555584,1.78232,-3.00616,-2.05183,0.11279,0.80163,-0.17885,0.55918,0.13350,-0.23168,-0.41943,\n1403636745363555328,1.79713,-3.03020,-2.08724,0.11273,0.79981,-0.17270,0.56371,0.14275,-0.23687,-0.31204,\n1403636745463555584,1.81266,-3.05314,-2.10821,0.11486,0.79536,-0.16788,0.57098,0.12995,-0.22581,-0.21361,\n1403636745563555584,1.82867,-3.07489,-2.12290,0.11522,0.79264,-0.16843,0.57453,0.11707,-0.20585,-0.15312,\n1403636745663555328,1.84294,-3.09475,-2.13063,0.11804,0.79096,-0.17147,0.57537,0.10085,-0.18159,-0.07574,\n1403636745763555584,1.85577,-3.11224,-2.13260,0.12391,0.79003,-0.17438,0.57454,0.09471,-0.15647,-0.00709,\n1403636745863555328,1.86704,-3.12698,-2.12636,0.13063,0.79028,-0.17760,0.57170,0.09632,-0.13116,0.06502,\n1403636745963555584,1.87937,-3.13881,-2.11501,0.13943,0.78972,-0.18021,0.56959,0.09797,-0.08992,0.12570,\n1403636746063555584,1.88837,-3.14749,-2.10064,0.14718,0.78971,-0.18233,0.56697,0.08605,-0.03858,0.19890,\n1403636746163555328,1.89876,-3.15064,-2.08068,0.15321,0.78958,-0.18401,0.56501,0.07848,0.03099,0.28636,\n1403636746263555584,1.90777,-3.14677,-2.05376,0.15633,0.78533,-0.18557,0.56955,0.08645,0.08682,0.33230,\n1403636746363555328,1.91856,-3.13958,-2.02753,0.15843,0.77645,-0.18590,0.58092,0.06798,0.15208,0.25726,\n1403636746463555584,1.92513,-3.12260,-2.00943,0.15571,0.77579,-0.18806,0.58184,0.04047,0.21824,0.13205,\n1403636746563555584,1.92953,-3.10256,-2.00212,0.14695,0.78413,-0.19220,0.57150,0.02435,0.26355,0.03824,\n1403636746663555328,1.93101,-3.07545,-2.00151,0.13464,0.79717,-0.19715,0.55455,0.03728,0.28061,-0.01333,\n1403636746763555584,1.93541,-3.04992,-2.00594,0.11949,0.80616,-0.20337,0.54265,0.06899,0.25668,-0.05198,\n1403636746863555328,1.94413,-3.02765,-2.01006,0.10921,0.80705,-0.20678,0.54220,0.10901,0.20497,-0.04611,\n1403636746963555584,1.95655,-3.01001,-2.01210,0.10648,0.80588,-0.20532,0.54504,0.13902,0.13840,-0.00916,\n1403636747063555584,1.97123,-2.99900,-2.00623,0.11119,0.80330,-0.19976,0.54994,0.16743,0.07555,0.11984,\n1403636747163555328,1.98977,-2.99195,-1.99078,0.12051,0.79952,-0.19292,0.55590,0.22504,0.03604,0.20296,\n1403636747263555584,2.01147,-2.98602,-1.96617,0.12990,0.79675,-0.18715,0.55972,0.25462,0.03697,0.29111,\n1403636747363555328,2.03474,-2.97665,-1.93402,0.13419,0.79577,-0.18594,0.56051,0.26031,0.06493,0.33470,\n1403636747463555584,2.05909,-2.96559,-1.90518,0.13713,0.79484,-0.18669,0.56087,0.26194,0.09279,0.27069,\n1403636747563555584,2.08067,-2.95226,-1.88240,0.13692,0.79559,-0.19004,0.55872,0.26690,0.11814,0.21484,\n1403636747663555328,2.10544,-2.93800,-1.86177,0.13700,0.79598,-0.19293,0.55715,0.26647,0.14682,0.25850,\n1403636747763555584,2.12964,-2.92187,-1.83559,0.13827,0.79596,-0.19541,0.55601,0.26957,0.16365,0.32716,\n1403636747863555328,2.15698,-2.90491,-1.80056,0.13812,0.79485,-0.19855,0.55651,0.30635,0.17125,0.42011,\n1403636747963555584,2.18612,-2.88668,-1.75844,0.12730,0.79034,-0.20934,0.56155,0.31163,0.17679,0.43931,\n1403636748063555584,2.21053,-2.86878,-1.71990,0.11781,0.78701,-0.21911,0.56456,0.28467,0.15106,0.37754,\n1403636748163555328,2.23465,-2.85417,-1.68633,0.11423,0.78646,-0.22403,0.56412,0.25543,0.11144,0.30993,\n1403636748263555584,2.25900,-2.84613,-1.66014,0.12096,0.78846,-0.22068,0.56125,0.23643,0.06162,0.24555,\n1403636748363555328,2.28118,-2.84220,-1.64052,0.13425,0.79124,-0.21166,0.55778,0.24043,0.03481,0.20281,\n1403636748463555584,2.30515,-2.83947,-1.62134,0.14231,0.79207,-0.20530,0.55698,0.24545,0.03418,0.21853,\n1403636748563555584,2.32894,-2.83630,-1.60055,0.14514,0.79169,-0.20292,0.55766,0.24591,0.05712,0.27107,\n1403636748663555328,2.35236,-2.83128,-1.57013,0.13915,0.78948,-0.20767,0.56056,0.23757,0.06020,0.31526,\n1403636748763555584,2.37541,-2.82713,-1.53950,0.13141,0.78721,-0.21420,0.56316,0.22369,0.05361,0.28577,\n1403636748863555328,2.39631,-2.82574,-1.51587,0.12736,0.78550,-0.21908,0.56460,0.20756,0.02772,0.23351,\n1403636748963555584,2.41714,-2.82552,-1.49370,0.12648,0.78375,-0.22362,0.56546,0.18462,-0.00771,0.19369,\n1403636749063555584,2.43693,-2.82724,-1.47721,0.12490,0.78193,-0.22847,0.56638,0.16296,-0.03255,0.16716,\n1403636749163555328,2.45256,-2.83327,-1.46204,0.12180,0.77970,-0.23351,0.56807,0.12756,-0.07447,0.14851,\n1403636749263555584,2.46460,-2.84375,-1.44488,0.13009,0.78009,-0.23075,0.56683,0.08878,-0.11626,0.21000,\n1403636749363555328,2.47372,-2.85711,-1.42145,0.14039,0.78119,-0.22556,0.56494,0.05440,-0.12586,0.28017,\n1403636749463555584,2.47873,-2.87080,-1.39080,0.14887,0.78225,-0.22049,0.56330,0.03496,-0.12005,0.31819,\n1403636749563555584,2.48378,-2.88524,-1.35967,0.15627,0.78434,-0.21475,0.56060,0.03795,-0.11033,0.33509,\n1403636749663555328,2.48775,-2.89734,-1.32843,0.16064,0.78484,-0.21055,0.56026,0.04198,-0.08197,0.26539,\n1403636749763555584,2.49449,-2.90789,-1.30504,0.15875,0.78278,-0.20966,0.56400,0.05450,-0.05422,0.21218,\n1403636749863555328,2.49857,-2.91346,-1.28602,0.15623,0.77768,-0.20881,0.57202,0.03961,-0.02659,0.15314,\n1403636749963555584,2.50188,-2.91685,-1.27323,0.15414,0.77458,-0.20658,0.57757,0.01553,0.00385,0.09500,\n1403636750063555584,2.50348,-2.91770,-1.26591,0.15130,0.77405,-0.20439,0.57981,-0.02233,0.04072,0.05596,\n1403636750163555328,2.49791,-2.91244,-1.26010,0.14823,0.77681,-0.20301,0.57739,-0.06941,0.07423,0.08168,\n1403636750263555584,2.48974,-2.90597,-1.24866,0.14528,0.78042,-0.20165,0.57374,-0.08568,0.09092,0.15385,\n1403636750363555328,2.47835,-2.89544,-1.22858,0.14265,0.78420,-0.19973,0.56990,-0.08750,0.09781,0.26407,\n1403636750463555584,2.46626,-2.88456,-1.19786,0.13925,0.78647,-0.19811,0.56818,-0.11050,0.12668,0.36235,\n1403636750563555584,2.45139,-2.87091,-1.15774,0.13728,0.78758,-0.19467,0.56831,-0.11209,0.13173,0.48207,\n1403636750663555328,2.43554,-2.85662,-1.11004,0.13658,0.78888,-0.19117,0.56787,-0.12095,0.12207,0.53280,\n1403636750763555584,2.41971,-2.84132,-1.05637,0.13490,0.79067,-0.18914,0.56646,-0.13135,0.13897,0.57503,\n1403636750863555328,2.40088,-2.82342,-0.99673,0.13461,0.79190,-0.18721,0.56544,-0.16742,0.16843,0.64997,\n1403636750963555584,2.38358,-2.80561,-0.93039,0.13672,0.79421,-0.18470,0.56252,-0.16576,0.17986,0.65225,\n1403636751063555584,2.36725,-2.78572,-0.86803,0.13797,0.79527,-0.18417,0.56089,-0.15356,0.20087,0.60420,\n1403636751163555328,2.34937,-2.76286,-0.81354,0.13635,0.79650,-0.18643,0.55879,-0.13889,0.21682,0.52417,\n1403636751263555584,2.33617,-2.74046,-0.76418,0.13503,0.80157,-0.18882,0.55100,-0.12184,0.23286,0.49885,\n1403636751363555328,2.32480,-2.71696,-0.71607,0.13022,0.80213,-0.19387,0.54959,-0.09178,0.22874,0.49085,\n1403636751463555584,2.31749,-2.69493,-0.66749,0.12372,0.79776,-0.20024,0.55514,-0.06939,0.21276,0.48338,\n1403636751563555584,2.31024,-2.67597,-0.62091,0.11737,0.79329,-0.20199,0.56223,-0.06029,0.19268,0.45713,\n1403636751663555328,2.30520,-2.65951,-0.57706,0.10979,0.79312,-0.20073,0.56446,-0.07204,0.15719,0.42433,\n1403636751763555584,2.32976,-2.66067,-0.59647,0.10310,0.79829,-0.19838,0.55924,-0.08465,0.11159,0.39058,\n1403636751863555328,2.32223,-2.65309,-0.55909,0.10109,0.80491,-0.19475,0.55134,-0.08555,0.05326,0.38266,\n1403636751963555584,2.31817,-2.65038,-0.52253,0.10664,0.80927,-0.18687,0.54662,-0.04758,0.00351,0.38091,\n1403636752063555584,2.31702,-2.65235,-0.48628,0.11161,0.81055,-0.18124,0.54562,-0.01118,-0.02592,0.36673,\n1403636752163555328,2.32254,-2.65659,-0.45421,0.11350,0.80753,-0.17724,0.55100,0.04194,-0.03977,0.35667,\n1403636752263555584,2.32870,-2.66253,-0.42282,0.11135,0.80334,-0.16988,0.55981,0.05038,-0.03757,0.36153,\n1403636752363555328,2.33725,-2.66557,-0.38802,0.10530,0.79953,-0.15910,0.56951,0.07117,-0.03298,0.40887,\n1403636752463555584,2.34552,-2.66850,-0.34686,0.09529,0.80050,-0.14516,0.57363,0.06559,-0.02649,0.44932,\n1403636752563555584,2.35225,-2.67102,-0.30468,0.08450,0.80419,-0.12768,0.57432,0.05224,-0.02345,0.41761,\n1403636752663555328,2.35645,-2.67381,-0.26822,0.07266,0.80903,-0.10903,0.57298,0.03407,-0.02224,0.34335,\n1403636752763555584,2.35899,-2.67608,-0.23822,0.05930,0.81197,-0.09052,0.57358,0.03107,-0.02630,0.28024,\n1403636752863555328,2.34439,-2.67857,-0.20756,0.04740,0.81537,-0.06839,0.57293,0.01534,-0.01533,0.21121,\n1403636752963555584,2.34554,-2.67788,-0.18962,0.03893,0.81655,-0.04426,0.57426,0.01667,0.00598,0.13617,\n1403636753063555584,2.34718,-2.67501,-0.17981,0.02882,0.81820,-0.02011,0.57386,0.01689,0.02942,0.07045,\n1403636753163555328,2.34938,-2.66856,-0.17626,0.01623,0.81895,0.00339,0.57363,0.02320,0.06893,0.00399,\n1403636753263555584,2.35046,-2.65905,-0.17956,0.00413,0.81882,0.02768,0.57337,0.02301,0.10966,-0.05534,\n1403636753363555328,2.35220,-2.64659,-0.18680,-0.00688,0.81689,0.05111,0.57449,0.02585,0.14340,-0.09385,\n1403636753363555328,2.34077,-2.65615,-0.23071,-0.00671,0.81690,0.05086,0.57449,0.02594,0.14338,-0.09385,\n1403636753463555584,2.34172,-2.64030,-0.23905,-0.02217,0.81509,0.06280,0.57549,0.03196,0.18559,-0.05839,\n1403636753563555584,2.34024,-2.62194,-0.24000,-0.03543,0.81418,0.06785,0.57554,-0.00321,0.20041,0.02436,\n1403636753663555328,2.33517,-2.60203,-0.23432,-0.04296,0.81268,0.07015,0.57687,-0.03921,0.20140,0.08057,\n1403636753763555584,2.33044,-2.58099,-0.22800,-0.04539,0.81189,0.07092,0.57770,-0.04204,0.20196,0.05816,\n1403636753863555328,2.32394,-2.55978,-0.22600,-0.04437,0.81108,0.07122,0.57888,-0.07148,0.21212,-0.01476,\n1403636753963555584,2.31677,-2.53771,-0.22938,-0.04383,0.81115,0.06927,0.57906,-0.08327,0.22320,-0.04277,\n1403636754063555584,2.30854,-2.51481,-0.23122,-0.04212,0.80910,0.06700,0.58231,-0.08218,0.23389,0.02322,\n1403636754163555328,2.29914,-2.49047,-0.22656,-0.04074,0.80570,0.06458,0.58738,-0.11871,0.24330,0.07779,\n1403636754263555584,2.27520,-2.50001,-0.22271,-0.03448,0.80388,0.06068,0.59069,-0.15555,0.25455,0.13337,\n1403636754363555328,2.25854,-2.47389,-0.20701,-0.03120,0.80377,0.06069,0.59102,-0.16868,0.27768,0.18336,\n1403636754463555584,2.23716,-2.44553,-0.18708,-0.03324,0.80398,0.06246,0.59043,-0.22209,0.30713,0.18549,\n1403636754563555584,2.21086,-2.41351,-0.17051,-0.03564,0.80500,0.06952,0.58811,-0.25895,0.33859,0.12676,\n1403636754663555328,2.18174,-2.38005,-0.16109,-0.04213,0.80528,0.08033,0.58591,-0.29199,0.36401,0.07108,\n1403636754763555584,2.14871,-2.34432,-0.15573,-0.05302,0.80434,0.09288,0.58447,-0.32395,0.38709,0.02586,\n1403636754863555328,2.11346,-2.30681,-0.15538,-0.06319,0.80303,0.10510,0.58318,-0.34869,0.38648,-0.02140,\n1403636754963555584,2.07627,-2.26720,-0.16000,-0.07059,0.80165,0.11460,0.58245,-0.37431,0.39475,-0.06770,\n1403636755063555584,2.03683,-2.22718,-0.16738,-0.07519,0.80067,0.12178,0.58176,-0.40606,0.40296,-0.07363,\n1403636755163555328,1.99229,-2.18720,-0.17158,-0.07948,0.80062,0.12586,0.58039,-0.46860,0.40394,-0.02048,\n1403636755263555584,1.94425,-2.14570,-0.16914,-0.08189,0.80191,0.12799,0.57780,-0.47975,0.41561,0.04821,\n1403636755363555328,1.89524,-2.10420,-0.16002,-0.08248,0.80831,0.12931,0.56843,-0.49660,0.41543,0.11441,\n1403636755463555584,1.80690,-2.05223,-0.13841,-0.08092,0.81335,0.12931,0.56142,-0.45553,0.43515,0.15999,\n1403636755563555584,1.76539,-2.00840,-0.12152,-0.07796,0.81396,0.13009,0.56077,-0.43087,0.45646,0.12813,\n1403636755663555328,1.72461,-1.95965,-0.11020,-0.07762,0.81245,0.12844,0.56338,-0.41186,0.48172,0.05891,\n1403636755763555584,1.68329,-1.90906,-0.10625,-0.07857,0.81137,0.12593,0.56537,-0.39904,0.49640,0.01760,\n1403636755863555328,1.64325,-1.85704,-0.10641,-0.07994,0.80895,0.12369,0.56912,-0.39508,0.51102,-0.03540,\n1403636755963555584,1.60277,-1.80260,-0.10729,-0.08104,0.80709,0.12225,0.57192,-0.38903,0.52475,-0.00684,\n1403636756063555584,1.56239,-1.74816,-0.10528,-0.08221,0.80589,0.12121,0.57366,-0.37533,0.53109,0.05161,\n1403636756163555328,1.51957,-1.68989,-0.09499,-0.08064,0.80462,0.12205,0.57549,-0.37032,0.55135,0.13311,\n1403636756263555584,1.47781,-1.63456,-0.08267,-0.08169,0.80372,0.12167,0.57668,-0.39613,0.55193,0.14299,\n1403636756363555328,1.43353,-1.57567,-0.07057,-0.08184,0.80320,0.12229,0.57724,-0.40186,0.55522,0.10341,\n1403636756463555584,1.38910,-1.52000,-0.06567,-0.08209,0.80251,0.12309,0.57800,-0.42219,0.55148,0.03003,\n1403636756563555584,1.34127,-1.44432,-0.06727,-0.08257,0.80224,0.12347,0.57822,-0.43755,0.55828,-0.03123,\n1403636756663555328,1.29569,-1.38950,-0.07192,-0.08399,0.80165,0.12362,0.57881,-0.46193,0.55693,-0.02701,\n1403636756763555584,1.24059,-1.33220,-0.07310,-0.08478,0.80028,0.12384,0.58054,-0.51344,0.54182,0.01340,\n1403636756863555328,1.18835,-1.27836,-0.06760,-0.08643,0.79997,0.12411,0.58067,-0.54158,0.52926,0.05818,\n1403636756963555584,1.12610,-1.22416,-0.05503,-0.08730,0.80014,0.12423,0.58028,-0.58255,0.52042,0.13243,\n1403636757063555584,1.07044,-1.17416,-0.04074,-0.08924,0.80036,0.12396,0.57974,-0.59233,0.51081,0.11568,\n1403636757163555328,1.00772,-1.12393,-0.02961,-0.08935,0.80048,0.12414,0.57951,-0.61054,0.50159,0.06632,\n1403636757263555584,0.94693,-1.07266,-0.02494,-0.08913,0.80037,0.12466,0.57960,-0.62717,0.49002,0.01237,\n1403636757363555328,0.88035,-1.02086,-0.02448,-0.08714,0.80128,0.12640,0.57825,-0.64694,0.49192,-0.04504,\n1403636757463555584,0.81644,-0.96905,-0.02992,-0.08386,0.80324,0.12925,0.57539,-0.65292,0.49906,-0.03789,\n1403636757563555584,0.74784,-0.91442,-0.03010,-0.08361,0.80629,0.13487,0.56983,-0.67778,0.52194,0.00104,\n1403636757663555328,0.68086,-0.86165,-0.02597,-0.08587,0.81213,0.14238,0.55929,-0.65851,0.54646,0.07600,\n1403636757763555584,0.61423,-0.80088,-0.01443,-0.08980,0.81745,0.14835,0.54927,-0.60459,0.58244,0.16344,\n1403636757863555328,0.55413,-0.73741,0.00578,-0.09441,0.82008,0.15256,0.54340,-0.53646,0.61666,0.29458,\n1403636757963555584,0.47467,-0.63291,0.05952,-0.10237,0.81756,0.15767,0.54430,-0.48814,0.64172,0.35149,\n1403636758063555584,0.42412,-0.56149,0.09269,-0.10701,0.80880,0.15831,0.55618,-0.44754,0.65495,0.29972,\n1403636758063555584,0.36256,-0.46810,0.11725,-0.11125,0.80205,0.15691,0.56543,-0.43894,0.65671,0.23901,\n1403636758163555328,0.37768,-0.49003,0.11921,-0.11151,0.80198,0.15727,0.56538,-0.43953,0.65632,0.23901,\n1403636758263555584,0.32985,-0.41845,0.13792,-0.11441,0.79808,0.15642,0.57053,-0.45470,0.64179,0.16515,\n1403636758363555328,0.28334,-0.35250,0.14817,-0.11687,0.79808,0.15511,0.57039,-0.46644,0.62317,0.06070,\n1403636758463555584,0.23574,-0.29061,0.14726,-0.12024,0.80749,0.15261,0.55696,-0.46580,0.61791,-0.03269,\n1403636758563555584,0.34115,-0.39904,0.13699,-0.12911,0.82040,0.14839,0.53689,-0.42067,0.58745,-0.05785,\n1403636758663555328,0.30426,-0.34494,0.12979,-0.13350,0.82655,0.14460,0.52734,-0.30189,0.54869,-0.03333,\n1403636758763555584,0.27805,-0.29422,0.12363,-0.13087,0.81806,0.14501,0.54094,-0.20701,0.50795,-0.03501,\n1403636758863555328,0.25961,-0.24654,0.11451,-0.12927,0.80830,0.14595,0.55555,-0.16132,0.46259,-0.05693,\n1403636758963555584,0.24914,-0.20383,0.10130,-0.12316,0.80200,0.14969,0.56500,-0.15381,0.42060,-0.12412,\n1403636759063555584,0.23667,-0.16358,0.08694,-0.12392,0.79932,0.14925,0.56873,-0.15751,0.40747,-0.14890,\n1403636759163555328,0.22274,-0.12560,0.07077,-0.13958,0.79961,0.13854,0.56740,-0.15304,0.36849,-0.17659,\n1403636759263555584,0.20837,-0.09367,0.05244,-0.15609,0.80056,0.12731,0.56438,-0.13160,0.27965,-0.21593,\n1403636759363555328,0.19861,-0.07361,0.02984,-0.15276,0.79966,0.13045,0.56586,-0.10533,0.17082,-0.24324,\n1403636759463555584,0.19996,-0.04665,-0.00775,-0.13553,0.79898,0.13482,0.57017,-0.09104,0.08734,-0.28660,\n1403636759563555584,0.19179,-0.04000,-0.03826,-0.12483,0.79746,0.14342,0.57263,-0.10225,0.05359,-0.30168,\n1403636759663555328,0.18216,-0.03765,-0.06345,-0.11733,0.79641,0.14998,0.57399,-0.12745,0.01405,-0.20587,\n1403636759763555584,0.17120,-0.03689,-0.07745,-0.11668,0.79613,0.15176,0.57405,-0.12857,0.01628,-0.09797,\n1403636759863555328,0.15852,-0.03600,-0.08304,-0.11580,0.79598,0.15243,0.57425,-0.14052,0.00459,-0.05964,\n1403636759963555584,0.14289,-0.03732,-0.08954,-0.11402,0.80214,0.15375,0.56562,-0.16390,-0.00550,-0.08929,\n1403636760063555584,0.12676,-0.03824,-0.09891,-0.11466,0.80740,0.15273,0.55824,-0.15119,-0.00494,-0.11020,\n1403636760163555328,0.11288,-0.03884,-0.11004,-0.11254,0.81198,0.15366,0.55173,-0.12696,0.00884,-0.10966,\n1403636760263555584,0.10258,-0.03859,-0.12017,-0.11500,0.80556,0.15166,0.56111,-0.09170,0.00292,-0.10068,\n1403636760363555328,0.09308,-0.03964,-0.13251,-0.11672,0.79917,0.15049,0.57013,-0.09101,-0.01065,-0.14065,\n1403636760463555584,0.08300,-0.04153,-0.14791,-0.11465,0.79587,0.15218,0.57470,-0.11319,-0.02426,-0.16017,\n1403636760563555584,0.07565,0.01494,-0.16219,-0.11278,0.79933,0.15603,0.56922,-0.13722,-0.01894,-0.17074,\n1403636760663555328,0.06210,0.01256,-0.17969,-0.10896,0.80442,0.15923,0.56185,-0.14225,-0.01672,-0.16328,\n1403636760763555584,0.04901,0.01158,-0.19472,-0.10572,0.80582,0.16234,0.55957,-0.12606,0.00505,-0.13457,\n1403636760863555328,0.03705,0.01321,-0.20753,-0.10458,0.80193,0.16394,0.56488,-0.12495,0.02557,-0.12090,\n1403636760963555584,0.02442,0.01720,-0.21968,-0.10427,0.80011,0.16511,0.56718,-0.13915,0.05437,-0.13119,\n1403636761063555584,0.01015,0.02400,-0.23429,-0.10687,0.80526,0.16369,0.55977,-0.14637,0.08111,-0.14404,\n1403636761163555328,-0.00355,0.03300,-0.24967,-0.10828,0.81346,0.16325,0.54764,-0.12399,0.10489,-0.14559,\n1403636761263555584,-0.01447,0.04433,-0.26368,-0.10753,0.81969,0.16436,0.53809,-0.08352,0.13307,-0.11406,\n1403636761363555328,-0.01931,0.05892,-0.27232,-0.10781,0.81875,0.16451,0.53941,-0.02175,0.16574,-0.04979,\n1403636761463555584,-0.01823,0.07617,-0.27473,-0.10918,0.81046,0.16337,0.55185,0.01618,0.17955,-0.01042,\n1403636761563555584,0.00234,0.04343,-0.26070,-0.11058,0.80327,0.16009,0.56294,0.02845,0.19027,0.00927,\n1403636761663555328,0.00553,0.06253,-0.25883,-0.11532,0.79988,0.15591,0.56797,0.02023,0.18731,0.01989,\n1403636761763555584,0.00774,0.08097,-0.25580,-0.12567,0.79995,0.14769,0.56787,0.02132,0.16948,0.03698,\n1403636761863555328,0.00941,0.09582,-0.25105,-0.13208,0.80063,0.14242,0.56680,0.01405,0.12415,0.05484,\n1403636761963555584,0.01082,0.10492,-0.24455,-0.12730,0.80116,0.14493,0.56650,0.02166,0.07355,0.07644,\n1403636762063555584,0.00801,0.10146,-0.24529,-0.11131,0.79886,0.15501,0.57045,0.01849,0.05176,0.10130,\n1403636762163555328,0.00773,0.10651,-0.23527,-0.09314,0.79731,0.16696,0.57249,-0.01460,0.06967,0.09186,\n1403636762263555584,0.00436,0.11555,-0.22775,-0.08925,0.80445,0.16882,0.56249,-0.02764,0.12430,0.04473,\n1403636762363555328,0.00107,0.12957,-0.22743,-0.09225,0.81360,0.16597,0.54955,-0.01609,0.16938,-0.03487,\n1403636762463555584,0.00009,0.14826,-0.23550,-0.10211,0.81762,0.15879,0.54392,0.01800,0.21919,-0.10716,\n1403636762563555584,0.00272,0.17037,-0.25023,-0.12698,0.81437,0.14125,0.54839,0.04257,0.22295,-0.17769,\n1403636762663555328,0.00903,0.18925,-0.27302,-0.14594,0.80861,0.12806,0.55539,0.08679,0.16083,-0.26247,\n1403636762763555584,0.01883,0.20093,-0.30319,-0.13856,0.80368,0.13395,0.56299,0.11014,0.09171,-0.33740,\n1403636762863555328,0.02594,0.20307,-0.32280,-0.14167,0.78386,0.13301,0.58975,-0.07444,-0.11984,-0.03054,\n1403636762863555328,0.03490,0.22115,-0.36264,-0.11547,0.79527,0.13540,0.57955,-0.15850,-0.18554,-0.17257,\n1403636762963555584,0.04888,0.21413,-0.36749,-0.11457,0.79554,0.13393,0.57969,-0.15658,-0.18664,-0.16625,\n1403636763063555584,0.03800,0.19410,-0.37656,-0.11103,0.80519,0.10816,0.57240,-0.10347,-0.19496,-0.20919,\n1403636763163555328,0.03104,0.18313,-0.37451,-0.10919,0.79244,0.09008,0.59330,-0.07071,-0.05428,-0.11721,\n1403636763263555584,0.03206,0.18037,-0.38225,-0.11005,0.80281,0.08615,0.57963,0.02791,0.00740,-0.07198,\n1403636763363555328,0.03369,0.18008,-0.37824,-0.11070,0.80115,0.08584,0.58183,-0.00923,0.00039,-0.01067,\n1403636763463555584,0.03784,0.18338,-0.37588,-0.11047,0.80205,0.08557,0.58069,-0.00934,0.00124,-0.00238,\n1403636763563555584,0.03809,0.18350,-0.37579,-0.11075,0.80212,0.08540,0.58055,-0.00975,-0.00227,-0.00203,\n1403636763663555328,0.03804,0.18341,-0.37565,-0.11080,0.80206,0.08538,0.58063,-0.01222,-0.00109,-0.00117,\n1403636763763555584,0.03804,0.18338,-0.37567,-0.11059,0.80218,0.08537,0.58051,-0.00457,-0.00235,0.00362,\n"
  },
  {
    "path": "config/vins_rviz_config.rviz",
    "content": "Panels:\n  - Class: rviz/Displays\n    Help Height: 0\n    Name: Displays\n    Property Tree Widget:\n      Expanded:\n        - /Global Options1\n      Splitter Ratio: 0.465116\n    Tree Height: 156\n  - Class: rviz/Selection\n    Name: Selection\n  - Class: rviz/Tool Properties\n    Expanded:\n      - /2D Pose Estimate1\n      - /2D Nav Goal1\n      - /Publish Point1\n    Name: Tool Properties\n    Splitter Ratio: 0.588679\n  - Class: rviz/Views\n    Expanded:\n      - /Current View1\n    Name: Views\n    Splitter Ratio: 0.5\n  - Class: rviz/Time\n    Experimental: false\n    Name: Time\n    SyncMode: 0\n    SyncSource: \"\"\n  - Class: rviz/Displays\n    Help Height: 78\n    Name: Displays\n    Property Tree Widget:\n      Expanded: ~\n      Splitter Ratio: 0.5\n    Tree Height: 359\nVisualization Manager:\n  Class: \"\"\n  Displays:\n    - Alpha: 0.5\n      Cell Size: 1\n      Class: rviz/Grid\n      Color: 130; 130; 130\n      Enabled: true\n      Line Style:\n        Line Width: 0.03\n        Value: Lines\n      Name: Grid\n      Normal Cell Count: 0\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Plane: XY\n      Plane Cell Count: 10\n      Reference Frame: <Fixed Frame>\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 255; 0; 0\n      Enabled: true\n      Head Diameter: 0.3\n      Head Length: 0.2\n      Length: 0.3\n      Line Style: Lines\n      Line Width: 0.5\n      Name: ground_truth_path\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Style: None\n      Radius: 0.03\n      Shaft Diameter: 0.1\n      Shaft Length: 0.1\n      Topic: /benchmark_publisher/path\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Autocompute Intensity Bounds: false\n      Autocompute Value Bounds:\n        Max Value: 10\n        Min Value: -10\n        Value: true\n      Axis: Z\n      Channel Name: intensity\n      Class: rviz/PointCloud\n      Color: 255; 255; 255\n      Color Transformer: FlatColor\n      Decay Time: 3000\n      Enabled: true\n      Invert Rainbow: true\n      Max Color: 0; 0; 0\n      Max Intensity: 4096\n      Min Color: 0; 0; 0\n      Min Intensity: 0\n      Name: history_point\n      Position Transformer: XYZ\n      Queue Size: 10\n      Selectable: true\n      Size (Pixels): 1\n      Size (m): 0.5\n      Style: Points\n      Topic: /vins_estimator/history_cloud\n      Unreliable: false\n      Use Fixed Frame: true\n      Use rainbow: false\n      Value: true\n    - Class: rviz/Image\n      Enabled: true\n      Image Topic: /vins_estimator/feature_img\n      Max Value: 1\n      Median window: 5\n      Min Value: 0\n      Name: tracked image\n      Normalize Range: true\n      Queue Size: 2\n      Transport Hint: raw\n      Unreliable: false\n      Value: true\n    - Class: rviz/MarkerArray\n      Enabled: true\n      Marker Topic: /vins_estimator/camera_pose_visual\n      Name: camera_visual\n      Namespaces:\n        {}\n      Queue Size: 100\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 0; 255; 0\n      Enabled: true\n      Head Diameter: 0.3\n      Head Length: 0.2\n      Length: 0.3\n      Line Style: Lines\n      Line Width: 0.03\n      Name: Path\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Style: None\n      Radius: 0.03\n      Shaft Diameter: 0.1\n      Shaft Length: 0.1\n      Topic: /vins_estimator/path\n      Unreliable: false\n      Value: true\n    - Class: rviz/MarkerArray\n      Enabled: true\n      Marker Topic: /vins_estimator/pose_graph\n      Name: loop_link\n      Namespaces:\n        {}\n      Queue Size: 100\n      Value: true\n    - Alpha: 1\n      Autocompute Intensity Bounds: true\n      Autocompute Value Bounds:\n        Max Value: 10\n        Min Value: -10\n        Value: true\n      Axis: Z\n      Channel Name: intensity\n      Class: rviz/PointCloud\n      Color: 0; 255; 0\n      Color Transformer: FlatColor\n      Decay Time: 0\n      Enabled: true\n      Invert Rainbow: false\n      Max Color: 255; 255; 255\n      Max Intensity: 4096\n      Min Color: 0; 0; 0\n      Min Intensity: 0\n      Name: current_point\n      Position Transformer: XYZ\n      Queue Size: 10\n      Selectable: true\n      Size (Pixels): 1\n      Size (m): 0.01\n      Style: Points\n      Topic: /vins_estimator/point_cloud\n      Unreliable: false\n      Use Fixed Frame: true\n      Use rainbow: true\n      Value: true\n    - Class: rviz/TF\n      Enabled: true\n      Frame Timeout: 15\n      Frames:\n        All Enabled: true\n      Marker Scale: 1\n      Name: TF\n      Show Arrows: true\n      Show Axes: true\n      Show Names: true\n      Tree:\n        {}\n      Update Interval: 0\n      Value: true\n    - Class: rviz/Axes\n      Enabled: true\n      Length: 1\n      Name: Axes\n      Radius: 0.1\n      Reference Frame: <Fixed Frame>\n      Value: true\n    - Class: rviz/Image\n      Enabled: true\n      Image Topic: /cam0/image_raw\n      Max Value: 1\n      Median window: 5\n      Min Value: 0\n      Name: raw_image\n      Normalize Range: true\n      Queue Size: 2\n      Transport Hint: raw\n      Unreliable: false\n      Value: true\n  Enabled: true\n  Global Options:\n    Background Color: 0; 0; 0\n    Fixed Frame: world\n    Frame Rate: 30\n  Name: root\n  Tools:\n    - Class: rviz/MoveCamera\n    - Class: rviz/Select\n    - Class: rviz/FocusCamera\n    - Class: rviz/Measure\n    - Class: rviz/SetInitialPose\n      Topic: /initialpose\n    - Class: rviz/SetGoal\n      Topic: /move_base_simple/goal\n    - Class: rviz/PublishPoint\n      Single click: true\n      Topic: /clicked_point\n  Value: true\n  Views:\n    Current:\n      Class: rviz/Orbit\n      Distance: 20.5065\n      Enable Stereo Rendering:\n        Stereo Eye Separation: 0.06\n        Stereo Focal Distance: 1\n        Swap Stereo Eyes: false\n        Value: false\n      Focal Point:\n        X: 1.21347\n        Y: -1.35133\n        Z: 1.5734\n      Name: Current View\n      Near Clip Distance: 0.01\n      Pitch: 1.1848\n      Target Frame: <Fixed Frame>\n      Value: Orbit (rviz)\n      Yaw: 3.1181\n    Saved: ~\nWindow Geometry:\n  Displays:\n    collapsed: false\n  Height: 746\n  Hide Left Dock: false\n  Hide Right Dock: true\n  QMainWindow State: 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\n  Selection:\n    collapsed: false\n  Time:\n    collapsed: false\n  Tool Properties:\n    collapsed: false\n  Views:\n    collapsed: true\n  Width: 1059\n  X: 101\n  Y: 55\n  raw_image:\n    collapsed: false\n  tracked image:\n    collapsed: false\n"
  },
  {
    "path": "generate.sh",
    "content": "#/bin/bash\ncd camera_model\nmkdir -p build\ncd build\ncmake ..\nmake \n\ncd ../../vins_estimator\nmkdir -p build\ncd build \ncmake ..\nmake\n\n"
  },
  {
    "path": "include/ChannelFloat32.h",
    "content": "#ifndef SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H\n#define SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/shared_ptr.hpp>\n\n\nnamespace sensor_msgs\n{\ntemplate <class ContainerAllocator>\nstruct ChannelFloat32_\n{\n  typedef ChannelFloat32_<ContainerAllocator> Type;\n\n  ChannelFloat32_()\n    : name()\n    , values()  {\n    }\n  ChannelFloat32_(const ContainerAllocator& _alloc)\n    : name(_alloc)\n    , values(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other >  _name_type;\n  _name_type name;\n\n   typedef std::vector<float, typename ContainerAllocator::template rebind<float>::other >  _values_type;\n  _values_type values;\n\n\n\n\n  typedef boost::shared_ptr< sensor_msgs::ChannelFloat32_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< sensor_msgs::ChannelFloat32_<ContainerAllocator> const> ConstPtr;\n\n}; // struct ChannelFloat32_\n\ntypedef sensor_msgs::ChannelFloat32_<std::allocator<void> > ChannelFloat32;\n\ntypedef boost::shared_ptr< sensor_msgs::ChannelFloat32 > ChannelFloat32Ptr;\ntypedef boost::shared_ptr< sensor_msgs::ChannelFloat32 const> ChannelFloat32ConstPtr;\n}\n\n#endif\n\n"
  },
  {
    "path": "include/Float32.h",
    "content": "\n#ifndef STD_MSGS_MESSAGE_FLOAT32_H\n#define STD_MSGS_MESSAGE_FLOAT32_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n\n\n\nnamespace std_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Float32_\n{\n  typedef Float32_<ContainerAllocator> Type;\n\n  Float32_()\n    : data(0.0)  {\n    }\n  Float32_(const ContainerAllocator& _alloc)\n    : data(0.0)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef float _data_type;\n  _data_type data;\n\n\n\n\n  typedef boost::shared_ptr< ::std_msgs::Float32_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::std_msgs::Float32_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Float32_\n}\n#endif\n\n"
  },
  {
    "path": "include/Header.h",
    "content": "#ifndef STD_MSGS_MESSAGE_HEADER_H\n#define STD_MSGS_MESSAGE_HEADER_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/shared_ptr.hpp>\n#include <stdint.h>\n#include \"Time.h\"\n\nnamespace std_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Header_\n{\n  typedef Header_<ContainerAllocator> Type;\n\n  Header_()\n    : seq(0)\n    , stamp()\n    , frame_id()  {\n    }\n  Header_(const ContainerAllocator& _alloc)\n    : seq(0)\n    , stamp()\n    , frame_id(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef uint32_t _seq_type;\n  _seq_type seq;\n\n   typedef ros::Time _stamp_type;\n  _stamp_type stamp;\n\n   typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other >  _frame_id_type;\n  _frame_id_type frame_id;\n\n\n\n\n  typedef boost::shared_ptr< std_msgs::Header_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< std_msgs::Header_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Header_\n\ntypedef std_msgs::Header_<std::allocator<void> > Header;\n\ntypedef boost::shared_ptr< std_msgs::Header > HeaderPtr;\ntypedef boost::shared_ptr< std_msgs::Header const> HeaderConstPtr;\n\n}\n#endif\n"
  },
  {
    "path": "include/Imu.h",
    "content": "#ifndef SENSOR_MSGS_MESSAGE_IMU_H\n#define SENSOR_MSGS_MESSAGE_IMU_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/array.hpp>\n\n#include \"Time.h\"\n#include \"Quaternion.h\"\n#include \"Vector3.h\"\n#include \"Header.h\"\n\n\nnamespace sensor_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Imu_\n{\n  typedef Imu_<ContainerAllocator> Type;\n\n  Imu_()\n    : header()\n    , orientation()\n    , orientation_covariance()\n    , angular_velocity()\n    , angular_velocity_covariance()\n    , linear_acceleration()\n    , linear_acceleration_covariance()  {\n      orientation_covariance.assign(0.0);\n\n      angular_velocity_covariance.assign(0.0);\n\n      linear_acceleration_covariance.assign(0.0);\n  }\n  Imu_(const ContainerAllocator& _alloc)\n    : header(_alloc)\n    , orientation(_alloc)\n    , orientation_covariance()\n    , angular_velocity(_alloc)\n    , angular_velocity_covariance()\n    , linear_acceleration(_alloc)\n    , linear_acceleration_covariance()  {\n  (void)_alloc;\n      orientation_covariance.assign(0.0);\n\n      angular_velocity_covariance.assign(0.0);\n\n      linear_acceleration_covariance.assign(0.0);\n  }\n\n\n\n   typedef  std_msgs::Header_<ContainerAllocator>  _header_type;\n  _header_type header;\n\n   typedef  geometry_msgs::Quaternion_<ContainerAllocator>  _orientation_type;\n  _orientation_type orientation;\n\n   typedef boost::array<double, 9>  _orientation_covariance_type;\n  _orientation_covariance_type orientation_covariance;\n\n   typedef  geometry_msgs::Vector3_<ContainerAllocator>  _angular_velocity_type;\n  _angular_velocity_type angular_velocity;\n\n   typedef boost::array<double, 9>  _angular_velocity_covariance_type;\n  _angular_velocity_covariance_type angular_velocity_covariance;\n\n   typedef  geometry_msgs::Vector3_<ContainerAllocator>  _linear_acceleration_type;\n  _linear_acceleration_type linear_acceleration;\n\n   typedef boost::array<double, 9>  _linear_acceleration_covariance_type;\n  _linear_acceleration_covariance_type linear_acceleration_covariance;\n\n\n\n\n  typedef boost::shared_ptr< sensor_msgs::Imu_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< sensor_msgs::Imu_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Imu_\n\ntypedef sensor_msgs::Imu_<std::allocator<void> > Imu;\n\ntypedef boost::shared_ptr< sensor_msgs::Imu > ImuPtr;\ntypedef boost::shared_ptr< sensor_msgs::Imu const> ImuConstPtr;\n}\n#endif\n\n"
  },
  {
    "path": "include/Odometry.h",
    "content": "#ifndef NAV_MSGS_MESSAGE_ODOMETRY_H\n#define NAV_MSGS_MESSAGE_ODOMETRY_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n#include \"Header.h\"\n#include \"PoseWithCovariance.h\"\n#include \"TwistWithCovariance.h\"\n\nnamespace nav_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Odometry_\n{\n  typedef Odometry_<ContainerAllocator> Type;\n\n  Odometry_()\n    : header()\n    , child_frame_id()\n    , pose()\n    , twist()  {\n    }\n  Odometry_(const ContainerAllocator& _alloc)\n    : header(_alloc)\n    , child_frame_id(_alloc)\n    , pose(_alloc)\n    , twist(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;\n  _header_type header;\n\n   typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other >  _child_frame_id_type;\n  _child_frame_id_type child_frame_id;\n\n   typedef  ::geometry_msgs::PoseWithCovariance_<ContainerAllocator>  _pose_type;\n  _pose_type pose;\n\n   typedef  ::geometry_msgs::TwistWithCovariance_<ContainerAllocator>  _twist_type;\n  _twist_type twist;\n\n\n\n\n  typedef boost::shared_ptr< ::nav_msgs::Odometry_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::nav_msgs::Odometry_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Odometry_\n\ntypedef ::nav_msgs::Odometry_<std::allocator<void> > Odometry;\n\ntypedef boost::shared_ptr< ::nav_msgs::Odometry > OdometryPtr;\ntypedef boost::shared_ptr< ::nav_msgs::Odometry const> OdometryConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/Path.h",
    "content": "#ifndef NAV_MSGS_MESSAGE_PATH_H\n#define NAV_MSGS_MESSAGE_PATH_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n\n#include \"Header.h\"\n#include \"PoseStamped.h\"\n\nnamespace nav_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Path_\n{\n  typedef Path_<ContainerAllocator> Type;\n\n  Path_()\n    : header()\n    , poses()  {\n    }\n  Path_(const ContainerAllocator& _alloc)\n    : header(_alloc)\n    , poses(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;\n  _header_type header;\n\n   typedef std::vector< ::geometry_msgs::PoseStamped_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::PoseStamped_<ContainerAllocator> >::other >  _poses_type;\n  _poses_type poses;\n\n\n\n\n  typedef boost::shared_ptr< ::nav_msgs::Path_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::nav_msgs::Path_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Path_\n\ntypedef ::nav_msgs::Path_<std::allocator<void> > Path;\n\ntypedef boost::shared_ptr< ::nav_msgs::Path > PathPtr;\ntypedef boost::shared_ptr< ::nav_msgs::Path const> PathConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/Point.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_POINT_H\n#define GEOMETRY_MSGS_MESSAGE_POINT_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Point_\n{\n  typedef Point_<ContainerAllocator> Type;\n\n  Point_()\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)  {\n    }\n  Point_(const ContainerAllocator& _alloc)\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef double _x_type;\n  _x_type x;\n\n   typedef double _y_type;\n  _y_type y;\n\n   typedef double _z_type;\n  _z_type z;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::Point_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::Point_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Point_\n\ntypedef ::geometry_msgs::Point_<std::allocator<void> > Point;\n\ntypedef boost::shared_ptr< ::geometry_msgs::Point > PointPtr;\ntypedef boost::shared_ptr< ::geometry_msgs::Point const> PointConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/Point32.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_POINT32_H\n#define GEOMETRY_MSGS_MESSAGE_POINT32_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/shared_ptr.hpp>\n\n\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Point32_\n{\n  typedef Point32_<ContainerAllocator> Type;\n\n  Point32_()\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)  {\n    }\n  Point32_(const ContainerAllocator& _alloc)\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef float _x_type;\n  _x_type x;\n\n   typedef float _y_type;\n  _y_type y;\n\n   typedef float _z_type;\n  _z_type z;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::Point32_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::Point32_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Point32_\n\ntypedef ::geometry_msgs::Point32_<std::allocator<void> > Point32;\n\ntypedef boost::shared_ptr< ::geometry_msgs::Point32 > Point32Ptr;\ntypedef boost::shared_ptr< ::geometry_msgs::Point32 const> Point32ConstPtr;\n\n}\n#endif\n\n"
  },
  {
    "path": "include/PointCloud.h",
    "content": "#ifndef SENSOR_MSGS_MESSAGE_POINTCLOUD_H\n#define SENSOR_MSGS_MESSAGE_POINTCLOUD_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/shared_ptr.hpp>\n\n\n#include \"Header.h\"\n#include \"Point32.h\"\n#include \"ChannelFloat32.h\"\n\nnamespace sensor_msgs\n{\ntemplate <class ContainerAllocator>\nstruct PointCloud_\n{\n  typedef PointCloud_<ContainerAllocator> Type;\n\n  PointCloud_()\n    : header()\n    , points()\n    , channels()  {\n    }\n  PointCloud_(const ContainerAllocator& _alloc)\n    : header(_alloc)\n    , points(_alloc)\n    , channels(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  std_msgs::Header_<ContainerAllocator>  _header_type;\n  _header_type header;\n\n   typedef std::vector< geometry_msgs::Point32_<ContainerAllocator> , typename ContainerAllocator::template rebind< geometry_msgs::Point32_<ContainerAllocator> >::other >  _points_type;\n  _points_type points;\n\n   typedef std::vector< sensor_msgs::ChannelFloat32_<ContainerAllocator> , typename ContainerAllocator::template rebind< sensor_msgs::ChannelFloat32_<ContainerAllocator> >::other >  _channels_type;\n  _channels_type channels;\n\n\n\n\n  typedef boost::shared_ptr< sensor_msgs::PointCloud_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< sensor_msgs::PointCloud_<ContainerAllocator> const> ConstPtr;\n\n}; // struct PointCloud_\n\ntypedef sensor_msgs::PointCloud_<std::allocator<void> > PointCloud;\n\ntypedef boost::shared_ptr< sensor_msgs::PointCloud > PointCloudPtr;\ntypedef boost::shared_ptr< sensor_msgs::PointCloud const> PointCloudConstPtr;\n}\n#endif\n\n"
  },
  {
    "path": "include/PointStamped",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H\n#define GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n#include \"Header.h\"\n#include \"Point.h\"\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct PointStamped_\n{\n  typedef PointStamped_<ContainerAllocator> Type;\n\n  PointStamped_()\n    : header()\n    , point()  {\n    }\n  PointStamped_(const ContainerAllocator& _alloc)\n    : header(_alloc)\n    , point(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;\n  _header_type header;\n\n   typedef  ::geometry_msgs::Point_<ContainerAllocator>  _point_type;\n  _point_type point;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::PointStamped_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::PointStamped_<ContainerAllocator> const> ConstPtr;\n\n}; // struct PointStamped_\n\ntypedef ::geometry_msgs::PointStamped_<std::allocator<void> > PointStamped;\n\ntypedef boost::shared_ptr< ::geometry_msgs::PointStamped > PointStampedPtr;\ntypedef boost::shared_ptr< ::geometry_msgs::PointStamped const> PointStampedConstPtr;\n\n}\n#endif\n"
  },
  {
    "path": "include/Pose.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_POSE_H\n#define GEOMETRY_MSGS_MESSAGE_POSE_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n#include \"Point.h\"\n#include \"Quaternion.h\"\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Pose_\n{\n  typedef Pose_<ContainerAllocator> Type;\n\n  Pose_()\n    : position()\n    , orientation()  {\n    }\n  Pose_(const ContainerAllocator& _alloc)\n    : position(_alloc)\n    , orientation(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  ::geometry_msgs::Point_<ContainerAllocator>  _position_type;\n  _position_type position;\n\n   typedef  ::geometry_msgs::Quaternion_<ContainerAllocator>  _orientation_type;\n  _orientation_type orientation;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::Pose_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::Pose_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Pose_\n\ntypedef ::geometry_msgs::Pose_<std::allocator<void> > Pose;\n\ntypedef boost::shared_ptr< ::geometry_msgs::Pose > PosePtr;\ntypedef boost::shared_ptr< ::geometry_msgs::Pose const> PoseConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/PoseStamped.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H\n#define GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n\n#include \"Header.h\"\n#include \"Pose.h\"\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct PoseStamped_\n{\n  typedef PoseStamped_<ContainerAllocator> Type;\n\n  PoseStamped_()\n    : header()\n    , pose()  {\n    }\n  PoseStamped_(const ContainerAllocator& _alloc)\n    : header(_alloc)\n    , pose(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;\n  _header_type header;\n\n   typedef  ::geometry_msgs::Pose_<ContainerAllocator>  _pose_type;\n  _pose_type pose;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_<ContainerAllocator> const> ConstPtr;\n\n}; // struct PoseStamped_\n\ntypedef ::geometry_msgs::PoseStamped_<std::allocator<void> > PoseStamped;\n\ntypedef boost::shared_ptr< ::geometry_msgs::PoseStamped > PoseStampedPtr;\ntypedef boost::shared_ptr< ::geometry_msgs::PoseStamped const> PoseStampedConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/PoseWithCovariance.h",
    "content": "\n#ifndef GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H\n#define GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/array.hpp>\n\n\n#include \"Pose.h\"\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct PoseWithCovariance_\n{\n  typedef PoseWithCovariance_<ContainerAllocator> Type;\n\n  PoseWithCovariance_()\n    : pose()\n    , covariance()  {\n      covariance.assign(0.0);\n  }\n  PoseWithCovariance_(const ContainerAllocator& _alloc)\n    : pose(_alloc)\n    , covariance()  {\n  (void)_alloc;\n      covariance.assign(0.0);\n  }\n\n\n\n   typedef  ::geometry_msgs::Pose_<ContainerAllocator>  _pose_type;\n  _pose_type pose;\n\n   typedef boost::array<double, 36>  _covariance_type;\n  _covariance_type covariance;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_<ContainerAllocator> const> ConstPtr;\n\n}; // struct PoseWithCovariance_\n\ntypedef ::geometry_msgs::PoseWithCovariance_<std::allocator<void> > PoseWithCovariance;\n\ntypedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance > PoseWithCovariancePtr;\ntypedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance const> PoseWithCovarianceConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/Quaternion.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_QUATERNION_H\n#define GEOMETRY_MSGS_MESSAGE_QUATERNION_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Quaternion_\n{\n  typedef Quaternion_<ContainerAllocator> Type;\n\n  Quaternion_()\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)\n    , w(0.0)  {\n    }\n  Quaternion_(const ContainerAllocator& _alloc)\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)\n    , w(0.0)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef double _x_type;\n  _x_type x;\n\n   typedef double _y_type;\n  _y_type y;\n\n   typedef double _z_type;\n  _z_type z;\n\n   typedef double _w_type;\n  _w_type w;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::Quaternion_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::Quaternion_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Quaternion_\n\ntypedef ::geometry_msgs::Quaternion_<std::allocator<void> > Quaternion;\n\ntypedef boost::shared_ptr< ::geometry_msgs::Quaternion > QuaternionPtr;\ntypedef boost::shared_ptr< ::geometry_msgs::Quaternion const> QuaternionConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/Time.h",
    "content": "/*********************************************************************\n *all edit by solomon\n *solomon.he@zhaoxin.com\n *********************************************************************/\n\n#ifndef ROS_TIME_H_INCLUDED\n#define ROS_TIME_H_INCLUDED\n\n/*********************************************************************\n ** Pragmas\n *********************************************************************/\n \n/*********************************************************************\n ** Headers\n *********************************************************************/\n\n//#include <ros/platform.h>\n#include <iostream>\n#include <cmath>\n#include <boost/math/special_functions/round.hpp>\n#include <sys/time.h>\n\n\n/*********************************************************************\n ** Cross Platform Headers\n *********************************************************************/\n\nnamespace  ros //DataStucture\n{\n /*********************************************************************\n ** Functions\n *********************************************************************/\n\n     inline void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)\n     {\n       uint64_t nsec_part = nsec % 1000000000UL;\n       uint64_t sec_part = nsec / 1000000000UL;\n     \n       if (sec_part > UINT_MAX)\n         throw std::runtime_error(\"Time is out of dual 32-bit range\");\n     \n       sec += sec_part;\n       nsec = nsec_part;\n     }\n     \n     inline void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)\n     {\n       uint64_t sec64 = sec;\n       uint64_t nsec64 = nsec;\n     \n       normalizeSecNSec(sec64, nsec64);\n     \n       sec = (uint32_t)sec64;\n       nsec = (uint32_t)nsec64;\n    }\n \n   inline void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)\n   {\n    int64_t nsec_part = nsec;\n    int64_t sec_part = sec;\n \n    while (nsec_part >= 1000000000L)\n    {\n      nsec_part -= 1000000000L;\n      ++sec_part;\n    }\n     while (nsec_part < 0)\n    {\n      nsec_part += 1000000000L;\n      --sec_part;\n    }\n    if (sec_part < 0 || sec_part > INT_MAX)\n     throw std::runtime_error(\"Time is out of dual 32-bit range\");\n \n    sec = sec_part;\n    nsec = nsec_part;\n   }\n\n  /*********************************************************************\n   ** Time Classes\n   *********************************************************************/\n\n  /**\n   * \\brief Base class for Time implementations.  Provides storage, common functions and operator overloads.\n   * This should not need to be used directly.\n   */\n  template<class T>\n  class TimeBase\n  {\n  public:\n    uint32_t sec, nsec;\n\n    TimeBase() : sec(0), nsec(0) { }\n    TimeBase(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec)\n    {\n      normalizeSecNSec(sec, nsec);\n    }\n    explicit TimeBase(double t) { fromSec(t); }\n    ~TimeBase() {}\n  \n    bool operator==(const T &rhs) const\n    {return sec == rhs.sec && nsec == rhs.nsec;}\n    inline bool operator!=(const T &rhs) const { return !(*static_cast<const T*>(this) == rhs); }\n    bool operator>(const T &rhs) const\n    {\n     if(sec > rhs.sec)\n       return true;\n     else if(sec==rhs.sec && nsec > rhs.nsec)\n       return true;\n     return false;\n    }\n    bool operator<(const T &rhs) const\n    {\n     if(sec < rhs.sec)\n       return true;\n     else if(sec==rhs.sec && nsec < rhs.nsec)\n       return true;\n     return false;\n    }\n    bool operator>=(const T &rhs) const\n    {\n     if(sec > rhs.sec)\n       return true;\n     else if(sec==rhs.sec && nsec >= rhs.nsec)\n       return true;\n     return false;\n    }\n    bool operator<=(const T &rhs) const\n     {\n     if(sec < rhs.sec)\n       return true;\n     else if(sec==rhs.sec && nsec <= rhs.nsec)\n       return true;\n     return false;\n    }\n\n    double toSec()  const { return (double)sec + 1e-9*(double)nsec; };\n    T& fromSec(double t) {\n      sec = (uint32_t)floor(t);\n      nsec = (uint32_t)boost::math::round((t-sec) * 1e9);\n      // avoid rounding errors\n      sec += (nsec / 1000000000ul);\n      nsec %= 1000000000ul;\n      return *static_cast<T*>(this);\n    }\n\n    uint64_t toNSec() const {return (uint64_t)sec*1000000000ull + (uint64_t)nsec;  }\n    T& fromNSec(uint64_t t);\n\n    inline bool isZero() const { return sec == 0 && nsec == 0; }\n    inline bool is_zero() const { return isZero(); }\n\n  };\n\n  /**\n   * \\brief Time representation.  May either represent wall clock time or ROS clock time.\n   *\n   * ros::TimeBase provides most of its functionality.\n   */\n  class  Time : public TimeBase<Time>\n  {\n  public:\n    Time()\n      : TimeBase<Time>()\n    {}\n\n    Time(uint32_t _sec, uint32_t _nsec)\n      : TimeBase<Time>(_sec, _nsec)\n    {}\n\n    explicit Time(double t) { fromSec(t); }\n\n\n  };\n}\n#endif\n"
  },
  {
    "path": "include/Twist.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_TWIST_H\n#define GEOMETRY_MSGS_MESSAGE_TWIST_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/shared_ptr.hpp>\n\n\n\n#include \"Vector3.h\"\n\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Twist_\n{\n  typedef Twist_<ContainerAllocator> Type;\n\n  Twist_()\n    : linear()\n    , angular()  {\n    }\n  Twist_(const ContainerAllocator& _alloc)\n    : linear(_alloc)\n    , angular(_alloc)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _linear_type;\n  _linear_type linear;\n\n   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _angular_type;\n  _angular_type angular;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::Twist_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::Twist_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Twist_\n\ntypedef ::geometry_msgs::Twist_<std::allocator<void> > Twist;\n\ntypedef boost::shared_ptr< ::geometry_msgs::Twist > TwistPtr;\ntypedef boost::shared_ptr< ::geometry_msgs::Twist const> TwistConstPtr;\n\n// constants requiring out of line definition\n\n} \n#endif\n"
  },
  {
    "path": "include/TwistWithCovariance.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H\n#define GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n#include <boost/array.hpp>\n\n\n#include \"Twist.h\"\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct TwistWithCovariance_\n{\n  typedef TwistWithCovariance_<ContainerAllocator> Type;\n\n  TwistWithCovariance_()\n    : twist()\n    , covariance()  {\n      covariance.assign(0.0);\n  }\n  TwistWithCovariance_(const ContainerAllocator& _alloc)\n    : twist(_alloc)\n    , covariance()  {\n  (void)_alloc;\n      covariance.assign(0.0);\n  }\n\n\n\n   typedef  ::geometry_msgs::Twist_<ContainerAllocator>  _twist_type;\n  _twist_type twist;\n\n   typedef boost::array<double, 36>  _covariance_type;\n  _covariance_type covariance;\n\n\n\n\n  typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance_<ContainerAllocator> const> ConstPtr;\n\n}; // struct TwistWithCovariance_\n\ntypedef ::geometry_msgs::TwistWithCovariance_<std::allocator<void> > TwistWithCovariance;\n\ntypedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance > TwistWithCovariancePtr;\ntypedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance const> TwistWithCovarianceConstPtr;\n}\n#endif\n"
  },
  {
    "path": "include/Vector3.h",
    "content": "#ifndef GEOMETRY_MSGS_MESSAGE_VECTOR3_H\n#define GEOMETRY_MSGS_MESSAGE_VECTOR3_H\n\n\n#include <string>\n#include <vector>\n#include <map>\n\n\nnamespace geometry_msgs\n{\ntemplate <class ContainerAllocator>\nstruct Vector3_\n{\n  typedef Vector3_<ContainerAllocator> Type;\n\n  Vector3_()\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)  {\n    }\n  Vector3_(const ContainerAllocator& _alloc)\n    : x(0.0)\n    , y(0.0)\n    , z(0.0)  {\n  (void)_alloc;\n    }\n\n\n\n   typedef double _x_type;\n  _x_type x;\n\n   typedef double _y_type;\n  _y_type y;\n\n   typedef double _z_type;\n  _z_type z;\n\n\n\n\n  typedef boost::shared_ptr< geometry_msgs::Vector3_<ContainerAllocator> > Ptr;\n  typedef boost::shared_ptr< geometry_msgs::Vector3_<ContainerAllocator> const> ConstPtr;\n\n}; // struct Vector3_\n\ntypedef geometry_msgs::Vector3_<std::allocator<void> > Vector3;\n\ntypedef boost::shared_ptr< geometry_msgs::Vector3 > Vector3Ptr;\ntypedef boost::shared_ptr< geometry_msgs::Vector3 const> Vector3ConstPtr;\n}\n#endif\n\n"
  },
  {
    "path": "src.kdev4",
    "content": "[Project]\nManager=KDevCMakeManager\nName=Remove_ROS_VINS\n"
  },
  {
    "path": "support_files/brief_pattern.yml",
    "content": "%YAML:1.0\nx1:\n  - 0\n  - 4\n  - 11\n  - -4\n  - 24\n  - -3\n  - -4\n  - -7\n  - -5\n  - -2\n  - 8\n  - 1\n  - -2\n  - -5\n  - -5\n  - -8\n  - 2\n  - -8\n  - 6\n  - 4\n  - -1\n  - -14\n  - 12\n  - -12\n  - -20\n  - -11\n  - 7\n  - -11\n  - 18\n  - -10\n  - 10\n  - -20\n  - 7\n  - -2\n  - 3\n  - 7\n  - -20\n  - -3\n  - -4\n  - -6\n  - -4\n  - 14\n  - 1\n  - 2\n  - -9\n  - 15\n  - 13\n  - 14\n  - -21\n  - -6\n  - 3\n  - -7\n  - 23\n  - -22\n  - -12\n  - 0\n  - -22\n  - 1\n  - -11\n  - -11\n  - -8\n  - -7\n  - -16\n  - -6\n  - 4\n  - -16\n  - 4\n  - 7\n  - 0\n  - 0\n  - -7\n  - -4\n  - 16\n  - -3\n  - 6\n  - 5\n  - 0\n  - 8\n  - -4\n  - -12\n  - 0\n  - -11\n  - 11\n  - 3\n  - -13\n  - 9\n  - 11\n  - -23\n  - 0\n  - 6\n  - 6\n  - 3\n  - -7\n  - 6\n  - 2\n  - -4\n  - 24\n  - -3\n  - 5\n  - -5\n  - 0\n  - 0\n  - -4\n  - -10\n  - -1\n  - 8\n  - 6\n  - 3\n  - -7\n  - 7\n  - -4\n  - 5\n  - 8\n  - 10\n  - 8\n  - -10\n  - 2\n  - 6\n  - 0\n  - 10\n  - -2\n  - 5\n  - -2\n  - -1\n  - -12\n  - -10\n  - -15\n  - 12\n  - 1\n  - -6\n  - 12\n  - -7\n  - -7\n  - -8\n  - 7\n  - -11\n  - 2\n  - -22\n  - 7\n  - 20\n  - -8\n  - 4\n  - 3\n  - 11\n  - -9\n  - 4\n  - -4\n  - -12\n  - 12\n  - -22\n  - -6\n  - 7\n  - 5\n  - -1\n  - 9\n  - 10\n  - -15\n  - -2\n  - 1\n  - 14\n  - 21\n  - 3\n  - 6\n  - -13\n  - 5\n  - -19\n  - 10\n  - 6\n  - 2\n  - -14\n  - -16\n  - -15\n  - 6\n  - -8\n  - 18\n  - 7\n  - 16\n  - -4\n  - -2\n  - -4\n  - 0\n  - -4\n  - -14\n  - 0\n  - -4\n  - -13\n  - -1\n  - -14\n  - 11\n  - -4\n  - -15\n  - -8\n  - -6\n  - 3\n  - -1\n  - -1\n  - 4\n  - 8\n  - 3\n  - 0\n  - 24\n  - -3\n  - -11\n  - -10\n  - -2\n  - 2\n  - -1\n  - 4\n  - 2\n  - -1\n  - 11\n  - 0\n  - 10\n  - -1\n  - 6\n  - -4\n  - -19\n  - 1\n  - 9\n  - -6\n  - -1\n  - 12\n  - 0\n  - -9\n  - -1\n  - 5\n  - 0\n  - -6\n  - -5\n  - 0\n  - 7\n  - -19\n  - 11\n  - -9\n  - 11\n  - 0\n  - 8\n  - -1\n  - 5\n  - 10\n  - -2\n  - -6\n  - -6\n  - 0\n  - -3\n  - -7\n  - 4\n  - -4\n  - 2\n  - -10\n  - -7\n  - 18\n  - 0\n  - 12\n  - -2\n  - 0\ny1:\n  - -10\n  - 12\n  - 13\n  - 8\n  - -2\n  - 2\n  - 4\n  - 4\n  - -11\n  - -4\n  - 7\n  - 15\n  - 17\n  - 0\n  - 10\n  - -2\n  - -7\n  - 10\n  - 11\n  - -3\n  - -4\n  - 11\n  - -5\n  - -15\n  - 0\n  - 3\n  - -3\n  - -11\n  - 3\n  - -4\n  - 4\n  - 22\n  - 16\n  - 3\n  - -2\n  - -3\n  - 0\n  - 5\n  - 15\n  - -8\n  - 2\n  - 2\n  - -12\n  - -10\n  - 5\n  - 4\n  - 10\n  - -4\n  - 20\n  - -7\n  - 5\n  - 0\n  - -18\n  - 2\n  - -2\n  - -23\n  - 15\n  - 4\n  - -1\n  - -2\n  - 3\n  - 0\n  - -16\n  - 5\n  - 0\n  - -16\n  - -2\n  - 5\n  - 6\n  - 4\n  - 13\n  - -1\n  - -16\n  - -13\n  - -7\n  - -3\n  - 4\n  - 0\n  - -10\n  - -10\n  - -9\n  - 4\n  - -8\n  - 0\n  - -15\n  - -2\n  - -10\n  - 16\n  - 1\n  - 0\n  - 6\n  - -11\n  - -3\n  - -7\n  - -2\n  - 10\n  - -3\n  - -2\n  - 0\n  - 10\n  - -7\n  - 0\n  - 4\n  - 22\n  - -5\n  - 0\n  - -1\n  - -8\n  - -1\n  - -4\n  - 12\n  - -2\n  - 18\n  - -2\n  - -6\n  - -4\n  - -2\n  - -16\n  - -8\n  - 1\n  - -5\n  - 2\n  - 3\n  - 2\n  - 7\n  - -8\n  - 3\n  - 8\n  - 0\n  - -10\n  - -4\n  - -18\n  - 9\n  - 12\n  - 3\n  - 17\n  - -8\n  - -13\n  - 5\n  - -1\n  - 8\n  - -10\n  - -3\n  - -9\n  - -2\n  - 7\n  - 5\n  - -5\n  - -8\n  - 15\n  - -12\n  - -5\n  - 0\n  - 2\n  - 15\n  - -5\n  - -4\n  - -1\n  - -9\n  - 0\n  - -5\n  - -11\n  - -5\n  - -5\n  - -10\n  - 2\n  - 16\n  - 6\n  - 17\n  - 0\n  - -7\n  - 15\n  - 5\n  - -14\n  - -2\n  - 6\n  - 5\n  - 2\n  - 12\n  - -3\n  - 2\n  - 4\n  - 13\n  - 1\n  - -1\n  - -3\n  - -1\n  - -11\n  - 8\n  - -3\n  - 19\n  - 8\n  - 7\n  - -3\n  - 5\n  - 19\n  - 4\n  - -6\n  - 4\n  - -14\n  - 3\n  - 10\n  - -15\n  - -4\n  - 14\n  - 1\n  - 14\n  - -1\n  - -9\n  - -5\n  - -16\n  - 10\n  - 6\n  - 3\n  - -13\n  - 19\n  - -7\n  - -7\n  - -12\n  - -7\n  - 1\n  - 7\n  - -7\n  - 15\n  - -2\n  - -15\n  - 6\n  - 6\n  - 2\n  - -6\n  - -16\n  - 0\n  - -5\n  - -9\n  - 11\n  - 21\n  - 3\n  - 8\n  - 23\n  - -5\n  - -20\n  - -7\n  - 13\n  - -7\n  - -7\n  - 0\n  - 6\n  - 10\n  - -15\n  - 1\n  - 0\n  - 0\n  - 0\n  - -7\n  - 10\n  - -3\nx2:\n  - 0\n  - 0\n  - 13\n  - -6\n  - 22\n  - -7\n  - -3\n  - -12\n  - 1\n  - -4\n  - 10\n  - 4\n  - -6\n  - 0\n  - -8\n  - -5\n  - 7\n  - -2\n  - -1\n  - 8\n  - -2\n  - -14\n  - 14\n  - -10\n  - -14\n  - -4\n  - 6\n  - -14\n  - 15\n  - -5\n  - 9\n  - -18\n  - 4\n  - 0\n  - -4\n  - 3\n  - -19\n  - -10\n  - -8\n  - 0\n  - -4\n  - 10\n  - 1\n  - 5\n  - -12\n  - 19\n  - 2\n  - 11\n  - -18\n  - -5\n  - -3\n  - -5\n  - 21\n  - -17\n  - -14\n  - 1\n  - -17\n  - 5\n  - -8\n  - -8\n  - -8\n  - -1\n  - -16\n  - 1\n  - -2\n  - -12\n  - 0\n  - 3\n  - 7\n  - -1\n  - -10\n  - -2\n  - 14\n  - -5\n  - 0\n  - 4\n  - -2\n  - 15\n  - 2\n  - -9\n  - -9\n  - -11\n  - 9\n  - 2\n  - -15\n  - 8\n  - 9\n  - -24\n  - -5\n  - 10\n  - 7\n  - 0\n  - -7\n  - 6\n  - 2\n  - 0\n  - 24\n  - 0\n  - 2\n  - 0\n  - 2\n  - -1\n  - -5\n  - -10\n  - -4\n  - 19\n  - 2\n  - -6\n  - -8\n  - 13\n  - -1\n  - 5\n  - 3\n  - 13\n  - 8\n  - -10\n  - 0\n  - 1\n  - -5\n  - 11\n  - -6\n  - 8\n  - -7\n  - 1\n  - -14\n  - -6\n  - -10\n  - 11\n  - 2\n  - -5\n  - 12\n  - -12\n  - -3\n  - -5\n  - 7\n  - -5\n  - -3\n  - -16\n  - 9\n  - 20\n  - -9\n  - 8\n  - 3\n  - 15\n  - -12\n  - 4\n  - -6\n  - -15\n  - 10\n  - -17\n  - -13\n  - 6\n  - 4\n  - -3\n  - 9\n  - 7\n  - -18\n  - -10\n  - -3\n  - 10\n  - 23\n  - -9\n  - 10\n  - -15\n  - 7\n  - -14\n  - 13\n  - 10\n  - 0\n  - -20\n  - -14\n  - -15\n  - 3\n  - -7\n  - 21\n  - 7\n  - 17\n  - 1\n  - 0\n  - -2\n  - -8\n  - -11\n  - -17\n  - 2\n  - -6\n  - -18\n  - -5\n  - -10\n  - 5\n  - 2\n  - -13\n  - -6\n  - -4\n  - 0\n  - 3\n  - 2\n  - 10\n  - 10\n  - 4\n  - 0\n  - 21\n  - -13\n  - -16\n  - -17\n  - 0\n  - 3\n  - -1\n  - 6\n  - 5\n  - 8\n  - 14\n  - 2\n  - 10\n  - -6\n  - 1\n  - 0\n  - -23\n  - 0\n  - 6\n  - -4\n  - 3\n  - 9\n  - -5\n  - -6\n  - 1\n  - 12\n  - 2\n  - -6\n  - -4\n  - 7\n  - -1\n  - -17\n  - 9\n  - -12\n  - 13\n  - -1\n  - 11\n  - 5\n  - 7\n  - 10\n  - -2\n  - -3\n  - -8\n  - 1\n  - 0\n  - -6\n  - 5\n  - -6\n  - 5\n  - -12\n  - -5\n  - 21\n  - 2\n  - 17\n  - -7\n  - -1\ny2:\n  - -6\n  - 6\n  - 10\n  - 9\n  - 0\n  - 4\n  - 1\n  - 1\n  - -12\n  - 3\n  - 1\n  - 9\n  - 15\n  - 0\n  - 14\n  - 3\n  - -13\n  - 11\n  - 10\n  - -4\n  - -2\n  - 7\n  - -7\n  - -17\n  - -6\n  - 7\n  - -4\n  - -16\n  - -6\n  - -2\n  - 6\n  - 19\n  - 8\n  - 4\n  - -1\n  - -2\n  - 3\n  - 3\n  - 21\n  - -3\n  - 6\n  - 5\n  - -12\n  - -5\n  - 0\n  - 5\n  - 12\n  - -5\n  - 24\n  - -11\n  - 7\n  - 0\n  - -20\n  - 0\n  - -4\n  - -21\n  - 15\n  - 2\n  - -4\n  - 0\n  - -3\n  - 0\n  - -19\n  - 5\n  - 4\n  - -19\n  - -1\n  - 5\n  - 4\n  - 0\n  - 10\n  - 3\n  - -15\n  - -15\n  - -7\n  - -1\n  - 8\n  - -6\n  - -6\n  - -14\n  - -15\n  - 1\n  - -8\n  - -1\n  - -5\n  - 2\n  - -10\n  - 17\n  - 5\n  - 0\n  - 9\n  - -8\n  - -5\n  - -2\n  - -1\n  - 9\n  - 0\n  - -5\n  - -2\n  - 13\n  - -4\n  - -1\n  - 0\n  - 16\n  - -10\n  - -1\n  - 0\n  - -6\n  - 3\n  - -6\n  - 10\n  - 1\n  - 18\n  - -2\n  - -10\n  - -4\n  - -1\n  - -13\n  - -4\n  - -1\n  - -8\n  - 7\n  - 4\n  - -3\n  - 4\n  - -14\n  - -4\n  - 11\n  - -1\n  - -12\n  - -5\n  - -14\n  - 9\n  - 16\n  - 1\n  - 20\n  - -9\n  - -10\n  - 10\n  - 2\n  - 4\n  - -2\n  - -4\n  - -6\n  - -6\n  - 5\n  - 5\n  - -4\n  - 0\n  - 18\n  - -15\n  - -5\n  - 0\n  - 1\n  - 8\n  - -4\n  - -2\n  - 0\n  - -11\n  - 0\n  - -7\n  - -5\n  - 0\n  - 0\n  - -3\n  - 5\n  - 12\n  - 3\n  - 13\n  - 1\n  - -9\n  - 19\n  - 4\n  - -9\n  - 4\n  - 2\n  - 3\n  - -1\n  - 14\n  - 0\n  - 2\n  - 4\n  - 14\n  - 8\n  - 0\n  - -4\n  - 0\n  - -12\n  - 9\n  - 0\n  - 23\n  - 7\n  - 0\n  - -9\n  - 2\n  - 22\n  - 2\n  - -14\n  - 6\n  - -16\n  - 1\n  - 4\n  - -14\n  - -10\n  - 19\n  - 6\n  - 16\n  - -4\n  - -9\n  - 0\n  - -7\n  - 12\n  - 5\n  - 5\n  - -14\n  - 21\n  - -5\n  - -9\n  - -17\n  - -5\n  - 4\n  - 14\n  - -5\n  - 9\n  - -4\n  - -21\n  - 4\n  - 1\n  - 5\n  - 0\n  - -12\n  - 2\n  - 0\n  - -1\n  - 6\n  - 24\n  - 0\n  - 15\n  - 23\n  - -10\n  - -12\n  - -2\n  - 12\n  - -4\n  - -5\n  - 0\n  - 5\n  - 13\n  - -21\n  - 0\n  - -2\n  - 1\n  - 6\n  - -4\n  - 9\n  - -2\n"
  },
  {
    "path": "support_files/cmake/FindEigen.cmake",
    "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n#   this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n#   this list of conditions and the following disclaimer in the documentation\n#   and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n#   used to endorse or promote products derived from this software without\n#   specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindEigen.cmake - Find Eigen library, version >= 3.\n#\n# This module defines the following variables:\n#\n# EIGEN_FOUND: TRUE iff Eigen is found.\n# EIGEN_INCLUDE_DIRS: Include directories for Eigen.\n#\n# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h\n# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0\n# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0\n# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0\n#\n# The following variables control the behaviour of this module:\n#\n# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to\n#                          search for eigen includes, e.g: /timbuktu/eigen3.\n#\n# The following variables are also defined by this module, but in line with\n# CMake recommended FindPackage() module style should NOT be referenced directly\n# by callers (use the plural variables detailed above instead).  These variables\n# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which\n# are NOT re-called (i.e. search for library is not repeated) if these variables\n# are set with valid values _in the CMake cache_. This means that if these\n# variables are set directly in the cache, either by the user in the CMake GUI,\n# or by the user passing -DVAR=VALUE directives to CMake when called (which\n# explicitly defines a cache variable), then they will be used verbatim,\n# bypassing the HINTS variables and other hard-coded search locations.\n#\n# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the\n#                    include directory of any dependencies.\n\n# Called if we failed to find Eigen or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.\nmacro(EIGEN_REPORT_NOT_FOUND REASON_MSG)\n  unset(EIGEN_FOUND)\n  unset(EIGEN_INCLUDE_DIRS)\n  # Make results of search visible in the CMake GUI if Eigen has not\n  # been found so that user does not have to toggle to advanced view.\n  mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)\n  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n  # use the camelcase library name, not uppercase.\n  if (Eigen_FIND_QUIETLY)\n    message(STATUS \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n  elseif (Eigen_FIND_REQUIRED)\n    message(FATAL_ERROR \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n  else()\n    # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n    # but continues configuration and allows generation.\n    message(\"-- Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n  endif ()\n  return()\nendmacro(EIGEN_REPORT_NOT_FOUND)\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set EIGEN_FOUND, but not\n# the other variables we require / set here which could cause the search logic\n# here to fail.\nunset(EIGEN_FOUND)\n\n# Search user-installed locations first, so that we prefer user installs\n# to system installs where both exist.\nlist(APPEND EIGEN_CHECK_INCLUDE_DIRS\n  /usr/local/include\n  /usr/local/homebrew/include # Mac OS X\n  /opt/local/var/macports/software # Mac OS X.\n  /opt/local/include\n  /usr/include)\n# Additional suffixes to try appending to each search path.\nlist(APPEND EIGEN_CHECK_PATH_SUFFIXES\n  eigen3 # Default root directory for Eigen.\n  Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3\n  Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3\n\n# Search supplied hint directories first if supplied.\nfind_path(EIGEN_INCLUDE_DIR\n  NAMES Eigen/Core\n  PATHS ${EIGEN_INCLUDE_DIR_HINTS}\n  ${EIGEN_CHECK_INCLUDE_DIRS}\n  PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})\n\nif (NOT EIGEN_INCLUDE_DIR OR\n    NOT EXISTS ${EIGEN_INCLUDE_DIR})\n  eigen_report_not_found(\n    \"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to \"\n    \"path to eigen3 include directory, e.g. /usr/local/include/eigen3.\")\nendif (NOT EIGEN_INCLUDE_DIR OR\n       NOT EXISTS ${EIGEN_INCLUDE_DIR})\n\n# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets\n# if called.\nset(EIGEN_FOUND TRUE)\n\n# Extract Eigen version from Eigen/src/Core/util/Macros.h\nif (EIGEN_INCLUDE_DIR)\n  set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)\n  if (NOT EXISTS ${EIGEN_VERSION_FILE})\n    eigen_report_not_found(\n      \"Could not find file: ${EIGEN_VERSION_FILE} \"\n      \"containing version information in Eigen install located at: \"\n      \"${EIGEN_INCLUDE_DIR}.\")\n  else (NOT EXISTS ${EIGEN_VERSION_FILE})\n    file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)\n\n    string(REGEX MATCH \"#define EIGEN_WORLD_VERSION [0-9]+\"\n      EIGEN_WORLD_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n    string(REGEX REPLACE \"#define EIGEN_WORLD_VERSION ([0-9]+)\" \"\\\\1\"\n      EIGEN_WORLD_VERSION \"${EIGEN_WORLD_VERSION}\")\n\n    string(REGEX MATCH \"#define EIGEN_MAJOR_VERSION [0-9]+\"\n      EIGEN_MAJOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n    string(REGEX REPLACE \"#define EIGEN_MAJOR_VERSION ([0-9]+)\" \"\\\\1\"\n      EIGEN_MAJOR_VERSION \"${EIGEN_MAJOR_VERSION}\")\n\n    string(REGEX MATCH \"#define EIGEN_MINOR_VERSION [0-9]+\"\n      EIGEN_MINOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n    string(REGEX REPLACE \"#define EIGEN_MINOR_VERSION ([0-9]+)\" \"\\\\1\"\n      EIGEN_MINOR_VERSION \"${EIGEN_MINOR_VERSION}\")\n\n    # This is on a single line s/t CMake does not interpret it as a list of\n    # elements and insert ';' separators which would result in 3.;2.;0 nonsense.\n    set(EIGEN_VERSION \"${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}\")\n  endif (NOT EXISTS ${EIGEN_VERSION_FILE})\nendif (EIGEN_INCLUDE_DIR)\n\n# Set standard CMake FindPackage variables if found.\nif (EIGEN_FOUND)\n  set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})\nendif (EIGEN_FOUND)\n\n# Handle REQUIRED / QUIET optional arguments and version.\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(Eigen\n  REQUIRED_VARS EIGEN_INCLUDE_DIRS\n  VERSION_VAR EIGEN_VERSION)\n\n# Only mark internal variables as advanced if we found Eigen, otherwise\n# leave it visible in the standard GUI for the user to set manually.\nif (EIGEN_FOUND)\n  mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)\nendif (EIGEN_FOUND)\n"
  },
  {
    "path": "vins_estimator/.kdev4/vins_estimator.kdev4",
    "content": "[Buildset]\nBuildItems=@Variant(\\x00\\x00\\x00\\t\\x00\\x00\\x00\\x00\\x01\\x00\\x00\\x00\\x0b\\x00\\x00\\x00\\x00\\x01\\x00\\x00\\x00\\x1c\\x00v\\x00i\\x00n\\x00s\\x00_\\x00e\\x00s\\x00t\\x00i\\x00m\\x00a\\x00t\\x00o\\x00r)\n\n[CMake]\nProjectRootRelative=./\n"
  },
  {
    "path": "vins_estimator/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(vins_estimator)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11\")\n#-DEIGEN_USE_MKL_ALL\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g -pthread -fopenmp\")\n\ninclude_directories(${CMAKE_SOURCE_DIR}/../camera_model/include)\n#add_library( lib_camera_model STATIC IMPORTED )\n#set_target_properties(lib_camera_model PROPERTIES IMPORTED_LOCATION  ${CMAKE_SOURCE_DIR}/../camera_model/build/libcamera_model.a)\n\ninclude_directories(\n\t\t\t\t\t/usr/local/include/\n\t\t\t\t\t/usr/local/include/opencv\n\t\t\t\t\t)\nset(OPENCV_LIB_DIR \"/usr/local/lib\")\nadd_library(opencv_calib3d SHARED IMPORTED)\nset_target_properties(opencv_calib3d PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_calib3d.so)\n\nadd_library(opencv_core SHARED IMPORTED)\nset_target_properties(opencv_core PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_core.so)\n\nadd_library(opencv_features2d SHARED IMPORTED)\nset_target_properties(opencv_features2d PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_features2d.so)\n\nadd_library(opencv_flann SHARED IMPORTED)\nset_target_properties(opencv_flann PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_flann.so)\n\nadd_library(opencv_highgui SHARED IMPORTED)\nset_target_properties(opencv_highgui PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_highgui.so)\n\nadd_library(opencv_imgcodecs SHARED IMPORTED)\nset_target_properties(opencv_imgcodecs PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_imgcodecs.so)\n\nadd_library(opencv_imgproc SHARED IMPORTED)\nset_target_properties(opencv_imgproc PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_imgproc.so)\n\nadd_library(opencv_ml SHARED IMPORTED)\nset_target_properties(opencv_ml PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_ml.so)\n\nadd_library(opencv_objdetect SHARED IMPORTED)\nset_target_properties(opencv_objdetect PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_objdetect.so)\n\nadd_library(opencv_photo SHARED IMPORTED)\nset_target_properties(opencv_photo PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_photo.so)\n\nadd_library(opencv_shape SHARED IMPORTED)\nset_target_properties(opencv_shape PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_shape.so)\n\nadd_library(opencv_stitching SHARED IMPORTED)\nset_target_properties(opencv_stitching PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_stitching.so)\n\nadd_library(opencv_superres SHARED IMPORTED)\nset_target_properties(opencv_superres PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_superres.so)\n\nadd_library(opencv_video SHARED IMPORTED)\nset_target_properties(opencv_video PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_video.so)\n\nadd_library(opencv_videoio SHARED IMPORTED)\nset_target_properties(opencv_videoio PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_videoio.so)\n\nadd_library(opencv_videostab SHARED IMPORTED)\nset_target_properties(opencv_videostab PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_videostab.so)\n\nadd_library(opencv_viz SHARED IMPORTED)\nset_target_properties(opencv_viz PROPERTIES IMPORTED_LOCATION ${OPENCV_LIB_DIR}/libopencv_viz.so)\n\nfind_package(Ceres REQUIRED)\n\ninclude_directories(${CERES_INCLUDE_DIRS})\n\n\n\n#message(\"CERES_INCLUDE_DIR:\" ${CERES_INCLUDE_DIR}  \" CERES_LIBS: \"  ${CERES_LIBRARIES} )\n#set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\n#message(\"CMAKE MOUDLE PATH:\", ${CMAKE_MODULE_PATH})\n#find_package(Eigen3  REQUIRED)\ninclude_directories(\n# ${EIGEN3_INCLUDE_DIR}\n\t\t/usr/include/eigen3\n)\n#message(\"EIGEN3_INCLUDE_DIR:\" ${EIGEN3_INCLUDE_DIR}  \" EIGEN3_LIBS: \"  ${Eigen3_LIBRARIES} )\n\n#add_library(ceres STATIC IMPORTED)\n#set_target_properties(ceres PROPERTIES IMPORTED_LOCATION /usr/local/lib/libceres.a)\n#include_directories(/usr/local/include)\n\n#add_library(glog STATIC IMPORTED)\n#set_target_properties(glog PROPERTIES IMPORTED_LOCATION /usr/lib/x86_64-linux-gnu/libglog.a)\n#include_directories(/usr/include/glog)\n\n#add_library(suitesparse STATIC IMPORTED)\n#set_target_properties(suitesparse PROPERTIES IMPORTED_LOCATION /usr/lib/x86_64-linux-gnu/libsuitesparseconfig.a)\n#include_directories(/usr/include/suitesparse) #cholmod\n\n#add_library(cxsparse STATIC IMPORTED)\n#set_target_properties(cxsparse PROPERTIES IMPORTED_LOCATION /usr/lib/x86_64-linux-gnu/libcxsparse.a)\n\n#add_library(cholmod SHARED IMPORTED)\n#set_target_properties(cholmod PROPERTIES IMPORTED_LOCATION /usr/lib/x86_64-linux-gnu/libcholmod.so)\n\n#add_library(atlas SHARED IMPORTED)\n#set_target_properties(atlas PROPERTIES IMPORTED_LOCATION /usr/lib/libatlas.so)\n#add_library(lapack_atlas SHARED IMPORTED)\n#set_target_properties(lapack_atlas PROPERTIES IMPORTED_LOCATION /usr/lib/liblapack_atlas.so)\n#include_directories(/usr/include/atlas)\n\t\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\n\nfind_package(Pangolin REQUIRED)\ninclude_directories(${Pangolin_INCLUDE_DIRS})\n\ninclude_directories(${Boost_INCLUDE_DIRS})\nadd_executable(vins_estimator\n    src/estimator_node.cpp\n    src/parameters.cpp\n    src/estimator.cpp\n    src/feature_manager.cpp\n    src/factor/pose_local_parameterization.cpp\n    src/factor/projection_factor.cpp\n    src/factor/marginalization_factor.cpp\n    src/utility/utility.cpp\n    src/utility/visualization.cpp\n#    src/utility/CameraPoseVisualization.cpp\n    src/initial/solve_5pts.cpp\n    src/initial/initial_aligment.cpp\n    src/initial/initial_sfm.cpp\n    src/initial/initial_ex_rotation.cpp\n    src/loop-closure/ThirdParty/DBoW/BowVector.cpp\n    src/loop-closure/ThirdParty/DBoW/FBrief.cpp\n    src/loop-closure/ThirdParty/DBoW/FeatureVector.cpp\n    src/loop-closure/ThirdParty/DBoW/QueryResults.cpp\n    src/loop-closure/ThirdParty/DBoW/ScoringObject.cpp\n    src/loop-closure/ThirdParty/DUtils/Random.cpp\n    src/loop-closure/ThirdParty/DUtils/Timestamp.cpp\n    src/loop-closure/ThirdParty/DVision/BRIEF.cpp\n    src/loop-closure/ThirdParty/VocabularyBinary.cpp\n    src/loop-closure/loop_closure.cpp\n    src/loop-closure/keyframe.cpp\n    src/loop-closure/keyframe_database.cpp\n    src/feature_tracker/feature_tracker.cpp \n  #  src/feature_tracker/feature_tracker_parameters.cpp\n    ../camera_model/src/chessboard/Chessboard.cc\n    ../camera_model/src/calib/CameraCalibration.cc\n    ../camera_model/src/camera_models/Camera.cc\n    ../camera_model/src/camera_models/CameraFactory.cc\n    ../camera_model/src/camera_models/CostFunctionFactory.cc\n    ../camera_model/src/camera_models/PinholeCamera.cc\n    ../camera_model/src/camera_models/CataCamera.cc\n    ../camera_model/src/camera_models/EquidistantCamera.cc\n    ../camera_model/src/camera_models/ScaramuzzaCamera.cc\n    ../camera_model/src/sparse_graph/Transform.cc\n    ../camera_model/src/gpl/gpl.cc\n    ../camera_model/src/gpl/EigenQuaternionParameterization.cc\n    )\n\n\ntarget_link_libraries(vins_estimator #lib_camera_model \n\t\t\t\t\topencv_calib3d opencv_core opencv_features2d opencv_flann opencv_highgui opencv_imgcodecs opencv_imgproc opencv_ml opencv_objdetect opencv_photo opencv_shape opencv_stitching opencv_superres opencv_video opencv_videoio opencv_videostab opencv_viz   \n\t\t\t\t\t${Pangolin_LIBRARIES}\n\t\t\t\t\t${CERES_LIBRARIES}\n#\t\t\t\t\tsuitesparse cxsparse cholmod atlas lapack_atlas\n#\t\t\t\t\tglog\n#\t\t\t\t\tceres\n\t\t) \n\n\n"
  },
  {
    "path": "vins_estimator/launch/3dm.launch",
    "content": "<launch>\n\n    <arg name=\"config_path\" default = \"$(find feature_tracker)/../config/3dm/3dm_config.yaml\" />\n\t<arg name=\"vins_path\" default = \"$(find feature_tracker)/../config/../\" />\n\n    <node name=\"feature_tracker\" pkg=\"feature_tracker\" type=\"feature_tracker\" output=\"log\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n        <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n    <node name=\"vins_estimator\" pkg=\"vins_estimator\" type=\"vins_estimator\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n        <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n     \n</launch>\n"
  },
  {
    "path": "vins_estimator/launch/A3.launch",
    "content": "<launch>\n\n    <arg name=\"config_path\" default = \"$(find feature_tracker)/../config/A3/A3_config.yaml\" />\n    <arg name=\"vins_path\" default = \"$(find feature_tracker)/../config/../\" />\n\n    <node name=\"feature_tracker\" pkg=\"feature_tracker\" type=\"feature_tracker\" output=\"log\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n        <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n\n    <node name=\"vins_estimator\" pkg=\"vins_estimator\" type=\"vins_estimator\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n        <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n\n</launch>\n"
  },
  {
    "path": "vins_estimator/launch/euroc.launch",
    "content": "<launch>\n    <arg name=\"config_path\" default = \"$(find vins_estimator)/../config/euroc/euroc_config.yaml\" />\n\t  <arg name=\"vins_path\" default = \"$(find vins_estimator)/../config/../\" />\n    \n\n\n    <node name=\"vins_estimator\" pkg=\"vins_estimator\" type=\"vins_estimator\" output=\"screen\">\n       <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n       <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n\n</launch>\n"
  },
  {
    "path": "vins_estimator/launch/euroc_no_extrinsic_param.launch",
    "content": "<launch>\n    <arg name=\"config_path\" default = \"$(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml\" />\n\t  <arg name=\"vins_path\" default = \"$(find feature_tracker)/../config/../\" />\n    \n    <node name=\"feature_tracker\" pkg=\"feature_tracker\" type=\"feature_tracker\" output=\"log\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n        <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n\n    <node name=\"vins_estimator\" pkg=\"vins_estimator\" type=\"vins_estimator\" output=\"screen\">\n       <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n       <param name=\"vins_folder\" type=\"string\" value=\"$(arg vins_path)\" />\n    </node>\n\n</launch>\n"
  },
  {
    "path": "vins_estimator/launch/vins_rviz.launch",
    "content": "<launch>\n    <node name=\"rvizvisualisation\" pkg=\"rviz\" type=\"rviz\" output=\"log\" args=\"-d $(find vins_estimator)/../config/vins_rviz_config.rviz\" />\n</launch>\n"
  },
  {
    "path": "vins_estimator/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>vins_estimator</name>\n  <version>0.0.0</version>\n  <description>The vins_estimator package</description>\n\n  <!-- One maintainer tag required, multiple allowed, one person per tag --> \n  <!-- Example:  -->\n  <!-- <maintainer email=\"jane.doe@example.com\">Jane Doe</maintainer> -->\n  <maintainer email=\"qintonguav@gmail.com\">qintong</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>TODO</license>\n\n\n  <!-- Url tags are optional, but mutiple are allowed, one per tag -->\n  <!-- Optional attribute type can be: website, bugtracker, or repository -->\n  <!-- Example: -->\n  <!-- <url type=\"website\">http://wiki.ros.org/vins_estimator</url> -->\n\n\n  <!-- Author tags are optional, mutiple are allowed, one per tag -->\n  <!-- Authors do not have to be maintianers, but could be -->\n  <!-- Example: -->\n  <!-- <author email=\"jane.doe@example.com\">Jane Doe</author> -->\n\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <!-- build_depend>roscpp</build_depend -->\n  <!-- run_depend>roscpp</run_depend -->\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- You can specify that this package is a metapackage here: -->\n    <!-- <metapackage/> -->\n\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>\n"
  },
  {
    "path": "vins_estimator/src/estimator.cpp",
    "content": "#include \"estimator.h\"\n\nEstimator::Estimator(): f_manager{Rs}\n{\n    //ROS_INFO(\"init begins\");\n    clearState();\n    failure_occur = 0;\n}\n\nvoid Estimator::setParameter()\n{\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        tic[i] = TIC[i];\n        ric[i] = RIC[i];\n    }\n    f_manager.setRic(ric);\n    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();\n}\n\nvoid Estimator::clearState()\n{\n    for (int i = 0; i < WINDOW_SIZE + 1; i++)\n    {\n        Rs[i].setIdentity();\n        Ps[i].setZero();\n        Vs[i].setZero();\n        Bas[i].setZero();\n        Bgs[i].setZero();\n        dt_buf[i].clear();\n        linear_acceleration_buf[i].clear();\n        angular_velocity_buf[i].clear();\n\n        if (pre_integrations[i] != nullptr)\n        {\n            delete pre_integrations[i];\n        }\n        pre_integrations[i] = nullptr;\n    }\n\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        tic[i] = Vector3d::Zero();\n        ric[i] = Matrix3d::Identity();\n    }\n\n    solver_flag = INITIAL;\n    first_imu = false,\n    sum_of_back = 0;\n    sum_of_front = 0;\n    frame_count = 0;\n    solver_flag = INITIAL;\n    initial_timestamp = 0;\n    all_image_frame.clear();\n    relocalize = false;\n    retrive_data_vector.clear();\n    relocalize_t = Eigen::Vector3d(0, 0, 0);\n    relocalize_r = Eigen::Matrix3d::Identity();\n\n    if (tmp_pre_integration != nullptr)\n        delete tmp_pre_integration;\n    if (last_marginalization_info != nullptr)\n        delete last_marginalization_info;\n\n    tmp_pre_integration = nullptr;\n    last_marginalization_info = nullptr;\n    last_marginalization_parameter_blocks.clear();\n\n    f_manager.clearState();\n}\n\nvoid Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)\n{\n    if (!first_imu)\n    {\n        first_imu = true;\n        acc_0 = linear_acceleration;\n        gyr_0 = angular_velocity;\n    }\n\n    if (!pre_integrations[frame_count])\n    {\n        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};\n    }\n    if (frame_count != 0)\n    {\n        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);\n        //if(solver_flag != NON_LINEAR)\n            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);\n\n        dt_buf[frame_count].push_back(dt);\n        linear_acceleration_buf[frame_count].push_back(linear_acceleration);\n        angular_velocity_buf[frame_count].push_back(angular_velocity);\n\n        int j = frame_count;         \n        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;\n        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];\n        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();\n        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;\n        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);\n        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;\n        Vs[j] += dt * un_acc;\n    }\n    acc_0 = linear_acceleration;\n    gyr_0 = angular_velocity;\n\t//std::cout << \"acc_0 : \" << acc_0 << std::endl;\n\t//std::cout << \"gyr_0 : \" << gyr_0 << std::endl;\n\n}\n\nvoid Estimator::processImage(const map<int, vector<pair<int, Vector3d>>> &image, const std_msgs::Header &header)\n{\n   // ROS_DEBUG(\"new image coming ------------------------------------------\");\n  //  ROS_DEBUG(\"Adding feature points %lu\", image.size());\n    if (f_manager.addFeatureCheckParallax(frame_count, image))\n        marginalization_flag = MARGIN_OLD;\n    else\n        marginalization_flag = MARGIN_SECOND_NEW;\n\n  // (\"this frame is--------------------%s\", marginalization_flag ? \"reject\" : \"accept\");\n   // ROS_DEBUG(\"%s\", marginalization_flag ? \"Non-keyframe\" : \"Keyframe\");\n   // ROS_DEBUG(\"Solving %d\", frame_count);\n   //std::cout  << \"number of feature: \" <<  f_manager.getFeatureCount() << std::endl;\n    Headers[frame_count] = header;\n\n    ImageFrame imageframe(image, header.stamp.toSec());\n    imageframe.pre_integration = tmp_pre_integration;\n    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));\n    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};\n\n    if(ESTIMATE_EXTRINSIC == 2)\n    {\n       // ROS_INFO(\"calibrating extrinsic param, rotation movement is needed\");\n        if (frame_count != 0)\n        {\n            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);\n            Matrix3d calib_ric;\n            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))\n            {\n          //      ROS_WARN(\"initial extrinsic rotation calib success\");\n         //       ROS_WARN_STREAM(\"initial extrinsic rotation: \" << endl << calib_ric);\n                ric[0] = calib_ric;\n                RIC[0] = calib_ric;\n                ESTIMATE_EXTRINSIC = 1;\n            }\n        }\n    }\n\n    if (solver_flag == INITIAL)\n    {\n        if (frame_count == WINDOW_SIZE)\n        {\n            bool result = false;\n            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)\n            {\n               result = initialStructure();\n               initial_timestamp = header.stamp.toSec();\n            }\n            if(result)\n            {\n                solver_flag = NON_LINEAR;\n                solveOdometry();\n                slideWindow();\n                f_manager.removeFailures();\n           //     ROS_INFO(\"Initialization finish!\");\n                last_R = Rs[WINDOW_SIZE];\n                last_P = Ps[WINDOW_SIZE];\n                last_R0 = Rs[0];\n                last_P0 = Ps[0];\n                \n            }\n            else\n                slideWindow();\n        }\n        else\n            frame_count++;\n    }\n    else\n    {\n        TicToc t_solve;\n        solveOdometry();\n      //  ROS_DEBUG(\"solver costs: %fms\", t_solve.toc());\n\n        if (failureDetection())\n        {\n          //  ROS_WARN(\"failure detection!\");\n            failure_occur = 1;\n            clearState();\n            setParameter();\n        //    ROS_WARN(\"system reboot!\");\n            return;\n        }\n\n        TicToc t_margin;\n        slideWindow();\n        f_manager.removeFailures();\n    //    ROS_DEBUG(\"marginalization costs: %fms\", t_margin.toc());\n        // prepare output of VINS\n        key_poses.clear();\n        for (int i = 0; i <= WINDOW_SIZE; i++)\n            key_poses.push_back(Ps[i]);\n\n        last_R = Rs[WINDOW_SIZE];\n        last_P = Ps[WINDOW_SIZE];\n        last_R0 = Rs[0];\n        last_P0 = Ps[0];\n    }\n}\nbool Estimator::initialStructure()\n{\n    TicToc t_sfm;\n    //check imu observibility\n    {\n        map<double, ImageFrame>::iterator frame_it;\n        Vector3d sum_g;\n        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)\n        {\n            double dt = frame_it->second.pre_integration->sum_dt;\n            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;\n            sum_g += tmp_g;\n        }\n        Vector3d aver_g;\n        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);\n        double var = 0;\n        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)\n        {\n            double dt = frame_it->second.pre_integration->sum_dt;\n            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;\n            var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);\n            //cout << \"frame g \" << tmp_g.transpose() << endl;\n        }\n        var = sqrt(var / ((int)all_image_frame.size() - 1));\n        //ROS_WARN(\"IMU variation %f!\", var);\n        if(var < 0.25)\n        {\n          //  ROS_INFO(\"IMU excitation not enouth!\");\n            //return false;\n        }\n    }\n    // global sfm\n    Quaterniond Q[frame_count + 1];\n    Vector3d T[frame_count + 1];\n    map<int, Vector3d> sfm_tracked_points;\n    vector<SFMFeature> sfm_f;\n    for (auto &it_per_id : f_manager.feature)\n    {\n        int imu_j = it_per_id.start_frame - 1;\n        SFMFeature tmp_feature;\n        tmp_feature.state = false;\n        tmp_feature.id = it_per_id.feature_id;\n        for (auto &it_per_frame : it_per_id.feature_per_frame)\n        {\n            imu_j++;\n            Vector3d pts_j = it_per_frame.point;\n            tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));\n        }\n        sfm_f.push_back(tmp_feature);\n    } \n    Matrix3d relative_R;\n    Vector3d relative_T;\n    int l;\n    if (!relativePose(relative_R, relative_T, l))\n    {\n     //   ROS_INFO(\"Not enough features or parallax; Move device around\");\n        return false;\n    }\n\tstd::cout << \"relative_R:\" << std::endl;\n\n\tstd::cout << relative_R << std::endl;\n\n\tstd::cout << \"relative_T:\" << std::endl;\n\n\tstd::cout << relative_T << std::endl;\n\n\tstd::cout << \"l :\" << l << std::endl;\n    \n\tGlobalSFM sfm;\n    if(!sfm.construct(frame_count + 1, Q, T, l,\n              relative_R, relative_T,\n              sfm_f, sfm_tracked_points))\n    {\n     //   ROS_DEBUG(\"global SFM failed!\");\n        marginalization_flag = MARGIN_OLD;\n        return false;\n    }\n\n    //solve pnp for all frame\n    map<double, ImageFrame>::iterator frame_it;\n    map<int, Vector3d>::iterator it;\n    frame_it = all_image_frame.begin( );\n    for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)\n    {\n        // provide initial guess\n        cv::Mat r, rvec, t, D, tmp_r;\n        if((frame_it->first) == Headers[i].stamp.toSec())\n        {\n            frame_it->second.is_key_frame = true;\n            frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();\n            frame_it->second.T = T[i];\n            i++;\n            continue;\n        }\n        if((frame_it->first) > Headers[i].stamp.toSec())\n        {\n            i++;\n        }\n        Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();\n        Vector3d P_inital = - R_inital * T[i];\n        cv::eigen2cv(R_inital, tmp_r);\n        cv::Rodrigues(tmp_r, rvec);\n        cv::eigen2cv(P_inital, t);\n\n        frame_it->second.is_key_frame = false;\n        vector<cv::Point3f> pts_3_vector;\n        vector<cv::Point2f> pts_2_vector;\n        for (auto &id_pts : frame_it->second.points)\n        {\n            int feature_id = id_pts.first;\n            //cout << \"feature id \" << feature_id;\n            for (auto &i_p : id_pts.second)\n            {\n                //cout << \" pts image_frame \" << (i_p.second.head<2>() * 460 ).transpose() << endl;\n                it = sfm_tracked_points.find(feature_id);\n                if(it != sfm_tracked_points.end())\n                {\n                    Vector3d world_pts = it->second;\n                    cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));\n                    pts_3_vector.push_back(pts_3);\n                    Vector2d img_pts = i_p.second.head<2>();\n                    cv::Point2f pts_2(img_pts(0), img_pts(1));\n                    pts_2_vector.push_back(pts_2);\n                }\n            }\n        }\n        cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);     \n        if(pts_3_vector.size() < 6)\n        {\n            //cout << \"pts_3_vector size \" << pts_3_vector.size() << endl;\n        //    ROS_DEBUG(\"Not enough points for solve pnp !\");\n            return false;\n        }\n        if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))\n        {\n       //     ROS_DEBUG(\"solve pnp fail!\");\n            return false;\n        }\n        cv::Rodrigues(rvec, r);\n        MatrixXd R_pnp,tmp_R_pnp;\n        cv::cv2eigen(r, tmp_R_pnp);\n        R_pnp = tmp_R_pnp.transpose();\n        MatrixXd T_pnp;\n        cv::cv2eigen(t, T_pnp);\n        T_pnp = R_pnp * (-T_pnp);\n        frame_it->second.R = R_pnp * RIC[0].transpose();\n        frame_it->second.T = T_pnp;\n    }\n    if (visualInitialAlign())\n        return true;\n    else\n    {\n      //  ROS_INFO(\"misalign visual structure with IMU\");\n        return false;\n    }\n\n}\n\nbool Estimator::visualInitialAlign()\n{\n    TicToc t_g;\n    VectorXd x;\n    //solve scale\n    bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);\n    if(!result)\n    {\n      //  ROS_DEBUG(\"solve g failed!\");\n        return false;\n    }\n\n    // change state\n    for (int i = 0; i <= frame_count; i++)\n    {\n        Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;\n        Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;\n        Ps[i] = Pi;\n        Rs[i] = Ri;\n        all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;\n    }\n\n    VectorXd dep = f_manager.getDepthVector();\n    for (int i = 0; i < dep.size(); i++)\n        dep[i] = -1;\n    f_manager.clearDepth(dep);\n\n    //triangulat on cam pose , no tic\n    Vector3d TIC_TMP[NUM_OF_CAM];\n    for(int i = 0; i < NUM_OF_CAM; i++)\n        TIC_TMP[i].setZero();\n    ric[0] = RIC[0];\n    f_manager.setRic(ric);\n    f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));\n\n    double s = (x.tail<1>())(0);\n    for (int i = 0; i <= WINDOW_SIZE; i++)\n    {\n        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);\n    }\n    for (int i = frame_count; i >= 0; i--)\n        Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);\n    int kv = -1;\n    map<double, ImageFrame>::iterator frame_i;\n    for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)\n    {\n        if(frame_i->second.is_key_frame)\n        {\n            kv++;\n            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);\n        }\n    }\n    for (auto &it_per_id : f_manager.feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n        it_per_id.estimated_depth *= s;\n    }\n\n    Matrix3d R0 = Utility::g2R(g);\n    double yaw = Utility::R2ypr(R0 * Rs[0]).x();\n    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n    g = R0 * g;\n    //Matrix3d rot_diff = R0 * Rs[0].transpose();\n    Matrix3d rot_diff = R0;\n    for (int i = 0; i <= frame_count; i++)\n    {\n        Ps[i] = rot_diff * Ps[i];\n        Rs[i] = rot_diff * Rs[i];\n        Vs[i] = rot_diff * Vs[i];\n    }\n //   ROS_DEBUG_STREAM(\"g0     \" << g.transpose());\n  //  ROS_DEBUG_STREAM(\"my R0  \" << Utility::R2ypr(Rs[0]).transpose()); \n\n    return true;\n}\n\nbool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)\n{\n    // find previous frame which contians enough correspondance and parallex with newest frame\n    for (int i = 0; i < WINDOW_SIZE; i++)\n    {\n        vector<pair<Vector3d, Vector3d>> corres;\n        corres = f_manager.getCorresponding(i, WINDOW_SIZE);\n        if (corres.size() > 20)\n        {\n            double sum_parallax = 0;\n            double average_parallax;\n            for (int j = 0; j < int(corres.size()); j++)\n            {\n                Vector2d pts_0(corres[j].first(0), corres[j].first(1));\n                Vector2d pts_1(corres[j].second(0), corres[j].second(1));\n                double parallax = (pts_0 - pts_1).norm();\n                sum_parallax = sum_parallax + parallax;\n\n            }\n            average_parallax = 1.0 * sum_parallax / int(corres.size());\n            if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))\n            {\n                l = i;\n\t\t\t\tstd::cout << \"average_parallax:\" << average_parallax*460 <<  \" choose l :\" << l << \" and newest frame to triangulate the whole structure, corres.size():\" <<  corres.size()  << std::endl;\n                return true;\n            }\n        }\n    }\n    return false;\n}\n\nvoid Estimator::solveOdometry()\n{\n    if (frame_count < WINDOW_SIZE)\n        return;\n    if (solver_flag == NON_LINEAR)\n    {\n        TicToc t_tri;\n        f_manager.triangulate(Ps, tic, ric);\n    //    ROS_DEBUG(\"triangulation costs %f\", t_tri.toc());\n        optimization();\n    }\n}\n\nvoid Estimator::vector2double()\n{\n    for (int i = 0; i <= WINDOW_SIZE; i++)\n    {\n        para_Pose[i][0] = Ps[i].x();\n        para_Pose[i][1] = Ps[i].y();\n        para_Pose[i][2] = Ps[i].z();\n        Quaterniond q{Rs[i]};\n        para_Pose[i][3] = q.x();\n        para_Pose[i][4] = q.y();\n        para_Pose[i][5] = q.z();\n        para_Pose[i][6] = q.w();\n\n        para_SpeedBias[i][0] = Vs[i].x();\n        para_SpeedBias[i][1] = Vs[i].y();\n        para_SpeedBias[i][2] = Vs[i].z();\n\n        para_SpeedBias[i][3] = Bas[i].x();\n        para_SpeedBias[i][4] = Bas[i].y();\n        para_SpeedBias[i][5] = Bas[i].z();\n\n        para_SpeedBias[i][6] = Bgs[i].x();\n        para_SpeedBias[i][7] = Bgs[i].y();\n        para_SpeedBias[i][8] = Bgs[i].z();\n    }\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        para_Ex_Pose[i][0] = tic[i].x();\n        para_Ex_Pose[i][1] = tic[i].y();\n        para_Ex_Pose[i][2] = tic[i].z();\n        Quaterniond q{ric[i]};\n        para_Ex_Pose[i][3] = q.x();\n        para_Ex_Pose[i][4] = q.y();\n        para_Ex_Pose[i][5] = q.z();\n        para_Ex_Pose[i][6] = q.w();\n    }\n\n    VectorXd dep = f_manager.getDepthVector();\n    for (int i = 0; i < f_manager.getFeatureCount(); i++)\n        para_Feature[i][0] = dep(i);\n}\n\nvoid Estimator::double2vector()\n{\n    Vector3d origin_R0 = Utility::R2ypr(Rs[0]);\n    Vector3d origin_P0 = Ps[0];\n\n    if (failure_occur)\n    {\n        origin_R0 = Utility::R2ypr(last_R0);\n        origin_P0 = last_P0;\n        failure_occur = 0;\n    }\n    Vector3d origin_R00 = Utility::R2ypr(Quaterniond(para_Pose[0][6],\n                                                      para_Pose[0][3],\n                                                      para_Pose[0][4],\n                                                      para_Pose[0][5]).toRotationMatrix());\n    double y_diff = origin_R0.x() - origin_R00.x();\n    //TODO\n    Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));\n\n    for (int i = 0; i <= WINDOW_SIZE; i++)\n    {\n\n        Rs[i] = rot_diff * Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();\n        Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0],\n                                para_Pose[i][1] - para_Pose[0][1],\n                                para_Pose[i][2] - para_Pose[0][2]) + origin_P0;\n        Vs[i] = rot_diff * Vector3d(para_SpeedBias[i][0],\n                                    para_SpeedBias[i][1],\n                                    para_SpeedBias[i][2]);\n\n        Bas[i] = Vector3d(para_SpeedBias[i][3],\n                          para_SpeedBias[i][4],\n                          para_SpeedBias[i][5]);\n\n        Bgs[i] = Vector3d(para_SpeedBias[i][6],\n                          para_SpeedBias[i][7],\n                          para_SpeedBias[i][8]);\n    }\n\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        tic[i] = Vector3d(para_Ex_Pose[i][0],\n                          para_Ex_Pose[i][1],\n                          para_Ex_Pose[i][2]);\n        ric[i] = Quaterniond(para_Ex_Pose[i][6],\n                             para_Ex_Pose[i][3],\n                             para_Ex_Pose[i][4],\n                             para_Ex_Pose[i][5]).toRotationMatrix();\n    }\n\n    VectorXd dep = f_manager.getDepthVector();\n    for (int i = 0; i < f_manager.getFeatureCount(); i++)\n        dep(i) = para_Feature[i][0];\n    f_manager.setDepth(dep);\n\n    if (LOOP_CLOSURE && relocalize && retrive_data_vector[0].relative_pose && !retrive_data_vector[0].relocalized)\n    {\n        for (int i = 0; i < (int)retrive_data_vector.size();i++)\n            retrive_data_vector[i].relocalized = true;\n        Matrix3d vio_loop_r;\n        Vector3d vio_loop_t;\n        vio_loop_r = rot_diff * Quaterniond(retrive_data_vector[0].loop_pose[6], retrive_data_vector[0].loop_pose[3], retrive_data_vector[0].loop_pose[4], retrive_data_vector[0].loop_pose[5]).normalized().toRotationMatrix();\n        vio_loop_t = rot_diff * Vector3d(retrive_data_vector[0].loop_pose[0] - para_Pose[0][0],\n                                retrive_data_vector[0].loop_pose[1] - para_Pose[0][1],\n                                retrive_data_vector[0].loop_pose[2] - para_Pose[0][2]) + origin_P0;\n        Quaterniond vio_loop_q(vio_loop_r);\n        double relocalize_yaw;\n        relocalize_yaw = Utility::R2ypr(retrive_data_vector[0].R_old).x() - Utility::R2ypr(vio_loop_r).x();\n        relocalize_r = Utility::ypr2R(Vector3d(relocalize_yaw, 0, 0));\n        relocalize_t = retrive_data_vector[0].P_old- relocalize_r * vio_loop_t;\n    }\n}\n\nbool Estimator::failureDetection()\n{\n    if (f_manager.last_track_num < 2)\n    {\n     //   ROS_INFO(\" little feature %d\", f_manager.last_track_num);\n        return true;\n    }\n    if (Bas[WINDOW_SIZE].norm() > 2.5)\n    {\n     //   ROS_INFO(\" big IMU acc bias estimation %f\", Bas[WINDOW_SIZE].norm());\n        return true;\n    }\n    if (Bgs[WINDOW_SIZE].norm() > 1.0)\n    {\n   //     ROS_INFO(\" big IMU gyr bias estimation %f\", Bgs[WINDOW_SIZE].norm());\n        return true;\n    }\n    /*\n    if (tic(0) > 1)\n    {\n        ROS_INFO(\" big extri param estimation %d\", tic(0) > 1);\n        return true;\n    }\n    */\n    Vector3d tmp_P = Ps[WINDOW_SIZE];\n    if ((tmp_P - last_P).norm() > 5)\n    {\n     //   ROS_INFO(\" big translation\");\n        return true;\n    }\n    if (abs(tmp_P.z() - last_P.z()) > 1)\n    {\n    //    ROS_INFO(\" big z translation\");\n        return true; \n    }\n    Matrix3d tmp_R = Rs[WINDOW_SIZE];\n    Matrix3d delta_R = tmp_R.transpose() * last_R;\n    Quaterniond delta_Q(delta_R);\n    double delta_angle;\n    delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;\n    if (delta_angle > 50)\n    {\n       // ROS_INFO(\" big delta_angle \");\n        return true;\n    }\n    return false;\n}\n\n\nvoid Estimator::optimization()\n{\n    ceres::Problem problem;\n    ceres::LossFunction *loss_function;\n    //loss_function = new ceres::HuberLoss(1.0);\n    loss_function = new ceres::CauchyLoss(1.0);\n    for (int i = 0; i < WINDOW_SIZE + 1; i++)\n    {\n        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();\n        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);\n        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);\n    }\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();\n        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);\n        if (!ESTIMATE_EXTRINSIC)\n        {\n        //    ROS_DEBUG(\"fix extinsic param\");\n            problem.SetParameterBlockConstant(para_Ex_Pose[i]);\n        }\n        else\n       //     ROS_DEBUG(\"estimate extinsic param\");\n\t  ;\n    }\n\n    TicToc t_whole, t_prepare;\n    vector2double();\n\n    if (last_marginalization_info)\n    {\n        // construct new marginlization_factor\n        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);\n        problem.AddResidualBlock(marginalization_factor, NULL,\n                                 last_marginalization_parameter_blocks);\n    }\n\n    for (int i = 0; i < WINDOW_SIZE; i++)\n    {\n        int j = i + 1;\n        if (pre_integrations[j]->sum_dt > 10.0)\n            continue;\n        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);\n        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);\n    }\n    int f_m_cnt = 0;\n    int feature_index = -1;\n    for (auto &it_per_id : f_manager.feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n \n        ++feature_index;\n\n        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n        \n        Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n\n        for (auto &it_per_frame : it_per_id.feature_per_frame)\n        {\n            imu_j++;\n            if (imu_i == imu_j)\n            {\n                continue;\n            }\n            Vector3d pts_j = it_per_frame.point;\n            ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);\n            problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);\n            f_m_cnt++;\n        }\n    }\n    relocalize = false;\n    //loop close factor\n    if(LOOP_CLOSURE)\n    {\n        int loop_constraint_num = 0;\n        for (int k = 0; k < (int)retrive_data_vector.size(); k++)\n        {    \n            for(int i = 0; i < WINDOW_SIZE; i++)\n            {\n                if(retrive_data_vector[k].header == Headers[i].stamp.toSec())\n                {\n                    relocalize = true;\n                    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();\n                    problem.AddParameterBlock(retrive_data_vector[k].loop_pose, SIZE_POSE, local_parameterization);\n                    loop_window_index = i;\n                    loop_constraint_num++;\n                    int retrive_feature_index = 0;\n                    int feature_index = -1;\n                    for (auto &it_per_id : f_manager.feature)\n                    {\n                        it_per_id.used_num = it_per_id.feature_per_frame.size();\n                        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n                            continue;\n\n                        ++feature_index;\n                        int start = it_per_id.start_frame;\n                        if(start <= i)\n                        {   \n                            while(retrive_data_vector[k].features_ids[retrive_feature_index] < it_per_id.feature_id)\n                            {\n                                retrive_feature_index++;\n                            }\n\n                            if(retrive_data_vector[k].features_ids[retrive_feature_index] == it_per_id.feature_id)\n                            {\n                                Vector3d pts_j = Vector3d(retrive_data_vector[k].measurements[retrive_feature_index].x, retrive_data_vector[k].measurements[retrive_feature_index].y, 1.0);\n                                Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n                                \n                                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);\n                                problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);\n                            \n                                retrive_feature_index++;\n                            }     \n                        }\n                    }\n                            \n                }\n            }\n        }\n      //  ROS_DEBUG(\"loop constraint num: %d\", loop_constraint_num);\n    }\n   // ROS_DEBUG(\"visual measurement count: %d\", f_m_cnt);\n  //  ROS_DEBUG(\"prepare for ceres: %f\", t_prepare.toc());\n\n    ceres::Solver::Options options;\n\n    options.linear_solver_type = ceres::DENSE_SCHUR;\n    //options.num_threads = 2;\n    options.trust_region_strategy_type = ceres::DOGLEG;\n    options.max_num_iterations = NUM_ITERATIONS;\n    //options.use_explicit_schur_complement = true;\n    //options.minimizer_progress_to_stdout = true;\n    //options.use_nonmonotonic_steps = true;\n    if (marginalization_flag == MARGIN_OLD)\n        options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;\n    else\n        options.max_solver_time_in_seconds = SOLVER_TIME;\n    TicToc t_solver;\n    ceres::Solver::Summary summary;\n    ceres::Solve(options, &problem, &summary);\n    //cout << summary.BriefReport() << endl;\n  //  ROS_DEBUG(\"Iterations : %d\", static_cast<int>(summary.iterations.size()));\n  //  ROS_DEBUG(\"solver costs: %f\", t_solver.toc());\n\n    // relative info between two loop frame\n    if(LOOP_CLOSURE && relocalize)\n    { \n        for (int k = 0; k < (int)retrive_data_vector.size(); k++)\n        {\n            for(int i = 0; i< WINDOW_SIZE; i++)\n            {\n                if(retrive_data_vector[k].header == Headers[i].stamp.toSec())\n                {\n                    retrive_data_vector[k].relative_pose = true;\n                    Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();\n                    Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]);\n                    Quaterniond Qs_loop;\n                    Qs_loop = Quaterniond(retrive_data_vector[k].loop_pose[6],  retrive_data_vector[k].loop_pose[3],  retrive_data_vector[k].loop_pose[4],  retrive_data_vector[k].loop_pose[5]).normalized().toRotationMatrix();\n                    Matrix3d Rs_loop = Qs_loop.toRotationMatrix();\n                    Vector3d Ps_loop = Vector3d( retrive_data_vector[k].loop_pose[0],  retrive_data_vector[k].loop_pose[1],  retrive_data_vector[k].loop_pose[2]);\n\n                    retrive_data_vector[k].relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop);\n                    retrive_data_vector[k].relative_q = Rs_loop.transpose() * Rs_i;\n                    retrive_data_vector[k].relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x());\n                    if (abs(retrive_data_vector[k].relative_yaw) > 30.0 || retrive_data_vector[k].relative_t.norm() > 20.0)\n                        retrive_data_vector[k].relative_pose = false;\n                        \n                }\n            } \n        } \n    }\n\n    double2vector();\n\n    TicToc t_whole_marginalization;\n    if (marginalization_flag == MARGIN_OLD)\n    {\n        MarginalizationInfo *marginalization_info = new MarginalizationInfo();\n        vector2double();\n\n        if (last_marginalization_info)\n        {\n            vector<int> drop_set;\n            for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)\n            {\n                if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||\n                    last_marginalization_parameter_blocks[i] == para_SpeedBias[0])\n                    drop_set.push_back(i);\n            }\n            // construct new marginlization_factor\n            MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);\n            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,\n                                                                           last_marginalization_parameter_blocks,\n                                                                           drop_set);\n\n            marginalization_info->addResidualBlockInfo(residual_block_info);\n        }\n\n        {\n            if (pre_integrations[1]->sum_dt < 10.0)\n            {\n                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);\n                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,\n                                                                           vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},\n                                                                           vector<int>{0, 1});\n                marginalization_info->addResidualBlockInfo(residual_block_info);\n            }\n        }\n\n        {\n            int feature_index = -1;\n            for (auto &it_per_id : f_manager.feature)\n            {\n                it_per_id.used_num = it_per_id.feature_per_frame.size();\n                if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n                    continue;\n\n                ++feature_index;\n\n                int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n                if (imu_i != 0)\n                    continue;\n\n                Vector3d pts_i = it_per_id.feature_per_frame[0].point;\n\n                for (auto &it_per_frame : it_per_id.feature_per_frame)\n                {\n                    imu_j++;\n                    if (imu_i == imu_j)\n                        continue;\n\n                    Vector3d pts_j = it_per_frame.point;\n                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);\n                    ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,\n                                                                                   vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},\n                                                                                   vector<int>{0, 3});\n                    marginalization_info->addResidualBlockInfo(residual_block_info);\n                }\n            }\n        }\n\n        TicToc t_pre_margin;\n        marginalization_info->preMarginalize();\n       // ROS_DEBUG(\"pre marginalization %f ms\", t_pre_margin.toc());\n        \n        TicToc t_margin;\n        marginalization_info->marginalize();\n     //   ROS_DEBUG(\"marginalization %f ms\", t_margin.toc());\n\n        std::unordered_map<long, double *> addr_shift;\n        for (int i = 1; i <= WINDOW_SIZE; i++)\n        {\n            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];\n            addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];\n        }\n        for (int i = 0; i < NUM_OF_CAM; i++)\n            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];\n\n        vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);\n\n        if (last_marginalization_info)\n            delete last_marginalization_info;\n        last_marginalization_info = marginalization_info;\n        last_marginalization_parameter_blocks = parameter_blocks;\n        \n    }\n    else\n    {\n        if (last_marginalization_info &&\n            std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))\n        {\n\n            MarginalizationInfo *marginalization_info = new MarginalizationInfo();\n            vector2double();\n            if (last_marginalization_info)\n            {\n                vector<int> drop_set;\n                for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)\n                {\n                    //ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);\n\t\t  assert(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);\n                    if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])\n                        drop_set.push_back(i);\n                }\n                // construct new marginlization_factor\n                MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);\n                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,\n                                                                               last_marginalization_parameter_blocks,\n                                                                               drop_set);\n\n                marginalization_info->addResidualBlockInfo(residual_block_info);\n            }\n\n            TicToc t_pre_margin;\n        //    ROS_DEBUG(\"begin marginalization\");\n            marginalization_info->preMarginalize();\n         //   ROS_DEBUG(\"end pre marginalization, %f ms\", t_pre_margin.toc());\n\n            TicToc t_margin;\n         //   ROS_DEBUG(\"begin marginalization\");\n            marginalization_info->marginalize();\n         //   ROS_DEBUG(\"end marginalization, %f ms\", t_margin.toc());\n            \n            std::unordered_map<long, double *> addr_shift;\n            for (int i = 0; i <= WINDOW_SIZE; i++)\n            {\n                if (i == WINDOW_SIZE - 1)\n                    continue;\n                else if (i == WINDOW_SIZE)\n                {\n                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];\n                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];\n                }\n                else\n                {\n                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];\n                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];\n                }\n            }\n            for (int i = 0; i < NUM_OF_CAM; i++)\n                addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];\n\n            vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);\n            if (last_marginalization_info)\n                delete last_marginalization_info;\n            last_marginalization_info = marginalization_info;\n            last_marginalization_parameter_blocks = parameter_blocks;\n            \n        }\n    }\n  //  ROS_DEBUG(\"whole marginalization costs: %f\", t_whole_marginalization.toc());\n   \n   // ROS_DEBUG(\"whole time for ceres: %f\", t_whole.toc());\n}\n\nvoid Estimator::slideWindow()\n{\n    TicToc t_margin;\n    if (marginalization_flag == MARGIN_OLD)\n    {\n        back_R0 = Rs[0];\n        back_P0 = Ps[0];\n        if (frame_count == WINDOW_SIZE)\n        {\n            for (int i = 0; i < WINDOW_SIZE; i++)\n            {\n                Rs[i].swap(Rs[i + 1]);\n\n                std::swap(pre_integrations[i], pre_integrations[i + 1]);\n\n                dt_buf[i].swap(dt_buf[i + 1]);\n                linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);\n                angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);\n\n                Headers[i] = Headers[i + 1];\n                Ps[i].swap(Ps[i + 1]);\n                Vs[i].swap(Vs[i + 1]);\n                Bas[i].swap(Bas[i + 1]);\n                Bgs[i].swap(Bgs[i + 1]);\n            }\n            Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];\n            Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];\n            Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];\n            Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];\n            Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];\n            Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];\n\n            delete pre_integrations[WINDOW_SIZE];\n            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};\n\n            dt_buf[WINDOW_SIZE].clear();\n            linear_acceleration_buf[WINDOW_SIZE].clear();\n            angular_velocity_buf[WINDOW_SIZE].clear();\n\n            if (true || solver_flag == INITIAL)\n            {\n                double t_0 = Headers[0].stamp.toSec();\n                map<double, ImageFrame>::iterator it_0;\n                it_0 = all_image_frame.find(t_0);\n                delete it_0->second.pre_integration;\n                all_image_frame.erase(all_image_frame.begin(), it_0);\n\n            }\n            slideWindowOld();\n        }\n    }\n    else\n    {\n        if (frame_count == WINDOW_SIZE)\n        {\n            for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)\n            {\n                double tmp_dt = dt_buf[frame_count][i];\n                Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];\n                Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];\n\n                pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);\n\n                dt_buf[frame_count - 1].push_back(tmp_dt);\n                linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);\n                angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);\n            }\n\n            Headers[frame_count - 1] = Headers[frame_count];\n            Ps[frame_count - 1] = Ps[frame_count];\n            Vs[frame_count - 1] = Vs[frame_count];\n            Rs[frame_count - 1] = Rs[frame_count];\n            Bas[frame_count - 1] = Bas[frame_count];\n            Bgs[frame_count - 1] = Bgs[frame_count];\n\n            delete pre_integrations[WINDOW_SIZE];\n            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};\n\n            dt_buf[WINDOW_SIZE].clear();\n            linear_acceleration_buf[WINDOW_SIZE].clear();\n            angular_velocity_buf[WINDOW_SIZE].clear();\n\n            slideWindowNew();\n        }\n    }\n}\n\n// real marginalization is removed in solve_ceres()\nvoid Estimator::slideWindowNew()\n{\n    sum_of_front++;\n    f_manager.removeFront(frame_count);\n}\n// real marginalization is removed in solve_ceres()\nvoid Estimator::slideWindowOld()\n{\n    sum_of_back++;\n\n    bool shift_depth = solver_flag == NON_LINEAR ? true : false;\n    if (shift_depth)\n    {\n        Matrix3d R0, R1;\n        Vector3d P0, P1;\n        R0 = back_R0 * ric[0];\n        R1 = Rs[0] * ric[0];\n        P0 = back_P0 + back_R0 * tic[0];\n        P1 = Ps[0] + Rs[0] * tic[0];\n        f_manager.removeBackShiftDepth(R0, P0, R1, P1);\n    }\n    else\n        f_manager.removeBack();\n\n}\n\n"
  },
  {
    "path": "vins_estimator/src/estimator.h",
    "content": "#pragma once\n\n#include \"parameters.h\"\n#include \"feature_manager.h\"\n#include \"utility/utility.h\"\n#include \"utility/tic_toc.h\"\n#include \"initial/solve_5pts.h\"\n#include \"initial/initial_sfm.h\"\n#include \"initial/initial_alignment.h\"\n#include \"initial/initial_ex_rotation.h\"\n//#include <std_msgs/Header.h>\n//#include <std_msgs/Float32.h>\n#include \"../../include/Header.h\"\n#include \"../../include/Float32.h\"\n#include <assert.h>\n#include <ceres/ceres.h>\n#include \"factor/imu_factor.h\"\n#include \"factor/pose_local_parameterization.h\"\n#include \"factor/projection_factor.h\"\n#include \"factor/marginalization_factor.h\"\n\n#include <unordered_map>\n#include <queue>\n#include <opencv2/core/eigen.hpp>\n\nstruct RetriveData\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    /* data */\n    int old_index;\n    int cur_index;\n    double header;\n    Vector3d P_old;\n    Matrix3d R_old;\n    vector<cv::Point2f> measurements;\n    vector<int> features_ids; \n    bool relocalized;\n    bool relative_pose;\n    Vector3d relative_t;\n    Quaterniond relative_q;\n    double relative_yaw;\n    double loop_pose[7];\n};\n\nclass Estimator\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    Estimator();\n\n    void setParameter();\n\n    // interface\n    void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity);\n    void processImage(const map<int, vector<pair<int, Vector3d>>> &image, const std_msgs::Header &header);\n\n    // internal\n    void clearState();\n    bool initialStructure();\n    bool visualInitialAlign();\n    bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l);\n    void slideWindow();\n    void solveOdometry();\n    void slideWindowNew();\n    void slideWindowOld();\n    void optimization();\n    void vector2double();\n    void double2vector();\n    bool failureDetection();\n\n\n    enum SolverFlag\n    {\n        INITIAL,\n        NON_LINEAR\n    };\n\n    enum MarginalizationFlag\n    {\n        MARGIN_OLD = 0,\n        MARGIN_SECOND_NEW = 1\n    };\n\n    SolverFlag solver_flag;\n    MarginalizationFlag  marginalization_flag;\n    Vector3d g;\n    MatrixXd Ap[2], backup_A;\n    VectorXd bp[2], backup_b;\n\n    Matrix3d ric[NUM_OF_CAM];\n    Vector3d tic[NUM_OF_CAM];\n\n    Vector3d Ps[(WINDOW_SIZE + 1)];\n    Vector3d Vs[(WINDOW_SIZE + 1)];\n    Matrix3d Rs[(WINDOW_SIZE + 1)];\n    Vector3d Bas[(WINDOW_SIZE + 1)];\n    Vector3d Bgs[(WINDOW_SIZE + 1)];\n\n    Matrix3d back_R0, last_R, last_R0;\n    Vector3d back_P0, last_P, last_P0;\n    std_msgs::Header Headers[(WINDOW_SIZE + 1)];\n\n    IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];\n    Vector3d acc_0, gyr_0;\n\n    vector<double> dt_buf[(WINDOW_SIZE + 1)];\n    vector<Vector3d> linear_acceleration_buf[(WINDOW_SIZE + 1)];\n    vector<Vector3d> angular_velocity_buf[(WINDOW_SIZE + 1)];\n\n    int frame_count;\n    int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid;\n\n    FeatureManager f_manager;\n    MotionEstimator m_estimator;\n    InitialEXRotation initial_ex_rotation;\n\n    bool first_imu;\n    bool is_valid, is_key;\n    bool failure_occur;\n\n    vector<Vector3d> point_cloud;\n    vector<Vector3d> margin_cloud;\n    vector<Vector3d> key_poses;\n    double initial_timestamp;\n\n\n    double para_Pose[WINDOW_SIZE + 1][SIZE_POSE];\n    double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS];\n    double para_Feature[NUM_OF_F][SIZE_FEATURE];\n    double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE];\n    double para_Retrive_Pose[SIZE_POSE];\n\n    RetriveData retrive_pose_data, front_pose;\n    vector<RetriveData> retrive_data_vector;\n    int loop_window_index;\n    bool relocalize;\n    Vector3d relocalize_t;\n    Matrix3d relocalize_r;\n\n    MarginalizationInfo *last_marginalization_info;\n    vector<double *> last_marginalization_parameter_blocks;\n\n    map<double, ImageFrame> all_image_frame;\n    IntegrationBase *tmp_pre_integration;\n\n};\n"
  },
  {
    "path": "vins_estimator/src/estimator_node.cpp",
    "content": "#include <stdio.h>\n#include <queue>\n#include <map>\n#include <thread>\n#include <mutex>\n#include <condition_variable>\n//#include <ros/ros.h>\n//#include <cv_bridge/cv_bridge.h>\n#include <opencv2/opencv.hpp>\n\n#include \"estimator.h\"\n#include \"parameters.h\"\n#include \"utility/visualization.h\"\n#include \"loop-closure/loop_closure.h\"\n#include \"loop-closure/keyframe.h\"\n#include \"loop-closure/keyframe_database.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n/****************** load image section ***********************/\n#include <iostream>\n#include <fstream>\n#include <chrono>\n/****************** load image section ***********************/\n\n/****************** feature tracker section ***********************/\n#include \"../../include/PointCloud.h\"\n#include \"../../include/Imu.h\"\n#include \"feature_tracker/feature_tracker.h\"\n\n/************************* visualization ***********************/\n#include <pangolin/pangolin.h>\n\n#define SHOW_UNDISTORTION 0\n\nvector<uchar> r_status;\nvector<float> r_err;\n\n//ros::Publisher pub_img,pub_match;\n//ros::Publisher pub_match;\n\nFeatureTracker trackerData[NUM_OF_CAM];\ndouble first_image_time;\nint pub_count = 1;\nbool first_image_flag = true;\nbool running_flag = true;\nbool view_done = false;\n/****************** feature tracker section ***********************/\n\n/****************** load image section ***********************/\nusing namespace std;\n/****************** load image section ***********************/\n\nEstimator estimator;\n\nstd::condition_variable con;\ndouble current_time = -1;\nqueue<sensor_msgs::ImuConstPtr> imu_buf;\nqueue<sensor_msgs::PointCloudConstPtr> feature_buf;\nstd::mutex m_posegraph_buf;\nqueue<int> optimize_posegraph_buf;\nqueue<KeyFrame*> keyframe_buf;\nqueue<RetriveData> retrive_data_buf;\n\nint sum_of_wait = 0;\n\nstd::mutex m_buf;\nstd::mutex m_state;\nstd::mutex i_buf;\nstd::mutex m_loop_drift;\nstd::mutex m_keyframedatabase_resample;\nstd::mutex m_update_visualization;\nstd::mutex m_keyframe_buf;\nstd::mutex m_retrive_data_buf;\n\ndouble latest_time;\nEigen::Vector3d tmp_P;\nEigen::Quaterniond tmp_Q;\nEigen::Vector3d tmp_V;\nEigen::Vector3d tmp_Ba;\nEigen::Vector3d tmp_Bg;\nEigen::Vector3d acc_0;\nEigen::Vector3d gyr_0;\n\nqueue<pair<cv::Mat, double>> image_buf;\nLoopClosure *loop_closure;\nKeyFrameDatabase keyframe_database;\n\nint global_frame_cnt = 0;\n//camera param\ncamodocal::CameraPtr m_camera;\nvector<int> erase_index;\nstd_msgs::Header cur_header;\nEigen::Vector3d relocalize_t{Eigen::Vector3d(0, 0, 0)};\nEigen::Matrix3d relocalize_r{Eigen::Matrix3d::Identity()};\n\nnav_msgs::Path  loop_path;\nvoid updateLoopPath(nav_msgs::Path _loop_path)\n{\n    loop_path = _loop_path;\n}\nvoid ViewCameraPose(Eigen::Vector3d loop_correct_t, Eigen::Matrix3d loop_correct_r, pangolin::OpenGlMatrix &M)\n{\n\tint idx2 = WINDOW_SIZE - 1;\n\tif (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n\t{\n\t\tint i = idx2;\n\t\tVector3d P = (loop_correct_r * estimator.Ps[i] + loop_correct_t) + (loop_correct_r * estimator.Rs[i]) * estimator.tic[0];\n\t\t//Quaterniond R = Quaterniond((loop_correct_r * estimator.Rs[i]) * estimator.ric[0]);\n\t\tEigen::Matrix3d R = (loop_correct_r * estimator.Rs[i]) * estimator.ric[0];\n\n\t\tM.m[0] = R(0,0); \n\t\tM.m[1] = R(1,0); \n\t\tM.m[2] = R(2,0); \n\t\tM.m[3] = 0.0; \n\n\t\tM.m[4] = R(0,1); \n\t\tM.m[5] = R(1,1); \n\t\tM.m[6] = R(2,1); \n\t\tM.m[7] = 0.0; \n\n\t\tM.m[8] = R(0,2); \n\t\tM.m[9] = R(1,2); \n\t\tM.m[10] = R(2,2); \n\t\tM.m[11] = 0.0; \n\t\t\n\t\t\n\t\tM.m[12] = P.x(); \n\t\tM.m[13] = P.y(); \n\t\tM.m[14] = P.z(); \n\t\tM.m[15] = 1.0;\t\n\t//    cout << \"M.m[0]:\" <<M.m[0] << \"M.m[1]:\" << M.m[1] << \"M.m[2]\" << M.m[2] << endl; \t\n\t//    cout << \"M.m[4]:\" <<M.m[4] << \"M.m[5]:\" << M.m[5] << \"M.m[6]\" << M.m[6] << endl; \t\n\t//    cout << \"M.m[8]:\" <<M.m[8] << \"M.m[9]:\" << M.m[9] << \"M.m[10]\" << M.m[10] << endl; \t\n\t  //  cout << \"M.m[12]:\" <<M.m[12] << \"M.m[13]:\" << M.m[13] << \"M.m[14]\" << M.m[14] << endl; \t\n\n\t}\n\telse\n\t\t M.SetIdentity();\n}\nvoid DrawCurrentCamera(pangolin::OpenGlMatrix &Twc)\n{\n\tconst float &w = 0.08f; //mCameraSize;\n\tconst float h = w*0.75;\n\tconst float z = w*0.6;\n\n\tglPushMatrix();\n\n#ifdef HAVE_GLES\n\tglMultMatrixf(Twc.m);\n#else\n\tglMultMatrixd(Twc.m);\n#endif\n\n\tglLineWidth(2);  //set line width\n\tglColor3f(0.0f,0.0f,1.0f);   //blue\n\tglBegin(GL_LINES);           //draw camera \n\tglVertex3f(0,0,0);\n\tglVertex3f(w,h,z);\n\tglVertex3f(0,0,0);\n\tglVertex3f(w,-h,z);\n\tglVertex3f(0,0,0);\n\tglVertex3f(-w,-h,z);\n\tglVertex3f(0,0,0);\n\tglVertex3f(-w,h,z);\n\n\tglVertex3f(w,h,z);\n\tglVertex3f(w,-h,z);\n\tglVertex3f(-w,h,z);\n\tglVertex3f(-w,-h,z);\n\tglVertex3f(-w,h,z);\n\tglVertex3f(w,h,z);\n\tglVertex3f(-w,-h,z);\n\tglVertex3f(w,-h,z);\n\tglEnd();\n\tglPopMatrix();\n}\nvoid visualization()\n{\n\tfloat mViewpointX = -0;\n\tfloat mViewpointY = -5;\n\tfloat mViewpointZ = -10;\n\tfloat mViewpointF = 500;\n\tpangolin::CreateWindowAndBind(\"VINS: Map Visualization\",1024,768); //create a display window\n\tglEnable(GL_DEPTH_TEST); //launch depth test\n\tglEnable(GL_BLEND);      //use blend function\n\tglBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); //set blend alpha value\n\tpangolin::CreatePanel(\"menu\").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175)); //new button and menu\n \tpangolin::Var<bool> menuFollowCamera(\"menu.Follow Camera\", true, true);\n \tpangolin::Var<bool> menuShowPoints(\"menu.Show Points\", true, true);\n \tpangolin::Var<bool> menuShowPath(\"menu.Show Path\", true, true);\n   \t// Define Camera Render Object (for view / scene browsing)\n\tpangolin::OpenGlRenderState s_cam(\n\t\t\t\t    pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),\n\t\t\t\t    pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,VISUALLOOKATX, VISUALLOOKATY, VISUALLOOKATZ)\n\t\t\t\t\t);\n\n// Add named OpenGL viewport to window and provide 3D Handler\n        pangolin::View& d_cam = pangolin::CreateDisplay()\n                    .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)\n\t\t    .SetHandler(new pangolin::Handler3D(s_cam));\n\t\n\tpangolin::OpenGlMatrix Twc;\n\tTwc.SetIdentity();\n\twhile(!pangolin::ShouldQuit() & running_flag)\n\t{\n\t  \n\t\tglClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);\n\t\tViewCameraPose(relocalize_t, relocalize_r, Twc);\n\t        if(menuFollowCamera)\n\t\t   s_cam.Follow(Twc);\n\t\td_cam.Activate(s_cam);\n\t\tglClearColor(0.0f,0.0f,0.0f,0.0f); //背景色设置为白色\n\t\tDrawCurrentCamera(Twc);\t\t\n\t\tif(menuShowPoints)\n\t\t\tkeyframe_database.viewPointClouds();\n\t\tif(menuShowPath)\n\t\t\tkeyframe_database.viewPath();\n\t\tpangolin::FinishFrame();\n\n\t}\n\tcout << \"pangolin thread end\" << endl;\n    view_done = true;\t\n\t//exit(1);\n}\n\n\nvoid predict(const sensor_msgs::ImuConstPtr &imu_msg)\n{\n    double t = imu_msg->header.stamp.toSec();\n    double dt = t - latest_time;\n    latest_time = t;\n\n    double dx = imu_msg->linear_acceleration.x;\n    double dy = imu_msg->linear_acceleration.y;\n    double dz = imu_msg->linear_acceleration.z;\n    Eigen::Vector3d linear_acceleration{dx, dy, dz};\n\n    double rx = imu_msg->angular_velocity.x;\n    double ry = imu_msg->angular_velocity.y;\n    double rz = imu_msg->angular_velocity.z;\n    Eigen::Vector3d angular_velocity{rx, ry, rz};\n\n    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba - tmp_Q.inverse() * estimator.g);\n\n    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;\n    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);\n\n    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba - tmp_Q.inverse() * estimator.g);\n\n    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);\n\n    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;\n    tmp_V = tmp_V + dt * un_acc;\n\n    acc_0 = linear_acceleration;\n    gyr_0 = angular_velocity;\n}\n\nvoid update()\n{\n    TicToc t_predict;\n    latest_time = current_time;\n    tmp_P = relocalize_r * estimator.Ps[WINDOW_SIZE] + relocalize_t;\n    tmp_Q = relocalize_r * estimator.Rs[WINDOW_SIZE];\n    tmp_V = estimator.Vs[WINDOW_SIZE];\n    tmp_Ba = estimator.Bas[WINDOW_SIZE];\n    tmp_Bg = estimator.Bgs[WINDOW_SIZE];\n    acc_0 = estimator.acc_0;\n    gyr_0 = estimator.gyr_0;\n\n    queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;\n    for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())\n        predict(tmp_imu_buf.front());\n\n}\n\nstd::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>\ngetMeasurements()\n{\n    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;\n\n    while (true)\n    {\n      \n        if (imu_buf.empty() || feature_buf.empty())\t   \n            return measurements;\n        if (!(imu_buf.back()->header.stamp > feature_buf.front()->header.stamp))\n        {\n       //     ROS_WARN(\"wait for imu, only should happen at the beginning\");\n\t    cout << \"WARN: wait for imu, only should happen at the beginning\" << endl;\n            sum_of_wait++;\n            return measurements;\n        }\n\n        if (!(imu_buf.front()->header.stamp < feature_buf.front()->header.stamp))\n        {\n        //    ROS_WARN(\"throw img, only should happen at the beginning\");\n\t    cout << \"WARN: throw img, only should happen at the beginning\" << endl;\n            feature_buf.pop();\n            continue;\n        }\n        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();\n        feature_buf.pop();\n\n        std::vector<sensor_msgs::ImuConstPtr> IMUs;\n        while (imu_buf.front()->header.stamp <= img_msg->header.stamp)\n        {\n            IMUs.emplace_back(imu_buf.front());\n            imu_buf.pop();\n        }\n// ROS_INFO_STREAM(\"IMUs end data timestamp: \" << IMUs.back()->header.stamp << \"IMUs size: \"<< IMUs.size() << \"img_msg timestamp\" << img_msg->header.stamp );\n        measurements.emplace_back(IMUs, img_msg);\n    }\n    return measurements;\n}\n\nvoid imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)\n{\n    m_buf.lock();\n    imu_buf.push(imu_msg);\n   // ROS_INFO(\"----------IMU DATA. timestamp %f------------\",imu_msg->header.stamp.toSec());\n    m_buf.unlock();\n  //  con.notify_one();   //remove by solomon\n    \n\n    {\n        std::lock_guard<std::mutex> lg(m_state);\n        predict(imu_msg);\n   //     std_msgs::Header header = imu_msg->header;\n    //    header.frame_id = \"world\";\n     //   if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n      //      pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);\n    }\n}\n\nvoid feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)\n{\n    m_buf.lock();\n    \n    feature_buf.push(feature_msg);\n    //ROS_INFO(\"----------feature timestamp %f------------\",feature_msg->header.stamp.toSec());\n    m_buf.unlock();\n    con.notify_one();\n}\n\nvoid send_imu(const sensor_msgs::ImuConstPtr &imu_msg)\n{\n    double t = imu_msg->header.stamp.toSec();\n    if (current_time < 0)\n        current_time = t;\n    double dt = t - current_time;\n    current_time = t;\n\n    double ba[]{0.0, 0.0, 0.0};\n    double bg[]{0.0, 0.0, 0.0};\n\n    double dx = imu_msg->linear_acceleration.x - ba[0];\n    double dy = imu_msg->linear_acceleration.y - ba[1];\n    double dz = imu_msg->linear_acceleration.z - ba[2];\n\n    double rx = imu_msg->angular_velocity.x - bg[0];\n    double ry = imu_msg->angular_velocity.y - bg[1];\n    double rz = imu_msg->angular_velocity.z - bg[2];\n    //ROS_DEBUG(\"IMU %f, dt: %f, acc: %f %f %f, gyr: %f %f %f\", t, dt, dx, dy, dz, rx, ry, rz);\n\n    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));\n}\n\n//thread:loop detection\nvoid process_loop_detection()\n{\n    if(loop_closure == NULL)\n    {\n        const char *voc_file = VOC_FILE.c_str();\n        TicToc t_load_voc;\n       // ROS_DEBUG(\"loop start loop\");\n\tcout << \"loop start loop\" << endl;\n        cout << \"voc file: \" << voc_file << endl;\n        loop_closure = new LoopClosure(voc_file, IMAGE_COL, IMAGE_ROW);\n     //   ROS_DEBUG(\"loop load vocbulary %lf\", t_load_voc.toc());\n\tcout << \"loop load vocbulary:\"<< t_load_voc.toc() << endl;\n        loop_closure->initCameraModel(CAM_NAMES_ESTIMATOR);  //add\n    }\n\n    while(LOOP_CLOSURE)   \n    {\n        KeyFrame* cur_kf = NULL; \n        m_keyframe_buf.lock();\n        while(!keyframe_buf.empty())\n        {\n            if(cur_kf!=NULL)\n                delete cur_kf;\n            cur_kf = keyframe_buf.front();\n            keyframe_buf.pop();\n        }\n        m_keyframe_buf.unlock();\n        if (cur_kf != NULL)\n        {\n            cur_kf->global_index = global_frame_cnt;\n            m_keyframedatabase_resample.lock();\n            keyframe_database.add(cur_kf);\n            m_keyframedatabase_resample.unlock();\n\n            cv::Mat current_image;\n            current_image = cur_kf->image;   \n\n            bool loop_succ = false;\n            int old_index = -1;\n            vector<cv::Point2f> cur_pts;\n            vector<cv::Point2f> old_pts;\n            TicToc t_brief;\n            cur_kf->extractBrief(current_image);\n            //printf(\"loop extract %d feature using %lf\\n\", cur_kf->keypoints.size(), t_brief.toc());\n            TicToc t_loopdetect;\n            loop_succ = loop_closure->startLoopClosure(cur_kf->keypoints, cur_kf->descriptors, cur_pts, old_pts, old_index);\n            double t_loop = t_loopdetect.toc();\n            //ROS_DEBUG(\"t_loopdetect %f ms\", t_loop);\n\t    // cout << \"t_loopdetect %f ms\" << t_loop << endl;\n            if(loop_succ)\n            {\n                KeyFrame* old_kf = keyframe_database.getKeyframe(old_index);\n                if (old_kf == NULL)\n                {\n                   // ROS_WARN(\"NO such frame in keyframe_database\");\n\t\t  cout << \"WARN: NO such frame in keyframe_database\" << endl;\n                   // ROS_BREAK();\n\t\t  //break;\n\t\t  continue;\n                }\n           //     ROS_DEBUG(\"loop succ %d with %drd image\", global_frame_cnt, old_index);\n\t\t//cout << \"loop succ \" <<global_frame_cnt <<  \" with \" << old_index << \"rd image\" << endl;\n                assert(old_index!=-1);\n                \n                Vector3d T_w_i_old, PnP_T_old;\n                Matrix3d R_w_i_old, PnP_R_old;\n\n                old_kf->getPose(T_w_i_old, R_w_i_old);\n                std::vector<cv::Point2f> measurements_old;\n                std::vector<cv::Point2f> measurements_old_norm;\n                std::vector<cv::Point2f> measurements_cur;\n                std::vector<int> features_id_matched;  \n                cur_kf->findConnectionWithOldFrame(old_kf, measurements_old, measurements_old_norm, PnP_T_old, PnP_R_old, m_camera);\n                measurements_cur = cur_kf->measurements_matched;\n                features_id_matched = cur_kf->features_id_matched;\n                // send loop info to VINS relocalization\n                int loop_fusion = 0;\n                if( (int)measurements_old_norm.size() > MIN_LOOP_NUM && global_frame_cnt - old_index > 35 && old_index > 30)\n                {\n\n                    Quaterniond PnP_Q_old(PnP_R_old);\n                    RetriveData retrive_data;\n                    retrive_data.cur_index = cur_kf->global_index;\n                    retrive_data.header = cur_kf->header;\n                    retrive_data.P_old = T_w_i_old;\n                    retrive_data.R_old = R_w_i_old;\n                    retrive_data.relative_pose = false;\n                    retrive_data.relocalized = false;\n                    retrive_data.measurements = measurements_old_norm;\n                    retrive_data.features_ids = features_id_matched;\n                    retrive_data.loop_pose[0] = PnP_T_old.x();\n                    retrive_data.loop_pose[1] = PnP_T_old.y();\n                    retrive_data.loop_pose[2] = PnP_T_old.z();\n                    retrive_data.loop_pose[3] = PnP_Q_old.x();\n                    retrive_data.loop_pose[4] = PnP_Q_old.y();\n                    retrive_data.loop_pose[5] = PnP_Q_old.z();\n                    retrive_data.loop_pose[6] = PnP_Q_old.w();\n                    m_retrive_data_buf.lock();\n                    retrive_data_buf.push(retrive_data);\n                    m_retrive_data_buf.unlock();\n                    cur_kf->detectLoop(old_index);\n                    old_kf->is_looped = 1;\n                    loop_fusion = 1;\n\n                    m_update_visualization.lock();\n                    keyframe_database.addLoop(old_index);\n                 //   CameraPoseVisualization* posegraph_visualization = keyframe_database.getPosegraphVisualization();\n                //    pubPoseGraph(posegraph_visualization, cur_header);  \n                    m_update_visualization.unlock();\n                }\n\n\n                // visualization loop info\n                if(0 && loop_fusion)\n                {\n                    int COL = current_image.cols;\n                    //int ROW = current_image.rows;\n                    cv::Mat gray_img, loop_match_img;\n                    cv::Mat old_img = old_kf->image;\n                    cv::hconcat(old_img, current_image, gray_img);\n                    cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);\n                    cv::Mat loop_match_img2;\n                    loop_match_img2 = loop_match_img.clone();\n                    /*\n                    for(int i = 0; i< (int)cur_pts.size(); i++)\n                    {\n                        cv::Point2f cur_pt = cur_pts[i];\n                        cur_pt.x += COL;\n                        cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));\n                    }\n                    for(int i = 0; i< (int)old_pts.size(); i++)\n                    {\n                        cv::circle(loop_match_img, old_pts[i], 5, cv::Scalar(0, 255, 0));\n                    }\n                    for (int i = 0; i< (int)old_pts.size(); i++)\n                    {\n                        cv::Point2f cur_pt = cur_pts[i];\n                        cur_pt.x += COL ;\n                        cv::line(loop_match_img, old_pts[i], cur_pt, cv::Scalar(0, 255, 0), 1, 8, 0);\n                    }\n                    ostringstream convert;\n                    convert << \"/home/tony-ws/raw_data/loop_image/\"\n                            << cur_kf->global_index << \"-\" \n                            << old_index << \"-\" << loop_fusion <<\".jpg\";\n                    cv::imwrite( convert.str().c_str(), loop_match_img);\n                    */\n                    for(int i = 0; i< (int)measurements_cur.size(); i++)\n                    {\n                        cv::Point2f cur_pt = measurements_cur[i];\n                        cur_pt.x += COL;\n                        cv::circle(loop_match_img2, cur_pt, 5, cv::Scalar(0, 255, 0));\n                    }\n                    for(int i = 0; i< (int)measurements_old.size(); i++)\n                    {\n                        cv::circle(loop_match_img2, measurements_old[i], 5, cv::Scalar(0, 255, 0));\n                    }\n                    for (int i = 0; i< (int)measurements_old.size(); i++)\n                    {\n                        cv::Point2f cur_pt = measurements_cur[i];\n                        cur_pt.x += COL ;\n                        cv::line(loop_match_img2, measurements_old[i], cur_pt, cv::Scalar(0, 255, 0), 1, 8, 0);\n                    }\n\n                    ostringstream convert2;\n                    convert2 << \"/home/tony-ws/raw_data/loop_image/\"\n                            << cur_kf->global_index << \"-\" \n                            << old_index << \"-\" << loop_fusion <<\"-2.jpg\";\n                    cv::imwrite( convert2.str().c_str(), loop_match_img2);\n                }\n                  \n            }\n            //release memory\n            cur_kf->image.release();\n            global_frame_cnt++;\n\t //   cout << \"---------keyframe_database.size():\" << keyframe_database.size() << endl;\n            if (t_loop > 1000 || keyframe_database.size() > MAX_KEYFRAME_NUM)\n            {\n                m_keyframedatabase_resample.lock();\n                erase_index.clear();\n                keyframe_database.downsample(erase_index);\n                m_keyframedatabase_resample.unlock();\n                if(!erase_index.empty())\n                    loop_closure->eraseIndex(erase_index);\n            }\n        }\n        std::chrono::milliseconds dura(10);\n        std::this_thread::sleep_for(dura);\n    }\n}\n\n//thread: pose_graph optimization\nvoid process_pose_graph()\n{\n    while(true)            \n    {\n        m_posegraph_buf.lock();\n        int index = -1;\n        while (!optimize_posegraph_buf.empty())\n        {\n            index = optimize_posegraph_buf.front();\n            optimize_posegraph_buf.pop();\n        }\n        m_posegraph_buf.unlock();\n        if(index != -1)\n        {\n            Vector3d correct_t = Vector3d::Zero();\n            Matrix3d correct_r = Matrix3d::Identity();\n            TicToc t_posegraph;\n            keyframe_database.optimize4DoFLoopPoseGraph(index,\n                                                    correct_t,\n                                                    correct_r);\n\t    cout << \"t_posegraph \" <<  t_posegraph.toc() << \" ms\"<<endl; \n            m_loop_drift.lock();\n            relocalize_r = correct_r;\n            relocalize_t = correct_t;\n            m_loop_drift.unlock();\n            m_update_visualization.lock();\n            keyframe_database.updateVisualization();\n         //   CameraPoseVisualization* posegraph_visualization = keyframe_database.getPosegraphVisualization();\n            m_update_visualization.unlock();\n            pubOdometry(estimator, cur_header, relocalize_t, relocalize_r);\n        //    pubPoseGraph(posegraph_visualization, cur_header); \n            nav_msgs::Path refine_path = keyframe_database.getPath();\n            updateLoopPath(refine_path);\n        }\n\n        std::chrono::milliseconds dura(5000);\n        std::this_thread::sleep_for(dura);\n    }\n}\n\n// thread: visual-inertial odometry\nvoid process()\n{\n    while (true) \n    {\n        std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;\n        std::unique_lock<std::mutex> lk(m_buf);\n        con.wait(lk, [&]\n                 {\n            return (measurements = getMeasurements()).size() != 0;\n                 });\n        lk.unlock();\n\n        for (auto &measurement : measurements)\n        {\n            for (auto &imu_msg : measurement.first)\n                send_imu(imu_msg);\n\n            auto img_msg = measurement.second;\n      //      ROS_DEBUG(\"processing vision data with stamp %f \\n\", img_msg->header.stamp.toSec());\n\t//    cout << \"processing vision data with stamp \"<<  img_msg->header.stamp.toSec() << endl;\n\n            TicToc t_s;\n            map<int, vector<pair<int, Vector3d>>> image;\n            for (unsigned int i = 0; i < img_msg->points.size(); i++)\n            {\n                int v = img_msg->channels[0].values[i] + 0.5;\n                int feature_id = v / NUM_OF_CAM;\n                int camera_id = v % NUM_OF_CAM;\n                double x = img_msg->points[i].x;\n                double y = img_msg->points[i].y;\n                double z = img_msg->points[i].z;\n                //ROS_ASSERT(z == 1);\n\t\tassert(z == 1);\n                image[feature_id].emplace_back(camera_id, Vector3d(x, y, z));\n            }\n            estimator.processImage(image, img_msg->header);\n            /**\n            *** start build keyframe database for loop closure\n            **/\n            if(LOOP_CLOSURE)\n            {\n                // remove previous loop\n                vector<RetriveData>::iterator it = estimator.retrive_data_vector.begin();\n                for(; it != estimator.retrive_data_vector.end(); )\n                {\n                    if ((*it).header < estimator.Headers[0].stamp.toSec())\n                    {\n                        it = estimator.retrive_data_vector.erase(it);\n                    }\n                    else\n                        it++;\n                }\n                m_retrive_data_buf.lock();\n                while(!retrive_data_buf.empty())\n                {\n                    RetriveData tmp_retrive_data = retrive_data_buf.front();\n                    retrive_data_buf.pop();\n                    estimator.retrive_data_vector.push_back(tmp_retrive_data);\n                }\n                m_retrive_data_buf.unlock();\n                //WINDOW_SIZE - 2 is key frame\n                if(estimator.marginalization_flag == 0 && estimator.solver_flag == estimator.NON_LINEAR)\n                {   \n                    Vector3d vio_T_w_i = estimator.Ps[WINDOW_SIZE - 2];\n                    Matrix3d vio_R_w_i = estimator.Rs[WINDOW_SIZE - 2];\n                    i_buf.lock();\n                    while(!image_buf.empty() && image_buf.front().second < estimator.Headers[WINDOW_SIZE - 2].stamp.toSec())\n                    {\n                        image_buf.pop();\n                    }\n                    i_buf.unlock();\n                    //assert(estimator.Headers[WINDOW_SIZE - 1].stamp.toSec() == image_buf.front().second);\n                    // relative_T   i-1_T_i relative_R  i-1_R_i\n                    cv::Mat KeyFrame_image;\n                    KeyFrame_image = image_buf.front().first;\n                    \n                    const char *pattern_file = PATTERN_FILE.c_str();\n                    Vector3d cur_T;\n                    Matrix3d cur_R;\n                    cur_T = relocalize_r * vio_T_w_i + relocalize_t;\n                    cur_R = relocalize_r * vio_R_w_i;\n                    KeyFrame* keyframe = new KeyFrame(estimator.Headers[WINDOW_SIZE - 2].stamp.toSec(), vio_T_w_i, vio_R_w_i, cur_T, cur_R, image_buf.front().first, pattern_file, relocalize_t, relocalize_r);\n                    keyframe->setExtrinsic(estimator.tic[0], estimator.ric[0]);\n                    keyframe->buildKeyFrameFeatures(estimator, m_camera);\n                    m_keyframe_buf.lock();\n                    keyframe_buf.push(keyframe);\n                    m_keyframe_buf.unlock();\n                    // update loop info\n                    if (!estimator.retrive_data_vector.empty() && estimator.retrive_data_vector[0].relative_pose)\n                    {\n                        if(estimator.Headers[0].stamp.toSec() == estimator.retrive_data_vector[0].header)\n                        {\n                            KeyFrame* cur_kf = keyframe_database.getKeyframe(estimator.retrive_data_vector[0].cur_index);                            \n                            if (abs(estimator.retrive_data_vector[0].relative_yaw) > 30.0 || estimator.retrive_data_vector[0].relative_t.norm() > 20.0)\n                            {\n                           //     ROS_DEBUG(\"Wrong loop\");\n\t\t\t      cout << \"Wrong loop\" <<endl;\n                                cur_kf->removeLoop();\n                            }\n                            else \n                            {\n                                cur_kf->updateLoopConnection( estimator.retrive_data_vector[0].relative_t, \n                                                              estimator.retrive_data_vector[0].relative_q, \n                                                              estimator.retrive_data_vector[0].relative_yaw);\n                                m_posegraph_buf.lock();\n                                optimize_posegraph_buf.push(estimator.retrive_data_vector[0].cur_index);\n                                m_posegraph_buf.unlock();\n                            }\n                        }\n                    }\n                }\n            }\n         //   double whole_t = t_s.toc();\n        //    printStatistics(estimator, whole_t);\n\t    cout << \"position: \" << estimator.Ps[WINDOW_SIZE].transpose() << endl;\n            std_msgs::Header header = img_msg->header;\n            header.frame_id = \"world\";\n            cur_header = header;\n            m_loop_drift.lock();\n            if (estimator.relocalize)\n            {\n                relocalize_t = estimator.relocalize_t;\n                relocalize_r = estimator.relocalize_r;\n            }\n            pubOdometry(estimator, header, relocalize_t, relocalize_r);\n        //    pubKeyPoses(estimator, header, relocalize_t, relocalize_r);\n        //    pubCameraPose(estimator, header, relocalize_t, relocalize_r);\n        //    pubPointCloud(estimator, header, relocalize_t, relocalize_r);\n       //     pubTF(estimator, header, relocalize_t, relocalize_r);\n            m_loop_drift.unlock();\n            //ROS_ERROR(\"end: %f, at %f\", img_msg->header.stamp.toSec(), ros::Time::now().toSec());\n        }\n        m_buf.lock();\n        m_state.lock();\n        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n            update();\n        m_state.unlock();\n        m_buf.unlock();\n    }\n}\n\n\nvoid img_callback(const cv::Mat &show_img, const ros::Time &timestamp)\n{\n    if(LOOP_CLOSURE)\n    {\n        i_buf.lock();\n        image_buf.push(make_pair(show_img, timestamp.toSec()));\n        i_buf.unlock();\n    }\n    if(first_image_flag)\n    {\n        first_image_flag = false;\n        first_image_time = timestamp.toSec();\n    }\n\n    // frequency control\n    if (round(1.0 * pub_count / (timestamp.toSec() - first_image_time)) <= FREQ)\n    {\n        PUB_THIS_FRAME = true;\n        // reset the frequency control\n        if (abs(1.0 * pub_count / (timestamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)\n        {\n            first_image_time = timestamp.toSec();\n            pub_count = 0;\n        }\n    }\n    else\n        PUB_THIS_FRAME = false;\n\n  //  cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);\n  //  cv::Mat show_img = ptr->image;\n    TicToc t_r;\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n       // ROS_DEBUG(\"processing camera %d\", i);\n        if (i != 1 || !STEREO_TRACK)\n            //trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)));\n\t  trackerData[i].readImage(show_img.rowRange(ROW * i, ROW * (i + 1)));\n        else\n        {\n            if (EQUALIZE)\n            {\n                cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();\n               // clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);\n\t\tclahe->apply(show_img.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);\n            }\n            else\n              //  trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));\n\t      trackerData[i].cur_img = show_img.rowRange(ROW * i, ROW * (i + 1));\n        }\n\n#if SHOW_UNDISTORTION\n        trackerData[i].showUndistortion(\"undistrotion_\" + std::to_string(i));\n#endif\n    }\n\n    if ( PUB_THIS_FRAME && STEREO_TRACK && trackerData[0].cur_pts.size() > 0)\n    {\n        pub_count++;\n        r_status.clear();\n        r_err.clear();\n        TicToc t_o;\n        cv::calcOpticalFlowPyrLK(trackerData[0].cur_img, trackerData[1].cur_img, trackerData[0].cur_pts, trackerData[1].cur_pts, r_status, r_err, cv::Size(21, 21), 3);\n   //     ROS_DEBUG(\"spatial optical flow costs: %fms\", t_o.toc());\n        vector<cv::Point2f> ll, rr;\n        vector<int> idx;\n        for (unsigned int i = 0; i < r_status.size(); i++)\n        {\n            if (!inBorder(trackerData[1].cur_pts[i]))\n                r_status[i] = 0;\n\n            if (r_status[i])\n            {\n                idx.push_back(i);\n\n                Eigen::Vector3d tmp_p;\n                trackerData[0].m_camera->liftProjective(Eigen::Vector2d(trackerData[0].cur_pts[i].x, trackerData[0].cur_pts[i].y), tmp_p);\n                tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n                tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n                ll.push_back(cv::Point2f(tmp_p.x(), tmp_p.y()));\n\n                trackerData[1].m_camera->liftProjective(Eigen::Vector2d(trackerData[1].cur_pts[i].x, trackerData[1].cur_pts[i].y), tmp_p);\n                tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n                tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n                rr.push_back(cv::Point2f(tmp_p.x(), tmp_p.y()));\n            }\n        }\n        if (ll.size() >= 8)\n        {\n            vector<uchar> status;\n            TicToc t_f;\n            cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 1.0, 0.5, status);\n         //   ROS_DEBUG(\"find f cost: %f\", t_f.toc());\n            int r_cnt = 0;\n            for (unsigned int i = 0; i < status.size(); i++)\n            {\n                if (status[i] == 0)\n                    r_status[idx[i]] = 0;\n                r_cnt += r_status[idx[i]];\n            }\n        }\n    }\n\n    for (unsigned int i = 0;; i++)\n    {\n        bool completed = false;\n        for (int j = 0; j < NUM_OF_CAM; j++)\n            if (j != 1 || !STEREO_TRACK)\n                completed |= trackerData[j].updateID(i);\n        if (!completed)\n            break;\n    }\n\n   if (PUB_THIS_FRAME)\n   {\n        pub_count++;\n        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);\n        sensor_msgs::ChannelFloat32 id_of_point;\n        sensor_msgs::ChannelFloat32 u_of_point;\n        sensor_msgs::ChannelFloat32 v_of_point;\n\n      //  feature_points->header = img_msg->header;\n\tfeature_points->header.stamp = timestamp; //here need to double check,because of missing seq variable assignment\n        feature_points->header.frame_id = \"world\";\n\n        vector<set<int>> hash_ids(NUM_OF_CAM);\n        for (int i = 0; i < NUM_OF_CAM; i++)\n        {\n            if (i != 1 || !STEREO_TRACK)\n            {\n                auto un_pts = trackerData[i].undistortedPoints();\n                auto &cur_pts = trackerData[i].cur_pts;\n                auto &ids = trackerData[i].ids;\n                for (unsigned int j = 0; j < ids.size(); j++)\n                {\n                    int p_id = ids[j];\n                    hash_ids[i].insert(p_id);\n                    geometry_msgs::Point32 p;\n                    p.x = un_pts[j].x;\n                    p.y = un_pts[j].y;\n                    p.z = 1;\n\n                    feature_points->points.push_back(p);\n                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);\n                    u_of_point.values.push_back(cur_pts[j].x);\n                    v_of_point.values.push_back(cur_pts[j].y);\n                 //   ROS_ASSERT(inBorder(cur_pts[j]));\n\t\t    assert(inBorder(cur_pts[j]));\n                }\n            }\n            else if (STEREO_TRACK)\n            {\n                auto r_un_pts = trackerData[1].undistortedPoints();\n                auto &ids = trackerData[0].ids;\n                for (unsigned int j = 0; j < ids.size(); j++)\n                {\n                    if (r_status[j])\n                    {\n                        int p_id = ids[j];\n                        hash_ids[i].insert(p_id);\n                        geometry_msgs::Point32 p;\n                        p.x = r_un_pts[j].x;\n                        p.y = r_un_pts[j].y;\n                        p.z = 1;\n\n                        feature_points->points.push_back(p);\n                        id_of_point.values.push_back(p_id * NUM_OF_CAM + i);\n                    }\n                }\n            }\n        }\n        feature_points->channels.push_back(id_of_point);\n        feature_points->channels.push_back(u_of_point);\n        feature_points->channels.push_back(v_of_point);\n        feature_callback(feature_points);          //add\n\n\t\t/*----------------add ui ---------------------*/\n        cv::Mat tmp_img = show_img.rowRange(0, ROW);\n        cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);\n        for (unsigned int j = 0; j < trackerData[0].cur_pts.size(); j++)\n        {\n            double len = std::min(1.0, 1.0 * trackerData[0].track_cnt[j] / WINDOW_SIZE_FEATURE_TRACKER);\n            cv::circle(tmp_img, trackerData[0].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);\n\t\t}\n\t\tcv::namedWindow(\"vins\", CV_WINDOW_NORMAL); \n        cv::imshow(\"vins\", tmp_img);\n        cv::waitKey(5);\n\t\t/*----------------add ui ---------------------*/\n    }\n  //  ROS_INFO(\"whole feature tracker processing costs: %f\", t_r.toc());\n   \n}\n/******************* load image begin ***********************/\nvoid LoadImages(const string &strImagePath, const string &strTimesStampsPath,\n\t\tvector<string> &strImagesFileNames, vector<double> &timeStamps)\n{\n    ifstream fTimes;\n    fTimes.open(strTimesStampsPath.c_str());\n    timeStamps.reserve(5000); //reserve vector space\n    strImagesFileNames.reserve(5000); \n    while(!fTimes.eof())\n    {\n\tstring s;\n\tgetline(fTimes,s);\n\tif(!s.empty())\n\t{\n\t    stringstream ss;\n\t    ss << s;\n\t    strImagesFileNames.push_back(strImagePath + \"/\" + ss.str() + \".png\");\n\t    double t;\n\t    ss >> t;\n\t    timeStamps.push_back(t/1e9);\n\t}\n    }\n}\n/******************* load image end ***********************/\n\n/******************* load IMU begin ***********************/\n\nvoid LoadImus(ifstream & fImus, const ros::Time &imageTimestamp)\n{\n\n    while(!fImus.eof())\n    {\n\tstring s;\n\tgetline(fImus,s);\n\tif(!s.empty())\n\t{\n\t   char c = s.at(0);\n \t   if(c<'0' || c>'9')      //remove first line in data.csv\n\t\t       continue;       \n\t    stringstream ss;\n\t    ss << s;\n\t    double tmpd;\n\t    int cnt=0;\n\t    double data[7];\n\t    while(ss >> tmpd)\n\t    {\n\t\tdata[cnt] = tmpd;\n\t\tcnt++;\n\t\tif(cnt ==7)\n\t\t  break;\n\t\tif(ss.peek() == ',' || ss.peek() == ' ')\n\t\t  ss.ignore();\n\t    }\n\t    data[0] *=1e-9; //convert to second unit\n\t    sensor_msgs::ImuPtr imudata(new sensor_msgs::Imu);\n\t    imudata->angular_velocity.x = data[1];\n\t    imudata->angular_velocity.y = data[2];\n\t    imudata->angular_velocity.z = data[3];\n\t    imudata->linear_acceleration.x = data[4];\n\t    imudata->linear_acceleration.y = data[5];\n\t    imudata->linear_acceleration.z = data[6];\n\t\tuint32_t  sec = data[0];\n\t\tuint32_t nsec = (data[0]-sec)*1e9;\n\t\tnsec = (nsec/1000)*1000+500;\n\t    imudata->header.stamp = ros::Time(sec,nsec);\n\t    imu_callback(imudata);\n\t    if (imudata->header.stamp > imageTimestamp)       //load all imu data produced in interval time between two consecutive frams \n\t      break;\n\t}\n    }\n}\n/******************* load IMU end ***********************/\n\nint main(int argc, char **argv)\n{\n  /******************* load image begin ***********************/\n    if(argc != 5)\n    {\n\tcerr << endl << \"Usage: ./vins_estimator path_to_setting_file path_to_image_folder path_to_times_file path_to_imu_data_file\" <<endl;\n\treturn 1;\n    }\n    \n    //imu data file \n    ifstream fImus;\n    fImus.open(argv[4]);\n    \n    cv::Mat image;\n    int ni;//num image\n    \n    //read parameters section\n    readParameters(argv[1]);\n    \n    estimator.setParameter();\n    for (int i = 0; i < NUM_OF_CAM; i++)\n        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); //add\n    \n\tvector<string> vStrImagesFileNames;\n    vector<double> vTimeStamps;\n    LoadImages(string(argv[2]),string(argv[3]),vStrImagesFileNames,vTimeStamps);\n    \n    int imageNum = vStrImagesFileNames.size();\n    \n    if(imageNum<=0)\n    {\n\tcerr << \"ERROR: Failed to load images\" << endl;\n\treturn 1;\n    }\n    \n    std::thread measurement_process{process};\n    \n     measurement_process.detach();\n   \n    std::thread loop_detection, pose_graph;\n    if (LOOP_CLOSURE)\n     {     \n\tloop_detection = std::thread(process_loop_detection);   \n\tpose_graph = std::thread(process_pose_graph);\n\tstd::thread visualization_thread{visualization}; //visualization thread\n\tloop_detection.detach();\n\tpose_graph.detach();\n        visualization_thread.detach();\n\t//loop_detection.join();\n\t//pose_graph.join();\n\tm_camera = CameraFactory::instance()->generateCameraFromYamlFile(CAM_NAMES_ESTIMATOR);\n     }\n    for(ni=0; ni<imageNum; ni++)\n    {\n      \n      double  tframe = vTimeStamps[ni];   //timestamp\n\t  uint32_t  sec = tframe;\n      uint32_t nsec = (tframe-sec)*1e9;\n      nsec = (nsec/1000)*1000+500;\n      ros::Time image_timestamp = ros::Time(sec, nsec);\n       // read imu data\n      LoadImus(fImus,image_timestamp);\n       \n\t//read image from file\n      image = cv::imread(vStrImagesFileNames[ni],CV_LOAD_IMAGE_UNCHANGED);\n      \n      if(image.empty())\n      {\n\t  cerr << endl << \"Failed to load image: \" << vStrImagesFileNames[ni] <<endl;\n\t  return 1;\n      }\n      std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();\n      img_callback(image, image_timestamp);\n      std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();\n      double timeSpent =std::chrono::duration_cast<std::chrono::duration<double>>(t2-t1).count();\n      \n      //wait to load the next frame image\n      double T=0;\n      if(ni < imageNum-1)\n\tT = vTimeStamps[ni+1]-tframe; //interval time between two consecutive frames,unit:second\n      else if(ni>0)    //lastest frame\n\tT = tframe-vTimeStamps[ni-1];\n      \n      if(timeSpent < T)\n\tusleep((T-timeSpent)*1e6); //sec->us:1e6\n      else\n\tcerr << endl << \"process image speed too slow, larger than interval time between two consecutive frames\" << endl;\n      \n    }\n\trunning_flag = false;\n\twhile(!view_done)     //main thread wait view thread used its data structure\n\t\tusleep(5000);\n/******************* load image end ***********************/\n    return 0;\n}\n"
  },
  {
    "path": "vins_estimator/src/factor/imu_factor.h",
    "content": "#pragma once\n//#include <ros/assert.h>\n#include <iostream>\n#include <eigen3/Eigen/Dense>\n\n#include \"../utility/utility.h\"\n#include \"../parameters.h\"\n#include \"integration_base.h\"\n\n#include <ceres/ceres.h>\n\nclass IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    IMUFactor() = delete;\n    IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration)\n    {\n    }\n    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const\n    {\n\n        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n        Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);\n        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);\n        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);\n\n        Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);\n        Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n\n        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);\n        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);\n        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);\n\n//Eigen::Matrix<double, 15, 15> Fd;\n//Eigen::Matrix<double, 15, 12> Gd;\n\n//Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;\n//Eigen::Quaterniond pQj = Qi * delta_q;\n//Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;\n//Eigen::Vector3d pBaj = Bai;\n//Eigen::Vector3d pBgj = Bgi;\n\n//Vi + Qi * delta_v - g * sum_dt = Vj;\n//Qi * delta_q = Qj;\n\n//delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);\n//delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);\n//delta_q = Qi.inverse() * Qj;\n\n#if 0\n        if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||\n            (Bgi - pre_integration->linearized_bg).norm() > 0.01)\n        {\n            pre_integration->repropagate(Bai, Bgi);\n        }\n#endif\n\n        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);\n        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,\n                                            Pj, Qj, Vj, Baj, Bgj);\n\n        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();\n        //sqrt_info.setIdentity();\n        residual = sqrt_info * residual;\n\n        if (jacobians)\n        {\n            double sum_dt = pre_integration->sum_dt;\n            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);\n            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);\n\n            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);\n\n            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);\n            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);\n\n            if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)\n            {\n               // ROS_WARN(\"numerical unstable in preintegration\");\n                //std::cout << pre_integration->jacobian << std::endl;\n///                ROS_BREAK();\n            }\n\n            if (jacobians[0])\n            {\n                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);\n                jacobian_pose_i.setZero();\n\n                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();\n                jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));\n\n#if 0\n            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();\n#else\n                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();\n#endif\n\n                jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));\n\n                jacobian_pose_i = sqrt_info * jacobian_pose_i;\n\n                if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)\n                {\n                  //  ROS_WARN(\"numerical unstable in preintegration\");\n\t\t  cout << \"WARN: numerical unstable in preintegration\" << endl;\n                    //std::cout << sqrt_info << std::endl;\n                 //   ROS_BREAK();\n\t\t  return false;\n                }\n            }\n            if (jacobians[1])\n            {\n                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);\n                jacobian_speedbias_i.setZero();\n                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;\n                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;\n                jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;\n\n#if 0\n            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;\n#else\n                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;\n#endif\n\n                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();\n                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;\n                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;\n\n                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();\n\n                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();\n\n                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;\n\n            //    ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);\n            //    ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);\n\t\tassert(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);\n\t\tassert(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);\n            }\n            if (jacobians[2])\n            {\n                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);\n                jacobian_pose_j.setZero();\n\n                jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();\n\n#if 0\n            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();\n#else\n                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));\n                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();\n#endif\n\n                jacobian_pose_j = sqrt_info * jacobian_pose_j;\n\n             //   ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);\n             //   ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);\n\t\tassert(fabs(jacobian_pose_j.maxCoeff()) < 1e8);\n\t\tassert(fabs(jacobian_pose_j.minCoeff()) < 1e8);\n            }\n            if (jacobians[3])\n            {\n                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);\n                jacobian_speedbias_j.setZero();\n\n                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();\n\n                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();\n\n                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();\n\n                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;\n\n               // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);\n               // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);\n\t\tassert(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);\n\t\tassert(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);\n            }\n        }\n\n        return true;\n    }\n\n    //bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix<double, 15, 1> &residuals, Eigen::Matrix<double, 15, 30> &jacobians);\n\n    //void checkCorrection();\n    //void checkTransition();\n    //void checkJacobian(double **parameters);\n    IntegrationBase* pre_integration;\n\n};\n\n"
  },
  {
    "path": "vins_estimator/src/factor/integration_base.h",
    "content": "#pragma once\n\n#include \"../utility/utility.h\"\n#include \"../parameters.h\"\n\n#include <ceres/ceres.h>\nusing namespace Eigen;\n\nclass IntegrationBase\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    IntegrationBase() = delete;\n    IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,\n                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)\n        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},\n          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},\n            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},\n          sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}\n\n    {\n        noise = Eigen::Matrix<double, 18, 18>::Zero();\n        noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();\n        noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();\n        noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();\n        noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();\n        noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();\n        noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();\n    }\n\n    void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)\n    {\n        dt_buf.push_back(dt);\n        acc_buf.push_back(acc);\n        gyr_buf.push_back(gyr);\n        propagate(dt, acc, gyr);\n    }\n\n    void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)\n    {\n        sum_dt = 0.0;\n        acc_0 = linearized_acc;\n        gyr_0 = linearized_gyr;\n        delta_p.setZero();\n        delta_q.setIdentity();\n        delta_v.setZero();\n        linearized_ba = _linearized_ba;\n        linearized_bg = _linearized_bg;\n        jacobian.setIdentity();\n        covariance.setZero();\n        for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)\n            propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);\n    }\n\n    void midPointIntegration(double _dt, \n                            const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,\n                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,\n                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,\n                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,\n                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,\n                            Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)\n    {\n        //ROS_INFO(\"midpoint integration\");\n        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);\n        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;\n        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);\n        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);\n        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);\n        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;\n        result_delta_v = delta_v + un_acc * _dt;\n        result_linearized_ba = linearized_ba;\n        result_linearized_bg = linearized_bg;         \n\n        if(update_jacobian)\n        {\n            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;\n            Vector3d a_0_x = _acc_0 - linearized_ba;\n            Vector3d a_1_x = _acc_1 - linearized_ba;\n            Matrix3d R_w_x, R_a_0_x, R_a_1_x;\n\n            R_w_x<<0, -w_x(2), w_x(1),\n                w_x(2), 0, -w_x(0),\n                -w_x(1), w_x(0), 0;\n            R_a_0_x<<0, -a_0_x(2), a_0_x(1),\n                a_0_x(2), 0, -a_0_x(0),\n                -a_0_x(1), a_0_x(0), 0;\n            R_a_1_x<<0, -a_1_x(2), a_1_x(1),\n                a_1_x(2), 0, -a_1_x(0),\n                -a_1_x(1), a_1_x(0), 0;\n\n            MatrixXd F = MatrixXd::Zero(15, 15);\n            F.block<3, 3>(0, 0) = Matrix3d::Identity();\n            F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + \n                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;\n            F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;\n            F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;\n            F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;\n            F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;\n            F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;\n            F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + \n                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;\n            F.block<3, 3>(6, 6) = Matrix3d::Identity();\n            F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;\n            F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;\n            F.block<3, 3>(9, 9) = Matrix3d::Identity();\n            F.block<3, 3>(12, 12) = Matrix3d::Identity();\n            //cout<<\"A\"<<endl<<A<<endl;\n\n            MatrixXd V = MatrixXd::Zero(15,18);\n            V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;\n            V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;\n            V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;\n            V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);\n            V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;\n            V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;\n            V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;\n            V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;\n            V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;\n            V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);\n            V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;\n            V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;\n\n            //step_jacobian = F;\n            //step_V = V;\n            jacobian = F * jacobian;\n            covariance = F * covariance * F.transpose() + V * noise * V.transpose();\n        }\n\n    }\n\n    void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)\n    {\n        dt = _dt;\n        acc_1 = _acc_1;\n        gyr_1 = _gyr_1;\n        Vector3d result_delta_p;\n        Quaterniond result_delta_q;\n        Vector3d result_delta_v;\n        Vector3d result_linearized_ba;\n        Vector3d result_linearized_bg;\n\n        midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            result_delta_p, result_delta_q, result_delta_v,\n                            result_linearized_ba, result_linearized_bg, 1);\n\n        //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,\n        //                    linearized_ba, linearized_bg);\n        delta_p = result_delta_p;\n        delta_q = result_delta_q;\n        delta_v = result_delta_v;\n        linearized_ba = result_linearized_ba;\n        linearized_bg = result_linearized_bg;\n        delta_q.normalize();\n        sum_dt += dt;\n        acc_0 = acc_1;\n        gyr_0 = gyr_1;  \n     \n    }\n\n    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,\n                                          const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)\n    {\n        Eigen::Matrix<double, 15, 1> residuals;\n\n        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);\n        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);\n\n        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);\n\n        Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);\n        Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);\n\n        Eigen::Vector3d dba = Bai - linearized_ba;\n        Eigen::Vector3d dbg = Bgi - linearized_bg;\n\n        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);\n        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;\n        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;\n\n        residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;\n        residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();\n        residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;\n        residuals.block<3, 1>(O_BA, 0) = Baj - Bai;\n        residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;\n        return residuals;\n    }\n\n    double dt;\n    Eigen::Vector3d acc_0, gyr_0;\n    Eigen::Vector3d acc_1, gyr_1;\n\n    const Eigen::Vector3d linearized_acc, linearized_gyr;\n    Eigen::Vector3d linearized_ba, linearized_bg;\n\n    Eigen::Matrix<double, 15, 15> jacobian, covariance;\n    Eigen::Matrix<double, 15, 15> step_jacobian;\n    Eigen::Matrix<double, 15, 18> step_V;\n    Eigen::Matrix<double, 18, 18> noise;\n\n    double sum_dt;\n    Eigen::Vector3d delta_p;\n    Eigen::Quaterniond delta_q;\n    Eigen::Vector3d delta_v;\n\n    std::vector<double> dt_buf;\n    std::vector<Eigen::Vector3d> acc_buf;\n    std::vector<Eigen::Vector3d> gyr_buf;\n\n};\n/*\n\n    void eulerIntegration(double _dt, const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,\n                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,\n                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,\n                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,\n                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,\n                            Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)\n    {\n        result_delta_p = delta_p + delta_v * _dt + 0.5 * (delta_q * (_acc_1 - linearized_ba)) * _dt * _dt;\n        result_delta_v = delta_v + delta_q * (_acc_1 - linearized_ba) * _dt;\n        Vector3d omg = _gyr_1 - linearized_bg;\n        omg = omg * _dt / 2;\n        Quaterniond dR(1, omg(0), omg(1), omg(2));\n        result_delta_q = (delta_q * dR);   \n        result_linearized_ba = linearized_ba;\n        result_linearized_bg = linearized_bg;         \n\n        if(update_jacobian)\n        {\n            Vector3d w_x = _gyr_1 - linearized_bg;\n            Vector3d a_x = _acc_1 - linearized_ba;\n            Matrix3d R_w_x, R_a_x;\n\n            R_w_x<<0, -w_x(2), w_x(1),\n                w_x(2), 0, -w_x(0),\n                -w_x(1), w_x(0), 0;\n            R_a_x<<0, -a_x(2), a_x(1),\n                a_x(2), 0, -a_x(0),\n                -a_x(1), a_x(0), 0;\n\n            MatrixXd A = MatrixXd::Zero(15, 15);\n            // one step euler 0.5\n            A.block<3, 3>(0, 3) = 0.5 * (-1 * delta_q.toRotationMatrix()) * R_a_x * _dt;\n            A.block<3, 3>(0, 6) = MatrixXd::Identity(3,3);\n            A.block<3, 3>(0, 9) = 0.5 * (-1 * delta_q.toRotationMatrix()) * _dt;\n            A.block<3, 3>(3, 3) = -R_w_x;\n            A.block<3, 3>(3, 12) = -1 * MatrixXd::Identity(3,3);\n            A.block<3, 3>(6, 3) = (-1 * delta_q.toRotationMatrix()) * R_a_x;\n            A.block<3, 3>(6, 9) = (-1 * delta_q.toRotationMatrix());\n            //cout<<\"A\"<<endl<<A<<endl;\n\n            MatrixXd U = MatrixXd::Zero(15,12);\n            U.block<3, 3>(0, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;\n            U.block<3, 3>(3, 3) =  MatrixXd::Identity(3,3);\n            U.block<3, 3>(6, 0) =  delta_q.toRotationMatrix();\n            U.block<3, 3>(9, 6) = MatrixXd::Identity(3,3);\n            U.block<3, 3>(12, 9) = MatrixXd::Identity(3,3);\n\n            // put outside\n            Eigen::Matrix<double, 12, 12> noise = Eigen::Matrix<double, 12, 12>::Zero();\n            noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();\n            noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();\n            noise.block<3, 3>(6, 6) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();\n            noise.block<3, 3>(9, 9) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();\n\n            //write F directly\n            MatrixXd F, V;\n            F = (MatrixXd::Identity(15,15) + _dt * A);\n            V = _dt * U;\n            step_jacobian = F;\n            step_V = V;\n            jacobian = F * jacobian;\n            covariance = F * covariance * F.transpose() + V * noise * V.transpose();\n        }\n\n    }     \n\n\n    void checkJacobian(double _dt, const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, \n                                   const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,\n                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,\n                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg)\n    {\n        Vector3d result_delta_p;\n        Quaterniond result_delta_q;\n        Vector3d result_delta_v;\n        Vector3d result_linearized_ba;\n        Vector3d result_linearized_bg;\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            result_delta_p, result_delta_q, result_delta_v,\n                            result_linearized_ba, result_linearized_bg, 0);\n\n        Vector3d turb_delta_p;\n        Quaterniond turb_delta_q;\n        Vector3d turb_delta_v;\n        Vector3d turb_linearized_ba;\n        Vector3d turb_linearized_bg;\n\n        Vector3d turb(0.0001, -0.003, 0.003);\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p + turb, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb p       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_jacobian.block<3, 3>(0, 0) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_jacobian.block<3, 3>(3, 0) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_jacobian.block<3, 3>(6, 0) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_jacobian.block<3, 3>(9, 0) * turb).transpose() << endl;\n        cout << \"bg diff \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff \" << (step_jacobian.block<3, 3>(12, 0) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q * Quaterniond(1, turb(0) / 2, turb(1) / 2, turb(2) / 2), delta_v,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb q       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_jacobian.block<3, 3>(0, 3) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_jacobian.block<3, 3>(3, 3) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_jacobian.block<3, 3>(6, 3) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_jacobian.block<3, 3>(9, 3) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_jacobian.block<3, 3>(12, 3) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v + turb,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb v       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_jacobian.block<3, 3>(0, 6) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_jacobian.block<3, 3>(3, 6) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_jacobian.block<3, 3>(6, 6) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_jacobian.block<3, 3>(9, 6) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_jacobian.block<3, 3>(12, 6) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba + turb, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb ba       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_jacobian.block<3, 3>(0, 9) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_jacobian.block<3, 3>(3, 9) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_jacobian.block<3, 3>(6, 9) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_jacobian.block<3, 3>(9, 9) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_jacobian.block<3, 3>(12, 9) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg + turb,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb bg       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_jacobian.block<3, 3>(0, 12) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_jacobian.block<3, 3>(3, 12) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_jacobian.block<3, 3>(6, 12) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_jacobian.block<3, 3>(9, 12) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_jacobian.block<3, 3>(12, 12) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0 + turb, _gyr_0, _acc_1 , _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb acc_0       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_V.block<3, 3>(0, 0) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_V.block<3, 3>(3, 0) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_V.block<3, 3>(6, 0) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_V.block<3, 3>(9, 0) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_V.block<3, 3>(12, 0) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0 + turb, _acc_1 , _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb _gyr_0       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_V.block<3, 3>(0, 3) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_V.block<3, 3>(3, 3) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_V.block<3, 3>(6, 3) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_V.block<3, 3>(9, 3) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_V.block<3, 3>(12, 3) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1 + turb, _gyr_1, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb acc_1       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_V.block<3, 3>(0, 6) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_V.block<3, 3>(3, 6) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_V.block<3, 3>(6, 6) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_V.block<3, 3>(9, 6) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_V.block<3, 3>(12, 6) * turb).transpose() << endl;\n\n        midPointIntegration(_dt, _acc_0, _gyr_0, _acc_1 , _gyr_1 + turb, delta_p, delta_q, delta_v,\n                            linearized_ba, linearized_bg,\n                            turb_delta_p, turb_delta_q, turb_delta_v,\n                            turb_linearized_ba, turb_linearized_bg, 0);\n        cout << \"turb _gyr_1       \" << endl;\n        cout << \"p diff       \" << (turb_delta_p - result_delta_p).transpose() << endl;\n        cout << \"p jacob diff \" << (step_V.block<3, 3>(0, 9) * turb).transpose() << endl;\n        cout << \"q diff       \" << ((result_delta_q.inverse() * turb_delta_q).vec() * 2).transpose() << endl;\n        cout << \"q jacob diff \" << (step_V.block<3, 3>(3, 9) * turb).transpose() << endl;\n        cout << \"v diff       \" << (turb_delta_v - result_delta_v).transpose() << endl;\n        cout << \"v jacob diff \" << (step_V.block<3, 3>(6, 9) * turb).transpose() << endl;\n        cout << \"ba diff      \" << (turb_linearized_ba - result_linearized_ba).transpose() << endl;\n        cout << \"ba jacob diff\" << (step_V.block<3, 3>(9, 9) * turb).transpose() << endl;\n        cout << \"bg diff      \" << (turb_linearized_bg - result_linearized_bg).transpose() << endl;\n        cout << \"bg jacob diff\" << (step_V.block<3, 3>(12, 9) * turb).transpose() << endl;\n    }\n    */\n"
  },
  {
    "path": "vins_estimator/src/factor/marginalization_factor.cpp",
    "content": "#include \"marginalization_factor.h\"\nusing namespace std;\nvoid ResidualBlockInfo::Evaluate()\n{\n    residuals.resize(cost_function->num_residuals());\n\n    std::vector<int> block_sizes = cost_function->parameter_block_sizes();\n    raw_jacobians = new double *[block_sizes.size()];\n    jacobians.resize(block_sizes.size());\n\n    for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)\n    {\n        jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);\n        raw_jacobians[i] = jacobians[i].data();\n        //dim += block_sizes[i] == 7 ? 6 : block_sizes[i];\n    }\n    cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);\n\n    //std::vector<int> tmp_idx(block_sizes.size());\n    //Eigen::MatrixXd tmp(dim, dim);\n    //for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)\n    //{\n    //    int size_i = localSize(block_sizes[i]);\n    //    Eigen::MatrixXd jacobian_i = jacobians[i].leftCols(size_i);\n    //    for (int j = 0, sub_idx = 0; j < static_cast<int>(parameter_blocks.size()); sub_idx += block_sizes[j] == 7 ? 6 : block_sizes[j], j++)\n    //    {\n    //        int size_j = localSize(block_sizes[j]);\n    //        Eigen::MatrixXd jacobian_j = jacobians[j].leftCols(size_j);\n    //        tmp_idx[j] = sub_idx;\n    //        tmp.block(tmp_idx[i], tmp_idx[j], size_i, size_j) = jacobian_i.transpose() * jacobian_j;\n    //    }\n    //}\n    //Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(tmp);\n    //std::cout << saes.eigenvalues() << std::endl;\n    //ROS_ASSERT(saes.eigenvalues().minCoeff() >= -1e-6);\n\n    if (loss_function)\n    {\n        double residual_scaling_, alpha_sq_norm_;\n\n        double sq_norm, rho[3];\n\n        sq_norm = residuals.squaredNorm();\n        loss_function->Evaluate(sq_norm, rho);\n        //printf(\"sq_norm: %f, rho[0]: %f, rho[1]: %f, rho[2]: %f\\n\", sq_norm, rho[0], rho[1], rho[2]);\n\n        double sqrt_rho1_ = sqrt(rho[1]);\n\n        if ((sq_norm == 0.0) || (rho[2] <= 0.0))\n        {\n            residual_scaling_ = sqrt_rho1_;\n            alpha_sq_norm_ = 0.0;\n        }\n        else\n        {\n            const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];\n            const double alpha = 1.0 - sqrt(D);\n            residual_scaling_ = sqrt_rho1_ / (1 - alpha);\n            alpha_sq_norm_ = alpha / sq_norm;\n        }\n\n        for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)\n        {\n            jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));\n        }\n\n        residuals *= residual_scaling_;\n    }\n}\n\nMarginalizationInfo::~MarginalizationInfo()\n{\n    //ROS_WARN(\"release marginlizationinfo\");\n    \n    for (auto it = parameter_block_data.begin(); it != parameter_block_data.end(); ++it)\n        delete it->second;\n\n    for (int i = 0; i < (int)factors.size(); i++)\n    {\n\n        delete[] factors[i]->raw_jacobians;\n        \n        delete factors[i]->cost_function;\n\n        delete factors[i];\n    }\n}\n\nvoid MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)\n{\n    factors.emplace_back(residual_block_info);\n\n    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;\n    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();\n\n    for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)\n    {\n        double *addr = parameter_blocks[i];\n        int size = parameter_block_sizes[i];\n        parameter_block_size[reinterpret_cast<long>(addr)] = size;\n    }\n\n    for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)\n    {\n        double *addr = parameter_blocks[residual_block_info->drop_set[i]];\n        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;\n    }\n}\n\nvoid MarginalizationInfo::preMarginalize()\n{\n    for (auto it : factors)\n    {\n        it->Evaluate();\n\n        std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();\n        for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)\n        {\n            long addr = reinterpret_cast<long>(it->parameter_blocks[i]);\n            int size = block_sizes[i];\n            if (parameter_block_data.find(addr) == parameter_block_data.end())\n            {\n                double *data = new double[size];\n                memcpy(data, it->parameter_blocks[i], sizeof(double) * size);\n                parameter_block_data[addr] = data;\n            }\n        }\n    }\n}\n\nint MarginalizationInfo::localSize(int size) const\n{\n    return size == 7 ? 6 : size;\n}\n\nint MarginalizationInfo::globalSize(int size) const\n{\n    return size == 6 ? 7 : size;\n}\n\nvoid* ThreadsConstructA(void* threadsstruct)\n{\n    ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);\n    for (auto it : p->sub_factors)\n    {\n        for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)\n        {\n            int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];\n            int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];\n            if (size_i == 7)\n                size_i = 6;\n            Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);\n            for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)\n            {\n                int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];\n                int size_j = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])];\n                if (size_j == 7)\n                    size_j = 6;\n                Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);\n                if (i == j)\n                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n                else\n                {\n                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n                    p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();\n                }\n            }\n            p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;\n        }\n    }\n    return threadsstruct;\n}\n\nvoid MarginalizationInfo::marginalize()\n{\n    int pos = 0;\n    for (auto &it : parameter_block_idx)\n    {\n        it.second = pos;\n        pos += localSize(parameter_block_size[it.first]);\n    }\n\n    m = pos;\n\n    for (const auto &it : parameter_block_size)\n    {\n        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())\n        {\n            parameter_block_idx[it.first] = pos;\n            pos += localSize(it.second);\n        }\n    }\n\n    n = pos - m;\n\n    //ROS_DEBUG(\"marginalization, pos: %d, m: %d, n: %d, size: %d\", pos, m, n, (int)parameter_block_idx.size());\n\n    TicToc t_summing;\n    Eigen::MatrixXd A(pos, pos);\n    Eigen::VectorXd b(pos);\n    A.setZero();\n    b.setZero();\n    /*\n    for (auto it : factors)\n    {\n        for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)\n        {\n            int idx_i = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];\n            int size_i = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])]);\n            Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);\n            for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)\n            {\n                int idx_j = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];\n                int size_j = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])]);\n                Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);\n                if (i == j)\n                    A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n                else\n                {\n                    A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;\n                    A.block(idx_j, idx_i, size_j, size_i) = A.block(idx_i, idx_j, size_i, size_j).transpose();\n                }\n            }\n            b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;\n        }\n    }\n    ROS_INFO(\"summing up costs %f ms\", t_summing.toc());\n    */\n    //multi thread\n\n\n    TicToc t_thread_summing;\n    pthread_t tids[NUM_THREADS];\n    ThreadsStruct threadsstruct[NUM_THREADS];\n    int i = 0;\n    for (auto it : factors)\n    {\n        threadsstruct[i].sub_factors.push_back(it);\n        i++;\n        i = i % NUM_THREADS;\n    }\n    for (int i = 0; i < NUM_THREADS; i++)\n    {\n        TicToc zero_matrix;\n        threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);\n        threadsstruct[i].b = Eigen::VectorXd::Zero(pos);\n        threadsstruct[i].parameter_block_size = parameter_block_size;\n        threadsstruct[i].parameter_block_idx = parameter_block_idx;\n        int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));\n        if (ret != 0)\n        {\n        //    ROS_WARN(\"pthread_create error\");\n         //   ROS_BREAK();\n\t  cout << \"WARN: pthread_create error\" << endl;\n\t  break;\n        }\n    }\n    for( int i = NUM_THREADS - 1; i >= 0; i--)  \n    {\n        pthread_join( tids[i], NULL ); \n        A += threadsstruct[i].A;\n        b += threadsstruct[i].b;\n    }\n    //ROS_DEBUG(\"thread summing up costs %f ms\", t_thread_summing.toc());\n    //ROS_INFO(\"A diff %f , b diff %f \", (A - tmp_A).sum(), (b - tmp_b).sum());\n\n\n    //TODO\n    Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());\n    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);\n\n    //ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, \"min eigenvalue %f\", saes.eigenvalues().minCoeff());\n\n    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();\n    //printf(\"error1: %f\\n\", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());\n\n    Eigen::VectorXd bmm = b.segment(0, m);\n    Eigen::MatrixXd Amr = A.block(0, m, m, n);\n    Eigen::MatrixXd Arm = A.block(m, 0, n, m);\n    Eigen::MatrixXd Arr = A.block(m, m, n, n);\n    Eigen::VectorXd brr = b.segment(m, n);\n    A = Arr - Arm * Amm_inv * Amr;\n    b = brr - Arm * Amm_inv * bmm;\n\n    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);\n    Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));\n    Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));\n\n    Eigen::VectorXd S_sqrt = S.cwiseSqrt();\n    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();\n\n    linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();\n    linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;\n    //std::cout << A << std::endl\n    //          << std::endl;\n    //std::cout << linearized_jacobians << std::endl;\n    //printf(\"error2: %f %f\\n\", (linearized_jacobians.transpose() * linearized_jacobians - A).sum(),\n    //      (linearized_jacobians.transpose() * linearized_residuals - b).sum());\n}\n\nstd::vector<double *> MarginalizationInfo::getParameterBlocks(std::unordered_map<long, double *> &addr_shift)\n{\n    std::vector<double *> keep_block_addr;\n    keep_block_size.clear();\n    keep_block_idx.clear();\n    keep_block_data.clear();\n\n    for (const auto &it : parameter_block_idx)\n    {\n        if (it.second >= m)\n        {\n            keep_block_size.push_back(parameter_block_size[it.first]);\n            keep_block_idx.push_back(parameter_block_idx[it.first]);\n            keep_block_data.push_back(parameter_block_data[it.first]);\n            keep_block_addr.push_back(addr_shift[it.first]);\n        }\n    }\n    sum_block_size = std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0);\n\n    return keep_block_addr;\n}\n\nMarginalizationFactor::MarginalizationFactor(MarginalizationInfo* _marginalization_info):marginalization_info(_marginalization_info)\n{\n    int cnt = 0;\n    for (auto it : marginalization_info->keep_block_size)\n    {\n        mutable_parameter_block_sizes()->push_back(it);\n        cnt += it;\n    }\n    //printf(\"residual size: %d, %d\\n\", cnt, n);\n    set_num_residuals(marginalization_info->n);\n};\n\nbool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const\n{\n    //printf(\"internal addr,%d, %d\\n\", (int)parameter_block_sizes().size(), num_residuals());\n    //for (int i = 0; i < static_cast<int>(keep_block_size.size()); i++)\n    //{\n    //    //printf(\"unsigned %x\\n\", reinterpret_cast<unsigned long>(parameters[i]));\n    //    //printf(\"signed %x\\n\", reinterpret_cast<long>(parameters[i]));\n    //printf(\"jacobian %x\\n\", reinterpret_cast<long>(jacobians));\n    //printf(\"residual %x\\n\", reinterpret_cast<long>(residuals));\n    //}\n    int n = marginalization_info->n;\n    int m = marginalization_info->m;\n    Eigen::VectorXd dx(n);\n    for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)\n    {\n        int size = marginalization_info->keep_block_size[i];\n        int idx = marginalization_info->keep_block_idx[i] - m;\n        Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);\n        Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);\n        if (size != 7)\n            dx.segment(idx, size) = x - x0;\n        else\n        {\n            dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();\n            dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();\n            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))\n            {\n                dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();\n            }\n        }\n    }\n    Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;\n    if (jacobians)\n    {\n\n        for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)\n        {\n            if (jacobians[i])\n            {\n                int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);\n                int idx = marginalization_info->keep_block_idx[i] - m;\n                Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);\n                jacobian.setZero();\n                jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);\n            }\n        }\n    }\n    return true;\n}\n"
  },
  {
    "path": "vins_estimator/src/factor/marginalization_factor.h",
    "content": "#pragma once\n\n//#include <ros/ros.h>\n//#include <ros/console.h>\n#include <cstdlib>\n#include <pthread.h>\n#include <ceres/ceres.h>\n#include <unordered_map>\n#include <iostream>\n#include \"../utility/utility.h\"\n#include \"../utility/tic_toc.h\"\n\nconst int NUM_THREADS = 4;\n\nstruct ResidualBlockInfo\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)\n        : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}\n\n    void Evaluate();\n\n    ceres::CostFunction *cost_function;\n    ceres::LossFunction *loss_function;\n    std::vector<double *> parameter_blocks;\n    std::vector<int> drop_set;\n\n    double **raw_jacobians;\n    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;\n    Eigen::VectorXd residuals;\n\n    int localSize(int size)\n    {\n        return size == 7 ? 6 : size;\n    }\n};\n\nstruct ThreadsStruct\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    std::vector<ResidualBlockInfo *> sub_factors;\n    Eigen::MatrixXd A;\n    Eigen::VectorXd b;\n    std::unordered_map<long, int> parameter_block_size; //global size\n    std::unordered_map<long, int> parameter_block_idx; //local size\n};\n\nclass MarginalizationInfo\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    ~MarginalizationInfo();\n    int localSize(int size) const;\n    int globalSize(int size) const;\n    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);\n    void preMarginalize();\n    void marginalize();\n    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);\n\n    std::vector<ResidualBlockInfo *> factors;\n    int m, n;\n    std::unordered_map<long, int> parameter_block_size; //global size\n    int sum_block_size;\n    std::unordered_map<long, int> parameter_block_idx; //local size\n    std::unordered_map<long, double *> parameter_block_data;\n\n    std::vector<int> keep_block_size; //global size\n    std::vector<int> keep_block_idx;  //local size\n    std::vector<double *> keep_block_data;\n\n    Eigen::MatrixXd linearized_jacobians;\n    Eigen::VectorXd linearized_residuals;\n    const double eps = 1e-8;\n\n};\n\nclass MarginalizationFactor : public ceres::CostFunction\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    MarginalizationFactor(MarginalizationInfo* _marginalization_info);\n    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;\n\n    MarginalizationInfo* marginalization_info;\n};\n"
  },
  {
    "path": "vins_estimator/src/factor/pose_local_parameterization.cpp",
    "content": "#include \"pose_local_parameterization.h\"\n\nbool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const\n{\n    Eigen::Map<const Eigen::Vector3d> _p(x);\n    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);\n\n    Eigen::Map<const Eigen::Vector3d> dp(delta);\n\n    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));\n\n    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);\n    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);\n\n    p = _p + dp;\n    q = (_q * dq).normalized();\n\n    return true;\n}\nbool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const\n{\n    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);\n    j.topRows<6>().setIdentity();\n    j.bottomRows<1>().setZero();\n\n    return true;\n}\n"
  },
  {
    "path": "vins_estimator/src/factor/pose_local_parameterization.h",
    "content": "#pragma once\n\n#include <eigen3/Eigen/Dense>\n#include <ceres/ceres.h>\n#include \"../utility/utility.h\"\n\nclass PoseLocalParameterization : public ceres::LocalParameterization\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\nprivate:\n    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;\n    virtual bool ComputeJacobian(const double *x, double *jacobian) const;\n    virtual int GlobalSize() const { return 7; };\n    virtual int LocalSize() const { return 6; };\n};\n"
  },
  {
    "path": "vins_estimator/src/factor/projection_factor.cpp",
    "content": "#include \"projection_factor.h\"\n\nEigen::Matrix2d ProjectionFactor::sqrt_info;\ndouble ProjectionFactor::sum_t;\n\nProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)\n{\n#ifdef UNIT_SPHERE_ERROR\n    Eigen::Vector3d b1, b2;\n    Eigen::Vector3d a = pts_j.normalized();\n    Eigen::Vector3d tmp(0, 0, 1);\n    if(a == tmp)\n        tmp << 1, 0, 0;\n    b1 = (tmp - a * (a.transpose() * tmp)).normalized();\n    b2 = a.cross(b1);\n    tangent_base.block<1, 3>(0, 0) = b1.transpose();\n    tangent_base.block<1, 3>(1, 0) = b2.transpose();\n#endif\n};\n\nbool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const\n{\n    TicToc tic_toc;\n    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n\n    double inv_dep_i = parameters[3][0];\n\n    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;\n    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n    Eigen::Map<Eigen::Vector2d> residual(residuals);\n\n#ifdef UNIT_SPHERE_ERROR \n    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());\n#else\n    double dep_j = pts_camera_j.z();\n    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();\n#endif\n\n    residual = sqrt_info * residual;\n\n    if (jacobians)\n    {\n        Eigen::Matrix3d Ri = Qi.toRotationMatrix();\n        Eigen::Matrix3d Rj = Qj.toRotationMatrix();\n        Eigen::Matrix3d ric = qic.toRotationMatrix();\n        Eigen::Matrix<double, 2, 3> reduce(2, 3);\n#ifdef UNIT_SPHERE_ERROR\n        double norm = pts_camera_j.norm();\n        Eigen::Matrix3d norm_jaco;\n        double x1, x2, x3;\n        x1 = pts_camera_j(0);\n        x2 = pts_camera_j(1);\n        x3 = pts_camera_j(2);\n        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),\n                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),\n                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);\n        reduce = tangent_base * norm_jaco;\n#else\n        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),\n            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);\n#endif\n        reduce = sqrt_info * reduce;\n\n        if (jacobians[0])\n        {\n            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);\n\n            Eigen::Matrix<double, 3, 6> jaco_i;\n            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();\n            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);\n\n            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;\n            jacobian_pose_i.rightCols<1>().setZero();\n        }\n\n        if (jacobians[1])\n        {\n            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);\n\n            Eigen::Matrix<double, 3, 6> jaco_j;\n            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();\n            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);\n\n            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;\n            jacobian_pose_j.rightCols<1>().setZero();\n        }\n        if (jacobians[2])\n        {\n            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);\n            Eigen::Matrix<double, 3, 6> jaco_ex;\n            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());\n            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;\n            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +\n                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));\n            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;\n            jacobian_ex_pose.rightCols<1>().setZero();\n        }\n        if (jacobians[3])\n        {\n            Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);\n#if 1\n            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);\n#else\n            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;\n#endif\n        }\n    }\n    sum_t += tic_toc.toc();\n\n    return true;\n}\n\nvoid ProjectionFactor::check(double **parameters)\n{\n    double *res = new double[15];\n    double **jaco = new double *[4];\n    jaco[0] = new double[2 * 7];\n    jaco[1] = new double[2 * 7];\n    jaco[2] = new double[2 * 7];\n    jaco[3] = new double[2 * 1];\n    Evaluate(parameters, res, jaco);\n    puts(\"check begins\");\n\n    puts(\"my\");\n\n    std::cout << Eigen::Map<Eigen::Matrix<double, 2, 1>>(res).transpose() << std::endl\n              << std::endl;\n    std::cout << Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>>(jaco[0]) << std::endl\n              << std::endl;\n    std::cout << Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>>(jaco[1]) << std::endl\n              << std::endl;\n    std::cout << Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>>(jaco[2]) << std::endl\n              << std::endl;\n    std::cout << Eigen::Map<Eigen::Vector2d>(jaco[3]) << std::endl\n              << std::endl;\n\n    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n    double inv_dep_i = parameters[3][0];\n\n    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;\n    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n\n    double dep_j = pts_camera_j.z();\n\n    Eigen::Vector2d residual;\n    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();\n\n    residual = sqrt_info * residual;\n\n    puts(\"num\");\n    std::cout << residual.transpose() << std::endl;\n\n    const double eps = 1e-6;\n    Eigen::Matrix<double, 2, 19> num_jacobian;\n    for (int k = 0; k < 19; k++)\n    {\n        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);\n        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);\n\n        Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);\n        Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);\n\n        Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);\n        Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);\n        double inv_dep_i = parameters[3][0];\n\n        int a = k / 3, b = k % 3;\n        Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;\n\n        if (a == 0)\n            Pi += delta;\n        else if (a == 1)\n            Qi = Qi * Utility::deltaQ(delta);\n        else if (a == 2)\n            Pj += delta;\n        else if (a == 3)\n            Qj = Qj * Utility::deltaQ(delta);\n        else if (a == 4)\n            tic += delta;\n        else if (a == 5)\n            qic = qic * Utility::deltaQ(delta);\n        else if (a == 6)\n            inv_dep_i += delta.x();\n\n        Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;\n        Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;\n        Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;\n        Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);\n        Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);\n\n        double dep_j = pts_camera_j.z();\n\n        Eigen::Vector2d tmp_residual;\n        tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();\n        tmp_residual = sqrt_info * tmp_residual;\n\n        num_jacobian.col(k) = (tmp_residual - residual) / eps;\n    }\n    std::cout << num_jacobian << std::endl;\n}\n"
  },
  {
    "path": "vins_estimator/src/factor/projection_factor.h",
    "content": "#pragma once\n\n//#include <ros/assert.h>\n#include <ceres/ceres.h>\n#include <Eigen/Dense>\n#include \"../utility/utility.h\"\n#include \"../utility/tic_toc.h\"\n#include \"../parameters.h\"\n\nclass ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j);\n    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;\n    void check(double **parameters);\n\n    Eigen::Vector3d pts_i, pts_j;\n    Eigen::Matrix<double, 2, 3> tangent_base;\n    static Eigen::Matrix2d sqrt_info;\n    static double sum_t;\n};\n"
  },
  {
    "path": "vins_estimator/src/feature_manager.cpp",
    "content": "#include \"feature_manager.h\"\n\nint FeaturePerId::endFrame()\n{\n    return start_frame + feature_per_frame.size() - 1;\n}\n\nFeatureManager::FeatureManager(Matrix3d _Rs[])\n    : Rs(_Rs)\n{\n    for (int i = 0; i < NUM_OF_CAM; i++)\n        ric[i].setIdentity();\n}\n\nvoid FeatureManager::setRic(Matrix3d _ric[])\n{\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        ric[i] = _ric[i];\n    }\n}\n\nvoid FeatureManager::clearState()\n{\n    feature.clear();\n}\n\nint FeatureManager::getFeatureCount()\n{\n    int cnt = 0;\n    for (auto &it : feature)\n    {\n\n        it.used_num = it.feature_per_frame.size();\n\n        if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)\n        {\n            cnt++;\n        }\n    }\n    return cnt;\n}\n\n\nbool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Vector3d>>> &image)\n{\n  //  ROS_DEBUG(\"input feature: %d\", (int)image.size());\n  //  ROS_DEBUG(\"num of feature: %d\", getFeatureCount());\n    double parallax_sum = 0;\n    int parallax_num = 0;\n    last_track_num = 0;\n    for (auto &id_pts : image)\n    {\n        FeaturePerFrame f_per_fra(id_pts.second[0].second);\n\n        int feature_id = id_pts.first;\n        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)\n                          {\n            return it.feature_id == feature_id;\n                          });\n\n        if (it == feature.end())\n        {\n            feature.push_back(FeaturePerId(feature_id, frame_count));\n            feature.back().feature_per_frame.push_back(f_per_fra);\n        }\n        else if (it->feature_id == feature_id)\n        {\n            it->feature_per_frame.push_back(f_per_fra);\n            last_track_num++;\n        }\n    }\n\n    if (frame_count < 2 || last_track_num < 20)\n        return true;\n\n    for (auto &it_per_id : feature)\n    {\n        if (it_per_id.start_frame <= frame_count - 2 &&\n            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)\n        {\n            parallax_sum += compensatedParallax2(it_per_id, frame_count);\n            parallax_num++;\n        }\n    }\n\n    if (parallax_num == 0)\n    {\n        return true;\n    }\n    else\n    {\n     //   ROS_DEBUG(\"parallax_sum: %lf, parallax_num: %d\", parallax_sum, parallax_num);\n     //   ROS_DEBUG(\"current parallax: %lf\", parallax_sum / parallax_num * FOCAL_LENGTH);\n        return parallax_sum / parallax_num >= MIN_PARALLAX;\n    }\n}\n\nvoid FeatureManager::debugShow()\n{\n  //  ROS_DEBUG(\"debug show\");\n    for (auto &it : feature)\n    {\n    //    ROS_ASSERT(it.feature_per_frame.size() != 0);\n     //   ROS_ASSERT(it.start_frame >= 0);\n     //   ROS_ASSERT(it.used_num >= 0);\n      assert(it.feature_per_frame.size() != 0);\n      assert(it.start_frame >= 0);\n      assert(it.used_num >= 0);\n\n    //    ROS_DEBUG(\"%d,%d,%d \", it.feature_id, it.used_num, it.start_frame);\n        int sum = 0;\n        for (auto &j : it.feature_per_frame)\n        {\n        //    ROS_DEBUG(\"%d,\", int(j.is_used));\n            sum += j.is_used;\n            printf(\"(%lf,%lf) \",j.point(0), j.point(1));\n        }\n       assert(it.used_num == sum);\n   //     ROS_ASSERT(it.used_num == sum);\n    }\n}\n\nvector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)\n{\n    vector<pair<Vector3d, Vector3d>> corres;\n    for (auto &it : feature)\n    {\n        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)\n        {\n            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();\n            int idx_l = frame_count_l - it.start_frame;\n            int idx_r = frame_count_r - it.start_frame;\n\n            a = it.feature_per_frame[idx_l].point;\n\n            b = it.feature_per_frame[idx_r].point;\n            \n            corres.push_back(make_pair(a, b));\n        }\n    }\n    return corres;\n}\n\nvoid FeatureManager::setDepth(const VectorXd &x)\n{\n    int feature_index = -1;\n    for (auto &it_per_id : feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n\n        it_per_id.estimated_depth = 1.0 / x(++feature_index);\n        //ROS_INFO(\"feature id %d , start_frame %d, depth %f \", it_per_id->feature_id, it_per_id-> start_frame, it_per_id->estimated_depth);\n        if (it_per_id.estimated_depth < 0)\n        {\n            it_per_id.solve_flag = 2;\n        }\n        else\n            it_per_id.solve_flag = 1;\n    }\n}\n\nvoid FeatureManager::removeFailures()\n{\n    for (auto it = feature.begin(), it_next = feature.begin();\n         it != feature.end(); it = it_next)\n    {\n        it_next++;\n        if (it->solve_flag == 2)\n            feature.erase(it);\n    }\n}\n\nvoid FeatureManager::clearDepth(const VectorXd &x)\n{\n    int feature_index = -1;\n    for (auto &it_per_id : feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n        it_per_id.estimated_depth = 1.0 / x(++feature_index);\n    }\n}\n\nVectorXd FeatureManager::getDepthVector()\n{\n    VectorXd dep_vec(getFeatureCount());\n    int feature_index = -1;\n    for (auto &it_per_id : feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n#if 1\n        dep_vec(++feature_index) = 1. / it_per_id.estimated_depth;\n#else\n        dep_vec(++feature_index) = it_per_id->estimated_depth;\n#endif\n    }\n    return dep_vec;\n}\n\nvoid FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])\n{\n    for (auto &it_per_id : feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n\n        if (it_per_id.estimated_depth > 0)\n            continue;\n        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;\n\n       // ROS_ASSERT(NUM_OF_CAM == 1);\n\tassert(NUM_OF_CAM == 1);\n        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);\n        int svd_idx = 0;\n\n        Eigen::Matrix<double, 3, 4> P0;\n        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];\n        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];\n        P0.leftCols<3>() = Eigen::Matrix3d::Identity();\n        P0.rightCols<1>() = Eigen::Vector3d::Zero();\n\n        for (auto &it_per_frame : it_per_id.feature_per_frame)\n        {\n            imu_j++;\n\n            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];\n            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];\n            Eigen::Vector3d t = R0.transpose() * (t1 - t0);\n            Eigen::Matrix3d R = R0.transpose() * R1;\n            Eigen::Matrix<double, 3, 4> P;\n            P.leftCols<3>() = R.transpose();\n            P.rightCols<1>() = -R.transpose() * t;\n            Eigen::Vector3d f = it_per_frame.point.normalized();\n            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);\n            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);\n\n            if (imu_i == imu_j)\n                continue;\n        }\n      //  ROS_ASSERT(svd_idx == svd_A.rows());\n        assert(svd_idx == svd_A.rows());\n        Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();\n        double svd_method = svd_V[2] / svd_V[3];\n        //it_per_id->estimated_depth = -b / A;\n        //it_per_id->estimated_depth = svd_V[2] / svd_V[3];\n\n        it_per_id.estimated_depth = svd_method;\n        //it_per_id->estimated_depth = INIT_DEPTH;\n\n        if (it_per_id.estimated_depth < 0.1)\n        {\n            it_per_id.estimated_depth = INIT_DEPTH;\n        }\n\n    }\n}\n\nvoid FeatureManager::removeOutlier()\n{\n  //  ROS_BREAK();\n    int i = -1;\n    for (auto it = feature.begin(), it_next = feature.begin();\n         it != feature.end(); it = it_next)\n    {\n        it_next++;\n        i += it->used_num != 0;\n        if (it->used_num != 0 && it->is_outlier == true)\n        {\n            feature.erase(it);\n        }\n    }\n}\n\nvoid FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)\n{\n    for (auto it = feature.begin(), it_next = feature.begin();\n         it != feature.end(); it = it_next)\n    {\n        it_next++;\n\n        if (it->start_frame != 0)\n            it->start_frame--;\n        else\n        {\n            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  \n            it->feature_per_frame.erase(it->feature_per_frame.begin());\n            if (it->feature_per_frame.size() < 2)\n            {\n                feature.erase(it);\n                continue;\n            }\n            else\n            {\n                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;\n                Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;\n                Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);\n                double dep_j = pts_j(2);\n                if (dep_j > 0)\n                    it->estimated_depth = dep_j;\n                else\n                    it->estimated_depth = INIT_DEPTH;\n            }\n        }\n        // remove tracking-lost feature after marginalize\n        /*\n        if (it->endFrame() < WINDOW_SIZE - 1)\n        {\n            feature.erase(it);\n        }\n        */\n    }\n}\n\nvoid FeatureManager::removeBack()\n{\n    for (auto it = feature.begin(), it_next = feature.begin();\n         it != feature.end(); it = it_next)\n    {\n        it_next++;\n\n        if (it->start_frame != 0)\n            it->start_frame--;\n        else\n        {\n            it->feature_per_frame.erase(it->feature_per_frame.begin());\n            if (it->feature_per_frame.size() == 0)\n                feature.erase(it);\n        }\n    }\n}\n\nvoid FeatureManager::removeFront(int frame_count)\n{\n    for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)\n    {\n        it_next++;\n\n        if (it->start_frame == frame_count)\n        {\n            it->start_frame--;\n        }\n        else\n        {\n            int j = WINDOW_SIZE - 1 - it->start_frame;\n            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);\n            if (it->feature_per_frame.size() == 0)\n                feature.erase(it);\n        }\n    }\n}\n\ndouble FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)\n{\n    //check the second last frame is keyframe or not\n    //parallax betwwen seconde last frame and third last frame\n    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];\n    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];\n\n    double ans = 0;\n    Vector3d p_j = frame_j.point;\n\n    double u_j = p_j(0);\n    double v_j = p_j(1);\n\n    Vector3d p_i = frame_i.point;\n    Vector3d p_i_comp;\n\n    //int r_i = frame_count - 2;\n    //int r_j = frame_count - 1;\n    //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;\n    p_i_comp = p_i;\n    double dep_i = p_i(2);\n    double u_i = p_i(0) / dep_i;\n    double v_i = p_i(1) / dep_i;\n    double du = u_i - u_j, dv = v_i - v_j;\n\n    double dep_i_comp = p_i_comp(2);\n    double u_i_comp = p_i_comp(0) / dep_i_comp;\n    double v_i_comp = p_i_comp(1) / dep_i_comp;\n    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;\n\n    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));\n\n    return ans;\n}"
  },
  {
    "path": "vins_estimator/src/feature_manager.h",
    "content": "#ifndef FEATURE_MANAGER_H\n#define FEATURE_MANAGER_H\n\n#include <list>\n#include <algorithm>\n#include <vector>\n#include <numeric>\nusing namespace std;\n\n#include <eigen3/Eigen/Dense>\nusing namespace Eigen;\n\n//#include <ros/console.h>\n//#include <ros/assert.h>\n#include <assert.h>\n#include \"parameters.h\"\n\nclass FeaturePerFrame\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    FeaturePerFrame(const Vector3d &_point)\n    {\n        z = _point(2);\n        point = _point / z;\n    }\n    Vector3d point;\n    double z;\n    bool is_used;\n    double parallax;\n    MatrixXd A;\n    VectorXd b;\n    double dep_gradient;\n};\n\nclass FeaturePerId\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    const int feature_id;\n    int start_frame;\n    vector<FeaturePerFrame> feature_per_frame;\n\n    int used_num;\n    bool is_outlier;\n    bool is_margin;\n    double estimated_depth;\n    int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;\n\n    Vector3d gt_p;\n\n    FeaturePerId(int _feature_id, int _start_frame)\n        : feature_id(_feature_id), start_frame(_start_frame),\n          used_num(0), estimated_depth(-1.0), solve_flag(0)\n    {\n    }\n\n    int endFrame();\n};\n\nclass FeatureManager\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    FeatureManager(Matrix3d _Rs[]);\n\n    void setRic(Matrix3d _ric[]);\n\n    void clearState();\n\n    int getFeatureCount();\n\n    bool addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Vector3d>>> &image);\n    void debugShow();\n    vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r);\n\n    //void updateDepth(const VectorXd &x);\n    void setDepth(const VectorXd &x);\n    void removeFailures();\n    void clearDepth(const VectorXd &x);\n    VectorXd getDepthVector();\n    void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]);\n    void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P);\n    void removeBack();\n    void removeFront(int frame_count);\n    void removeOutlier();\n    list<FeaturePerId> feature;\n    int last_track_num;\n\n  private:\n    double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count);\n    const Matrix3d *Rs;\n    Matrix3d ric[NUM_OF_CAM];\n};\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/feature_tracker/feature_tracker.cpp",
    "content": "#include \"feature_tracker.h\"\n\nint FeatureTracker::n_id = 0;\n\nbool inBorder(const cv::Point2f &pt)\n{\n    const int BORDER_SIZE = 1;\n    int img_x = cvRound(pt.x);\n    int img_y = cvRound(pt.y);\n    return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;\n}\n\nvoid reduceVector(vector<cv::Point2f> &v, vector<uchar> status)\n{\n    int j = 0;\n    for (int i = 0; i < int(v.size()); i++)\n        if (status[i])\n            v[j++] = v[i];\n    v.resize(j);\n}\n\nvoid reduceVector(vector<int> &v, vector<uchar> status)\n{\n    int j = 0;\n    for (int i = 0; i < int(v.size()); i++)\n        if (status[i])\n            v[j++] = v[i];\n    v.resize(j);\n}\n\nFeatureTracker::FeatureTracker()\n{\n}\n\nvoid FeatureTracker::setMask()\n{\n    if(FISHEYE)\n        mask = fisheye_mask.clone();\n    else\n        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));\n    \n\n    // prefer to keep features that are tracked for long time\n    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;\n\n    for (unsigned int i = 0; i < forw_pts.size(); i++)\n        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));\n\n    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)\n         {\n            return a.first > b.first;\n         });\n\n    forw_pts.clear();\n    ids.clear();\n    track_cnt.clear();\n\n    for (auto &it : cnt_pts_id)\n    {\n        if (mask.at<uchar>(it.second.first) == 255)\n        {\n            forw_pts.push_back(it.second.first);\n            ids.push_back(it.second.second);\n            track_cnt.push_back(it.first);\n            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);\n        }\n    }\n}\n\nvoid FeatureTracker::addPoints()\n{\n    for (auto &p : n_pts)\n    {\n        forw_pts.push_back(p);\n        ids.push_back(-1);\n        track_cnt.push_back(1);\n    }\n}\n\nvoid FeatureTracker::readImage(const cv::Mat &_img)\n{\n    cv::Mat img;\n    TicToc t_r;\n\n    if (EQUALIZE)\n    {\n        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));\n        TicToc t_c;\n        clahe->apply(_img, img);\n      //  ROS_DEBUG(\"CLAHE costs: %fms\", t_c.toc());\n    }\n    else\n        img = _img;\n\n    if (forw_img.empty())\n    {\n        prev_img = cur_img = forw_img = img;\n    }\n    else\n    {\n        forw_img = img;\n    }\n\n    forw_pts.clear();\n\n    if (cur_pts.size() > 0)\n    {\n        TicToc t_o;\n        vector<uchar> status;\n        vector<float> err;\n        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);\n\n        for (int i = 0; i < int(forw_pts.size()); i++)\n            if (status[i] && !inBorder(forw_pts[i]))\n                status[i] = 0;\n        reduceVector(prev_pts, status);\n        reduceVector(cur_pts, status);\n        reduceVector(forw_pts, status);\n        reduceVector(ids, status);\n        reduceVector(track_cnt, status);\n      //  ROS_DEBUG(\"temporal optical flow costs: %fms\", t_o.toc());\n    }\n\n    if (PUB_THIS_FRAME)\n    {\n        rejectWithF();\n\n        for (auto &n : track_cnt)\n            n++;\n\n     //   ROS_DEBUG(\"set mask begins\");\n        TicToc t_m;\n        setMask();\n     //   ROS_DEBUG(\"set mask costs %fms\", t_m.toc());\n\n     //   ROS_DEBUG(\"detect feature begins\");\n        TicToc t_t;\n        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());\n        if (n_max_cnt > 0)\n        {\n            if(mask.empty())\n                cout << \"mask is empty \" << endl;\n            if (mask.type() != CV_8UC1)\n                cout << \"mask type wrong \" << endl;\n            if (mask.size() != forw_img.size())\n                cout << \"wrong size \" << endl;\n            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);\n        }\n        else\n            n_pts.clear();\n     //   ROS_DEBUG(\"detect feature costs: %fms\", t_t.toc());\n\n      //  ROS_DEBUG(\"add feature begins\");\n        TicToc t_a;\n        addPoints();\n     //   ROS_DEBUG(\"selectFeature costs: %fms\", t_a.toc());\n\n        prev_img = forw_img;\n        prev_pts = forw_pts;\n    }\n    cur_img = forw_img;\n    cur_pts = forw_pts;\n}\n\nvoid FeatureTracker::rejectWithF()\n{\n    if (forw_pts.size() >= 8)\n    {\n       // ROS_DEBUG(\"FM ransac begins\");\n        TicToc t_f;\n        vector<cv::Point2f> un_prev_pts(prev_pts.size()), un_forw_pts(forw_pts.size());\n        for (unsigned int i = 0; i < prev_pts.size(); i++)\n        {\n            Eigen::Vector3d tmp_p;\n            m_camera->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_pts[i].y), tmp_p);\n            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n            un_prev_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n\n            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);\n            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n        }\n\n        vector<uchar> status;\n        cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);\n        int size_a = prev_pts.size();\n        reduceVector(prev_pts, status);\n        reduceVector(cur_pts, status);\n        reduceVector(forw_pts, status);\n        reduceVector(ids, status);\n        reduceVector(track_cnt, status);\n      //  ROS_DEBUG(\"FM ransac: %d -> %lu: %f\", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);\n      //  ROS_DEBUG(\"FM ransac costs: %fms\", t_f.toc());\n    }\n}\n\nbool FeatureTracker::updateID(unsigned int i)\n{\n    if (i < ids.size())\n    {\n        if (ids[i] == -1)\n            ids[i] = n_id++;\n        return true;\n    }\n    else\n        return false;\n}\n\nvoid FeatureTracker::readIntrinsicParameter(const string &calib_file)\n{\n  //  ROS_INFO(\"reading paramerter of camera %s\", calib_file.c_str());\n   cout << \"reading paramerter of camera \" <<  calib_file.c_str() <<endl;\n    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n}\n\nvoid FeatureTracker::showUndistortion(const string &name)\n{\n    cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));\n    vector<Eigen::Vector2d> distortedp, undistortedp;\n    for (int i = 0; i < COL; i++)\n        for (int j = 0; j < ROW; j++)\n        {\n            Eigen::Vector2d a(i, j);\n            Eigen::Vector3d b;\n            m_camera->liftProjective(a, b);\n            distortedp.push_back(a);\n            undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));\n            //printf(\"%f,%f->%f,%f,%f\\n)\\n\", a.x(), a.y(), b.x(), b.y(), b.z());\n        }\n    for (int i = 0; i < int(undistortedp.size()); i++)\n    {\n        cv::Mat pp(3, 1, CV_32FC1);\n        pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;\n        pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;\n        pp.at<float>(2, 0) = 1.0;\n        //cout << trackerData[0].K << endl;\n        //printf(\"%lf %lf\\n\", p.at<float>(1, 0), p.at<float>(0, 0));\n        //printf(\"%lf %lf\\n\", pp.at<float>(1, 0), pp.at<float>(0, 0));\n        if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)\n        {\n            undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());\n        }\n        else\n        {\n            //ROS_ERROR(\"(%f %f) -> (%f %f)\", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));\n        }\n    }\n    cv::imshow(name, undistortedImg);\n    cv::waitKey(0);\n}\n\nvector<cv::Point2f> FeatureTracker::undistortedPoints()\n{\n    vector<cv::Point2f> un_pts;\n    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());\n    for (unsigned int i = 0; i < cur_pts.size(); i++)\n    {\n        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);\n        Eigen::Vector3d b;\n        m_camera->liftProjective(a, b);\n        un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));\n    }\n\n    return un_pts;\n}\n"
  },
  {
    "path": "vins_estimator/src/feature_tracker/feature_tracker.h",
    "content": "#pragma once\n\n#include <cstdio>\n#include <iostream>\n#include <queue>\n//#include <execinfo.h>\n#include <csignal>\n\n#include <opencv2/opencv.hpp>\n#include <eigen3/Eigen/Dense>\n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n\n#include \"../parameters.h\"\n#include \"tic_toc.h\"\n\nusing namespace std;\nusing namespace camodocal;\nusing namespace Eigen;\n\nbool inBorder(const cv::Point2f &pt);\n\nvoid reduceVector(vector<cv::Point2f> &v, vector<uchar> status);\nvoid reduceVector(vector<int> &v, vector<uchar> status);\n\nclass FeatureTracker\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    FeatureTracker();\n\n    void readImage(const cv::Mat &_img);\n\n    void setMask();\n\n    void addPoints();\n\n    bool updateID(unsigned int i);\n\n    void readIntrinsicParameter(const string &calib_file);\n\n    void showUndistortion(const string &name);\n\n    void rejectWithF();\n\n    vector<cv::Point2f> undistortedPoints();\n\n    cv::Mat mask;\n    cv::Mat fisheye_mask;\n    cv::Mat prev_img, cur_img, forw_img;\n    vector<cv::Point2f> n_pts;\n    vector<cv::Point2f> prev_pts, cur_pts, forw_pts;\n    vector<int> ids;\n    vector<int> track_cnt;\n    camodocal::CameraPtr m_camera;\n\n    static int n_id;\n};\n"
  },
  {
    "path": "vins_estimator/src/feature_tracker/tic_toc.h",
    "content": "#pragma once\n\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n\nclass TicToc\n{\n  public:\n    TicToc()\n    {\n        tic();\n    }\n\n    void tic()\n    {\n        start = std::chrono::system_clock::now();\n    }\n\n    double toc()\n    {\n        end = std::chrono::system_clock::now();\n        std::chrono::duration<double> elapsed_seconds = end - start;\n        return elapsed_seconds.count() * 1000;\n    }\n\n  private:\n    std::chrono::time_point<std::chrono::system_clock> start, end;\n};\n"
  },
  {
    "path": "vins_estimator/src/initial/initial_aligment.cpp",
    "content": "#include \"initial_alignment.h\"\n\nvoid solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)\n{\n    Matrix3d A;\n    Vector3d b;\n    Vector3d delta_bg;\n    A.setZero();\n    b.setZero();\n    map<double, ImageFrame>::iterator frame_i;\n    map<double, ImageFrame>::iterator frame_j;\n    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)\n    {\n        frame_j = next(frame_i);\n        MatrixXd tmp_A(3, 3);\n        tmp_A.setZero();\n        VectorXd tmp_b(3);\n        tmp_b.setZero();\n        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);\n        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);\n        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();\n        A += tmp_A.transpose() * tmp_A;\n        b += tmp_A.transpose() * tmp_b;\n\n    }\n    delta_bg = A.ldlt().solve(b);\n  //  ROS_WARN_STREAM(\"gyroscope bias initial calibration \" << delta_bg.transpose());\n    cout << \"gyroscope bias initial calibration \" << delta_bg.transpose() << endl;\n\n    for (int i = 0; i <= WINDOW_SIZE; i++)\n        Bgs[i] += delta_bg;\n\n    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)\n    {\n        frame_j = next(frame_i);\n        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);\n    }\n}\n\n\nMatrixXd TangentBasis(Vector3d &g0)\n{\n    Vector3d b, c;\n    Vector3d a = g0.normalized();\n    Vector3d tmp(0, 0, 1);\n    if(a == tmp)\n        tmp << 1, 0, 0;\n    b = (tmp - a * (a.transpose() * tmp)).normalized();\n    c = a.cross(b);\n    MatrixXd bc(3, 2);\n    bc.block<3, 1>(0, 0) = b;\n    bc.block<3, 1>(0, 1) = c;\n    return bc;\n}\n\nvoid RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)\n{\n    Vector3d g0 = g.normalized() * G.norm();\n    Vector3d lx, ly;\n    //VectorXd x;\n    int all_frame_count = all_image_frame.size();\n    int n_state = all_frame_count * 3 + 2 + 1;\n\n    MatrixXd A{n_state, n_state};\n    A.setZero();\n    VectorXd b{n_state};\n    b.setZero();\n\n    map<double, ImageFrame>::iterator frame_i;\n    map<double, ImageFrame>::iterator frame_j;\n    for(int k = 0; k < 4; k++)\n    {\n        MatrixXd lxly(3, 2);\n        lxly = TangentBasis(g0);\n        int i = 0;\n        for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)\n        {\n            frame_j = next(frame_i);\n\n            MatrixXd tmp_A(6, 9);\n            tmp_A.setZero();\n            VectorXd tmp_b(6);\n            tmp_b.setZero();\n\n            double dt = frame_j->second.pre_integration->sum_dt;\n\n\n            tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n            tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;\n            tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     \n            tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;\n\n            tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n            tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n            tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;\n            tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;\n\n\n            Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();\n            //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];\n            //MatrixXd cov_inv = cov.inverse();\n            cov_inv.setIdentity();\n\n            MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n            VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n            A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n            b.segment<6>(i * 3) += r_b.head<6>();\n\n            A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();\n            b.tail<3>() += r_b.tail<3>();\n\n            A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();\n            A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();\n        }\n            A = A * 1000.0;\n            b = b * 1000.0;\n            x = A.ldlt().solve(b);\n            VectorXd dg = x.segment<2>(n_state - 3);\n            g0 = (g0 + lxly * dg).normalized() * G.norm();\n            //double s = x(n_state - 1);\n    }   \n    g = g0;\n}\n\nbool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)\n{\n    int all_frame_count = all_image_frame.size();\n    int n_state = all_frame_count * 3 + 3 + 1;\n\n    MatrixXd A{n_state, n_state};\n    A.setZero();\n    VectorXd b{n_state};\n    b.setZero();\n\n    map<double, ImageFrame>::iterator frame_i;\n    map<double, ImageFrame>::iterator frame_j;\n    int i = 0;\n    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)\n    {\n        frame_j = next(frame_i);\n\n        MatrixXd tmp_A(6, 10);\n        tmp_A.setZero();\n        VectorXd tmp_b(6);\n        tmp_b.setZero();\n\n        double dt = frame_j->second.pre_integration->sum_dt;\n\n        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();\n        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();\n        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     \n        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];\n        //cout << \"delta_p   \" << frame_j->second.pre_integration->delta_p.transpose() << endl;\n        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();\n        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;\n        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();\n        tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;\n        //cout << \"delta_v   \" << frame_j->second.pre_integration->delta_v.transpose() << endl;\n\n        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();\n        //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];\n        //MatrixXd cov_inv = cov.inverse();\n        cov_inv.setIdentity();\n\n        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;\n        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;\n\n        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();\n        b.segment<6>(i * 3) += r_b.head<6>();\n\n        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();\n        b.tail<4>() += r_b.tail<4>();\n\n        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();\n        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();\n    }\n    A = A * 1000.0;\n    b = b * 1000.0;\n    x = A.ldlt().solve(b);\n    double s = x(n_state - 1) / 100.0;\n  //  ROS_DEBUG(\"estimated scale: %f\", s);\n    g = x.segment<3>(n_state - 4);\n    //ROS_DEBUG_STREAM(\" result g     \" << g.norm() << \" \" << g.transpose());\n    cout << \" result g     \" << g.norm() << \" \" << g.transpose() <<endl;\n    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)\n    {\n        return false;\n    }\n\n    RefineGravity(all_image_frame, g, x);\n    s = (x.tail<1>())(0) / 100.0;\n    (x.tail<1>())(0) = s;\n  //  ROS_DEBUG_STREAM(\" refine     \" << g.norm() << \" \" << g.transpose());\n    cout << \" refine     \" << g.norm() << \" \" << g.transpose() << endl;\n    if(s < 0.0 )\n        return false;   \n    else\n        return true;\n}\n\nbool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)\n{\n    solveGyroscopeBias(all_image_frame, Bgs);\n\n    if(LinearAlignment(all_image_frame, g, x))\n        return true;\n    else \n        return false;\n}\n"
  },
  {
    "path": "vins_estimator/src/initial/initial_alignment.h",
    "content": "#pragma once\n#include <eigen3/Eigen/Dense>\n#include <iostream>\n#include \"../factor/imu_factor.h\"\n#include \"../utility/utility.h\"\n//#include <ros/ros.h>\n#include <map>\n#include \"../feature_manager.h\"\n#include <iostream>\n\nusing namespace Eigen;\nusing namespace std;\n\nclass ImageFrame\n{\n    public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        ImageFrame(){};\n        ImageFrame(const map<int, vector<pair<int, Vector3d>>>& _points, double _t):points{_points},t{_t},is_key_frame{false}\n        {\n        };\n        map<int, vector<pair<int, Vector3d> > > points;\n        double t;\n        Matrix3d R;\n        Vector3d T;\n        IntegrationBase *pre_integration;\n        bool is_key_frame;\n};\n\nbool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x);\n"
  },
  {
    "path": "vins_estimator/src/initial/initial_ex_rotation.cpp",
    "content": "#include \"initial_ex_rotation.h\"\n\nInitialEXRotation::InitialEXRotation(){\n    frame_count = 0;\n    Rc.push_back(Matrix3d::Identity());\n    Rc_g.push_back(Matrix3d::Identity());\n    Rimu.push_back(Matrix3d::Identity());\n    ric = Matrix3d::Identity();\n}\n\nbool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)\n{\n    frame_count++;\n    Rc.push_back(solveRelativeR(corres));\n    Rimu.push_back(delta_q_imu.toRotationMatrix());\n    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);\n\n    Eigen::MatrixXd A(frame_count * 4, 4);\n    A.setZero();\n    int sum_ok = 0;\n    for (int i = 1; i <= frame_count; i++)\n    {\n        Quaterniond r1(Rc[i]);\n        Quaterniond r2(Rc_g[i]);\n\n        double angular_distance = 180 / M_PI * r1.angularDistance(r2);\n      //  ROS_DEBUG( \"%d %f\", i, angular_distance);\n\n        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;\n        ++sum_ok;\n        Matrix4d L, R;\n\n        double w = Quaterniond(Rc[i]).w();\n        Vector3d q = Quaterniond(Rc[i]).vec();\n        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);\n        L.block<3, 1>(0, 3) = q;\n        L.block<1, 3>(3, 0) = -q.transpose();\n        L(3, 3) = w;\n\n        Quaterniond R_ij(Rimu[i]);\n        w = R_ij.w();\n        q = R_ij.vec();\n        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);\n        R.block<3, 1>(0, 3) = q;\n        R.block<1, 3>(3, 0) = -q.transpose();\n        R(3, 3) = w;\n\n        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);\n    }\n\n    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);\n    Matrix<double, 4, 1> x = svd.matrixV().col(3);\n    Quaterniond estimated_R(x);\n    ric = estimated_R.toRotationMatrix().inverse();\n    //cout << svd.singularValues().transpose() << endl;\n    //cout << ric << endl;\n    Vector3d ric_cov;\n    ric_cov = svd.singularValues().tail<3>();\n    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)\n    {\n        calib_ric_result = ric;\n        return true;\n    }\n    else\n        return false;\n}\n\nMatrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)\n{\n    if (corres.size() >= 9)\n    {\n        vector<cv::Point2f> ll, rr;\n        for (int i = 0; i < int(corres.size()); i++)\n        {\n            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));\n            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));\n        }\n        cv::Mat E = cv::findFundamentalMat(ll, rr);\n        cv::Mat_<double> R1, R2, t1, t2;\n        decomposeE(E, R1, R2, t1, t2);\n\n        if (determinant(R1) + 1.0 < 1e-09)\n        {\n            E = -E;\n            decomposeE(E, R1, R2, t1, t2);\n        }\n        double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));\n        double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));\n        cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;\n\n        Matrix3d ans_R_eigen;\n        for (int i = 0; i < 3; i++)\n            for (int j = 0; j < 3; j++)\n                ans_R_eigen(j, i) = ans_R_cv(i, j);\n        return ans_R_eigen;\n    }\n    return Matrix3d::Identity();\n}\n\ndouble InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,\n                                          const vector<cv::Point2f> &r,\n                                          cv::Mat_<double> R, cv::Mat_<double> t)\n{\n    cv::Mat pointcloud;\n    cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,\n                                0, 1, 0, 0,\n                                0, 0, 1, 0);\n    cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),\n                                 R(1, 0), R(1, 1), R(1, 2), t(1),\n                                 R(2, 0), R(2, 1), R(2, 2), t(2));\n    cv::triangulatePoints(P, P1, l, r, pointcloud);\n    int front_count = 0;\n    for (int i = 0; i < pointcloud.cols; i++)\n    {\n        double normal_factor = pointcloud.col(i).at<float>(3);\n\n        cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);\n        cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);\n        if (p_3d_l(2) > 0 && p_3d_r(2) > 0)\n            front_count++;\n    }\n  //  ROS_DEBUG(\"MotionEstimator: %f\", 1.0 * front_count / pointcloud.cols);\n    return 1.0 * front_count / pointcloud.cols;\n}\n\nvoid InitialEXRotation::decomposeE(cv::Mat E,\n                                 cv::Mat_<double> &R1, cv::Mat_<double> &R2,\n                                 cv::Mat_<double> &t1, cv::Mat_<double> &t2)\n{\n    cv::SVD svd(E, cv::SVD::MODIFY_A);\n    cv::Matx33d W(0, -1, 0,\n                  1, 0, 0,\n                  0, 0, 1);\n    cv::Matx33d Wt(0, 1, 0,\n                   -1, 0, 0,\n                   0, 0, 1);\n    R1 = svd.u * cv::Mat(W) * svd.vt;\n    R2 = svd.u * cv::Mat(Wt) * svd.vt;\n    t1 = svd.u.col(2);\n    t2 = -svd.u.col(2);\n}\n"
  },
  {
    "path": "vins_estimator/src/initial/initial_ex_rotation.h",
    "content": "#pragma once \n\n#include <vector>\n#include \"../parameters.h\"\nusing namespace std;\n\n#include <opencv2/opencv.hpp>\n\n#include <eigen3/Eigen/Dense>\nusing namespace Eigen;\n//#include <ros/console.h>\n\n/* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */\nclass InitialEXRotation\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tInitialEXRotation();\n    bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);\nprivate:\n\tMatrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);\n\n    double testTriangulation(const vector<cv::Point2f> &l,\n                             const vector<cv::Point2f> &r,\n                             cv::Mat_<double> R, cv::Mat_<double> t);\n    void decomposeE(cv::Mat E,\n                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,\n                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);\n    int frame_count;\n\n    vector< Matrix3d > Rc;\n    vector< Matrix3d > Rimu;\n    vector< Matrix3d > Rc_g;\n    Matrix3d ric;\n};\n\n\n"
  },
  {
    "path": "vins_estimator/src/initial/initial_sfm.cpp",
    "content": "#include \"initial_sfm.h\"\n\nGlobalSFM::GlobalSFM(){}\n\nvoid GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,\n\t\t\t\t\t\tVector2d &point0, Vector2d &point1, Vector3d &point_3d)\n{\n\tMatrix4d design_matrix = Matrix4d::Zero();\n\tdesign_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);\n\tdesign_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);\n\tdesign_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);\n\tdesign_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);\n\tVector4d triangulated_point;\n\ttriangulated_point =\n\t\t      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();\n\tpoint_3d(0) = triangulated_point(0) / triangulated_point(3);\n\tpoint_3d(1) = triangulated_point(1) / triangulated_point(3);\n\tpoint_3d(2) = triangulated_point(2) / triangulated_point(3);\n}\n\n\nbool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,\n\t\t\t\t\t\t\t\tvector<SFMFeature> &sfm_f)\n{\n\tvector<cv::Point2f> pts_2_vector;\n\tvector<cv::Point3f> pts_3_vector;\n\tfor (int j = 0; j < feature_num; j++)\n\t{\n\t\tif (sfm_f[j].state != true)\n\t\t\tcontinue;\n\t\tVector2d point2d;\n\t\tfor (int k = 0; k < (int)sfm_f[j].observation.size(); k++)\n\t\t{\n\t\t\tif (sfm_f[j].observation[k].first == i)\n\t\t\t{\n\t\t\t\tVector2d img_pts = sfm_f[j].observation[k].second;\n\t\t\t\tcv::Point2f pts_2(img_pts(0), img_pts(1));\n\t\t\t\tpts_2_vector.push_back(pts_2);\n\t\t\t\tcv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);\n\t\t\t\tpts_3_vector.push_back(pts_3);\n\t\t\t\tbreak;\n\t\t\t}\n\t\t}\n\t}\n\tif (int(pts_2_vector.size()) < 15)\n\t{\n\t\tprintf(\"unstable features tracking, please slowly move you device!\\n\");\n\t\tif (int(pts_2_vector.size()) < 10)\n\t\t\treturn false;\n\t}\n\tcv::Mat r, rvec, t, D, tmp_r;\n\tcv::eigen2cv(R_initial, tmp_r);\n\tcv::Rodrigues(tmp_r, rvec);\n\tcv::eigen2cv(P_initial, t);\n\tcv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n\tbool pnp_succ;\n\tpnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);\n\tif(!pnp_succ)\n\t{\n\t\treturn false;\n\t}\n\tcv::Rodrigues(rvec, r);\n\t//cout << \"r \" << endl << r << endl;\n\tMatrixXd R_pnp;\n\tcv::cv2eigen(r, R_pnp);\n\tMatrixXd T_pnp;\n\tcv::cv2eigen(t, T_pnp);\n\tR_initial = R_pnp;\n\tP_initial = T_pnp;\n\treturn true;\n\n}\n\nvoid GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, \n\t\t\t\t\t\t\t\t\t int frame1, Eigen::Matrix<double, 3, 4> &Pose1,\n\t\t\t\t\t\t\t\t\t vector<SFMFeature> &sfm_f)\n{\n\tassert(frame0 != frame1);\n\tfor (int j = 0; j < feature_num; j++)\n\t{\n\t\tif (sfm_f[j].state == true)\n\t\t\tcontinue;\n\t\tbool has_0 = false, has_1 = false;\n\t\tVector2d point0;\n\t\tVector2d point1;\n\t\tfor (int k = 0; k < (int)sfm_f[j].observation.size(); k++)\n\t\t{\n\t\t\tif (sfm_f[j].observation[k].first == frame0)\n\t\t\t{\n\t\t\t\tpoint0 = sfm_f[j].observation[k].second;\n\t\t\t\thas_0 = true;\n\t\t\t}\n\t\t\tif (sfm_f[j].observation[k].first == frame1)\n\t\t\t{\n\t\t\t\tpoint1 = sfm_f[j].observation[k].second;\n\t\t\t\thas_1 = true;\n\t\t\t}\n\t\t}\n\t\tif (has_0 && has_1)\n\t\t{\n\t\t\tVector3d point_3d;\n\t\t\ttriangulatePoint(Pose0, Pose1, point0, point1, point_3d);\n\t\t\tsfm_f[j].state = true;\n\t\t\tsfm_f[j].position[0] = point_3d(0);\n\t\t\tsfm_f[j].position[1] = point_3d(1);\n\t\t\tsfm_f[j].position[2] = point_3d(2);\n\t\t\t//cout << \"trangulated : \" << frame1 << \"  3d point : \"  << j << \"  \" << point_3d.transpose() << endl;\n\t\t}\t\t\t\t\t\t\t  \n\t}\n}\n\n// \t q w_R_cam t w_R_cam\n//  c_rotation cam_R_w \n//  c_translation cam_R_w\n// relative_q[i][j]  j_q_i\n// relative_t[i][j]  j_t_ji  (j < i)\nbool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,\n\t\t\t  const Matrix3d relative_R, const Vector3d relative_T,\n\t\t\t  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)\n{\n\tfeature_num = sfm_f.size();\n\t//cout << \"set 0 and \" << l << \" as known \" << endl;\n\t// have relative_r relative_t\n\t// intial two view\n\tq[l].w() = 1;\n\tq[l].x() = 0;\n\tq[l].y() = 0;\n\tq[l].z() = 0;\n\tT[l].setZero();\n\tq[frame_num - 1] = q[l] * Quaterniond(relative_R);\n\tT[frame_num - 1] = relative_T;\n\t//cout << \"init q_l \" << q[l].w() << \" \" << q[l].vec().transpose() << endl;\n\t//cout << \"init t_l \" << T[l].transpose() << endl;\n\n\t//rotate to cam frame\n\tMatrix3d c_Rotation[frame_num];\n\tVector3d c_Translation[frame_num];\n\tQuaterniond c_Quat[frame_num];\n\tdouble c_rotation[frame_num][4];\n\tdouble c_translation[frame_num][3];\n\tEigen::Matrix<double, 3, 4> Pose[frame_num];\n\n\tc_Quat[l] = q[l].inverse();\n\tc_Rotation[l] = c_Quat[l].toRotationMatrix();\n\tc_Translation[l] = -1 * (c_Rotation[l] * T[l]);\n\tPose[l].block<3, 3>(0, 0) = c_Rotation[l];\n\tPose[l].block<3, 1>(0, 3) = c_Translation[l];\n\n\tc_Quat[frame_num - 1] = q[frame_num - 1].inverse();\n\tc_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();\n\tc_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);\n\tPose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];\n\tPose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];\n\n\n\t//1: trangulate between l ----- frame_num - 1\n\t//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1; \n\tfor (int i = l; i < frame_num - 1 ; i++)\n\t{\n\t\t// solve pnp\n\t\tif (i > l)\n\t\t{\n\t\t\tMatrix3d R_initial = c_Rotation[i - 1];\n\t\t\tVector3d P_initial = c_Translation[i - 1];\n\t\t\tif(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))\n\t\t\t\treturn false;\n\t\t\tc_Rotation[i] = R_initial;\n\t\t\tc_Translation[i] = P_initial;\n\t\t\tc_Quat[i] = c_Rotation[i];\n\t\t\tPose[i].block<3, 3>(0, 0) = c_Rotation[i];\n\t\t\tPose[i].block<3, 1>(0, 3) = c_Translation[i];\n\t\t}\n\n\t\t// triangulate point based on the solve pnp result\n\t\ttriangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);\n\t}\n\t//3: triangulate l-----l+1 l+2 ... frame_num -2\n\tfor (int i = l + 1; i < frame_num - 1; i++)\n\t\ttriangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);\n\t//4: solve pnp l-1; triangulate l-1 ----- l\n\t//             l-2              l-2 ----- l\n\tfor (int i = l - 1; i >= 0; i--)\n\t{\n\t\t//solve pnp\n\t\tMatrix3d R_initial = c_Rotation[i + 1];\n\t\tVector3d P_initial = c_Translation[i + 1];\n\t\tif(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))\n\t\t\treturn false;\n\t\tc_Rotation[i] = R_initial;\n\t\tc_Translation[i] = P_initial;\n\t\tc_Quat[i] = c_Rotation[i];\n\t\tPose[i].block<3, 3>(0, 0) = c_Rotation[i];\n\t\tPose[i].block<3, 1>(0, 3) = c_Translation[i];\n\t\t//triangulate\n\t\ttriangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);\n\t}\n\t//5: triangulate all other points\n\tfor (int j = 0; j < feature_num; j++)\n\t{\n\t\tif (sfm_f[j].state == true)\n\t\t\tcontinue;\n\t\tif ((int)sfm_f[j].observation.size() >= 2)\n\t\t{\n\t\t\tVector2d point0, point1;\n\t\t\tint frame_0 = sfm_f[j].observation[0].first;\n\t\t\tpoint0 = sfm_f[j].observation[0].second;\n\t\t\tint frame_1 = sfm_f[j].observation.back().first;\n\t\t\tpoint1 = sfm_f[j].observation.back().second;\n\t\t\tVector3d point_3d;\n\t\t\ttriangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);\n\t\t\tsfm_f[j].state = true;\n\t\t\tsfm_f[j].position[0] = point_3d(0);\n\t\t\tsfm_f[j].position[1] = point_3d(1);\n\t\t\tsfm_f[j].position[2] = point_3d(2);\n\t\t\t//cout << \"trangulated : \" << frame_0 << \" \" << frame_1 << \"  3d point : \"  << j << \"  \" << point_3d.transpose() << endl;\n\t\t}\t\t\n\t}\n\n/*\n\tfor (int i = 0; i < frame_num; i++)\n\t{\n\t\tq[i] = c_Rotation[i].transpose(); \n\t\tcout << \"solvePnP  q\" << \" i \" << i <<\"  \" <<q[i].w() << \"  \" << q[i].vec().transpose() << endl;\n\t}\n\tfor (int i = 0; i < frame_num; i++)\n\t{\n\t\tVector3d t_tmp;\n\t\tt_tmp = -1 * (q[i] * c_Translation[i]);\n\t\tcout << \"solvePnP  t\" << \" i \" << i <<\"  \" << t_tmp.x() <<\"  \"<< t_tmp.y() <<\"  \"<< t_tmp.z() << endl;\n\t}\n*/\n\t//full BA\n\tceres::Problem problem;\n\tceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();\n\t//cout << \" begin full BA \" << endl;\n\tfor (int i = 0; i < frame_num; i++)\n\t{\n\t\t//double array for ceres\n\t\tc_translation[i][0] = c_Translation[i].x();\n\t\tc_translation[i][1] = c_Translation[i].y();\n\t\tc_translation[i][2] = c_Translation[i].z();\n\t\tc_rotation[i][0] = c_Quat[i].w();\n\t\tc_rotation[i][1] = c_Quat[i].x();\n\t\tc_rotation[i][2] = c_Quat[i].y();\n\t\tc_rotation[i][3] = c_Quat[i].z();\n\t\tproblem.AddParameterBlock(c_rotation[i], 4, local_parameterization);\n\t\tproblem.AddParameterBlock(c_translation[i], 3);\n\t\tif (i == l)\n\t\t{\n\t\t\tproblem.SetParameterBlockConstant(c_rotation[i]);\n\t\t}\n\t\tif (i == l || i == frame_num - 1)\n\t\t{\n\t\t\tproblem.SetParameterBlockConstant(c_translation[i]);\n\t\t}\n\t}\n\n\tfor (int i = 0; i < feature_num; i++)\n\t{\n\t\tif (sfm_f[i].state != true)\n\t\t\tcontinue;\n\t\tfor (int j = 0; j < int(sfm_f[i].observation.size()); j++)\n\t\t{\n\t\t\tint l = sfm_f[i].observation[j].first;\n\t\t\tceres::CostFunction* cost_function = ReprojectionError3D::Create(\n\t\t\t\t\t\t\t\t\t\t\t\tsfm_f[i].observation[j].second.x(),\n\t\t\t\t\t\t\t\t\t\t\t\tsfm_f[i].observation[j].second.y());\n\n    \t\tproblem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], \n    \t\t\t\t\t\t\t\tsfm_f[i].position);\t \n\t\t}\n\n\t}\n\tceres::Solver::Options options;\n\toptions.linear_solver_type = ceres::DENSE_SCHUR;\n\t//options.minimizer_progress_to_stdout = true;\n\toptions.max_solver_time_in_seconds = 0.2;\n\tceres::Solver::Summary summary;\n\tceres::Solve(options, &problem, &summary);\n\t//std::cout << summary.BriefReport() << \"\\n\";\n\tif (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)\n\t{\n\t\t//cout << \"vision only BA converge\" << endl;\n\t}\n\telse\n\t{\n\t\t//cout << \"vision only BA not converge \" << endl;\n\t\treturn false;\n\t}\n\tfor (int i = 0; i < frame_num; i++)\n\t{\n\t\tq[i].w() = c_rotation[i][0]; \n\t\tq[i].x() = c_rotation[i][1]; \n\t\tq[i].y() = c_rotation[i][2]; \n\t\tq[i].z() = c_rotation[i][3]; \n\t\tq[i] = q[i].inverse();\n\t\t//cout << \"final  q\" << \" i \" << i <<\"  \" <<q[i].w() << \"  \" << q[i].vec().transpose() << endl;\n\t}\n\tfor (int i = 0; i < frame_num; i++)\n\t{\n\n\t\tT[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));\n\t\t//cout << \"final  t\" << \" i \" << i <<\"  \" << T[i](0) <<\"  \"<< T[i](1) <<\"  \"<< T[i](2) << endl;\n\t}\n\tfor (int i = 0; i < (int)sfm_f.size(); i++)\n\t{\n\t\tif(sfm_f[i].state)\n\t\t\tsfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);\n\t}\n\treturn true;\n\n}\n\n"
  },
  {
    "path": "vins_estimator/src/initial/initial_sfm.h",
    "content": "#pragma once \n#include <ceres/ceres.h>\n#include <ceres/rotation.h>\n#include <eigen3/Eigen/Dense>\n#include <iostream>\n#include <cstdlib>\n#include <deque>\n#include <map>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/opencv.hpp>\nusing namespace Eigen;\nusing namespace std;\n\n\n\nstruct SFMFeature\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    bool state;\n    int id;\n    vector<pair<int,Vector2d>> observation;\n    double position[3];\n    double depth;\n};\n\nstruct ReprojectionError3D\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tReprojectionError3D(double observed_u, double observed_v)\n\t\t:observed_u(observed_u), observed_v(observed_v)\n\t\t{}\n\n\ttemplate <typename T>\n\tbool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const\n\t{\n\t\tT p[3];\n\t\tceres::QuaternionRotatePoint(camera_R, point, p);\n\t\tp[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2];\n\t\tT xp = p[0] / p[2];\n    \tT yp = p[1] / p[2];\n    \tresiduals[0] = xp - T(observed_u);\n    \tresiduals[1] = yp - T(observed_v);\n    \treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double observed_x,\n\t                                   const double observed_y) \n\t{\n\t  return (new ceres::AutoDiffCostFunction<\n\t          ReprojectionError3D, 2, 4, 3, 3>(\n\t          \tnew ReprojectionError3D(observed_x,observed_y)));\n\t}\n\n\tdouble observed_u;\n\tdouble observed_v;\n};\n\nclass GlobalSFM\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tGlobalSFM();\n\tbool construct(int frame_num, Quaterniond* q, Vector3d* T, int l,\n\t\t\t  const Matrix3d relative_R, const Vector3d relative_T,\n\t\t\t  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points);\n\nprivate:\n\tbool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector<SFMFeature> &sfm_f);\n\n\tvoid triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,\n\t\t\t\t\t\t\tVector2d &point0, Vector2d &point1, Vector3d &point_3d);\n\tvoid triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, \n\t\t\t\t\t\t\t  int frame1, Eigen::Matrix<double, 3, 4> &Pose1,\n\t\t\t\t\t\t\t  vector<SFMFeature> &sfm_f);\n\n\tint feature_num;\n};\n"
  },
  {
    "path": "vins_estimator/src/initial/solve_5pts.cpp",
    "content": "#include \"solve_5pts.h\"\n\n\nnamespace cv {\n    void decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )\n    {\n\n        Mat E = _E.getMat().reshape(1, 3);\n        CV_Assert(E.cols == 3 && E.rows == 3);\n\n        Mat D, U, Vt;\n        SVD::compute(E, D, U, Vt);\n\n        if (determinant(U) < 0) U *= -1.;\n        if (determinant(Vt) < 0) Vt *= -1.;\n\n        Mat W = (Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);\n        W.convertTo(W, E.type());\n\n        Mat R1, R2, t;\n        R1 = U * W * Vt;\n        R2 = U * W.t() * Vt;\n        t = U.col(2) * 1.0;\n\n        R1.copyTo(_R1);\n        R2.copyTo(_R2);\n        t.copyTo(_t);\n    }\n\n    int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,\n                         OutputArray _R, OutputArray _t, InputOutputArray _mask)\n    {\n\n        Mat points1, points2, cameraMatrix;\n        _points1.getMat().convertTo(points1, CV_64F);\n        _points2.getMat().convertTo(points2, CV_64F);\n        _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);\n\n        int npoints = points1.checkVector(2);\n        CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&\n                                  points1.type() == points2.type());\n\n        CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);\n\n        if (points1.channels() > 1)\n        {\n            points1 = points1.reshape(1, npoints);\n            points2 = points2.reshape(1, npoints);\n        }\n\n        double fx = cameraMatrix.at<double>(0,0);\n        double fy = cameraMatrix.at<double>(1,1);\n        double cx = cameraMatrix.at<double>(0,2);\n        double cy = cameraMatrix.at<double>(1,2);\n\n        points1.col(0) = (points1.col(0) - cx) / fx;\n        points2.col(0) = (points2.col(0) - cx) / fx;\n        points1.col(1) = (points1.col(1) - cy) / fy;\n        points2.col(1) = (points2.col(1) - cy) / fy;\n\n        points1 = points1.t();\n        points2 = points2.t();\n\n        Mat R1, R2, t;\n        decomposeEssentialMat(E, R1, R2, t);\n        Mat P0 = Mat::eye(3, 4, R1.type());\n        Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());\n        P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0;\n        P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0;\n        P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;\n        P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;\n\n        // Do the cheirality check.\n        // Notice here a threshold dist is used to filter\n        // out far away points (i.e. infinite points) since\n        // there depth may vary between postive and negtive.\n        double dist = 50.0;\n        Mat Q;\n        triangulatePoints(P0, P1, points1, points2, Q);\n        Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;\n        Q.row(0) /= Q.row(3);\n        Q.row(1) /= Q.row(3);\n        Q.row(2) /= Q.row(3);\n        Q.row(3) /= Q.row(3);\n        mask1 = (Q.row(2) < dist) & mask1;\n        Q = P1 * Q;\n        mask1 = (Q.row(2) > 0) & mask1;\n        mask1 = (Q.row(2) < dist) & mask1;\n\n        triangulatePoints(P0, P2, points1, points2, Q);\n        Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;\n        Q.row(0) /= Q.row(3);\n        Q.row(1) /= Q.row(3);\n        Q.row(2) /= Q.row(3);\n        Q.row(3) /= Q.row(3);\n        mask2 = (Q.row(2) < dist) & mask2;\n        Q = P2 * Q;\n        mask2 = (Q.row(2) > 0) & mask2;\n        mask2 = (Q.row(2) < dist) & mask2;\n\n        triangulatePoints(P0, P3, points1, points2, Q);\n        Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;\n        Q.row(0) /= Q.row(3);\n        Q.row(1) /= Q.row(3);\n        Q.row(2) /= Q.row(3);\n        Q.row(3) /= Q.row(3);\n        mask3 = (Q.row(2) < dist) & mask3;\n        Q = P3 * Q;\n        mask3 = (Q.row(2) > 0) & mask3;\n        mask3 = (Q.row(2) < dist) & mask3;\n\n        triangulatePoints(P0, P4, points1, points2, Q);\n        Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;\n        Q.row(0) /= Q.row(3);\n        Q.row(1) /= Q.row(3);\n        Q.row(2) /= Q.row(3);\n        Q.row(3) /= Q.row(3);\n        mask4 = (Q.row(2) < dist) & mask4;\n        Q = P4 * Q;\n        mask4 = (Q.row(2) > 0) & mask4;\n        mask4 = (Q.row(2) < dist) & mask4;\n\n        mask1 = mask1.t();\n        mask2 = mask2.t();\n        mask3 = mask3.t();\n        mask4 = mask4.t();\n\n        // If _mask is given, then use it to filter outliers.\n        if (!_mask.empty())\n        {\n            Mat mask = _mask.getMat();\n            CV_Assert(mask.size() == mask1.size());\n            bitwise_and(mask, mask1, mask1);\n            bitwise_and(mask, mask2, mask2);\n            bitwise_and(mask, mask3, mask3);\n            bitwise_and(mask, mask4, mask4);\n        }\n        if (_mask.empty() && _mask.needed())\n        {\n            _mask.create(mask1.size(), CV_8U);\n        }\n\n        CV_Assert(_R.needed() && _t.needed());\n        _R.create(3, 3, R1.type());\n        _t.create(3, 1, t.type());\n\n        int good1 = countNonZero(mask1);\n        int good2 = countNonZero(mask2);\n        int good3 = countNonZero(mask3);\n        int good4 = countNonZero(mask4);\n\n        if (good1 >= good2 && good1 >= good3 && good1 >= good4)\n        {\n            R1.copyTo(_R);\n            t.copyTo(_t);\n            if (_mask.needed()) mask1.copyTo(_mask);\n            return good1;\n        }\n        else if (good2 >= good1 && good2 >= good3 && good2 >= good4)\n        {\n            R2.copyTo(_R);\n            t.copyTo(_t);\n            if (_mask.needed()) mask2.copyTo(_mask);\n            return good2;\n        }\n        else if (good3 >= good1 && good3 >= good2 && good3 >= good4)\n        {\n            t = -t;\n            R1.copyTo(_R);\n            t.copyTo(_t);\n            if (_mask.needed()) mask3.copyTo(_mask);\n            return good3;\n        }\n        else\n        {\n            t = -t;\n            R2.copyTo(_R);\n            t.copyTo(_t);\n            if (_mask.needed()) mask4.copyTo(_mask);\n            return good4;\n        }\n    }\n\n    int recoverPose( InputArray E, InputArray _points1, InputArray _points2, OutputArray _R,\n                         OutputArray _t, double focal, Point2d pp, InputOutputArray _mask)\n    {\n        Mat cameraMatrix = (Mat_<double>(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);\n        return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask);\n    }\n}\n\n\nbool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)\n{\n    if (corres.size() >= 15)\n    {\n        vector<cv::Point2f> ll, rr;\n        for (int i = 0; i < int(corres.size()); i++)\n        {\n            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));\n            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));\n        }\n        cv::Mat mask;\n        cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);\n        cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);\n        cv::Mat rot, trans;\n        int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);\n        //cout << \"inlier_cnt \" << inlier_cnt << endl;\n\n        Eigen::Matrix3d R;\n        Eigen::Vector3d T;\n        for (int i = 0; i < 3; i++)\n        {   \n            T(i) = trans.at<double>(i, 0);\n            for (int j = 0; j < 3; j++)\n                R(i, j) = rot.at<double>(i, j);\n        }\n\n        Rotation = R.transpose();\n        Translation = -R.transpose() * T;\n        if(inlier_cnt > 12)\n            return true;\n        else\n            return false;\n    }\n    return false;\n}\n\n\n\n"
  },
  {
    "path": "vins_estimator/src/initial/solve_5pts.h",
    "content": "#pragma once\n\n#include <vector>\nusing namespace std;\n\n#include <opencv2/opencv.hpp>\n//#include <opencv2/core/eigen.hpp>\n#include <eigen3/Eigen/Dense>\nusing namespace Eigen;\n\n//#include <ros/console.h>\n\nclass MotionEstimator\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n    bool solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &R, Vector3d &T);\n\n  private:\n    double testTriangulation(const vector<cv::Point2f> &l,\n                             const vector<cv::Point2f> &r,\n                             cv::Mat_<double> R, cv::Mat_<double> t);\n    void decomposeE(cv::Mat E,\n                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,\n                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);\n};\n\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/DLoopDetector.h",
    "content": "/*\n * File: DLoopDetector.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: Generic include file for the DLoopDetector classes and\n *   the specialized loop detectors\n * License: see the LICENSE.txt file\n *\n */\n\n/*! \\mainpage DLoopDetector Library\n *\n * DLoopDetector library for C++:\n * Loop detector for monocular image sequences based on bags of words\n *\n * Written by Dorian Galvez-Lopez,\n * University of Zaragoza\n * \n * Check my website to obtain updates: http://webdiis.unizar.es/~dorian\n *\n * \\section requirements Requirements\n * This library requires the DUtils, DUtilsCV, DVision, DBoW2 and OpenCV libraries,\n * as well as the boost::dynamic_bitset class.\n *\n * \\section citation Citation\n * If you use this software in academic works, please cite:\n <pre>\n   @@ARTICLE{GalvezTRO12,\n    author={Galvez-Lopez, Dorian and Tardos, J. D.}, \n    journal={IEEE Transactions on Robotics},\n    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},\n    year={2012},\n    month={October},\n    volume={28},\n    number={5},\n    pages={1188--1197},\n    doi={10.1109/TRO.2012.2197158},\n    ISSN={1552-3098}\n  }\n </pre>\n *\n * \\section license License\n * This file is licensed under a Creative Commons \n * Attribution-NonCommercial-ShareAlike 3.0 license. \n * This file can be freely used and users can use, download and edit this file \n * provided that credit is attributed to the original author. No users are \n * permitted to use this file for commercial purposes unless explicit permission\n * is given by the original author. Derivative works must be licensed using the\n * same or similar license.\n * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further\n * details.\n *\n */\n\n\n#ifndef __D_T_LOOP_DETECTOR__\n#define __D_T_LOOP_DETECTOR__\n\n/// Loop detector for sequences of monocular images\nnamespace DLoopDetector\n{\n}\n\n//#include \"DBoW2.h\"\n#include \"TemplatedLoopDetector.h\"\n#include \"ThirdParty/DBoW/FBrief.h\"\n\n/// BRIEF Loop Detector\ntypedef DLoopDetector::TemplatedLoopDetector\n  <FBrief::TDescriptor, FBrief> BriefLoopDetector;\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/TemplatedLoopDetector.h",
    "content": "/**\n * File: TemplatedLoopDetector\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated loop detector\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_TEMPLATED_LOOP_DETECTOR__\n#define __D_T_TEMPLATED_LOOP_DETECTOR__\n\n#include <vector>\n#include <numeric>\n#include <fstream>\n#include <string>\n\n#include <opencv/cv.h>\n\n#include \"ThirdParty/DBoW/TemplatedVocabulary.h\"\n#include \"ThirdParty/DBoW/TemplatedDatabase.h\"\n#include \"ThirdParty/DBoW/QueryResults.h\"\n#include \"ThirdParty/DBoW/BowVector.h\"\n\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/EquidistantCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include \"camodocal/camera_models/ScaramuzzaCamera.h\"\n\nusing namespace std;\nusing namespace DBoW2;\nusing namespace camodocal;\n\nnamespace DLoopDetector {\n\n\n/// Geometrical checking methods\nenum GeometricalCheck\n{\n  /// Exhaustive search\n  GEOM_EXHAUSTIVE,\n  /// Use direct index\n  GEOM_DI,\n  /// Use a Flann structure\n  GEOM_FLANN,\n  /// Do not perform geometrical checking\n  GEOM_NONE\n};\n\n/// Reasons for dismissing loops\nenum DetectionStatus\n{\n  /// Loop correctly detected\n  LOOP_DETECTED,\n  /// All the matches are very recent\n  CLOSE_MATCHES_ONLY,\n  /// No matches against the database\n  NO_DB_RESULTS,\n  /// Score of current image against previous one too low\n  LOW_NSS_FACTOR,\n  /// Scores (or NS Scores) were below the alpha threshold\n  LOW_SCORES,\n  /// Not enough matches to create groups  \n  NO_GROUPS,\n  /// Not enough temporary consistent matches (k)\n  NO_TEMPORAL_CONSISTENCY,\n  /// The geometrical consistency failed\n  NO_GEOMETRICAL_CONSISTENCY\n};\n\n/// Result of a detection\nstruct DetectionResult\n{\n  /// Detection status. LOOP_DETECTED iff loop detected\n  DetectionStatus status;\n  /// Query id\n  EntryId query;\n  /// Matched id if loop detected, otherwise, best candidate \n  EntryId match;\n  \n  /**\n   * Checks if the loop was detected\n   * @return true iff a loop was detected\n   */\n  inline bool detection() const\n  {\n    return status == LOOP_DETECTED;\n  }\n};\n\n/// TDescriptor: class of descriptor\n/// F: class of descriptor functions\ntemplate<class TDescriptor, class F>\n/// Generic Loop detector\nclass TemplatedLoopDetector\n{\npublic:\n  \n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n  /// Parameters to create a loop detector\n  struct Parameters\n  {\n    /// Height of expected images\n    int image_rows;\n    /// Width of expected images\n    int image_cols;\n    \n    // Main loop detector parameters\n    \n    /// Use normalized similarity score?\n    bool use_nss;\n    /// Alpha threshold\n    float alpha;\n    /// Min consistent matches to pass the temporal check\n    int k;\n    /// Geometrical check\n    GeometricalCheck geom_check;\n    /// If using direct index for geometrical checking, direct index levels\n    int di_levels;\n    \n    // These are less deciding parameters of the system\n    \n    /// Distance between entries to be consider a match\n    int dislocal;\n    /// Max number of results from db queries to consider\n    int max_db_results;\n    /// Min raw score between current entry and previous one to consider a match \n    float min_nss_factor;\n    /// Min number of close matches to consider some of them\n    int min_matches_per_group; \n    /// Max separation between matches to consider them of the same group\n    int max_intragroup_gap; \n    /// Max separation between groups of matches to consider them consistent\n    int max_distance_between_groups;\n    /// Max separation between two queries to consider them consistent\n    int max_distance_between_queries; \n  \n    // These are for the RANSAC to compute the F\n    \n    /// Min number of inliers when computing a fundamental matrix\n    int min_Fpoints;\n    /// Max number of iterations of RANSAC\n    int max_ransac_iterations;\n    /// Success probability of RANSAC\n    double ransac_probability;\n    /// Max reprojection error of fundamental matrices\n    double max_reprojection_error;\n    \n    // This is to compute correspondences\n    \n    /// Max value of the neighbour-ratio of accepted correspondences\n    double max_neighbor_ratio;\n  \n    /**\n     * Creates parameters by default\n     */ \n    Parameters();\n    \n    /**\n     * Creates parameters by default\n     * @param height image height\n     * @param width image width\n     * @param frequency set the value of some parameters according to the \n     *   expected working frequency (Hz) \n     * @param nss use normalized similarity score\n     * @param _alpha alpha parameter\n     * @param _k k parameter (number of temporary consistent matches)\n     * @param geom type of geometrical check\n     * @param dilevels direct index levels when geom == GEOM_DI\n     */ \n    Parameters(int height, int width, float frequency = 1, bool nss = true,\n      float _alpha = 0.3, int _k = 1, \n      GeometricalCheck geom = GEOM_DI, int dilevels = 2);\n      \n  private:\n    /**\n     * Sets some parameters according to the frequency\n     * @param frequency\n     */\n    void set(float frequency);\n  };\n\n  //camera param\n  camodocal::CameraPtr m_camera;\n  \npublic:\n\n  /**\n   * Empty constructor\n   */\n  TemplatedLoopDetector(const Parameters &params = Parameters());\n\n  /**\n   * Creates a loop detector with the given parameters and with a BoW2 database \n   * with the given vocabulary\n   * @param voc vocabulary\n   * @param params loop detector parameters\n   */\n  TemplatedLoopDetector(const TemplatedVocabulary<TDescriptor, F> &voc,\n    const Parameters &params = Parameters());\n  \n  /**\n   * Creates a loop detector with a copy of the given database, but clearing\n   * its contents\n   * @param db database to copy\n   * @param params loop detector parameters\n   */\n  TemplatedLoopDetector(const TemplatedDatabase<TDescriptor, F> &db,\n    const Parameters &params = Parameters());\n\n  /**\n   * Creates a loop detector with a copy of the given database, but clearing\n   * its contents\n   * @param T class derived from TemplatedDatabase\n   * @param db database to copy\n   * @param params loop detector parameters\n   */\n  template<class T>\n  TemplatedLoopDetector(const T &db, const Parameters &params = Parameters());\n\n  /**\n   * Destructor\n   */\n  virtual ~TemplatedLoopDetector(void);\n  \n  /**\n   * Retrieves a reference to the database used by the loop detector\n   * @return const reference to database\n   */\n  inline const TemplatedDatabase<TDescriptor, F>& getDatabase() const;\n  \n  /**\n   * Retrieves a reference to the vocabulary used by the loop detector\n   * @return const reference to vocabulary\n   */\n  inline const TemplatedVocabulary<TDescriptor, F>& getVocabulary() const;\n  \n  /**\n   * Init camera model\n  **/\n  void initCameraModel(const std::string &calib_file);\n  \n  /**\n   * Sets the database to use. The contents of the database and the detector\n   * entries are cleared\n   * @param T class derived from TemplatedDatabase\n   * @param db database to copy\n   */\n  template<class T>\n  void setDatabase(const T &db);\n  \n  /**\n   * Sets a new DBoW2 database created from the given vocabulary\n   * @param voc vocabulary to copy\n   */\n  void setVocabulary(const TemplatedVocabulary<TDescriptor, F>& voc);\n  \n  /**\n   * Allocates some memory for the first entries\n   * @param nentries number of expected entries\n   * @param nkeys number of keypoints per image expected\n   */\n  void allocate(int nentries, int nkeys = 0);\n\n  /**\n   * Adds the given tuple <keys, descriptors, current_t> to the database\n   * and returns the match if any\n   * @param keys keypoints of the image\n   * @param descriptors descriptors associated to the given keypoints\n   * @param match (out) match or failing information\n   * @return true iff there was match\n   */\n  bool detectLoop(const std::vector<cv::KeyPoint> &keys, \n    const std::vector<TDescriptor> &descriptors,\n    DetectionResult &match,std::vector<cv::Point2f> &cur_pts,\n                          std::vector<cv::Point2f> &old_pts);\n\n  /**\n   * Resets the detector and clears the database, such that the next entry\n   * will be 0 again\n   */\n  inline void clear();\n\n  void eraseIndex(std::vector<int> &erase_index);\n\nprotected:\n  \n  /// Matching island\n  struct tIsland\n  {\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    /// Island starting entry\n    EntryId first;\n    /// Island ending entry\n    EntryId last;\n    /// Island score\n    double score; // score of island\n    \n    /// Entry of the island with the highest score\n    EntryId best_entry; // id and score of the entry with the highest score\n    /// Highest single score in the island\n    double best_score;  // in the island\n    \n    /**\n     * Creates an empty island\n     */\n    tIsland(){}\n    \n    /**\n     * Creates an island\n     * @param f first entry\n     * @param l last entry\n     */\n    tIsland(EntryId f, EntryId l): first(f), last(l){}\n    \n    /**\n     * Creates an island\n     * @param f first entry\n     * @param l last entry\n     * @param s island score\n     */\n    tIsland(EntryId f, EntryId l, double s): first(f), last(l), score(s){}\n    \n    /**\n     * Says whether this score is less than the score of another island\n     * @param b\n     * @return true iff this score < b.score\n     */\n    inline bool operator < (const tIsland &b) const\n    {\n      return this->score < b.score;\n    }\n    \n    /**\n     * Says whether this score is greater than the score of another island\n     * @param b\n     * @return true iff this score > b.score\n     */\n    inline bool operator > (const tIsland &b) const\n    {\n      return this->score > b.score;\n    }\n    \n    /** \n     * Returns true iff a > b\n     * This function is used to sort in descending order\n     * @param a\n     * @param b\n     * @return a > b\n     */\n    static inline bool gt(const tIsland &a, const tIsland &b)\n    {\n      return a.score > b.score;\n    }\n        \n    /**\n     * Returns true iff entry ids of a are less then those of b.\n     * Assumes there is no overlap between the islands\n     * @param a\n     * @param b\n     * @return a.first < b.first\n     */\n    static inline bool ltId(const tIsland &a, const tIsland &b)\n    {\n      return a.first < b.first;\n    }\n    \n    /**\n     * Returns the length of the island\n     * @return length of island\n     */\n    inline int length() const { return last - first + 1; }\n    \n    /**\n     * Returns a printable version of the island\n     * @return printable island\n     */\n    std::string toString() const\n    {\n      stringstream ss;\n      ss << \"[\" << first << \"-\" << last << \": \" << score << \" | best: <\"\n        << best_entry << \": \" << best_score << \"> ] \";\n      return ss.str();\n    }\n  };\n  \n  /// Temporal consistency window\n  struct tTemporalWindow\n  {\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    /// Island matched in the last query\n    tIsland last_matched_island;\n    /// Last query id\n    EntryId last_query_id;\n    /// Number of consistent entries in the window\n    int nentries;\n    \n    /**\n     * Creates an empty temporal window\n     */\n    tTemporalWindow(): nentries(0) {}\n  };\n  \n  \nprotected:\n  \n  /**\n   * Removes from q those results whose score is lower than threshold\n   * (that should be alpha * ns_factor)\n   * @param q results from query\n   * @param threshold min value of the resting results\n   */\n  void removeLowScores(QueryResults &q, double threshold) const;\n  \n  /**\n   * Returns the islands of the given matches in ascending order of entry ids\n   * @param q \n   * @param islands (out) computed islands\n   */\n  void computeIslands(QueryResults &q, vector<tIsland> &islands) const;\n  \n  /**\n   * Returns the score of the island composed of the entries of q whose indices\n   * are in [i_first, i_last] (both included)\n   * @param q\n   * @param i_first first index of q of the island\n   * @param i_last last index of q of the island\n   * @return island score\n   */\n  double calculateIslandScore(const QueryResults &q, unsigned int i_first, \n    unsigned int i_last) const;\n\n  /**\n   * Updates the temporal window by adding the given match <island, id>, such \n   * that the window will contain only those islands which are consistent\n   * @param matched_island\n   * @param entry_id\n   */\n  void updateTemporalWindow(const tIsland &matched_island, EntryId entry_id);\n  \n  /**\n   * Returns the number of consistent islands in the temporal window\n   * @return number of temporal consistent islands\n   */\n  inline int getConsistentEntries() const\n  {\n    return m_window.nentries;\n  }\n  \n  void reduceInputToOutput(vector<cv::Point2f> v_in, vector<cv::Point2f> &v_out, vector<uchar> status);\n  bool checkFoundamental(vector<cv::Point2f> cur_input_pts,\n                   vector<cv::Point2f> old_input_pts,\n                   vector<cv::Point2f> &cur_output_pts,\n                   vector<cv::Point2f> &old_output_pts);\n  /**\n   * Check if an old entry is geometrically consistent (by calculating a \n   * fundamental matrix) with the given set of keys and descriptors\n   * @param old_entry entry id of the stored image to check\n   * @param keys current keypoints\n   * @param descriptors current descriptors associated to the given keypoints\n   * @param curvec feature vector of the current entry \n   */\n  bool isGeometricallyConsistent_DI(EntryId old_entry, \n    const std::vector<cv::KeyPoint> &keys, \n    const std::vector<TDescriptor> &descriptors, \n    const FeatureVector &curvec,\n    std::vector<cv::Point2f> &cur_pts,\n    std::vector<cv::Point2f> &old_pts);\n\n  /**\n   * Creates a flann structure from a set of descriptors to perform queries\n   * @param descriptors\n   * @param flann_structure (out) flann matcher\n   */\n  void getFlannStructure(const std::vector<TDescriptor> &descriptors, \n    cv::FlannBasedMatcher &flann_structure) const;\n\n  /**\n   * Calculate the matches between the descriptors A[i_A] and the descriptors\n   * B[i_B]. Applies a left-right matching without neighbour ratio \n   * @param A set A of descriptors\n   * @param i_A only descriptors A[i_A] will be checked\n   * @param B set B of descriptors\n   * @param i_B only descriptors B[i_B] will be checked\n   * @param i_match_A (out) indices of descriptors matched (s.t. A[i_match_A])\n   * @param i_match_B (out) indices of descriptors matched (s.t. B[i_match_B])\n   */\n  void getMatches_neighratio(const std::vector<TDescriptor> &A, \n    const vector<unsigned int> &i_A, const vector<TDescriptor> &B,\n    const vector<unsigned int> &i_B,\n    vector<unsigned int> &i_match_A, vector<unsigned int> &i_match_B) const;\n\nprotected:\n\n  /// Database\n  // The loop detector stores its own copy of the database\n  TemplatedDatabase<TDescriptor,F> *m_database;\n  \n  /// KeyPoints of images\n  vector<vector<cv::KeyPoint> > m_image_keys;\n  \n  /// Descriptors of images\n  vector<vector<TDescriptor> > m_image_descriptors;\n  \n  /// Last bow vector added to database\n  BowVector m_last_bowvec;\n  \n  /// Temporal consistency window\n  tTemporalWindow m_window;\n  \n  /// Parameters of loop detector\n  Parameters m_params;\n  \n};\n\n// --------------------------------------------------------------------------\n\ntemplate <class TDescriptor, class F> \nTemplatedLoopDetector<TDescriptor,F>::Parameters::Parameters():\n  use_nss(true), alpha(0.3), k(4), geom_check(GEOM_DI), di_levels(0)\n{\n  set(1);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate <class TDescriptor, class F> \nTemplatedLoopDetector<TDescriptor,F>::Parameters::Parameters\n  (int height, int width, float frequency, bool nss, float _alpha, \n  int _k, GeometricalCheck geom, int dilevels)\n  : image_rows(height), image_cols(width), use_nss(nss), alpha(_alpha), k(_k),\n    geom_check(geom), di_levels(dilevels)\n{\n  set(frequency);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate <class TDescriptor, class F> \nvoid TemplatedLoopDetector<TDescriptor,F>::Parameters::set(float f)\n{\n  dislocal = 20 * f;\n  max_db_results = 50 * f;\n  min_nss_factor = 0.005;\n  min_matches_per_group = f;\n  max_intragroup_gap = 3 * f;\n  max_distance_between_groups = 3 * f;\n  max_distance_between_queries = 2 * f; \n\n  min_Fpoints = 12;\n  max_ransac_iterations = 500;\n  ransac_probability = 0.99;\n  max_reprojection_error = 2.0;\n  \n  max_neighbor_ratio = 0.6;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedLoopDetector<TDescriptor,F>::TemplatedLoopDetector\n  (const Parameters &params)\n  : m_database(NULL), m_params(params)\n{\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedLoopDetector<TDescriptor,F>::TemplatedLoopDetector\n  (const TemplatedVocabulary<TDescriptor, F> &voc, const Parameters &params)\n  : m_params(params) \n{\n  m_database = new TemplatedDatabase<TDescriptor, F>(voc, \n    params.geom_check == GEOM_DI, params.di_levels);\n  \n  //TODO change param trans method\n  //m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n  //printf(\"loop set camera model finish\\n\");\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor,F>::setVocabulary\n  (const TemplatedVocabulary<TDescriptor, F>& voc)\n{\n  delete m_database;\n  m_database = new TemplatedDatabase<TDescriptor, F>(voc, \n    m_params.geom_check == GEOM_DI, m_params.di_levels);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedLoopDetector<TDescriptor, F>::TemplatedLoopDetector\n  (const TemplatedDatabase<TDescriptor, F> &db, const Parameters &params)\n  : m_params(params)\n{\n  m_database = new TemplatedDatabase<TDescriptor, F>(db.getVocabulary(),\n    params.geom_check == GEOM_DI, params.di_levels);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ntemplate<class T>\nTemplatedLoopDetector<TDescriptor, F>::TemplatedLoopDetector\n  (const T &db, const Parameters &params)\n  : m_params(params)\n{\n  m_database = new T(db);\n  m_database->clear();\n}\n\n// --------------------------------------------------------------------------\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::initCameraModel(const std::string &calib_file)\n{\n  m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ntemplate<class T>\nvoid TemplatedLoopDetector<TDescriptor, F>::setDatabase(const T &db)\n{\n  delete m_database;\n  m_database = new T(db);\n  clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedLoopDetector<TDescriptor, F>::~TemplatedLoopDetector(void)\n{\n  delete m_database;\n  m_database = NULL;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor,F>::allocate\n  (int nentries, int nkeys)\n{\n  const int sz = (const int)m_image_keys.size();\n  \n  if(sz < nentries)\n  {\n    m_image_keys.resize(nentries);\n    m_image_descriptors.resize(nentries);\n  }\n  \n  if(nkeys > 0)\n  {\n    for(int i = sz; i < nentries; ++i)\n    {\n      m_image_keys[i].reserve(nkeys);\n      m_image_descriptors[i].reserve(nkeys);\n    }\n  }\n  \n  m_database->allocate(nentries, nkeys);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline const TemplatedDatabase<TDescriptor, F>& \nTemplatedLoopDetector<TDescriptor, F>::getDatabase() const\n{\n  return *m_database;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline const TemplatedVocabulary<TDescriptor, F>& \nTemplatedLoopDetector<TDescriptor, F>::getVocabulary() const\n{\n  return m_database->getVocabulary();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nbool TemplatedLoopDetector<TDescriptor, F>::detectLoop(\n  const std::vector<cv::KeyPoint> &keys, \n  const std::vector<TDescriptor> &descriptors,\n  DetectionResult &match,\n  std::vector<cv::Point2f> &cur_pts,\n  std::vector<cv::Point2f> &old_pts)\n{\n  EntryId entry_id = m_database->size();\n  match.query = entry_id;\n  \n  BowVector bowvec;\n  FeatureVector featvec;\n  \n  if(m_params.geom_check == GEOM_DI)\n    m_database->getVocabulary()->transform(descriptors, bowvec, featvec,\n      m_params.di_levels);\n  else\n    m_database->getVocabulary()->transform(descriptors, bowvec);\n\n  if((int)entry_id <= m_params.dislocal)\n  {\n    // only add the entry to the database and finish\n    m_database->add(bowvec, featvec);\n    match.status = CLOSE_MATCHES_ONLY;\n    //printf(\"entry_id %d <= m_params.dislocal %d\\n\",entry_id,m_params.dislocal);\n  }\n  else\n  {\n    //printf(\"entry_id %d > m_params.dislocal %d\\n\",entry_id,m_params.dislocal);\n    int max_id = (int)entry_id - m_params.dislocal;\n    \n    QueryResults qret;\n    m_database->query(bowvec, qret, m_params.max_db_results, max_id);\n\n    // update database\n    m_database->add(bowvec, featvec); // returns entry_id\n    \n    if(!qret.empty())\n    {\n      //printf(\"qret not empty\\n\");\n      // factor to compute normalized similarity score, if necessary\n      double ns_factor = 1.0;\n      \n      if(m_params.use_nss)\n      {\n        ns_factor = m_database->getVocabulary()->score(bowvec, m_last_bowvec);\n        //printf(\"use_nss, ns_factor : %lf\\n\", ns_factor);\n      }\n      \n      if(!m_params.use_nss || ns_factor >= m_params.min_nss_factor)\n      {\n        // scores in qret must be divided by ns_factor to obtain the\n        // normalized similarity score, but we can\n        // speed this up by moving ns_factor to alpha's\n        \n        // remove those scores whose nss is lower than alpha\n        // (ret is sorted in descending score order now)\n        //printf(\"ns_factor %lf >= m_params.min_nss_factor %lf\\n\",ns_factor,m_params.min_nss_factor);\n        removeLowScores(qret, m_params.alpha * ns_factor);\n        \n        if(!qret.empty())\n        {\n          //printf(\"qret not empty\\n\");\n          // the best candidate is the one with highest score by now\n          match.match = qret[0].Id;\n          \n          // compute islands\n          vector<tIsland> islands;\n          computeIslands(qret, islands); \n          // this modifies qret and changes the score order\n          \n          // get best island\n          if(!islands.empty())\n          {\n            //printf(\"island is not empty\\n\");\n            const tIsland& island = \n              *std::max_element(islands.begin(), islands.end());\n           \n            //printf(\"loop chose id:%d socre:%f------------\\n\",island.best_entry, island.best_score);\n            // check temporal consistency of this island\n            updateTemporalWindow(island, entry_id);\n            \n            // get the best candidate (maybe match)\n            match.match = island.best_entry;\n            \n            if(getConsistentEntries() > m_params.k)\n            {\n              //printf(\"temporal consistent entries:%d > n_params.k:%d\\n\", getConsistentEntries(),m_params.k);\n              // candidate loop detected\n              // check geometry\n              bool detection;\n\n              if(m_params.geom_check == GEOM_DI)\n              {\n                //printf(\"loop use direct index for geometrical checking\\n\");\n                // all the DI stuff is implicit in the database\n                detection = isGeometricallyConsistent_DI(island.best_entry, \n                  keys, descriptors, featvec, cur_pts, old_pts);\n              }\n              else // GEOM_NONE, accept the match\n              {\n                detection = true;\n                //printf(\"don't check detec true\\n\");\n              }\n              \n              if(detection)\n              {\n                match.status = LOOP_DETECTED;\n                //printf(\"LOOP_DETECTED\\n\");\n              }\n              else\n              {\n                match.status = NO_GEOMETRICAL_CONSISTENCY;\n                //printf(\"no geometry consistentcy\\n\");\n              }\n              \n            } // if enough temporal matches\n            else\n            {\n              match.status = NO_TEMPORAL_CONSISTENCY;\n              //printf(\"no temporal consistentcy\\n\");\n            }\n            \n          } // if there is some island\n          else\n          {\n            match.status = NO_GROUPS;\n            //printf(\"in island\\n\");\n          }\n        } // if !qret empty after removing low scores\n        else\n        {\n          match.status = LOW_SCORES;\n          //printf(\"query empty after remove low score\\n\");\n        }\n      } // if (ns_factor > min normal score)\n      else\n      {\n        //printf(\"low nss factor may be rotation\\n\");\n        match.status = LOW_NSS_FACTOR;\n      }\n    } // if(!qret.empty())\n    else\n    {\n      match.status = NO_DB_RESULTS;\n      //printf(\"data base no result\\n\");\n    }\n  }\n\n  // update record\n  // m_image_keys and m_image_descriptors have the same length\n  if(m_image_keys.size() == entry_id)\n  {\n    //printf(\"image size = entry_id %d\\n\", entry_id);\n    m_image_keys.push_back(keys);\n    m_image_descriptors.push_back(descriptors);\n  }\n  else\n  {\n    //printf(\"image size != entry_id %d\\n\", entry_id);\n    m_image_keys[entry_id] = keys;\n    m_image_descriptors[entry_id] = descriptors;\n  }\n  \n  // store this bowvec if we are going to use it in next iteratons\n  if(m_params.use_nss && (int)entry_id + 1 > m_params.dislocal)\n  {\n    m_last_bowvec = bowvec;\n  }\n\n  return match.detection();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline void TemplatedLoopDetector<TDescriptor, F>::clear()\n{\n  m_database->clear();\n  m_window.nentries = 0;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::computeIslands\n  (QueryResults &q, vector<tIsland> &islands) const\n{\n  islands.clear();\n  \n  if(q.size() == 1)\n  {\n    islands.push_back(tIsland(q[0].Id, q[0].Id, calculateIslandScore(q, 0, 0)));\n    islands.back().best_entry = q[0].Id;\n    islands.back().best_score = q[0].Score;\n  }\n  else if(!q.empty())\n  {\n    // sort query results in ascending order of ids\n    std::sort(q.begin(), q.end(), Result::ltId);\n    \n    // create long enough islands\n    QueryResults::const_iterator dit = q.begin();\n    int first_island_entry = dit->Id;\n    int last_island_entry = dit->Id;\n    \n    // these are indices of q\n    unsigned int i_first = 0;\n    unsigned int i_last = 0;\n    \n    double best_score = dit->Score;\n    EntryId best_entry = dit->Id;\n\n    ++dit;\n    for(unsigned int idx = 1; dit != q.end(); ++dit, ++idx)\n    {\n      if((int)dit->Id - last_island_entry < m_params.max_intragroup_gap)\n      {\n        // go on until find the end of the island\n        last_island_entry = dit->Id;\n        i_last = idx;\n        if(dit->Score > best_score)\n        {\n          best_score = dit->Score;\n          best_entry = dit->Id;\n        }\n      }\n      else\n      {\n        // end of island reached\n        int length = last_island_entry - first_island_entry + 1;\n        if(length >= m_params.min_matches_per_group)\n        {\n          islands.push_back( tIsland(first_island_entry, last_island_entry,\n            calculateIslandScore(q, i_first, i_last)) );\n          \n          islands.back().best_score = best_score;\n          islands.back().best_entry = best_entry;\n        }\n        \n        // prepare next island\n        first_island_entry = last_island_entry = dit->Id;\n        i_first = i_last = idx;\n        best_score = dit->Score;\n        best_entry = dit->Id;\n      }\n    }\n    // add last island\n    if(last_island_entry - first_island_entry + 1 >= \n      m_params.min_matches_per_group)\n    {\n      islands.push_back( tIsland(first_island_entry, last_island_entry,\n        calculateIslandScore(q, i_first, i_last)) );\n        \n      islands.back().best_score = best_score;\n      islands.back().best_entry = best_entry;\n    }\n  }\n  \n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ndouble TemplatedLoopDetector<TDescriptor, F>::calculateIslandScore(\n  const QueryResults &q, unsigned int i_first, unsigned int i_last) const\n{\n  // get the sum of the scores\n  double sum = 0;\n  while(i_first <= i_last) sum += q[i_first++].Score;\n  return sum;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::updateTemporalWindow\n  (const tIsland &matched_island, EntryId entry_id)\n{\n  // if m_window.nentries > 0, island > m_window.last_matched_island and\n  // entry_id > m_window.last_query_id hold\n  \n  if(m_window.nentries == 0 || int(entry_id - m_window.last_query_id)\n    > m_params.max_distance_between_queries)\n  {\n    m_window.nentries = 1;\n  }\n  else\n  {\n    EntryId a1 = m_window.last_matched_island.first;\n    EntryId a2 = m_window.last_matched_island.last;\n    EntryId b1 = matched_island.first;\n    EntryId b2 = matched_island.last;\n    \n    bool fit = (b1 <= a1 && a1 <= b2) || (a1 <= b1 && b1 <= a2);\n\n    if(!fit)\n    {\n      int d1 = (int)a1 - (int)b2;\n      int d2 = (int)b1 - (int)a2;\n      int gap = (d1 > d2 ? d1 : d2);\n      \n      fit = (gap <= m_params.max_distance_between_groups);\n    }\n    \n    if(fit) m_window.nentries++;\n    else m_window.nentries = 1;\n  }\n  \n  m_window.last_matched_island = matched_island;\n  m_window.last_query_id = entry_id;\n}\n\n// --------------------------------------------------------------------------\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::reduceInputToOutput(vector<cv::Point2f> v_in, vector<cv::Point2f> &v_out, vector<uchar> status)\n{\n    v_out.clear();\n    //v_out.reserve(v_in.size());\n    for (int i = 0; i < int(v_in.size()); i++)\n        if (status[i])\n            v_out.push_back(v_in[i]);\n}\n\ntemplate<class TDescriptor, class F>\nbool TemplatedLoopDetector<TDescriptor, F>::checkFoundamental(vector<cv::Point2f> cur_input_pts,\n                                                              vector<cv::Point2f> old_input_pts,\n                                                              vector<cv::Point2f> &cur_output_pts,\n                                                              vector<cv::Point2f> &old_output_pts)\n{\n    if (cur_input_pts.size() >= 8)\n    {\n        double ROW = 480.0;\n        double COL = 752.0;\n        double FOCAL_LENGTH = 460.0;\n        vector<cv::Point2f> un_cur_pts(cur_input_pts.size()), un_old_pts(cur_input_pts.size());\n        for (unsigned int i = 0; i < cur_input_pts.size(); i++)\n        {\n            Eigen::Vector3d tmp_p;\n            m_camera->liftProjective(Eigen::Vector2d(old_input_pts[i].x, old_input_pts[i].y), tmp_p);\n            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n            un_old_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n\n            m_camera->liftProjective(Eigen::Vector2d(cur_input_pts[i].x, cur_input_pts[i].y), tmp_p);\n            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n        }\n        vector<uchar> status;\n        cv::findFundamentalMat(un_old_pts, un_cur_pts, cv::FM_RANSAC, 1.0, 0.99, status);\n        cur_output_pts = cur_input_pts;\n        old_output_pts = old_input_pts;\n        reduceInputToOutput( cur_input_pts, cur_output_pts, status);\n        reduceInputToOutput( old_input_pts, old_output_pts, status);\n        if(cur_output_pts.size()>20)\n        {\n          return true;\n        }\n        else\n          return false;\n    }\n    else\n    {\n      return false;\n    }\n}\n\ntemplate<class TDescriptor, class F>\nbool TemplatedLoopDetector<TDescriptor, F>::isGeometricallyConsistent_DI(\n  EntryId old_entry, const std::vector<cv::KeyPoint> &keys, \n  const std::vector<TDescriptor> &descriptors, \n  const FeatureVector &bowvec,\n  std::vector<cv::Point2f> &cur_pts,\n  std::vector<cv::Point2f> &old_pts)\n{\n  const FeatureVector &oldvec = m_database->retrieveFeatures(old_entry);\n  \n  // for each word in common, get the closest descriptors\n  \n  vector<unsigned int> i_old, i_cur;\n  \n  FeatureVector::const_iterator old_it, cur_it; \n  const FeatureVector::const_iterator old_end = oldvec.end();\n  const FeatureVector::const_iterator cur_end = bowvec.end();\n  \n  old_it = oldvec.begin();\n  cur_it = bowvec.begin();\n  \n  while(old_it != old_end && cur_it != cur_end)\n  {\n    if(old_it->first == cur_it->first)\n    {\n      // compute matches between \n      // features old_it->second of m_image_keys[old_entry] and\n      // features cur_it->second of keys\n      vector<unsigned int> i_old_now, i_cur_now;\n      \n      getMatches_neighratio(\n        m_image_descriptors[old_entry], old_it->second, \n        descriptors, cur_it->second,  \n        i_old_now, i_cur_now);\n      \n      i_old.insert(i_old.end(), i_old_now.begin(), i_old_now.end());\n      i_cur.insert(i_cur.end(), i_cur_now.begin(), i_cur_now.end());\n      \n      // move old_it and cur_it forward\n      ++old_it;\n      ++cur_it;\n    }\n    else if(old_it->first < cur_it->first)\n    {\n      // move old_it forward\n      old_it = oldvec.lower_bound(cur_it->first);\n      // old_it = (first element >= cur_it.id)\n    }\n    else\n    {\n      // move cur_it forward\n      cur_it = bowvec.lower_bound(old_it->first);\n      // cur_it = (first element >= old_it.id)\n    }\n  }\n  \n  // calculate now the fundamental matrix\n  if((int)i_old.size() >= m_params.min_Fpoints)\n  {\n    vector<cv::Point2f> old_points, cur_points;\n    \n    // add matches to the vectors to calculate the fundamental matrix\n    vector<unsigned int>::const_iterator oit, cit;\n    oit = i_old.begin();\n    cit = i_cur.begin();\n    \n    for(; oit != i_old.end(); ++oit, ++cit)\n    {\n      const cv::KeyPoint &old_k = m_image_keys[old_entry][*oit];\n      const cv::KeyPoint &cur_k = keys[*cit];\n      \n      old_points.push_back(old_k.pt);\n      cur_points.push_back(cur_k.pt);\n    }\n  \n    cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]);\n    cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]);\n    //printf(\"-old size %d current size %d\\n\",old_points.size(),cur_points.size());\n    bool find_fundamental_suss = false;\n    find_fundamental_suss =  checkFoundamental(cur_points, old_points, cur_pts, old_pts);\n    if(find_fundamental_suss)\n    {\n      //cur_pts = cur_points;\n      //old_pts = old_points;\n      return true;\n    }\n  }\n  \n  return false;\n} \n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::getFlannStructure(\n  const std::vector<TDescriptor> &descriptors, \n  cv::FlannBasedMatcher &flann_structure) const\n{\n  vector<cv::Mat> features(1);\n  F::toMat32F(descriptors, features[0]);\n  \n  flann_structure.clear();\n  flann_structure.add(features);\n  flann_structure.train();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::getMatches_neighratio(\n  const vector<TDescriptor> &A, const vector<unsigned int> &i_A,\n  const vector<TDescriptor> &B, const vector<unsigned int> &i_B,\n  vector<unsigned int> &i_match_A, vector<unsigned int> &i_match_B) const \n{\n  i_match_A.resize(0);\n  i_match_B.resize(0);\n  i_match_A.reserve( min(i_A.size(), i_B.size()) );\n  i_match_B.reserve( min(i_A.size(), i_B.size()) );\n  \n  vector<unsigned int>::const_iterator ait, bit;\n  unsigned int i, j;\n  i = 0;\n  for(ait = i_A.begin(); ait != i_A.end(); ++ait, ++i)\n  {\n    int best_j_now = -1;\n    double best_dist_1 = 1e9;\n    double best_dist_2 = 1e9;\n    \n    j = 0;\n    for(bit = i_B.begin(); bit != i_B.end(); ++bit, ++j)\n    {\n      double d = F::distance(A[*ait], B[*bit]);\n            \n      // in i\n      if(d < best_dist_1)\n      {\n        best_j_now = j;\n        best_dist_2 = best_dist_1;\n        best_dist_1 = d;\n      }\n      else if(d < best_dist_2)\n      {\n        best_dist_2 = d;\n      }\n    }\n    \n    if(best_dist_1 / best_dist_2 <= m_params.max_neighbor_ratio)\n    {\n      unsigned int idx_B = i_B[best_j_now];\n      bit = find(i_match_B.begin(), i_match_B.end(), idx_B);\n      \n      if(bit == i_match_B.end())\n      {\n        i_match_B.push_back(idx_B);\n        i_match_A.push_back(*ait);\n      }\n      else\n      {\n        unsigned int idx_A = i_match_A[ bit - i_match_B.begin() ];\n        double d = F::distance(A[idx_A], B[idx_B]);\n        if(best_dist_1 < d)\n        {\n          i_match_A[ bit - i_match_B.begin() ] = *ait;\n        }\n      }\n        \n    }\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::removeLowScores(QueryResults &q,\n  double threshold) const\n{\n  // remember scores in q are in descending order now\n  //QueryResults::iterator qit = \n  //  lower_bound(q.begin(), q.end(), threshold, Result::geqv);\n  \n  Result aux(0, threshold);\n  QueryResults::iterator qit = \n    lower_bound(q.begin(), q.end(), aux, Result::geq);\n  \n  // qit = first element < m_alpha_minus || end\n  \n  if(qit != q.end())\n  {\n    int valid_entries = qit - q.begin();\n    q.resize(valid_entries);\n  }\n}\n\n// --------------------------------------------------------------------------\ntemplate<class TDescriptor, class F>\nvoid TemplatedLoopDetector<TDescriptor, F>::eraseIndex\n(std::vector<int> &erase_index)\n{\n  for (int i = 0; i < (int)erase_index.size(); i++)\n  {\n    DBoW2::EntryId entry;\n    entry = (unsigned int)erase_index[i];\n    m_database->delete_entry(entry);\n  }\n}\n} // namespace DLoopDetector\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/BowVector.cpp",
    "content": "/**\n * File: BowVector.cpp\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: bag of words vector\n * License: see the LICENSE.txt file\n *\n */\n\n#include <iostream>\n#include <fstream>\n#include <vector>\n#include <algorithm>\n#include <cmath>\n\n#include \"BowVector.h\"\n\nnamespace DBoW2 {\n\n// --------------------------------------------------------------------------\n\nBowVector::BowVector(void)\n{\n}\n\n// --------------------------------------------------------------------------\n\nBowVector::~BowVector(void)\n{\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::addWeight(WordId id, WordValue v)\n{\n  BowVector::iterator vit = this->lower_bound(id);\n  \n  if(vit != this->end() && !(this->key_comp()(id, vit->first)))\n  {\n    vit->second += v;\n  }\n  else\n  {\n    this->insert(vit, BowVector::value_type(id, v));\n  }\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::addIfNotExist(WordId id, WordValue v)\n{\n  BowVector::iterator vit = this->lower_bound(id);\n  \n  if(vit == this->end() || (this->key_comp()(id, vit->first)))\n  {\n    this->insert(vit, BowVector::value_type(id, v));\n  }\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::normalize(LNorm norm_type)\n{\n  double norm = 0.0; \n  BowVector::iterator it;\n\n  if(norm_type == DBoW2::L1)\n  {\n    for(it = begin(); it != end(); ++it)\n      norm += fabs(it->second);\n  }\n  else\n  {\n    for(it = begin(); it != end(); ++it)\n      norm += it->second * it->second;\n\t\tnorm = sqrt(norm);  \n  }\n\n  if(norm > 0.0)\n  {\n    for(it = begin(); it != end(); ++it)\n      it->second /= norm;\n  }\n}\n\n// --------------------------------------------------------------------------\n\nstd::ostream& operator<< (std::ostream &out, const BowVector &v)\n{\n  BowVector::const_iterator vit;\n  std::vector<unsigned int>::const_iterator iit;\n  unsigned int i = 0; \n  const unsigned int N = v.size();\n  for(vit = v.begin(); vit != v.end(); ++vit, ++i)\n  {\n    out << \"<\" << vit->first << \", \" << vit->second << \">\";\n    \n    if(i < N-1) out << \", \";\n  }\n  return out;\n}\n\n// --------------------------------------------------------------------------\n\nvoid BowVector::saveM(const std::string &filename, size_t W) const\n{\n  std::fstream f(filename.c_str(), std::ios::out);\n  \n  WordId last = 0;\n  BowVector::const_iterator bit;\n  for(bit = this->begin(); bit != this->end(); ++bit)\n  {\n    for(; last < bit->first; ++last)\n    {\n      f << \"0 \";\n    }\n    f << bit->second << \" \";\n    \n    last = bit->first + 1;\n  }\n  for(; last < (WordId)W; ++last)\n    f << \"0 \";\n  \n  f.close();\n}\n\n// --------------------------------------------------------------------------\n\n} // namespace DBoW2\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/BowVector.h",
    "content": "/**\n * File: BowVector.h\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: bag of words vector\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_BOW_VECTOR__\n#define __D_T_BOW_VECTOR__\n\n#include <iostream>\n#include <map>\n#include <vector>\n\nnamespace DBoW2 {\n\n/// Id of words\ntypedef unsigned int WordId;\n\n/// Value of a word\ntypedef double WordValue;\n\n/// Id of nodes in the vocabulary treee\ntypedef unsigned int NodeId;\n\n/// L-norms for normalization\nenum LNorm\n{\n  L1,\n  L2\n};\n\n/// Weighting type\nenum WeightingType\n{\n  TF_IDF,\n  TF,\n  IDF,\n  BINARY\n};\n\n/// Scoring type\nenum ScoringType\n{\n  L1_NORM,\n  L2_NORM,\n  CHI_SQUARE,\n  KL,\n  BHATTACHARYYA,\n  DOT_PRODUCT\n};\n\n/// Vector of words to represent images\nclass BowVector: \n\tpublic std::map<WordId, WordValue>\n{\npublic:\n\n\t/** \n\t * Constructor\n\t */\n\tBowVector(void);\n\n\t/**\n\t * Destructor\n\t */\n\t~BowVector(void);\n\t\n\t/**\n\t * Adds a value to a word value existing in the vector, or creates a new\n\t * word with the given value\n\t * @param id word id to look for\n\t * @param v value to create the word with, or to add to existing word\n\t */\n\tvoid addWeight(WordId id, WordValue v);\n\t\n\t/**\n\t * Adds a word with a value to the vector only if this does not exist yet\n\t * @param id word id to look for\n\t * @param v value to give to the word if this does not exist\n\t */\n\tvoid addIfNotExist(WordId id, WordValue v);\n\n\t/**\n\t * L1-Normalizes the values in the vector \n\t * @param norm_type norm used\n\t */\n\tvoid normalize(LNorm norm_type);\n\t\n\t/**\n\t * Prints the content of the bow vector\n\t * @param out stream\n\t * @param v\n\t */\n\tfriend std::ostream& operator<<(std::ostream &out, const BowVector &v);\n\t\n\t/**\n\t * Saves the bow vector as a vector in a matlab file\n\t * @param filename\n\t * @param W number of words in the vocabulary\n\t */\n\tvoid saveM(const std::string &filename, size_t W) const;\n};\n\n} // namespace DBoW2\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/DBoW2.h",
    "content": "/*\n * File: DBoW2.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: Generic include file for the DBoW2 classes and\n *   the specialized vocabularies and databases\n * License: see the LICENSE.txt file\n *\n */\n\n/*! \\mainpage DBoW2 Library\n *\n * DBoW2 library for C++:\n * Bag-of-word image database for image retrieval.\n *\n * Written by Dorian Galvez-Lopez,\n * University of Zaragoza\n * \n * Check my website to obtain updates: http://doriangalvez.com\n *\n * \\section requirements Requirements\n * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries,\n * as well as the boost::dynamic_bitset class.\n *\n * \\section citation Citation\n * If you use this software in academic works, please cite:\n <pre>\n   @@ARTICLE{GalvezTRO12,\n    author={Galvez-Lopez, Dorian and Tardos, J. D.}, \n    journal={IEEE Transactions on Robotics},\n    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},\n    year={2012},\n    month={October},\n    volume={28},\n    number={5},\n    pages={1188--1197},\n    doi={10.1109/TRO.2012.2197158},\n    ISSN={1552-3098}\n  }\n </pre>\n *\n * \\section license License\n * This file is licensed under a Creative Commons \n * Attribution-NonCommercial-ShareAlike 3.0 license. \n * This file can be freely used and users can use, download and edit this file \n * provided that credit is attributed to the original author. No users are \n * permitted to use this file for commercial purposes unless explicit permission\n * is given by the original author. Derivative works must be licensed using the\n * same or similar license.\n * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further\n * details.\n *\n */\n\n#ifndef __D_T_DBOW2__\n#define __D_T_DBOW2__\n\n/// Includes all the data structures to manage vocabularies and image databases\nnamespace DBoW2\n{\n}\n\n#include \"TemplatedVocabulary.h\"\n#include \"TemplatedDatabase.h\"\n#include \"BowVector.h\"\n#include \"FeatureVector.h\"\n#include \"QueryResults.h\"\n#include \"FBrief.h\"\n\n/// BRIEF Vocabulary\ntypedef DBoW2::TemplatedVocabulary<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> \n  BriefVocabulary;\n\n/// BRIEF Database\ntypedef DBoW2::TemplatedDatabase<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> \n  BriefDatabase;\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/FBrief.cpp",
    "content": "/**\r\n * File: FBrief.cpp\r\n * Date: November 2011\r\n * Author: Dorian Galvez-Lopez\r\n * Description: functions for BRIEF descriptors\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n \r\n#include <vector>\r\n#include <string>\r\n#include <sstream>\r\n\r\n#include \"FBrief.h\"\r\n\r\nusing namespace std;\r\n\r\nnamespace DBoW2 {\r\n\r\n// --------------------------------------------------------------------------\r\n\r\nvoid FBrief::meanValue(const std::vector<FBrief::pDescriptor> &descriptors, \r\n  FBrief::TDescriptor &mean)\r\n{\r\n  mean.reset();\r\n  \r\n  if(descriptors.empty()) return;\r\n  \r\n  const int N2 = descriptors.size() / 2;\r\n  const int L = descriptors[0]->size();\r\n  \r\n  vector<int> counters(L, 0);\r\n\r\n  vector<FBrief::pDescriptor>::const_iterator it;\r\n  for(it = descriptors.begin(); it != descriptors.end(); ++it)\r\n  {\r\n    const FBrief::TDescriptor &desc = **it;\r\n    for(int i = 0; i < L; ++i)\r\n    {\r\n      if(desc[i]) counters[i]++;\r\n    }\r\n  }\r\n  \r\n  for(int i = 0; i < L; ++i)\r\n  {\r\n    if(counters[i] > N2) mean.set(i);\r\n  }\r\n  \r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n  \r\ndouble FBrief::distance(const FBrief::TDescriptor &a, \r\n  const FBrief::TDescriptor &b)\r\n{\r\n  return (double)DVision::BRIEF::distance(a, b);\r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n  \r\nstd::string FBrief::toString(const FBrief::TDescriptor &a)\r\n{\r\n  // from boost::bitset\r\n  string s;\r\n  to_string(a, s); // reversed\r\n  return s;\r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n  \r\nvoid FBrief::fromString(FBrief::TDescriptor &a, const std::string &s)\r\n{\r\n  // from boost::bitset\r\n  stringstream ss(s);\r\n  ss >> a;\r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n\r\nvoid FBrief::toMat32F(const std::vector<TDescriptor> &descriptors, \r\n  cv::Mat &mat)\r\n{\r\n  if(descriptors.empty())\r\n  {\r\n    mat.release();\r\n    return;\r\n  }\r\n  \r\n  const int N = descriptors.size();\r\n  const int L = descriptors[0].size();\r\n  \r\n  mat.create(N, L, CV_32F);\r\n  \r\n  for(int i = 0; i < N; ++i)\r\n  {\r\n    const TDescriptor& desc = descriptors[i];\r\n    float *p = mat.ptr<float>(i);\r\n    for(int j = 0; j < L; ++j, ++p)\r\n    {\r\n      *p = (desc[j] ? 1 : 0);\r\n    }\r\n  } \r\n}\r\n\r\n// --------------------------------------------------------------------------\r\n\r\n} // namespace DBoW2\r\n\r\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/FBrief.h",
    "content": "/**\r\n * File: FBrief.h\r\n * Date: November 2011\r\n * Author: Dorian Galvez-Lopez\r\n * Description: functions for BRIEF descriptors\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n#ifndef __D_T_F_BRIEF__\r\n#define __D_T_F_BRIEF__\r\n\r\n#include <opencv2/opencv.hpp>\r\n#include <vector>\r\n#include <string>\r\n\r\n#include \"FClass.h\"\r\n#include \"../DVision/DVision.h\"\r\n\r\nnamespace DBoW2 {\r\n\r\n/// Functions to manipulate BRIEF descriptors\r\nclass FBrief: protected FClass\r\n{\r\npublic:\r\n\r\n  typedef DVision::BRIEF::bitset TDescriptor;\r\n  typedef const TDescriptor *pDescriptor;\r\n\r\n  /**\r\n   * Calculates the mean value of a set of descriptors\r\n   * @param descriptors\r\n   * @param mean mean descriptor\r\n   */\r\n  static void meanValue(const std::vector<pDescriptor> &descriptors, \r\n    TDescriptor &mean);\r\n  \r\n  /**\r\n   * Calculates the distance between two descriptors\r\n   * @param a\r\n   * @param b\r\n   * @return distance\r\n   */\r\n  static double distance(const TDescriptor &a, const TDescriptor &b);\r\n  \r\n  /**\r\n   * Returns a string version of the descriptor\r\n   * @param a descriptor\r\n   * @return string version\r\n   */\r\n  static std::string toString(const TDescriptor &a);\r\n  \r\n  /**\r\n   * Returns a descriptor from a string\r\n   * @param a descriptor\r\n   * @param s string version\r\n   */\r\n  static void fromString(TDescriptor &a, const std::string &s);\r\n  \r\n  /**\r\n   * Returns a mat with the descriptors in float format\r\n   * @param descriptors\r\n   * @param mat (out) NxL 32F matrix\r\n   */\r\n  static void toMat32F(const std::vector<TDescriptor> &descriptors, \r\n    cv::Mat &mat);\r\n\r\n};\r\n\r\n} // namespace DBoW2\r\n\r\n#endif\r\n\r\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/FClass.h",
    "content": "/**\n * File: FClass.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: generic FClass to instantiate templated classes\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_FCLASS__\n#define __D_T_FCLASS__\n\n#include <opencv2/opencv.hpp>\n#include <vector>\n#include <string>\n\nnamespace DBoW2 {\n\n/// Generic class to encapsulate functions to manage descriptors.\n/**\n * This class must be inherited. Derived classes can be used as the\n * parameter F when creating Templated structures\n * (TemplatedVocabulary, TemplatedDatabase, ...)\n */\nclass FClass\n{\n  class TDescriptor;\n  typedef const TDescriptor *pDescriptor;\n  \n  /**\n   * Calculates the mean value of a set of descriptors\n   * @param descriptors\n   * @param mean mean descriptor\n   */\n  virtual void meanValue(const std::vector<pDescriptor> &descriptors, \n    TDescriptor &mean) = 0;\n  \n  /**\n   * Calculates the distance between two descriptors\n   * @param a\n   * @param b\n   * @return distance\n   */\n  static double distance(const TDescriptor &a, const TDescriptor &b);\n  \n  /**\n   * Returns a string version of the descriptor\n   * @param a descriptor\n   * @return string version\n   */\n  static std::string toString(const TDescriptor &a);\n  \n  /**\n   * Returns a descriptor from a string\n   * @param a descriptor\n   * @param s string version\n   */\n  static void fromString(TDescriptor &a, const std::string &s);\n\n  /**\n   * Returns a mat with the descriptors in float format\n   * @param descriptors\n   * @param mat (out) NxL 32F matrix\n   */\n  static void toMat32F(const std::vector<TDescriptor> &descriptors, \n    cv::Mat &mat);\n};\n\n} // namespace DBoW2\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/FeatureVector.cpp",
    "content": "/**\n * File: FeatureVector.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: feature vector\n * License: see the LICENSE.txt file\n *\n */\n\n#include \"FeatureVector.h\"\n#include <map>\n#include <vector>\n#include <iostream>\n\nnamespace DBoW2 {\n\n// ---------------------------------------------------------------------------\n\nFeatureVector::FeatureVector(void)\n{\n}\n\n// ---------------------------------------------------------------------------\n\nFeatureVector::~FeatureVector(void)\n{\n}\n\n// ---------------------------------------------------------------------------\n\nvoid FeatureVector::addFeature(NodeId id, unsigned int i_feature)\n{\n  FeatureVector::iterator vit = this->lower_bound(id);\n  \n  if(vit != this->end() && vit->first == id)\n  {\n    vit->second.push_back(i_feature);\n  }\n  else\n  {\n    vit = this->insert(vit, FeatureVector::value_type(id, \n      std::vector<unsigned int>() ));\n    vit->second.push_back(i_feature);\n  }\n}\n\n// ---------------------------------------------------------------------------\n\nstd::ostream& operator<<(std::ostream &out, \n  const FeatureVector &v)\n{\n  if(!v.empty())\n  {\n    FeatureVector::const_iterator vit = v.begin();\n    \n    const std::vector<unsigned int>* f = &vit->second;\n\n    out << \"<\" << vit->first << \": [\";\n    if(!f->empty()) out << (*f)[0];\n    for(unsigned int i = 1; i < f->size(); ++i)\n    {\n      out << \", \" << (*f)[i];\n    }\n    out << \"]>\";\n    \n    for(++vit; vit != v.end(); ++vit)\n    {\n      f = &vit->second;\n      \n      out << \", <\" << vit->first << \": [\";\n      if(!f->empty()) out << (*f)[0];\n      for(unsigned int i = 1; i < f->size(); ++i)\n      {\n        out << \", \" << (*f)[i];\n      }\n      out << \"]>\";\n    }\n  }\n  \n  return out;  \n}\n\n// ---------------------------------------------------------------------------\n\n} // namespace DBoW2\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/FeatureVector.h",
    "content": "/**\n * File: FeatureVector.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: feature vector\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_FEATURE_VECTOR__\n#define __D_T_FEATURE_VECTOR__\n\n#include \"BowVector.h\"\n#include <map>\n#include <vector>\n#include <iostream>\n\nnamespace DBoW2 {\n\n/// Vector of nodes with indexes of local features\nclass FeatureVector: \n  public std::map<NodeId, std::vector<unsigned int> >\n{\npublic:\n\n  /**\n   * Constructor\n   */\n  FeatureVector(void);\n  \n  /**\n   * Destructor\n   */\n  ~FeatureVector(void);\n  \n  /**\n   * Adds a feature to an existing node, or adds a new node with an initial\n   * feature\n   * @param id node id to add or to modify\n   * @param i_feature index of feature to add to the given node\n   */\n  void addFeature(NodeId id, unsigned int i_feature);\n\n  /**\n   * Sends a string versions of the feature vector through the stream\n   * @param out stream\n   * @param v feature vector\n   */\n  friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v);\n    \n};\n\n} // namespace DBoW2\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/QueryResults.cpp",
    "content": "/**\n * File: QueryResults.cpp\n * Date: March, November 2011\n * Author: Dorian Galvez-Lopez\n * Description: structure to store results of database queries\n * License: see the LICENSE.txt file\n *\n */\n\n#include <iostream>\n#include <fstream>\n#include \"QueryResults.h\"\n\nusing namespace std;\n\nnamespace DBoW2\n{\n\n// ---------------------------------------------------------------------------\n\nostream & operator<<(ostream& os, const Result& ret )\n{\n  os << \"<EntryId: \" << ret.Id << \", Score: \" << ret.Score << \">\";\n  return os;\n}\n\n// ---------------------------------------------------------------------------\n\nostream & operator<<(ostream& os, const QueryResults& ret )\n{\n  if(ret.size() == 1)\n    os << \"1 result:\" << endl;\n  else\n    os << ret.size() << \" results:\" << endl;\n    \n  QueryResults::const_iterator rit;\n  for(rit = ret.begin(); rit != ret.end(); ++rit)\n  {\n    os << *rit;\n    if(rit + 1 != ret.end()) os << endl;\n  }\n  return os;\n}\n\n// ---------------------------------------------------------------------------\n\nvoid QueryResults::saveM(const std::string &filename) const\n{\n  fstream f(filename.c_str(), ios::out);\n  \n  QueryResults::const_iterator qit;\n  for(qit = begin(); qit != end(); ++qit)\n  {\n    f << qit->Id << \" \" << qit->Score << endl;\n  }\n  \n  f.close();\n}\n\n// ---------------------------------------------------------------------------\n\n} // namespace DBoW2\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/QueryResults.h",
    "content": "/**\n * File: QueryResults.h\n * Date: March, November 2011\n * Author: Dorian Galvez-Lopez\n * Description: structure to store results of database queries\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_QUERY_RESULTS__\n#define __D_T_QUERY_RESULTS__\n\n#include <vector>\n\nnamespace DBoW2 {\n\n/// Id of entries of the database\ntypedef unsigned int EntryId;\n\n/// Single result of a query\nclass Result\n{\npublic:\n  \n  /// Entry id\n  EntryId Id;\n  \n  /// Score obtained\n  double Score;\n  \n  /// debug\n  int nWords; // words in common\n  // !!! this is filled only by Bhatt score!\n  // (and for BCMatching, BCThresholding then)\n  \n  double bhatScore, chiScore;\n  /// debug\n  \n  // only done by ChiSq and BCThresholding \n  double sumCommonVi;\n  double sumCommonWi;\n  double expectedChiScore;\n  /// debug\n\n  /**\n   * Empty constructors\n   */\n  inline Result(){}\n  \n  /**\n   * Creates a result with the given data\n   * @param _id entry id\n   * @param _score score\n   */\n  inline Result(EntryId _id, double _score): Id(_id), Score(_score){}\n\n  /**\n   * Compares the scores of two results\n   * @return true iff this.score < r.score\n   */\n  inline bool operator<(const Result &r) const\n  {\n    return this->Score < r.Score;\n  }\n\n  /**\n   * Compares the scores of two results\n   * @return true iff this.score > r.score\n   */\n  inline bool operator>(const Result &r) const\n  {\n    return this->Score > r.Score;\n  }\n\n  /**\n   * Compares the entry id of the result\n   * @return true iff this.id == id\n   */\n  inline bool operator==(EntryId id) const\n  {\n    return this->Id == id;\n  }\n  \n  /**\n   * Compares the score of this entry with a given one\n   * @param s score to compare with\n   * @return true iff this score < s\n   */\n  inline bool operator<(double s) const\n  {\n    return this->Score < s;\n  }\n  \n  /**\n   * Compares the score of this entry with a given one\n   * @param s score to compare with\n   * @return true iff this score > s\n   */\n  inline bool operator>(double s) const\n  {\n    return this->Score > s;\n  }\n  \n  /**\n   * Compares the score of two results\n   * @param a\n   * @param b\n   * @return true iff a.Score > b.Score\n   */\n  static inline bool gt(const Result &a, const Result &b)\n  {\n    return a.Score > b.Score;\n  }\n  \n  /**\n   * Compares the scores of two results\n   * @return true iff a.Score > b.Score\n   */\n  inline static bool ge(const Result &a, const Result &b)\n  {\n    return a.Score > b.Score;\n  }\n  \n  /**\n   * Returns true iff a.Score >= b.Score\n   * @param a\n   * @param b\n   * @return true iff a.Score >= b.Score\n   */\n  static inline bool geq(const Result &a, const Result &b)\n  {\n    return a.Score >= b.Score;\n  }\n  \n  /**\n   * Returns true iff a.Score >= s\n   * @param a\n   * @param s\n   * @return true iff a.Score >= s\n   */\n  static inline bool geqv(const Result &a, double s)\n  {\n    return a.Score >= s;\n  }\n  \n  \n  /**\n   * Returns true iff a.Id < b.Id\n   * @param a\n   * @param b\n   * @return true iff a.Id < b.Id\n   */\n  static inline bool ltId(const Result &a, const Result &b)\n  {\n    return a.Id < b.Id;\n  }\n  \n  /**\n   * Prints a string version of the result\n   * @param os ostream\n   * @param ret Result to print\n   */\n  friend std::ostream & operator<<(std::ostream& os, const Result& ret );\n};\n\n/// Multiple results from a query\nclass QueryResults: public std::vector<Result>\n{\npublic:\n\n  /** \n   * Multiplies all the scores in the vector by factor\n   * @param factor\n   */\n  inline void scaleScores(double factor);\n  \n  /**\n   * Prints a string version of the results\n   * @param os ostream\n   * @param ret QueryResults to print\n   */\n  friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret );\n  \n  /**\n   * Saves a matlab file with the results \n   * @param filename \n   */\n  void saveM(const std::string &filename) const;\n  \n};\n\n// --------------------------------------------------------------------------\n\ninline void QueryResults::scaleScores(double factor)\n{\n  for(QueryResults::iterator qit = begin(); qit != end(); ++qit) \n    qit->Score *= factor;\n}\n\n// --------------------------------------------------------------------------\n\n} // namespace TemplatedBoW\n  \n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/ScoringObject.cpp",
    "content": "/**\n * File: ScoringObject.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: functions to compute bow scores \n * License: see the LICENSE.txt file\n *\n */\n\n#include <cfloat>\n#include \"TemplatedVocabulary.h\"\n#include \"BowVector.h\"\n\nusing namespace DBoW2;\n\n// If you change the type of WordValue, make sure you change also the\n// epsilon value (this is needed by the KL method)\nconst double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble L1Scoring::score(const BowVector &v1, const BowVector &v2) const\n{\n  BowVector::const_iterator v1_it, v2_it;\n  const BowVector::const_iterator v1_end = v1.end();\n  const BowVector::const_iterator v2_end = v2.end();\n  \n  v1_it = v1.begin();\n  v2_it = v2.begin();\n  \n  double score = 0;\n  \n  while(v1_it != v1_end && v2_it != v2_end)\n  {\n    const WordValue& vi = v1_it->second;\n    const WordValue& wi = v2_it->second;\n    \n    if(v1_it->first == v2_it->first)\n    {\n      score += fabs(vi - wi) - fabs(vi) - fabs(wi);\n      \n      // move v1 and v2 forward\n      ++v1_it;\n      ++v2_it;\n    }\n    else if(v1_it->first < v2_it->first)\n    {\n      // move v1 forward\n      v1_it = v1.lower_bound(v2_it->first);\n      // v1_it = (first element >= v2_it.id)\n    }\n    else\n    {\n      // move v2 forward\n      v2_it = v2.lower_bound(v1_it->first);\n      // v2_it = (first element >= v1_it.id)\n    }\n  }\n  \n  // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) \n  //\t\tfor all i | v_i != 0 and w_i != 0 \n  // (Nister, 2006)\n  // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}\n  score = -score/2.0;\n\n  return score; // [0..1]\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble L2Scoring::score(const BowVector &v1, const BowVector &v2) const\n{\n  BowVector::const_iterator v1_it, v2_it;\n  const BowVector::const_iterator v1_end = v1.end();\n  const BowVector::const_iterator v2_end = v2.end();\n  \n  v1_it = v1.begin();\n  v2_it = v2.begin();\n  \n  double score = 0;\n  \n  while(v1_it != v1_end && v2_it != v2_end)\n  {\n    const WordValue& vi = v1_it->second;\n    const WordValue& wi = v2_it->second;\n    \n    if(v1_it->first == v2_it->first)\n    {\n      score += vi * wi;\n      \n      // move v1 and v2 forward\n      ++v1_it;\n      ++v2_it;\n    }\n    else if(v1_it->first < v2_it->first)\n    {\n      // move v1 forward\n      v1_it = v1.lower_bound(v2_it->first);\n      // v1_it = (first element >= v2_it.id)\n    }\n    else\n    {\n      // move v2 forward\n      v2_it = v2.lower_bound(v1_it->first);\n      // v2_it = (first element >= v1_it.id)\n    }\n  }\n  \n  // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )\n\t//\t\tfor all i | v_i != 0 and w_i != 0 )\n\t// (Nister, 2006)\n\tif(score >= 1) // rounding errors\n\t  score = 1.0;\n\telse\n    score = 1.0 - sqrt(1.0 - score); // [0..1]\n\n  return score;\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) \n  const\n{\n  BowVector::const_iterator v1_it, v2_it;\n  const BowVector::const_iterator v1_end = v1.end();\n  const BowVector::const_iterator v2_end = v2.end();\n  \n  v1_it = v1.begin();\n  v2_it = v2.begin();\n  \n  double score = 0;\n  \n  // all the items are taken into account\n  \n  while(v1_it != v1_end && v2_it != v2_end)\n  {\n    const WordValue& vi = v1_it->second;\n    const WordValue& wi = v2_it->second;\n    \n    if(v1_it->first == v2_it->first)\n    {\n      // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)\n      // we move the -4 out\n      if(vi + wi != 0.0) score += vi * wi / (vi + wi);\n      \n      // move v1 and v2 forward\n      ++v1_it;\n      ++v2_it;\n    }\n    else if(v1_it->first < v2_it->first)\n    {\n      // move v1 forward\n      v1_it = v1.lower_bound(v2_it->first);\n    }\n    else\n    {\n      // move v2 forward\n      v2_it = v2.lower_bound(v1_it->first);\n    }\n  }\n    \n  // this takes the -4 into account\n  score = 2. * score; // [0..1]\n\n  return score;\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble KLScoring::score(const BowVector &v1, const BowVector &v2) const\n{ \n  BowVector::const_iterator v1_it, v2_it;\n  const BowVector::const_iterator v1_end = v1.end();\n  const BowVector::const_iterator v2_end = v2.end();\n  \n  v1_it = v1.begin();\n  v2_it = v2.begin();\n  \n  double score = 0;\n  \n  // all the items or v are taken into account\n  \n  while(v1_it != v1_end && v2_it != v2_end)\n  {\n    const WordValue& vi = v1_it->second;\n    const WordValue& wi = v2_it->second;\n    \n    if(v1_it->first == v2_it->first)\n    {\n      if(vi != 0 && wi != 0) score += vi * log(vi/wi);\n      \n      // move v1 and v2 forward\n      ++v1_it;\n      ++v2_it;\n    }\n    else if(v1_it->first < v2_it->first)\n    {\n      // move v1 forward\n      score += vi * (log(vi) - LOG_EPS);\n      ++v1_it;\n    }\n    else\n    {\n      // move v2_it forward, do not add any score\n      v2_it = v2.lower_bound(v1_it->first);\n      // v2_it = (first element >= v1_it.id)\n    }\n  }\n  \n  // sum rest of items of v\n  for(; v1_it != v1_end; ++v1_it) \n    if(v1_it->second != 0)\n      score += v1_it->second * (log(v1_it->second) - LOG_EPS);\n  \n  return score; // cannot be scaled\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble BhattacharyyaScoring::score(const BowVector &v1, \n  const BowVector &v2) const\n{\n  BowVector::const_iterator v1_it, v2_it;\n  const BowVector::const_iterator v1_end = v1.end();\n  const BowVector::const_iterator v2_end = v2.end();\n  \n  v1_it = v1.begin();\n  v2_it = v2.begin();\n  \n  double score = 0;\n  \n  while(v1_it != v1_end && v2_it != v2_end)\n  {\n    const WordValue& vi = v1_it->second;\n    const WordValue& wi = v2_it->second;\n    \n    if(v1_it->first == v2_it->first)\n    {\n      score += sqrt(vi * wi);\n      \n      // move v1 and v2 forward\n      ++v1_it;\n      ++v2_it;\n    }\n    else if(v1_it->first < v2_it->first)\n    {\n      // move v1 forward\n      v1_it = v1.lower_bound(v2_it->first);\n      // v1_it = (first element >= v2_it.id)\n    }\n    else\n    {\n      // move v2 forward\n      v2_it = v2.lower_bound(v1_it->first);\n      // v2_it = (first element >= v1_it.id)\n    }\n  }\n\n  return score; // already scaled\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\ndouble DotProductScoring::score(const BowVector &v1, \n  const BowVector &v2) const\n{\n  BowVector::const_iterator v1_it, v2_it;\n  const BowVector::const_iterator v1_end = v1.end();\n  const BowVector::const_iterator v2_end = v2.end();\n  \n  v1_it = v1.begin();\n  v2_it = v2.begin();\n  \n  double score = 0;\n  \n  while(v1_it != v1_end && v2_it != v2_end)\n  {\n    const WordValue& vi = v1_it->second;\n    const WordValue& wi = v2_it->second;\n    \n    if(v1_it->first == v2_it->first)\n    {\n      score += vi * wi;\n      \n      // move v1 and v2 forward\n      ++v1_it;\n      ++v2_it;\n    }\n    else if(v1_it->first < v2_it->first)\n    {\n      // move v1 forward\n      v1_it = v1.lower_bound(v2_it->first);\n      // v1_it = (first element >= v2_it.id)\n    }\n    else\n    {\n      // move v2 forward\n      v2_it = v2.lower_bound(v1_it->first);\n      // v2_it = (first element >= v1_it.id)\n    }\n  }\n\n  return score; // cannot scale\n}\n\n// ---------------------------------------------------------------------------\n// ---------------------------------------------------------------------------\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/ScoringObject.h",
    "content": "/**\n * File: ScoringObject.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: functions to compute bow scores \n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_SCORING_OBJECT__\n#define __D_T_SCORING_OBJECT__\n\n#include \"BowVector.h\"\n\nnamespace DBoW2 {\n\n/// Base class of scoring functions\nclass GeneralScoring\n{\npublic:\n  /**\n   * Computes the score between two vectors. Vectors must be sorted and \n   * normalized if necessary\n   * @param v (in/out)\n   * @param w (in/out)\n   * @return score\n   */\n  virtual double score(const BowVector &v, const BowVector &w) const = 0;\n\n  /**\n   * Returns whether a vector must be normalized before scoring according\n   * to the scoring scheme\n   * @param norm norm to use\n   * @return true iff must normalize\n   */\n  virtual bool mustNormalize(LNorm &norm) const = 0;\n\n  /// Log of epsilon\n\tstatic const double LOG_EPS; \n  // If you change the type of WordValue, make sure you change also the\n\t// epsilon value (this is needed by the KL method)\n\t\n  virtual ~GeneralScoring() {} //!< Required for virtual base classes\t\n};\n\n/** \n * Macro for defining Scoring classes\n * @param NAME name of class\n * @param MUSTNORMALIZE if vectors must be normalized to compute the score\n * @param NORM type of norm to use when MUSTNORMALIZE\n */\n#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \\\n  NAME: public GeneralScoring \\\n  { public: \\\n    /** \\\n     * Computes score between two vectors \\\n     * @param v \\\n     * @param w \\\n     * @return score between v and w \\\n     */ \\\n    virtual double score(const BowVector &v, const BowVector &w) const; \\\n    \\\n    /** \\\n     * Says if a vector must be normalized according to the scoring function \\\n     * @param norm (out) if true, norm to use\n     * @return true iff vectors must be normalized \\\n     */ \\\n    virtual inline bool mustNormalize(LNorm &norm) const  \\\n      { norm = NORM; return MUSTNORMALIZE; } \\\n  }\n  \n/// L1 Scoring object\nclass __SCORING_CLASS(L1Scoring, true, L1);\n\n/// L2 Scoring object\nclass __SCORING_CLASS(L2Scoring, true, L2);\n\n/// Chi square Scoring object\nclass __SCORING_CLASS(ChiSquareScoring, true, L1);\n\n/// KL divergence Scoring object\nclass __SCORING_CLASS(KLScoring, true, L1);\n\n/// Bhattacharyya Scoring object\nclass __SCORING_CLASS(BhattacharyyaScoring, true, L1);\n\n/// Dot product Scoring object\nclass __SCORING_CLASS(DotProductScoring, false, L1);\n\n#undef __SCORING_CLASS\n  \n} // namespace DBoW2\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/TemplatedDatabase.h",
    "content": "/**\n * File: TemplatedDatabase.h\n * Date: March 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated database of images\n * License: see the LICENSE.txt file\n *\n */\n \n#ifndef __D_T_TEMPLATED_DATABASE__\n#define __D_T_TEMPLATED_DATABASE__\n\n#include <vector>\n#include <numeric>\n#include <fstream>\n#include <string>\n#include <list>\n#include <set>\n\n#include \"TemplatedVocabulary.h\"\n#include \"QueryResults.h\"\n#include \"ScoringObject.h\"\n#include \"BowVector.h\"\n#include \"FeatureVector.h\"\n\n#include \"../DUtils/DUtils.h\"\n\nnamespace DBoW2 {\n\n// For query functions\nstatic int MIN_COMMON_WORDS = 5;\n\n/// @param TDescriptor class of descriptor\n/// @param F class of descriptor functions\ntemplate<class TDescriptor, class F>\n/// Generic Database\nclass TemplatedDatabase\n{\npublic:\n\n  /**\n   * Creates an empty database without vocabulary\n   * @param use_di a direct index is used to store feature indexes\n   * @param di_levels levels to go up the vocabulary tree to select the \n   *   node id to store in the direct index when adding images\n   */\n  explicit TemplatedDatabase(bool use_di = true, int di_levels = 0);\n\n  /**\n   * Creates a database with the given vocabulary\n   * @param T class inherited from TemplatedVocabulary<TDescriptor, F>\n   * @param voc vocabulary\n   * @param use_di a direct index is used to store feature indexes\n   * @param di_levels levels to go up the vocabulary tree to select the \n   *   node id to store in the direct index when adding images\n   */\n  template<class T>\n  explicit TemplatedDatabase(const T &voc, bool use_di = true, \n    int di_levels = 0);\n\n  /**\n   * Copy constructor. Copies the vocabulary too\n   * @param db object to copy\n   */\n  TemplatedDatabase(const TemplatedDatabase<TDescriptor, F> &db);\n\n  /** \n   * Creates the database from a file\n   * @param filename\n   */\n  TemplatedDatabase(const std::string &filename);\n\n  /** \n   * Creates the database from a file\n   * @param filename\n   */\n  TemplatedDatabase(const char *filename);\n\n  /**\n   * Destructor\n   */\n  virtual ~TemplatedDatabase(void);\n\n  /**\n   * Copies the given database and its vocabulary\n   * @param db database to copy\n   */\n  TemplatedDatabase<TDescriptor,F>& operator=(\n    const TemplatedDatabase<TDescriptor,F> &db);\n\n  /**\n   * Sets the vocabulary to use and clears the content of the database.\n   * @param T class inherited from TemplatedVocabulary<TDescriptor, F>\n   * @param voc vocabulary to copy\n   */\n  template<class T>\n  inline void setVocabulary(const T &voc);\n  \n  /**\n   * Sets the vocabulary to use and the direct index parameters, and clears\n   * the content of the database\n   * @param T class inherited from TemplatedVocabulary<TDescriptor, F>\n   * @param voc vocabulary to copy\n   * @param use_di a direct index is used to store feature indexes\n   * @param di_levels levels to go up the vocabulary tree to select the \n   *   node id to store in the direct index when adding images\n   */\n  template<class T>\n  void setVocabulary(const T& voc, bool use_di, int di_levels = 0);\n  \n  /**\n   * Returns a pointer to the vocabulary used\n   * @return vocabulary\n   */\n  inline const TemplatedVocabulary<TDescriptor,F>* getVocabulary() const;\n\n  /** \n   * Allocates some memory for the direct and inverted indexes\n   * @param nd number of expected image entries in the database \n   * @param ni number of expected words per image\n   * @note Use 0 to ignore a parameter\n   */\n  void allocate(int nd = 0, int ni = 0);\n\n  /**\n   * Adds an entry to the database and returns its index\n   * @param features features of the new entry\n   * @param bowvec if given, the bow vector of these features is returned\n   * @param fvec if given, the vector of nodes and feature indexes is returned\n   * @return id of new entry\n   */\n  EntryId add(const std::vector<TDescriptor> &features,\n    BowVector *bowvec = NULL, FeatureVector *fvec = NULL);\n\n  /**\n   * Adss an entry to the database and returns its index\n   * @param vec bow vector\n   * @param fec feature vector to add the entry. Only necessary if using the\n   *   direct index\n   * @return id of new entry\n   */\n  EntryId add(const BowVector &vec, \n    const FeatureVector &fec = FeatureVector() );\n\n  void delete_entry(const EntryId entry_id);\n\n  /**\n   * Empties the database\n   */\n  inline void clear();\n\n  /**\n   * Returns the number of entries in the database \n   * @return number of entries in the database\n   */\n  inline unsigned int size() const;\n  \n  /**\n   * Checks if the direct index is being used\n   * @return true iff using direct index\n   */\n  inline bool usingDirectIndex() const;\n  \n  /**\n   * Returns the di levels when using direct index\n   * @return di levels\n   */\n  inline int getDirectIndexLevels() const;\n  \n  /**\n   * Queries the database with some features\n   * @param features query features\n   * @param ret (out) query results\n   * @param max_results number of results to return. <= 0 means all\n   * @param max_id only entries with id <= max_id are returned in ret. \n   *   < 0 means all\n   */\n  void query(const std::vector<TDescriptor> &features, QueryResults &ret,\n    int max_results = 1, int max_id = -1) const;\n  \n  /**\n   * Queries the database with a vector\n   * @param vec bow vector already normalized\n   * @param ret results\n   * @param max_results number of results to return. <= 0 means all\n   * @param max_id only entries with id <= max_id are returned in ret. \n   *   < 0 means all\n   */\n  void query(const BowVector &vec, QueryResults &ret, \n    int max_results = 1, int max_id = -1) const;\n\n  /**\n   * Returns the a feature vector associated with a database entry\n   * @param id entry id (must be < size())\n   * @return const reference to map of nodes and their associated features in\n   *   the given entry\n   */\n  const FeatureVector& retrieveFeatures(EntryId id) const;\n\n  /**\n   * Stores the database in a file\n   * @param filename\n   */\n  void save(const std::string &filename) const;\n  \n  /**\n   * Loads the database from a file\n   * @param filename\n   */\n  void load(const std::string &filename);\n  \n  /** \n   * Stores the database in the given file storage structure\n   * @param fs\n   * @param name node name\n   */\n  virtual void save(cv::FileStorage &fs, \n    const std::string &name = \"database\") const;\n  \n  /** \n   * Loads the database from the given file storage structure\n   * @param fs\n   * @param name node name\n   */\n  virtual void load(const cv::FileStorage &fs, \n    const std::string &name = \"database\");\n\nprotected:\n  \n  /// Query with L1 scoring\n  void queryL1(const BowVector &vec, QueryResults &ret, \n    int max_results, int max_id) const;\n  \n  /// Query with L2 scoring\n  void queryL2(const BowVector &vec, QueryResults &ret, \n    int max_results, int max_id) const;\n  \n  /// Query with Chi square scoring\n  void queryChiSquare(const BowVector &vec, QueryResults &ret, \n    int max_results, int max_id) const;\n  \n  /// Query with Bhattacharyya scoring\n  void queryBhattacharyya(const BowVector &vec, QueryResults &ret, \n    int max_results, int max_id) const;\n  \n  /// Query with KL divergence scoring  \n  void queryKL(const BowVector &vec, QueryResults &ret, \n    int max_results, int max_id) const;\n  \n  /// Query with dot product scoring\n  void queryDotProduct(const BowVector &vec, QueryResults &ret, \n    int max_results, int max_id) const;\n\nprotected:\n\n  /* Inverted file declaration */\n  \n  /// Item of IFRow\n  struct IFPair\n  {\n    /// Entry id\n    EntryId entry_id;\n    \n    /// Word weight in this entry\n    WordValue word_weight;\n    \n    /**\n     * Creates an empty pair\n     */\n    IFPair(){}\n    \n    /**\n     * Creates an inverted file pair\n     * @param eid entry id\n     * @param wv word weight\n     */\n    IFPair(EntryId eid, WordValue wv): entry_id(eid), word_weight(wv) {}\n    \n    /**\n     * Compares the entry ids\n     * @param eid\n     * @return true iff this entry id is the same as eid\n     */\n    inline bool operator==(EntryId eid) const { return entry_id == eid; }\n  };\n  \n  /// Row of InvertedFile\n  typedef std::list<IFPair> IFRow;\n  // IFRows are sorted in ascending entry_id order\n  \n  /// Inverted index\n  typedef std::vector<IFRow> InvertedFile; \n  // InvertedFile[word_id] --> inverted file of that word\n  \n  /* Direct file declaration */\n\n  /// Direct index\n  typedef std::vector<FeatureVector> DirectFile;\n  // DirectFile[entry_id] --> [ directentry, ... ]\n\nprotected:\n\n  /// Associated vocabulary\n  TemplatedVocabulary<TDescriptor, F> *m_voc;\n  \n  /// Flag to use direct index\n  bool m_use_di;\n  \n  /// Levels to go up the vocabulary tree to select nodes to store\n  /// in the direct index\n  int m_dilevels;\n  \n  /// Inverted file (must have size() == |words|)\n  InvertedFile m_ifile;\n\n  \n  /// Direct file (resized for allocation)\n  DirectFile m_dfile;\n  \n  std::vector<BowVector> m_dBowfile;\n\n  /// Number of valid entries in m_dfile\n  int m_nentries;\n  \n};\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedDatabase<TDescriptor, F>::TemplatedDatabase\n  (bool use_di, int di_levels)\n  : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels), m_nentries(0)\n{\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ntemplate<class T>\nTemplatedDatabase<TDescriptor, F>::TemplatedDatabase\n  (const T &voc, bool use_di, int di_levels)\n  : m_voc(NULL), m_use_di(use_di), m_dilevels(di_levels)\n{\n  setVocabulary(voc);\n  clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedDatabase<TDescriptor,F>::TemplatedDatabase\n  (const TemplatedDatabase<TDescriptor,F> &db)\n  : m_voc(NULL)\n{\n  *this = db;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedDatabase<TDescriptor, F>::TemplatedDatabase\n  (const std::string &filename)\n  : m_voc(NULL)\n{\n  load(filename);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedDatabase<TDescriptor, F>::TemplatedDatabase\n  (const char *filename)\n  : m_voc(NULL)\n{\n  load(filename);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedDatabase<TDescriptor, F>::~TemplatedDatabase(void)\n{\n  delete m_voc;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedDatabase<TDescriptor,F>& TemplatedDatabase<TDescriptor,F>::operator=\n  (const TemplatedDatabase<TDescriptor,F> &db)\n{\n  if(this != &db)\n  {\n    m_dfile = db.m_dfile;\n    m_dBowfile = db.m_dBowfile;\n    m_dilevels = db.m_dilevels;\n    m_ifile = db.m_ifile;\n    m_nentries = db.m_nentries;\n    m_use_di = db.m_use_di;\n    setVocabulary(*db.m_voc);\n  }\n  return *this;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nEntryId TemplatedDatabase<TDescriptor, F>::add(\n  const std::vector<TDescriptor> &features,\n  BowVector *bowvec, FeatureVector *fvec)\n{\n  BowVector aux;\n  BowVector& v = (bowvec ? *bowvec : aux);\n  \n  if(m_use_di && fvec != NULL)\n  {\n    m_voc->transform(features, v, *fvec, m_dilevels); // with features\n    return add(v, *fvec);\n  }\n  else if(m_use_di)\n  {\n    FeatureVector fv;\n    m_voc->transform(features, v, fv, m_dilevels); // with features\n    return add(v, fv);\n  }\n  else if(fvec != NULL)\n  {\n    m_voc->transform(features, v, *fvec, m_dilevels); // with features\n    return add(v);\n  }\n  else\n  {\n    m_voc->transform(features, v); // with features\n    return add(v);\n  }\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nEntryId TemplatedDatabase<TDescriptor, F>::add(const BowVector &v,\n  const FeatureVector &fv)\n{\n  EntryId entry_id = m_nentries++;\n\n  BowVector::const_iterator vit;\n  std::vector<unsigned int>::const_iterator iit;\n\n  if(m_use_di)\n  {\n    // update direct file\n    if(entry_id == m_dfile.size())\n    {\n      m_dfile.push_back(fv);\n      m_dBowfile.push_back(v);\n    }\n    else\n    {\n      m_dfile[entry_id] = fv;\n      m_dBowfile[entry_id] = v;\n    }\n  }\n  \n  // update inverted file\n  for(vit = v.begin(); vit != v.end(); ++vit)\n  {\n    const WordId& word_id = vit->first;\n    const WordValue& word_weight = vit->second;\n    \n    IFRow& ifrow = m_ifile[word_id];\n    ifrow.push_back(IFPair(entry_id, word_weight));\n  }\n  \n  return entry_id;\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::delete_entry(const EntryId entry_id)\n{\n  BowVector v = m_dBowfile[entry_id];\n\n  BowVector::const_iterator vit;\n\n  for (vit = v.begin(); vit != v.end(); ++vit)\n  {\n    const WordId& word_id = vit->first;\n    IFRow& ifrow = m_ifile[word_id];\n    typename IFRow::iterator rit;\n    for (rit = ifrow.begin(); rit != ifrow.end(); ++rit)\n    {\n      if (rit->entry_id == entry_id)\n      {\n        ifrow.erase(rit);\n        break;\n      }\n    }\n  }\n  m_dBowfile[entry_id].clear();\n  m_dfile[entry_id].clear();\n}\n\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ntemplate<class T>\ninline void TemplatedDatabase<TDescriptor, F>::setVocabulary\n  (const T& voc)\n{\n  delete m_voc;\n  m_voc = new T(voc);\n  clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ntemplate<class T>\ninline void TemplatedDatabase<TDescriptor, F>::setVocabulary\n  (const T& voc, bool use_di, int di_levels)\n{\n  m_use_di = use_di;\n  m_dilevels = di_levels;\n  delete m_voc;\n  m_voc = new T(voc);\n  clear();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline const TemplatedVocabulary<TDescriptor,F>* \nTemplatedDatabase<TDescriptor, F>::getVocabulary() const\n{\n  return m_voc;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline void TemplatedDatabase<TDescriptor, F>::clear()\n{\n  // resize vectors\n  m_ifile.resize(0);\n  m_ifile.resize(m_voc->size());\n  m_dfile.resize(0);\n  m_dBowfile.resize(0);\n  m_nentries = 0;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::allocate(int nd, int ni)\n{\n  // m_ifile already contains |words| items\n  if(ni > 0)\n  {\n    typename std::vector<IFRow>::iterator rit;\n    for(rit = m_ifile.begin(); rit != m_ifile.end(); ++rit)\n    {\n      int n = (int)rit->size();\n      if(ni > n)\n      {\n        rit->resize(ni);\n        rit->resize(n);\n      }\n    }\n  }\n  \n  if(m_use_di && (int)m_dfile.size() < nd)\n  {\n    m_dfile.resize(nd);\n    m_dBowfile.resize(nd);\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline unsigned int TemplatedDatabase<TDescriptor, F>::size() const\n{\n  return m_nentries;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline bool TemplatedDatabase<TDescriptor, F>::usingDirectIndex() const\n{\n  return m_use_di;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline int TemplatedDatabase<TDescriptor, F>::getDirectIndexLevels() const\n{\n  return m_dilevels;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::query(\n  const std::vector<TDescriptor> &features,\n  QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector vec;\n  m_voc->transform(features, vec);\n  query(vec, ret, max_results, max_id);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::query(\n  const BowVector &vec, \n  QueryResults &ret, int max_results, int max_id) const\n{\n  ret.resize(0);\n  \n  switch(m_voc->getScoringType())\n  {\n    case L1_NORM:\n      queryL1(vec, ret, max_results, max_id);\n      break;\n      \n    case L2_NORM:\n      queryL2(vec, ret, max_results, max_id);\n      break;\n      \n    case CHI_SQUARE:\n      queryChiSquare(vec, ret, max_results, max_id);\n      break;\n      \n    case KL:\n      queryKL(vec, ret, max_results, max_id);\n      break;\n      \n    case BHATTACHARYYA:\n      queryBhattacharyya(vec, ret, max_results, max_id);\n      break;\n      \n    case DOT_PRODUCT:\n      queryDotProduct(vec, ret, max_results, max_id);\n      break;\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::queryL1(const BowVector &vec, \n  QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector::const_iterator vit;\n  typename IFRow::const_iterator rit;\n    \n  std::map<EntryId, double> pairs;\n  std::map<EntryId, double>::iterator pit;\n  \n  for(vit = vec.begin(); vit != vec.end(); ++vit)\n  {\n    const WordId word_id = vit->first;\n    const WordValue& qvalue = vit->second;\n        \n    const IFRow& row = m_ifile[word_id];\n    \n    // IFRows are sorted in ascending entry_id order\n    \n    for(rit = row.begin(); rit != row.end(); ++rit)\n    {\n      const EntryId entry_id = rit->entry_id;\n      const WordValue& dvalue = rit->word_weight;\n      \n      if((int)entry_id < max_id || max_id == -1)\n      {\n        double value = fabs(qvalue - dvalue) - fabs(qvalue) - fabs(dvalue);\n        \n        pit = pairs.lower_bound(entry_id);\n        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n        {\n          pit->second += value;\n        }\n        else\n        {\n          pairs.insert(pit, \n            std::map<EntryId, double>::value_type(entry_id, value));\n        }\n      }\n      \n    } // for each inverted row\n  } // for each query word\n\t\n  // move to vector\n  ret.reserve(pairs.size());\n  for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n  {\n    ret.push_back(Result(pit->first, pit->second));\n  }\n\t\n  // resulting \"scores\" are now in [-2 best .. 0 worst]\t\n  \n  // sort vector in ascending order of score\n  std::sort(ret.begin(), ret.end());\n  // (ret is inverted now --the lower the better--)\n\n  // cut vector\n  if(max_results > 0 && (int)ret.size() > max_results)\n    ret.resize(max_results);\n  \n  // complete and scale score to [0 worst .. 1 best]\n  // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) \n  //\t\tfor all i | v_i != 0 and w_i != 0 \n  // (Nister, 2006)\n  // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}\n  QueryResults::iterator qit;\n  for(qit = ret.begin(); qit != ret.end(); qit++) \n    qit->Score = -qit->Score/2.0;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::queryL2(const BowVector &vec, \n  QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector::const_iterator vit;\n  typename IFRow::const_iterator rit;\n  \n  std::map<EntryId, double> pairs;\n  std::map<EntryId, double>::iterator pit;\n  \n  //map<EntryId, int> counters;\n  //map<EntryId, int>::iterator cit;\n  \n  for(vit = vec.begin(); vit != vec.end(); ++vit)\n  {\n    const WordId word_id = vit->first;\n    const WordValue& qvalue = vit->second;\n    \n    const IFRow& row = m_ifile[word_id];\n    \n    // IFRows are sorted in ascending entry_id order\n    \n    for(rit = row.begin(); rit != row.end(); ++rit)\n    {\n      const EntryId entry_id = rit->entry_id;\n      const WordValue& dvalue = rit->word_weight;\n      \n      if((int)entry_id < max_id || max_id == -1)\n      {\n        double value = - qvalue * dvalue; // minus sign for sorting trick\n        \n        pit = pairs.lower_bound(entry_id);\n        //cit = counters.lower_bound(entry_id);\n        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n        {\n          pit->second += value; \n          //cit->second += 1;\n        }\n        else\n        {\n          pairs.insert(pit, \n            std::map<EntryId, double>::value_type(entry_id, value));\n          \n          //counters.insert(cit, \n          //  map<EntryId, int>::value_type(entry_id, 1));\n        }\n      }\n      \n    } // for each inverted row\n  } // for each query word\n\t\n  // move to vector\n  ret.reserve(pairs.size());\n  //cit = counters.begin();\n  for(pit = pairs.begin(); pit != pairs.end(); ++pit)//, ++cit)\n  {\n    ret.push_back(Result(pit->first, pit->second));// / cit->second));\n  }\n\t\n  // resulting \"scores\" are now in [-1 best .. 0 worst]\t\n  \n  // sort vector in ascending order of score\n  std::sort(ret.begin(), ret.end());\n  // (ret is inverted now --the lower the better--)\n\n  // cut vector\n  if(max_results > 0 && (int)ret.size() > max_results)\n    ret.resize(max_results);\n\n  // complete and scale score to [0 worst .. 1 best]\n  // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) \n\t//\t\tfor all i | v_i != 0 and w_i != 0 )\n\t// (Nister, 2006)\n\tQueryResults::iterator qit;\n  for(qit = ret.begin(); qit != ret.end(); qit++) \n  {\n    if(qit->Score <= -1.0) // rounding error\n      qit->Score = 1.0;\n    else\n      qit->Score = 1.0 - sqrt(1.0 + qit->Score); // [0..1]\n      // the + sign is ok, it is due to - sign in \n      // value = - qvalue * dvalue\n  }\n  \n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::queryChiSquare(const BowVector &vec, \n  QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector::const_iterator vit;\n  typename IFRow::const_iterator rit;\n  \n  std::map<EntryId, std::pair<double, int> > pairs;\n  std::map<EntryId, std::pair<double, int> >::iterator pit;\n  \n  std::map<EntryId, std::pair<double, double> > sums; // < sum vi, sum wi >\n  std::map<EntryId, std::pair<double, double> >::iterator sit;\n  \n  // In the current implementation, we suppose vec is not normalized\n  \n  //map<EntryId, double> expected;\n  //map<EntryId, double>::iterator eit;\n  \n  for(vit = vec.begin(); vit != vec.end(); ++vit)\n  {\n    const WordId word_id = vit->first;\n    const WordValue& qvalue = vit->second;\n    \n    const IFRow& row = m_ifile[word_id];\n    \n    // IFRows are sorted in ascending entry_id order\n    \n    for(rit = row.begin(); rit != row.end(); ++rit)\n    {\n      const EntryId entry_id = rit->entry_id;\n      const WordValue& dvalue = rit->word_weight;\n      \n      if((int)entry_id < max_id || max_id == -1)\n      {\n        // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)\n        // we move the 4 out\n        double value = 0;\n        if(qvalue + dvalue != 0.0) // words may have weight zero\n          value = - qvalue * dvalue / (qvalue + dvalue);\n        \n        pit = pairs.lower_bound(entry_id);\n        sit = sums.lower_bound(entry_id);\n        //eit = expected.lower_bound(entry_id);\n        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n        {\n          pit->second.first += value;\n          pit->second.second += 1;\n          //eit->second += dvalue;\n          sit->second.first += qvalue;\n          sit->second.second += dvalue;\n        }\n        else\n        {\n          pairs.insert(pit, \n            std::map<EntryId, std::pair<double, int> >::value_type(entry_id,\n              std::make_pair(value, 1) ));\n          //expected.insert(eit, \n          //  map<EntryId, double>::value_type(entry_id, dvalue));\n          \n          sums.insert(sit, \n            std::map<EntryId, std::pair<double, double> >::value_type(entry_id,\n              std::make_pair(qvalue, dvalue) ));\n        }\n      }\n      \n    } // for each inverted row\n  } // for each query word\n\t\n  // move to vector\n  ret.reserve(pairs.size());\n  sit = sums.begin();\n  for(pit = pairs.begin(); pit != pairs.end(); ++pit, ++sit)\n  {\n    if(pit->second.second >= MIN_COMMON_WORDS)\n    {\n      ret.push_back(Result(pit->first, pit->second.first));\n      ret.back().nWords = pit->second.second;\n      ret.back().sumCommonVi = sit->second.first;\n      ret.back().sumCommonWi = sit->second.second;\n      ret.back().expectedChiScore = \n        2 * sit->second.second / (1 + sit->second.second);\n    }\n  \n    //ret.push_back(Result(pit->first, pit->second));\n  }\n\t\n  // resulting \"scores\" are now in [-2 best .. 0 worst]\t\n  // we have to add +2 to the scores to obtain the chi square score\n  \n  // sort vector in ascending order of score\n  std::sort(ret.begin(), ret.end());\n  // (ret is inverted now --the lower the better--)\n\n  // cut vector\n  if(max_results > 0 && (int)ret.size() > max_results)\n    ret.resize(max_results);\n\n  // complete and scale score to [0 worst .. 1 best]\n  QueryResults::iterator qit;\n  for(qit = ret.begin(); qit != ret.end(); qit++)\n  {\n    // this takes the 4 into account\n    qit->Score = - 2. * qit->Score; // [0..1]\n    \n    qit->chiScore = qit->Score;\n  }\n  \n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::queryKL(const BowVector &vec, \n  QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector::const_iterator vit;\n  typename IFRow::const_iterator rit;\n  \n  std::map<EntryId, double> pairs;\n  std::map<EntryId, double>::iterator pit;\n  \n  for(vit = vec.begin(); vit != vec.end(); ++vit)\n  {\n    const WordId word_id = vit->first;\n    const WordValue& vi = vit->second;\n    \n    const IFRow& row = m_ifile[word_id];\n    \n    // IFRows are sorted in ascending entry_id order\n    \n    for(rit = row.begin(); rit != row.end(); ++rit)\n    {    \n      const EntryId entry_id = rit->entry_id;\n      const WordValue& wi = rit->word_weight;\n      \n      if((int)entry_id < max_id || max_id == -1)\n      {\n        double value = 0;\n        if(vi != 0 && wi != 0) value = vi * log(vi/wi);\n        \n        pit = pairs.lower_bound(entry_id);\n        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n        {\n          pit->second += value;\n        }\n        else\n        {\n          pairs.insert(pit, \n            std::map<EntryId, double>::value_type(entry_id, value));\n        }\n      }\n      \n    } // for each inverted row\n  } // for each query word\n\t\n  // resulting \"scores\" are now in [-X worst .. 0 best .. X worst]\n  // but we cannot make sure which ones are better without calculating\n  // the complete score\n\n  // complete scores and move to vector\n  ret.reserve(pairs.size());\n  for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n  {\n    EntryId eid = pit->first;\n    double value = 0.0;\n\n    for(vit = vec.begin(); vit != vec.end(); ++vit)\n    {\n      const WordValue &vi = vit->second;\n      const IFRow& row = m_ifile[vit->first];\n\n      if(vi != 0)\n      {\n        if(row.end() == find(row.begin(), row.end(), eid ))\n        {\n          value += vi * (log(vi) - GeneralScoring::LOG_EPS);\n        }\n      }\n    }\n    \n    pit->second += value;\n    \n    // to vector\n    ret.push_back(Result(pit->first, pit->second));\n  }\n  \n  // real scores are now in [0 best .. X worst]\n\n  // sort vector in ascending order\n  // (scores are inverted now --the lower the better--)\n  std::sort(ret.begin(), ret.end());\n\n  // cut vector\n  if(max_results > 0 && (int)ret.size() > max_results)\n    ret.resize(max_results);\n\n  // cannot scale scores\n    \n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::queryBhattacharyya(\n  const BowVector &vec, QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector::const_iterator vit;\n  typename IFRow::const_iterator rit;\n  \n  //map<EntryId, double> pairs;\n  //map<EntryId, double>::iterator pit;\n  \n  std::map<EntryId, std::pair<double, int> > pairs; // <eid, <score, counter> >\n  std::map<EntryId, std::pair<double, int> >::iterator pit;\n  \n  for(vit = vec.begin(); vit != vec.end(); ++vit)\n  {\n    const WordId word_id = vit->first;\n    const WordValue& qvalue = vit->second;\n    \n    const IFRow& row = m_ifile[word_id];\n    \n    // IFRows are sorted in ascending entry_id order\n    \n    for(rit = row.begin(); rit != row.end(); ++rit)\n    {\n      const EntryId entry_id = rit->entry_id;\n      const WordValue& dvalue = rit->word_weight;\n      \n      if((int)entry_id < max_id || max_id == -1)\n      {\n        double value = sqrt(qvalue * dvalue);\n        \n        pit = pairs.lower_bound(entry_id);\n        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n        {\n          pit->second.first += value;\n          pit->second.second += 1;\n        }\n        else\n        {\n          pairs.insert(pit, \n            std::map<EntryId, std::pair<double, int> >::value_type(entry_id,\n              std::make_pair(value, 1)));\n        }\n      }\n      \n    } // for each inverted row\n  } // for each query word\n\t\n  // move to vector\n  ret.reserve(pairs.size());\n  for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n  {\n    if(pit->second.second >= MIN_COMMON_WORDS)\n    {\n      ret.push_back(Result(pit->first, pit->second.first));\n      ret.back().nWords = pit->second.second;\n      ret.back().bhatScore = pit->second.first;\n    }\n  }\n\t\n  // scores are already in [0..1]\n\n  // sort vector in descending order\n  std::sort(ret.begin(), ret.end(), Result::gt);\n\n  // cut vector\n  if(max_results > 0 && (int)ret.size() > max_results)\n    ret.resize(max_results);\n\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::queryDotProduct(\n  const BowVector &vec, QueryResults &ret, int max_results, int max_id) const\n{\n  BowVector::const_iterator vit;\n  typename IFRow::const_iterator rit;\n  \n  std::map<EntryId, double> pairs;\n  std::map<EntryId, double>::iterator pit;\n  \n  for(vit = vec.begin(); vit != vec.end(); ++vit)\n  {\n    const WordId word_id = vit->first;\n    const WordValue& qvalue = vit->second;\n    \n    const IFRow& row = m_ifile[word_id];\n    \n    // IFRows are sorted in ascending entry_id order\n    \n    for(rit = row.begin(); rit != row.end(); ++rit)\n    {\n      const EntryId entry_id = rit->entry_id;\n      const WordValue& dvalue = rit->word_weight;\n      \n      if((int)entry_id < max_id || max_id == -1)\n      {\n        double value; \n        if(this->m_voc->getWeightingType() == BINARY)\n          value = 1;\n        else\n          value = qvalue * dvalue;\n        \n        pit = pairs.lower_bound(entry_id);\n        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))\n        {\n          pit->second += value;\n        }\n        else\n        {\n          pairs.insert(pit, \n            std::map<EntryId, double>::value_type(entry_id, value));\n        }\n      }\n      \n    } // for each inverted row\n  } // for each query word\n\t\n  // move to vector\n  ret.reserve(pairs.size());\n  for(pit = pairs.begin(); pit != pairs.end(); ++pit)\n  {\n    ret.push_back(Result(pit->first, pit->second));\n  }\n\t\n  // scores are the greater the better\n\n  // sort vector in descending order\n  std::sort(ret.begin(), ret.end(), Result::gt);\n\n  // cut vector\n  if(max_results > 0 && (int)ret.size() > max_results)\n    ret.resize(max_results);\n\n  // these scores cannot be scaled\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nconst FeatureVector& TemplatedDatabase<TDescriptor, F>::retrieveFeatures\n  (EntryId id) const\n{\n  assert(id < size());\n  return m_dfile[id];\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::save(const std::string &filename) const\n{\n  cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE);\n  if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n  \n  save(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::save(cv::FileStorage &fs,\n  const std::string &name) const\n{\n  // Format YAML:\n  // vocabulary { ... see TemplatedVocabulary::save }\n  // database \n  // {\n  //   nEntries: \n  //   usingDI: \n  //   diLevels: \n  //   invertedIndex\n  //   [\n  //     [\n  //        { \n  //          imageId: \n  //          weight: \n  //        }\n  //     ]\n  //   ]\n  //   directIndex\n  //   [\n  //      [\n  //        {\n  //          nodeId:\n  //          features: [ ]\n  //        }\n  //      ]\n  //   ]\n\n  // invertedIndex[i] is for the i-th word\n  // directIndex[i] is for the i-th entry\n  // directIndex may be empty if not using direct index\n  //\n  // imageId's and nodeId's must be stored in ascending order\n  // (according to the construction of the indexes)\n\n  m_voc->save(fs);\n \n  fs << name << \"{\";\n  \n  fs << \"nEntries\" << m_nentries;\n  fs << \"usingDI\" << (m_use_di ? 1 : 0);\n  fs << \"diLevels\" << m_dilevels;\n  \n  fs << \"invertedIndex\" << \"[\";\n  \n  typename InvertedFile::const_iterator iit;\n  typename IFRow::const_iterator irit;\n  for(iit = m_ifile.begin(); iit != m_ifile.end(); ++iit)\n  {\n    fs << \"[\"; // word of IF\n    for(irit = iit->begin(); irit != iit->end(); ++irit)\n    {\n      fs << \"{:\" \n        << \"imageId\" << (int)irit->entry_id\n        << \"weight\" << irit->word_weight\n        << \"}\";\n    }\n    fs << \"]\"; // word of IF\n  }\n  \n  fs << \"]\"; // invertedIndex\n  \n  fs << \"directIndex\" << \"[\";\n  \n  typename DirectFile::const_iterator dit;\n  typename FeatureVector::const_iterator drit;\n  for(dit = m_dfile.begin(); dit != m_dfile.end(); ++dit)\n  {\n    fs << \"[\"; // entry of DF\n    \n    for(drit = dit->begin(); drit != dit->end(); ++drit)\n    {\n      NodeId nid = drit->first;\n      const std::vector<unsigned int>& features = drit->second;\n      \n      // save info of last_nid\n      fs << \"{\";\n      fs << \"nodeId\" << (int)nid;\n      // msvc++ 2010 with opencv 2.3.1 does not allow FileStorage::operator<<\n      // with vectors of unsigned int\n      fs << \"features\" << \"[\" \n        << *(const std::vector<int>*)(&features) << \"]\";\n      fs << \"}\";\n    }\n    \n    fs << \"]\"; // entry of DF\n  }\n  \n  fs << \"]\"; // directIndex\n  \n  fs << \"}\"; // database\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::load(const std::string &filename)\n{\n  cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ);\n  if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n  \n  load(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedDatabase<TDescriptor, F>::load(const cv::FileStorage &fs,\n  const std::string &name)\n{ \n  // load voc first\n  // subclasses must instantiate m_voc before calling this ::load\n  if(!m_voc) m_voc = new TemplatedVocabulary<TDescriptor, F>;\n  \n  m_voc->load(fs);\n\n  // load database now\n  clear(); // resizes inverted file \n    \n  cv::FileNode fdb = fs[name];\n  \n  m_nentries = (int)fdb[\"nEntries\"]; \n  m_use_di = (int)fdb[\"usingDI\"] != 0;\n  m_dilevels = (int)fdb[\"diLevels\"];\n  \n  cv::FileNode fn = fdb[\"invertedIndex\"];\n  for(WordId wid = 0; wid < fn.size(); ++wid)\n  {\n    cv::FileNode fw = fn[wid];\n    \n    for(unsigned int i = 0; i < fw.size(); ++i)\n    {\n      EntryId eid = (int)fw[i][\"imageId\"];\n      WordValue v = fw[i][\"weight\"];\n      \n      m_ifile[wid].push_back(IFPair(eid, v));\n    }\n  }\n  \n  if(m_use_di)\n  {\n    fn = fdb[\"directIndex\"];\n    \n    m_dfile.resize(fn.size());\n    m_dBowfile.resize(fn.size());\n    assert(m_nentries == (int)fn.size());\n    \n    FeatureVector::iterator dit;\n    for(EntryId eid = 0; eid < fn.size(); ++eid)\n    {\n      cv::FileNode fe = fn[eid];\n      \n      m_dfile[eid].clear();\n      m_dBowfile[eid].clear();\n      for(unsigned int i = 0; i < fe.size(); ++i)\n      {\n        NodeId nid = (int)fe[i][\"nodeId\"];\n        \n        dit = m_dfile[eid].insert(m_dfile[eid].end(), \n          make_pair(nid, std::vector<unsigned int>() ));\n        \n        // this failed to compile with some opencv versions (2.3.1)\n        //fe[i][\"features\"] >> dit->second;\n        \n        // this was ok until OpenCV 2.4.1\n        //std::vector<int> aux;\n        //fe[i][\"features\"] >> aux; // OpenCV < 2.4.1\n        //dit->second.resize(aux.size());\n        //std::copy(aux.begin(), aux.end(), dit->second.begin());\n        \n        cv::FileNode ff = fe[i][\"features\"][0];\n        dit->second.reserve(ff.size());\n                \n        cv::FileNodeIterator ffit;\n        for(ffit = ff.begin(); ffit != ff.end(); ++ffit)\n        {\n          dit->second.push_back((int)*ffit); \n        }\n      }\n    } // for each entry\n  } // if use_id\n  \n}\n\n// --------------------------------------------------------------------------\n\n/**\n * Writes printable information of the database\n * @param os stream to write to\n * @param db\n */\ntemplate<class TDescriptor, class F>\nstd::ostream& operator<<(std::ostream &os, \n  const TemplatedDatabase<TDescriptor,F> &db)\n{\n  os << \"Database: Entries = \" << db.size() << \", \"\n    \"Using direct index = \" << (db.usingDirectIndex() ? \"yes\" : \"no\");\n  \n  if(db.usingDirectIndex())\n    os << \", Direct index levels = \" << db.getDirectIndexLevels();\n  \n  os << \". \" << *db.getVocabulary();\n  return os;\n}\n\n// --------------------------------------------------------------------------\n\n} // namespace DBoW2\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DBoW/TemplatedVocabulary.h",
    "content": "/**\n * File: TemplatedVocabulary.h\n * Date: February 2011\n * Author: Dorian Galvez-Lopez\n * Description: templated vocabulary \n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_T_TEMPLATED_VOCABULARY__\n#define __D_T_TEMPLATED_VOCABULARY__\n\n#include <cassert>\n\n#include <vector>\n#include <numeric>\n#include <fstream>\n#include <string>\n#include <algorithm>\n#include <opencv2/opencv.hpp>\n\n#include \"FeatureVector.h\"\n#include \"BowVector.h\"\n#include \"ScoringObject.h\"\n\n#include \"../DUtils/DUtils.h\"\n#include <iostream>\n// Added by VINS [[[\n#include \"../VocabularyBinary.hpp\"\n#include <boost/dynamic_bitset.hpp>\n// Added by VINS ]]]\n\nnamespace DBoW2 {\n\n/// @param TDescriptor class of descriptor\n/// @param F class of descriptor functions\ntemplate<class TDescriptor, class F>\n/// Generic Vocabulary\nclass TemplatedVocabulary\n{\t\t\npublic:\n  \n  /**\n   * Initiates an empty vocabulary\n   * @param k branching factor\n   * @param L depth levels\n   * @param weighting weighting type\n   * @param scoring scoring type\n   */\n  TemplatedVocabulary(int k = 10, int L = 5, \n    WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM);\n  \n  /**\n   * Creates the vocabulary by loading a file\n   * @param filename\n   */\n  TemplatedVocabulary(const std::string &filename);\n  \n  /**\n   * Creates the vocabulary by loading a file\n   * @param filename\n   */\n  TemplatedVocabulary(const char *filename);\n  \n  /** \n   * Copy constructor\n   * @param voc\n   */\n  TemplatedVocabulary(const TemplatedVocabulary<TDescriptor, F> &voc);\n  \n  /**\n   * Destructor\n   */\n  virtual ~TemplatedVocabulary();\n  \n  /** \n   * Assigns the given vocabulary to this by copying its data and removing\n   * all the data contained by this vocabulary before\n   * @param voc\n   * @return reference to this vocabulary\n   */\n  TemplatedVocabulary<TDescriptor, F>& operator=(\n    const TemplatedVocabulary<TDescriptor, F> &voc);\n  \n  /** \n   * Creates a vocabulary from the training features with the already\n   * defined parameters\n   * @param training_features\n   */\n  virtual void create\n    (const std::vector<std::vector<TDescriptor> > &training_features);\n  \n  /**\n   * Creates a vocabulary from the training features, setting the branching\n   * factor and the depth levels of the tree\n   * @param training_features\n   * @param k branching factor\n   * @param L depth levels\n   */\n  virtual void create\n    (const std::vector<std::vector<TDescriptor> > &training_features, \n      int k, int L);\n\n  /**\n   * Creates a vocabulary from the training features, setting the branching\n   * factor nad the depth levels of the tree, and the weighting and scoring\n   * schemes\n   */\n  virtual void create\n    (const std::vector<std::vector<TDescriptor> > &training_features,\n      int k, int L, WeightingType weighting, ScoringType scoring);\n\n  /**\n   * Returns the number of words in the vocabulary\n   * @return number of words\n   */\n  virtual inline unsigned int size() const;\n  \n  /**\n   * Returns whether the vocabulary is empty (i.e. it has not been trained)\n   * @return true iff the vocabulary is empty\n   */\n  virtual inline bool empty() const;\n\n  /**\n   * Transforms a set of descriptores into a bow vector\n   * @param features\n   * @param v (out) bow vector of weighted words\n   */\n  virtual void transform(const std::vector<TDescriptor>& features, BowVector &v) \n    const;\n  \n  /**\n   * Transform a set of descriptors into a bow vector and a feature vector\n   * @param features\n   * @param v (out) bow vector\n   * @param fv (out) feature vector of nodes and feature indexes\n   * @param levelsup levels to go up the vocabulary tree to get the node index\n   */\n  virtual void transform(const std::vector<TDescriptor>& features,\n    BowVector &v, FeatureVector &fv, int levelsup) const;\n\n  /**\n   * Transforms a single feature into a word (without weight)\n   * @param feature\n   * @return word id\n   */\n  virtual WordId transform(const TDescriptor& feature) const;\n  \n  /**\n   * Returns the score of two vectors\n   * @param a vector\n   * @param b vector\n   * @return score between vectors\n   * @note the vectors must be already sorted and normalized if necessary\n   */\n  inline double score(const BowVector &a, const BowVector &b) const;\n  \n  /**\n   * Returns the id of the node that is \"levelsup\" levels from the word given\n   * @param wid word id\n   * @param levelsup 0..L\n   * @return node id. if levelsup is 0, returns the node id associated to the\n   *   word id\n   */\n  virtual NodeId getParentNode(WordId wid, int levelsup) const;\n  \n  /**\n   * Returns the ids of all the words that are under the given node id,\n   * by traversing any of the branches that goes down from the node\n   * @param nid starting node id\n   * @param words ids of words\n   */\n  void getWordsFromNode(NodeId nid, std::vector<WordId> &words) const;\n  \n  /**\n   * Returns the branching factor of the tree (k)\n   * @return k\n   */\n  inline int getBranchingFactor() const { return m_k; }\n  \n  /** \n   * Returns the depth levels of the tree (L)\n   * @return L\n   */\n  inline int getDepthLevels() const { return m_L; }\n  \n  /**\n   * Returns the real depth levels of the tree on average\n   * @return average of depth levels of leaves\n   */\n  float getEffectiveLevels() const;\n  \n  /**\n   * Returns the descriptor of a word\n   * @param wid word id\n   * @return descriptor\n   */\n  virtual inline TDescriptor getWord(WordId wid) const;\n  \n  /**\n   * Returns the weight of a word\n   * @param wid word id\n   * @return weight\n   */\n  virtual inline WordValue getWordWeight(WordId wid) const;\n  \n  /** \n   * Returns the weighting method\n   * @return weighting method\n   */\n  inline WeightingType getWeightingType() const { return m_weighting; }\n  \n  /** \n   * Returns the scoring method\n   * @return scoring method\n   */\n  inline ScoringType getScoringType() const { return m_scoring; }\n  \n  /**\n   * Changes the weighting method\n   * @param type new weighting type\n   */\n  inline void setWeightingType(WeightingType type);\n  \n  /**\n   * Changes the scoring method\n   * @param type new scoring type\n   */\n  void setScoringType(ScoringType type);\n  \n  /**\n   * Saves the vocabulary into a file\n   * @param filename\n   */\n  void save(const std::string &filename) const;\n  \n  /**\n   * Loads the vocabulary from a file\n   * @param filename\n   */\n  void load(const std::string &filename);\n  \n  /** \n   * Saves the vocabulary to a file storage structure\n   * @param fn node in file storage\n   */\n  virtual void save(cv::FileStorage &fs, \n    const std::string &name = \"vocabulary\") const;\n  \n  /**\n   * Loads the vocabulary from a file storage node\n   * @param fn first node\n   * @param subname name of the child node of fn where the tree is stored.\n   *   If not given, the fn node is used instead\n   */  \n  virtual void load(const cv::FileStorage &fs, \n    const std::string &name = \"vocabulary\");\n    \n  // Added by VINS [[[\n  virtual void loadBin(const std::string &filename);\n  // Added by VINS ]]]\n    \n  /** \n   * Stops those words whose weight is below minWeight.\n   * Words are stopped by setting their weight to 0. There are not returned\n   * later when transforming image features into vectors.\n   * Note that when using IDF or TF_IDF, the weight is the idf part, which\n   * is equivalent to -log(f), where f is the frequency of the word\n   * (f = Ni/N, Ni: number of training images where the word is present, \n   * N: number of training images).\n   * Note that the old weight is forgotten, and subsequent calls to this \n   * function with a lower minWeight have no effect.\n   * @return number of words stopped now\n   */\n  virtual int stopWords(double minWeight);\n\nprotected:\n\n  /// Pointer to descriptor\n  typedef const TDescriptor *pDescriptor;\n\n  /// Tree node\n  struct Node \n  {\n    /// Node id\n    NodeId id;\n    /// Weight if the node is a word\n    WordValue weight;\n    /// Children \n    std::vector<NodeId> children;\n    /// Parent node (undefined in case of root)\n    NodeId parent;\n    /// Node descriptor\n    TDescriptor descriptor;\n\n    /// Word id if the node is a word\n    WordId word_id;\n\n    /**\n     * Empty constructor\n     */\n    Node(): id(0), weight(0), parent(0), word_id(0){}\n    \n    /**\n     * Constructor\n     * @param _id node id\n     */\n    Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){}\n\n    /**\n     * Returns whether the node is a leaf node\n     * @return true iff the node is a leaf\n     */\n    inline bool isLeaf() const { return children.empty(); }\n  };\n\nprotected:\n\n  /**\n   * Creates an instance of the scoring object accoring to m_scoring\n   */\n  void createScoringObject();\n\n  /** \n   * Returns a set of pointers to descriptores\n   * @param training_features all the features\n   * @param features (out) pointers to the training features\n   */\n  void getFeatures(\n    const std::vector<std::vector<TDescriptor> > &training_features,\n    std::vector<pDescriptor> &features) const;\n\n  /**\n   * Returns the word id associated to a feature\n   * @param feature\n   * @param id (out) word id\n   * @param weight (out) word weight\n   * @param nid (out) if given, id of the node \"levelsup\" levels up\n   * @param levelsup\n   */\n  virtual void transform(const TDescriptor &feature, \n    WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const;\n\n  /**\n   * Returns the word id associated to a feature\n   * @param feature\n   * @param id (out) word id\n   */\n  virtual void transform(const TDescriptor &feature, WordId &id) const;\n      \n  /**\n   * Creates a level in the tree, under the parent, by running kmeans with\n   * a descriptor set, and recursively creates the subsequent levels too\n   * @param parent_id id of parent node\n   * @param descriptors descriptors to run the kmeans on\n   * @param current_level current level in the tree\n   */\n  void HKmeansStep(NodeId parent_id, const std::vector<pDescriptor> &descriptors,\n    int current_level);\n\n  /**\n   * Creates k clusters from the given descriptors with some seeding algorithm.\n   * @note In this class, kmeans++ is used, but this function should be\n   *   overriden by inherited classes.\n   */\n  virtual void initiateClusters(const std::vector<pDescriptor> &descriptors,\n    std::vector<TDescriptor> &clusters) const;\n  \n  /**\n   * Creates k clusters from the given descriptor sets by running the\n   * initial step of kmeans++\n   * @param descriptors \n   * @param clusters resulting clusters\n   */\n  void initiateClustersKMpp(const std::vector<pDescriptor> &descriptors,\n    std::vector<TDescriptor> &clusters) const;\n  \n  /**\n   * Create the words of the vocabulary once the tree has been built\n   */\n  void createWords();\n  \n  /**\n   * Sets the weights of the nodes of tree according to the given features.\n   * Before calling this function, the nodes and the words must be already\n   * created (by calling HKmeansStep and createWords)\n   * @param features\n   */\n  void setNodeWeights(const std::vector<std::vector<TDescriptor> > &features);\n  \nprotected:\n\n  /// Branching factor\n  int m_k;\n  \n  /// Depth levels \n  int m_L;\n  \n  /// Weighting method\n  WeightingType m_weighting;\n  \n  /// Scoring method\n  ScoringType m_scoring;\n  \n  /// Object for computing scores\n  GeneralScoring* m_scoring_object;\n  \n  /// Tree nodes\n  std::vector<Node> m_nodes;\n  \n  /// Words of the vocabulary (tree leaves)\n  /// this condition holds: m_words[wid]->word_id == wid\n  std::vector<Node*> m_words;\n  \n};\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary\n  (int k, int L, WeightingType weighting, ScoringType scoring)\n  : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring),\n  m_scoring_object(NULL)\n{\n  //printf(\"loop start load bin\\n\");\n  createScoringObject();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary\n  (const std::string &filename): m_scoring_object(NULL)\n{\n    //m_scoring = KL;\n    // Changed by VINS [[[\n    //printf(\"loop start load bin\\n\");\n    loadBin(filename);\n    // Changed by VINS ]]]\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary\n  (const char *filename): m_scoring_object(NULL)\n{\n    //m_scoring = KL;\n    // Changed by VINS [[[\n    //printf(\"loop start load bin\\n\");\n    loadBin(filename);\n    // Changed by VINS ]]]\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::createScoringObject()\n{\n  delete m_scoring_object;\n  m_scoring_object = NULL;\n  \n  switch(m_scoring)\n  {\n    case L1_NORM: \n      m_scoring_object = new L1Scoring;\n      break;\n      \n    case L2_NORM:\n      m_scoring_object = new L2Scoring;\n      break;\n    \n    case CHI_SQUARE:\n      m_scoring_object = new ChiSquareScoring;\n      break;\n      \n    case KL:\n      m_scoring_object = new KLScoring;\n      break;\n      \n    case BHATTACHARYYA:\n      m_scoring_object = new BhattacharyyaScoring;\n      break;\n      \n    case DOT_PRODUCT:\n      m_scoring_object = new DotProductScoring;\n      break;\n    \n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::setScoringType(ScoringType type)\n{\n  m_scoring = type;\n  createScoringObject();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::setWeightingType(WeightingType type)\n{\n  this->m_weighting = type;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary(\n  const TemplatedVocabulary<TDescriptor, F> &voc)\n  : m_scoring_object(NULL)\n{\n  printf(\"loop start load vocabulary\\n\");\n  *this = voc;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedVocabulary<TDescriptor,F>::~TemplatedVocabulary()\n{\n  delete m_scoring_object;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTemplatedVocabulary<TDescriptor, F>& \nTemplatedVocabulary<TDescriptor,F>::operator=\n  (const TemplatedVocabulary<TDescriptor, F> &voc)\n{  \n  this->m_k = voc.m_k;\n  this->m_L = voc.m_L;\n  this->m_scoring = voc.m_scoring;\n  this->m_weighting = voc.m_weighting;\n\n  this->createScoringObject();\n  \n  this->m_nodes.clear();\n  this->m_words.clear();\n  \n  this->m_nodes = voc.m_nodes;\n  this->createWords();\n  \n  return *this;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::create(\n  const std::vector<std::vector<TDescriptor> > &training_features)\n{\n  m_nodes.clear();\n  m_words.clear();\n  \n  // expected_nodes = Sum_{i=0..L} ( k^i )\n\tint expected_nodes = \n\t\t(int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));\n\n  m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree\n  \n  \n  std::vector<pDescriptor> features;\n  getFeatures(training_features, features);\n\n\n  // create root  \n  m_nodes.push_back(Node(0)); // root\n  \n  // create the tree\n  HKmeansStep(0, features, 1);\n\n  // create the words\n  createWords();\n\n  // and set the weight of each node of the tree\n  setNodeWeights(training_features);\n  \n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::create(\n  const std::vector<std::vector<TDescriptor> > &training_features,\n  int k, int L)\n{\n  m_k = k;\n  m_L = L;\n  \n  create(training_features);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::create(\n  const std::vector<std::vector<TDescriptor> > &training_features,\n  int k, int L, WeightingType weighting, ScoringType scoring)\n{\n  m_k = k;\n  m_L = L;\n  m_weighting = weighting;\n  m_scoring = scoring;\n  createScoringObject();\n  \n  create(training_features);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::getFeatures(\n  const std::vector<std::vector<TDescriptor> > &training_features,\n  std::vector<pDescriptor> &features) const\n{\n  features.resize(0);\n  \n  typename std::vector<std::vector<TDescriptor> >::const_iterator vvit;\n  typename std::vector<TDescriptor>::const_iterator vit;\n  for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit)\n  {\n    features.reserve(features.size() + vvit->size());\n    for(vit = vvit->begin(); vit != vvit->end(); ++vit)\n    {\n      features.push_back(&(*vit));\n    }\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::HKmeansStep(NodeId parent_id, \n  const std::vector<pDescriptor> &descriptors, int current_level)\n{\n  if(descriptors.empty()) return;\n        \n  // features associated to each cluster\n  std::vector<TDescriptor> clusters;\n  std::vector<std::vector<unsigned int> > groups; // groups[i] = [j1, j2, ...]\n\t// j1, j2, ... indices of descriptors associated to cluster i\n\n  clusters.reserve(m_k);\n\tgroups.reserve(m_k);\n  \n  //const int msizes[] = { m_k, descriptors.size() };\n  //cv::SparseMat assoc(2, msizes, CV_8U);\n  //cv::SparseMat last_assoc(2, msizes, CV_8U);  \n  //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated\n  \n  if((int)descriptors.size() <= m_k)\n  {\n    // trivial case: one cluster per feature\n    groups.resize(descriptors.size());\n\n    for(unsigned int i = 0; i < descriptors.size(); i++)\n    {\n      groups[i].push_back(i);\n      clusters.push_back(*descriptors[i]);\n    }\n  }\n  else\n  {\n    // select clusters and groups with kmeans\n    \n    bool first_time = true;\n    bool goon = true;\n    \n    // to check if clusters move after iterations\n    std::vector<int> last_association, current_association;\n\n    while(goon)\n    {\n      // 1. Calculate clusters\n\n\t\t\tif(first_time)\n\t\t\t{\n        // random sample \n        initiateClusters(descriptors, clusters);\n      }\n      else\n      {\n        // calculate cluster centres\n\n        for(unsigned int c = 0; c < clusters.size(); ++c)\n        {\n          std::vector<pDescriptor> cluster_descriptors;\n          cluster_descriptors.reserve(groups[c].size());\n          \n          /*\n          for(unsigned int d = 0; d < descriptors.size(); ++d)\n          {\n            if( assoc.find<unsigned char>(c, d) )\n            {\n              cluster_descriptors.push_back(descriptors[d]);\n            }\n          }\n          */\n          \n          std::vector<unsigned int>::const_iterator vit;\n          for(vit = groups[c].begin(); vit != groups[c].end(); ++vit)\n          {\n            cluster_descriptors.push_back(descriptors[*vit]);\n          }\n          \n          \n          F::meanValue(cluster_descriptors, clusters[c]);\n        }\n        \n      } // if(!first_time)\n\n      // 2. Associate features with clusters\n\n      // calculate distances to cluster centers\n      groups.clear();\n      groups.resize(clusters.size(), std::vector<unsigned int>());\n      current_association.resize(descriptors.size());\n\n      //assoc.clear();\n\n      typename std::vector<pDescriptor>::const_iterator fit;\n      //unsigned int d = 0;\n      for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d)\n      {\n        double best_dist = F::distance(*(*fit), clusters[0]);\n        unsigned int icluster = 0;\n        \n        for(unsigned int c = 1; c < clusters.size(); ++c)\n        {\n          double dist = F::distance(*(*fit), clusters[c]);\n          if(dist < best_dist)\n          {\n            best_dist = dist;\n            icluster = c;\n          }\n        }\n\n        //assoc.ref<unsigned char>(icluster, d) = 1;\n\n        groups[icluster].push_back(fit - descriptors.begin());\n        current_association[ fit - descriptors.begin() ] = icluster;\n      }\n      \n      // kmeans++ ensures all the clusters has any feature associated with them\n\n      // 3. check convergence\n      if(first_time)\n      {\n        first_time = false;\n      }\n      else\n      {\n        //goon = !eqUChar(last_assoc, assoc);\n        \n        goon = false;\n        for(unsigned int i = 0; i < current_association.size(); i++)\n        {\n          if(current_association[i] != last_association[i]){\n            goon = true;\n            break;\n          }\n        }\n      }\n\n\t\t\tif(goon)\n\t\t\t{\n\t\t\t\t// copy last feature-cluster association\n\t\t\t\tlast_association = current_association;\n\t\t\t\t//last_assoc = assoc.clone();\n\t\t\t}\n\t\t\t\n\t\t} // while(goon)\n    \n  } // if must run kmeans\n  \n  // create nodes\n  for(unsigned int i = 0; i < clusters.size(); ++i)\n  {\n    NodeId id = m_nodes.size();\n    m_nodes.push_back(Node(id));\n    m_nodes.back().descriptor = clusters[i];\n    m_nodes.back().parent = parent_id;\n    m_nodes[parent_id].children.push_back(id);\n  }\n  \n  // go on with the next level\n  if(current_level < m_L)\n  {\n    // iterate again with the resulting clusters\n    const std::vector<NodeId> &children_ids = m_nodes[parent_id].children;\n    for(unsigned int i = 0; i < clusters.size(); ++i)\n    {\n      NodeId id = children_ids[i];\n\n      std::vector<pDescriptor> child_features;\n      child_features.reserve(groups[i].size());\n\n      std::vector<unsigned int>::const_iterator vit;\n      for(vit = groups[i].begin(); vit != groups[i].end(); ++vit)\n      {\n        child_features.push_back(descriptors[*vit]);\n      }\n\n      if(child_features.size() > 1)\n      {\n        HKmeansStep(id, child_features, current_level + 1);\n      }\n    }\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor, F>::initiateClusters\n  (const std::vector<pDescriptor> &descriptors,\n   std::vector<TDescriptor> &clusters) const\n{\n  initiateClustersKMpp(descriptors, clusters);  \n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::initiateClustersKMpp(\n  const std::vector<pDescriptor> &pfeatures,\n    std::vector<TDescriptor> &clusters) const\n{\n  // Implements kmeans++ seeding algorithm\n  // Algorithm:\n  // 1. Choose one center uniformly at random from among the data points.\n  // 2. For each data point x, compute D(x), the distance between x and the nearest \n  //    center that has already been chosen.\n  // 3. Add one new data point as a center. Each point x is chosen with probability \n  //    proportional to D(x)^2.\n  // 4. Repeat Steps 2 and 3 until k centers have been chosen.\n  // 5. Now that the initial centers have been chosen, proceed using standard k-means \n  //    clustering.\n\n  DUtils::Random::SeedRandOnce();\n\n  clusters.resize(0);\n  clusters.reserve(m_k);\n  std::vector<double> min_dists(pfeatures.size(), std::numeric_limits<double>::max());\n  \n  // 1.\n  \n  int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1);\n  \n  // create first cluster\n  clusters.push_back(*pfeatures[ifeature]);\n\n  // compute the initial distances\n  typename std::vector<pDescriptor>::const_iterator fit;\n  std::vector<double>::iterator dit;\n  dit = min_dists.begin();\n  for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit)\n  {\n    *dit = F::distance(*(*fit), clusters.back());\n  }  \n\n  while((int)clusters.size() < m_k)\n  {\n    // 2.\n    dit = min_dists.begin();\n    for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit)\n    {\n      if(*dit > 0)\n      {\n        double dist = F::distance(*(*fit), clusters.back());\n        if(dist < *dit) *dit = dist;\n      }\n    }\n    \n    // 3.\n    double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0);\n\n    if(dist_sum > 0)\n    {\n      double cut_d;\n      do\n      {\n        cut_d = DUtils::Random::RandomValue<double>(0, dist_sum);\n      } while(cut_d == 0.0);\n\n      double d_up_now = 0;\n      for(dit = min_dists.begin(); dit != min_dists.end(); ++dit)\n      {\n        d_up_now += *dit;\n        if(d_up_now >= cut_d) break;\n      }\n      \n      if(dit == min_dists.end()) \n        ifeature = pfeatures.size()-1;\n      else\n        ifeature = dit - min_dists.begin();\n      \n      clusters.push_back(*pfeatures[ifeature]);\n\n    } // if dist_sum > 0\n    else\n      break;\n      \n  } // while(used_clusters < m_k)\n\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::createWords()\n{\n  m_words.resize(0);\n  \n  if(!m_nodes.empty())\n  {\n    m_words.reserve( (int)pow((double)m_k, (double)m_L) );\n\n    typename std::vector<Node>::iterator nit;\n    \n    nit = m_nodes.begin(); // ignore root\n    for(++nit; nit != m_nodes.end(); ++nit)\n    {\n      if(nit->isLeaf())\n      {\n        nit->word_id = m_words.size();\n        m_words.push_back( &(*nit) );\n      }\n    }\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::setNodeWeights\n  (const std::vector<std::vector<TDescriptor> > &training_features)\n{\n  const unsigned int NWords = m_words.size();\n  const unsigned int NDocs = training_features.size();\n\n  if(m_weighting == TF || m_weighting == BINARY)\n  {\n    // idf part must be 1 always\n    for(unsigned int i = 0; i < NWords; i++)\n      m_words[i]->weight = 1;\n  }\n  else if(m_weighting == IDF || m_weighting == TF_IDF)\n  {\n    // IDF and TF-IDF: we calculte the idf path now\n\n    // Note: this actually calculates the idf part of the tf-idf score.\n    // The complete tf-idf score is calculated in ::transform\n\n    std::vector<unsigned int> Ni(NWords, 0);\n    std::vector<bool> counted(NWords, false);\n    \n    typename std::vector<std::vector<TDescriptor> >::const_iterator mit;\n    typename std::vector<TDescriptor>::const_iterator fit;\n\n    for(mit = training_features.begin(); mit != training_features.end(); ++mit)\n    {\n      fill(counted.begin(), counted.end(), false);\n\n      for(fit = mit->begin(); fit < mit->end(); ++fit)\n      {\n        WordId word_id;\n        transform(*fit, word_id);\n\n        if(!counted[word_id])\n        {\n          Ni[word_id]++;\n          counted[word_id] = true;\n        }\n      }\n    }\n\n    // set ln(N/Ni)\n    for(unsigned int i = 0; i < NWords; i++)\n    {\n      if(Ni[i] > 0)\n      {\n        m_words[i]->weight = log((double)NDocs / (double)Ni[i]);\n      }// else // This cannot occur if using kmeans++\n    }\n  \n  }\n\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline unsigned int TemplatedVocabulary<TDescriptor,F>::size() const\n{\n  return m_words.size();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\ninline bool TemplatedVocabulary<TDescriptor,F>::empty() const\n{\n  return m_words.empty();\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nfloat TemplatedVocabulary<TDescriptor,F>::getEffectiveLevels() const\n{\n  long sum = 0;\n  typename std::vector<Node*>::const_iterator wit;\n  for(wit = m_words.begin(); wit != m_words.end(); ++wit)\n  {\n    const Node *p = *wit;\n    \n    for(; p->id != 0; sum++) p = &m_nodes[p->parent];\n  }\n  \n  return (float)((double)sum / (double)m_words.size());\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nTDescriptor TemplatedVocabulary<TDescriptor,F>::getWord(WordId wid) const\n{\n  return m_words[wid]->descriptor;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nWordValue TemplatedVocabulary<TDescriptor, F>::getWordWeight(WordId wid) const\n{\n  return m_words[wid]->weight;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nWordId TemplatedVocabulary<TDescriptor, F>::transform\n  (const TDescriptor& feature) const\n{\n  if(empty())\n  {\n    return 0;\n  }\n  \n  WordId wid;\n  transform(feature, wid);\n  return wid;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::transform(\n  const std::vector<TDescriptor>& features, BowVector &v) const\n{\n  v.clear();\n  \n  if(empty())\n  {\n    return;\n  }\n\n  // normalize \n  LNorm norm;\n  bool must = m_scoring_object->mustNormalize(norm);\n\n  typename std::vector<TDescriptor>::const_iterator fit;\n\n  if(m_weighting == TF || m_weighting == TF_IDF)\n  {\n    for(fit = features.begin(); fit < features.end(); ++fit)\n    {\n      WordId id;\n      WordValue w; \n      // w is the idf value if TF_IDF, 1 if TF\n      \n      transform(*fit, id, w);\n      \n      // not stopped\n      if(w > 0) v.addWeight(id, w);\n    }\n    \n    if(!v.empty() && !must)\n    {\n      // unnecessary when normalizing\n      const double nd = v.size();\n      for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) \n        vit->second /= nd;\n    }\n    \n  }\n  else // IDF || BINARY\n  {\n    for(fit = features.begin(); fit < features.end(); ++fit)\n    {\n      WordId id;\n      WordValue w;\n      // w is idf if IDF, or 1 if BINARY\n      \n      transform(*fit, id, w);\n      \n      // not stopped\n      if(w > 0) v.addIfNotExist(id, w);\n      \n    } // if add_features\n  } // if m_weighting == ...\n  \n  if(must) v.normalize(norm);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F> \nvoid TemplatedVocabulary<TDescriptor,F>::transform(\n  const std::vector<TDescriptor>& features,\n  BowVector &v, FeatureVector &fv, int levelsup) const\n{\n  v.clear();\n  fv.clear();\n  \n  if(empty()) // safe for subclasses\n  {\n    return;\n  }\n  \n  // normalize \n  LNorm norm;\n  bool must = m_scoring_object->mustNormalize(norm);\n  \n  typename std::vector<TDescriptor>::const_iterator fit;\n  \n  if(m_weighting == TF || m_weighting == TF_IDF)\n  {\n    unsigned int i_feature = 0;\n    for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)\n    {\n      WordId id;\n      NodeId nid;\n      WordValue w; \n      // w is the idf value if TF_IDF, 1 if TF\n      \n      transform(*fit, id, w, &nid, levelsup);\n      \n      if(w > 0) // not stopped\n      { \n        v.addWeight(id, w);\n        fv.addFeature(nid, i_feature);\n      }\n    }\n    \n    if(!v.empty() && !must)\n    {\n      // unnecessary when normalizing\n      const double nd = v.size();\n      for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) \n        vit->second /= nd;\n    }\n  \n  }\n  else // IDF || BINARY\n  {\n    unsigned int i_feature = 0;\n    for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)\n    {\n      WordId id;\n      NodeId nid;\n      WordValue w;\n      // w is idf if IDF, or 1 if BINARY\n      \n      transform(*fit, id, w, &nid, levelsup);\n      \n      if(w > 0) // not stopped\n      {\n        v.addIfNotExist(id, w);\n        fv.addFeature(nid, i_feature);\n      }\n    }\n  } // if m_weighting == ...\n  \n  if(must) v.normalize(norm);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F> \ninline double TemplatedVocabulary<TDescriptor,F>::score\n  (const BowVector &v1, const BowVector &v2) const\n{\n  return m_scoring_object->score(v1, v2);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::transform\n  (const TDescriptor &feature, WordId &id) const\n{\n  WordValue weight;\n  transform(feature, id, weight);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature, \n  WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const\n{ \n  // propagate the feature down the tree\n  std::vector<NodeId> nodes;\n  typename std::vector<NodeId>::const_iterator nit;\n\n  // level at which the node must be stored in nid, if given\n  const int nid_level = m_L - levelsup;\n  if(nid_level <= 0 && nid != NULL) *nid = 0; // root\n\n  NodeId final_id = 0; // root\n  int current_level = 0;\n\n  do\n  {\n    ++current_level;\n    nodes = m_nodes[final_id].children;\n    final_id = nodes[0];\n \n    double best_d = F::distance(feature, m_nodes[final_id].descriptor);\n\n    for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit)\n    {\n      NodeId id = *nit;\n      double d = F::distance(feature, m_nodes[id].descriptor);\n      if(d < best_d)\n      {\n        best_d = d;\n        final_id = id;\n      }\n    }\n    \n    if(nid != NULL && current_level == nid_level)\n      *nid = final_id;\n    \n  } while( !m_nodes[final_id].isLeaf() );\n\n  // turn node id into word id\n  word_id = m_nodes[final_id].word_id;\n  weight = m_nodes[final_id].weight;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nNodeId TemplatedVocabulary<TDescriptor,F>::getParentNode\n  (WordId wid, int levelsup) const\n{\n  NodeId ret = m_words[wid]->id; // node id\n  while(levelsup > 0 && ret != 0) // ret == 0 --> root\n  {\n    --levelsup;\n    ret = m_nodes[ret].parent;\n  }\n  return ret;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::getWordsFromNode\n  (NodeId nid, std::vector<WordId> &words) const\n{\n  words.clear();\n  \n  if(m_nodes[nid].isLeaf())\n  {\n    words.push_back(m_nodes[nid].word_id);\n  }\n  else\n  {\n    words.reserve(m_k); // ^1, ^2, ...\n    \n    std::vector<NodeId> parents;\n    parents.push_back(nid);\n    \n    while(!parents.empty())\n    {\n      NodeId parentid = parents.back();\n      parents.pop_back();\n      \n      const std::vector<NodeId> &child_ids = m_nodes[parentid].children;\n      std::vector<NodeId>::const_iterator cit;\n      \n      for(cit = child_ids.begin(); cit != child_ids.end(); ++cit)\n      {\n        const Node &child_node = m_nodes[*cit];\n        \n        if(child_node.isLeaf())\n          words.push_back(child_node.word_id);\n        else\n          parents.push_back(*cit);\n        \n      } // for each child\n    } // while !parents.empty\n  }\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nint TemplatedVocabulary<TDescriptor,F>::stopWords(double minWeight)\n{\n  int c = 0;\n  typename std::vector<Node*>::iterator wit;\n  for(wit = m_words.begin(); wit != m_words.end(); ++wit)\n  {\n    if((*wit)->weight < minWeight)\n    {\n      ++c;\n      (*wit)->weight = 0;\n    }\n  }\n  return c;\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::save(const std::string &filename) const\n{\n  cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE);\n  if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n  \n  save(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::load(const std::string &filename)\n{\n  cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ);\n  if(!fs.isOpened()) throw std::string(\"Could not open file \") + filename;\n  \n  this->load(fs);\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::save(cv::FileStorage &f,\n  const std::string &name) const\n{\n  // Format YAML:\n  // vocabulary \n  // {\n  //   k:\n  //   L:\n  //   scoringType:\n  //   weightingType:\n  //   nodes \n  //   [\n  //     {\n  //       nodeId:\n  //       parentId:\n  //       weight:\n  //       descriptor: \n  //     }\n  //   ]\n  //   words\n  //   [\n  //     {\n  //       wordId:\n  //       nodeId:\n  //     }\n  //   ]\n  // }\n  //\n  // The root node (index 0) is not included in the node vector\n  //\n  \n  f << name << \"{\";\n  \n  f << \"k\" << m_k;\n  f << \"L\" << m_L;\n  f << \"scoringType\" << m_scoring;\n  f << \"weightingType\" << m_weighting;\n  \n  // tree\n  f << \"nodes\" << \"[\";\n  std::vector<NodeId> parents, children;\n  std::vector<NodeId>::const_iterator pit;\n\n  parents.push_back(0); // root\n\n  while(!parents.empty())\n  {\n    NodeId pid = parents.back();\n    parents.pop_back();\n\n    const Node& parent = m_nodes[pid];\n    children = parent.children;\n\n    for(pit = children.begin(); pit != children.end(); pit++)\n    {\n      const Node& child = m_nodes[*pit];\n\n      // save node data\n      f << \"{:\";\n      f << \"nodeId\" << (int)child.id;\n      f << \"parentId\" << (int)pid;\n      f << \"weight\" << (double)child.weight;\n      f << \"descriptor\" << F::toString(child.descriptor);\n      f << \"}\";\n      \n      // add to parent list\n      if(!child.isLeaf())\n      {\n        parents.push_back(*pit);\n      }\n    }\n  }\n  \n  f << \"]\"; // nodes\n\n  // words\n  f << \"words\" << \"[\";\n  \n  typename std::vector<Node*>::const_iterator wit;\n  for(wit = m_words.begin(); wit != m_words.end(); wit++)\n  {\n    WordId id = wit - m_words.begin();\n    f << \"{:\";\n    f << \"wordId\" << (int)id;\n    f << \"nodeId\" << (int)(*wit)->id;\n    f << \"}\";\n  }\n  \n  f << \"]\"; // words\n\n  f << \"}\";\n\n}\n\n// --------------------------------------------------------------------------\n\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::load(const cv::FileStorage &fs,\n  const std::string &name)\n{\n  m_words.clear();\n  m_nodes.clear();\n  \n  cv::FileNode fvoc = fs[name];\n  \n  m_k = (int)fvoc[\"k\"];\n  m_L = (int)fvoc[\"L\"];\n  m_scoring = (ScoringType)((int)fvoc[\"scoringType\"]);\n  m_weighting = (WeightingType)((int)fvoc[\"weightingType\"]);\n  \n  createScoringObject();\n\n  // nodes\n  cv::FileNode fn = fvoc[\"nodes\"];\n\n  m_nodes.resize(fn.size() + 1); // +1 to include root\n  m_nodes[0].id = 0;\n\n  for(unsigned int i = 0; i < fn.size(); ++i)\n  {\n    NodeId nid = (int)fn[i][\"nodeId\"];\n    NodeId pid = (int)fn[i][\"parentId\"];\n    WordValue weight = (WordValue)fn[i][\"weight\"];\n    std::string d = (std::string)fn[i][\"descriptor\"];\n    \n    m_nodes[nid].id = nid;\n    m_nodes[nid].parent = pid;\n    m_nodes[nid].weight = weight;\n    m_nodes[pid].children.push_back(nid);\n    \n    F::fromString(m_nodes[nid].descriptor, d);\n  }\n  \n  // words\n  fn = fvoc[\"words\"];\n  \n  m_words.resize(fn.size());\n\n  for(unsigned int i = 0; i < fn.size(); ++i)\n  {\n    NodeId wid = (int)fn[i][\"wordId\"];\n    NodeId nid = (int)fn[i][\"nodeId\"];\n    \n    m_nodes[nid].word_id = wid;\n    m_words[wid] = &m_nodes[nid];\n  }\n}\n    \n// Added by VINS [[[\ntemplate<class TDescriptor, class F>\nvoid TemplatedVocabulary<TDescriptor,F>::loadBin(const std::string &filename) {\n    \n  m_words.clear();\n  m_nodes.clear();\n  //printf(\"loop load bin\\n\");\n  std::ifstream ifStream(filename);\n  VINSLoop::Vocabulary voc;\n  voc.deserialize(ifStream);\n  ifStream.close();\n  \n  m_k = voc.k;\n  m_L = voc.L;\n  m_scoring = (ScoringType)voc.scoringType;\n  m_weighting = (WeightingType)voc.weightingType;\n  \n  createScoringObject();\n\n  m_nodes.resize(voc.nNodes + 1); // +1 to include root\n  m_nodes[0].id = 0;\n\n  for(unsigned int i = 0; i < voc.nNodes; ++i)\n  {\n    NodeId nid = voc.nodes[i].nodeId;\n    NodeId pid = voc.nodes[i].parentId;\n    WordValue weight = voc.nodes[i].weight;\n      \n    m_nodes[nid].id = nid;\n    m_nodes[nid].parent = pid;\n    m_nodes[nid].weight = weight;\n    m_nodes[pid].children.push_back(nid);\n      \n    // Sorry to break template here\n\n#if (__SIZEOF_PTRDIFF_T__ == 8)\n    m_nodes[nid].descriptor = boost::dynamic_bitset<>(voc.nodes[i].descriptor,voc.nodes[i].descriptor + 4); //solomon modified for debug 32 bit system compatibility issue(boost::dynamic_bitset size()!= rhs.size() issue)\n\n#elif (__SIZEOF_PTRDIFF_T__ == 4)\n    m_nodes[nid].descriptor = boost::dynamic_bitset<>(voc.nodes[i].descriptor,voc.nodes[i].descriptor + 8); //solomon modified for debug 32 bit system compatibility issue(boost::dynamic_bitset size()!= rhs.size() issue)\n#endif\n\n\tif (i < 2) {\n      std::string test;\n      boost::to_string(m_nodes[nid].descriptor, test);\n      //cout << \"descriptor[\" << i << \"] = \" << test << endl;\n    }\n  }\n  m_words.resize(voc.nWords);\n\n  for(unsigned int i = 0; i < voc.nWords; ++i)\n  {\n    NodeId wid = (int)voc.words[i].wordId;\n    NodeId nid = (int)voc.words[i].nodeId;\n    \n    m_nodes[nid].word_id = wid;\n    m_words[wid] = &m_nodes[nid];\n  }\n}\n    \n// Added by VINS ]]]\n\n// --------------------------------------------------------------------------\n\n/**\n * Writes printable information of the vocabulary\n * @param os stream to write to\n * @param voc\n */\ntemplate<class TDescriptor, class F>\nstd::ostream& operator<<(std::ostream &os, \n  const TemplatedVocabulary<TDescriptor,F> &voc)\n{\n  os << \"Vocabulary: k = \" << voc.getBranchingFactor() \n    << \", L = \" << voc.getDepthLevels()\n    << \", Weighting = \";\n\n  switch(voc.getWeightingType())\n  {\n    case TF_IDF: os << \"tf-idf\"; break;\n    case TF: os << \"tf\"; break;\n    case IDF: os << \"idf\"; break;\n    case BINARY: os << \"binary\"; break;\n  }\n\n  os << \", Scoring = \";\n  switch(voc.getScoringType())\n  {\n    case L1_NORM: os << \"L1-norm\"; break;\n    case L2_NORM: os << \"L2-norm\"; break;\n    case CHI_SQUARE: os << \"Chi square distance\"; break;\n    case KL: os << \"KL-divergence\"; break;\n    case BHATTACHARYYA: os << \"Bhattacharyya coefficient\"; break;\n    case DOT_PRODUCT: os << \"Dot product\"; break;\n  }\n  \n  os << \", Number of words = \" << voc.size();\n\n  return os;\n}\n\n} // namespace DBoW2\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DUtils/DException.h",
    "content": "/*\t\n * File: DException.h\n * Project: DUtils library\n * Author: Dorian Galvez-Lopez\n * Date: October 6, 2009\n * Description: general exception of the library\n * License: see the LICENSE.txt file\n *\n */\n\n#pragma once\n\n#ifndef __D_EXCEPTION__\n#define __D_EXCEPTION__\n\n#include <stdexcept>\n#include <string>\nusing namespace std;\n\nnamespace DUtils {\n\n/// General exception\nclass DException :\n\tpublic exception\n{\npublic:\n\t/**\n\t * Creates an exception with a general error message\n\t */\n\tDException(void) throw(): m_message(\"DUtils exception\"){}\n\n\t/**\n\t * Creates an exception with a custom error message\n\t * @param msg: message\n\t */\n\tDException(const char *msg) throw(): m_message(msg){}\n\t\n\t/**\n\t * Creates an exception with a custom error message\n\t * @param msg: message\n\t */\n\tDException(const string &msg) throw(): m_message(msg){}\n\n  /**\n\t * Destructor\n\t */\n\tvirtual ~DException(void) throw(){}\n\n\t/**\n\t * Returns the exception message\n\t */\n\tvirtual const char* what() const throw()\n\t{\n\t\treturn m_message.c_str();\n\t}\n\nprotected:\n  /// Error message\n\tstring m_message;\n};\n\n}\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DUtils/DUtils.h",
    "content": "/*\r\n * File: DUtils.h\r\n * Project: DUtils library\r\n * Author: Dorian Galvez-Lopez\r\n * Date: October 6, 2009\r\n * Description: include file for including all the library functionalities\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n/*! \\mainpage DUtils Library\r\n *\r\n * DUtils library for C++:\r\n * Collection of classes with general utilities for C++ applications.\r\n *\r\n * Written by Dorian Galvez-Lopez,\r\n * University of Zaragoza\r\n * \r\n * Check my website to obtain updates: http://webdiis.unizar.es/~dorian\r\n *\r\n * \\section license License\r\n * This program is free software: you can redistribute it and/or modify\r\n * it under the terms of the GNU Lesser General Public License (LGPL) as \r\n * published by the Free Software Foundation, either version 3 of the License, \r\n * or any later version.\r\n *\r\n */\r\n\r\n\r\n#pragma once\r\n\r\n#ifndef __D_UTILS__\r\n#define __D_UTILS__\r\n\r\n/// Several utilities for C++ programs\r\nnamespace DUtils\r\n{\r\n}\r\n\r\n// Exception\r\n#include \"DException.h\"\r\n\r\n#include \"Timestamp.h\"\r\n// Random numbers\r\n#include \"Random.h\"\r\n\r\n\r\n#endif\r\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DUtils/Random.cpp",
    "content": "/*\t\r\n * File: Random.cpp\r\n * Project: DUtils library\r\n * Author: Dorian Galvez-Lopez\r\n * Date: April 2010\r\n * Description: manages pseudo-random numbers\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n#include \"Random.h\"\r\n#include \"Timestamp.h\"\r\n#include <cstdlib>\r\nusing namespace std;\r\n\r\nbool DUtils::Random::m_already_seeded = false;\r\n\r\nvoid DUtils::Random::SeedRand(){\r\n\tTimestamp time;\r\n\ttime.setToCurrentTime();\r\n\tsrand((unsigned)time.getFloatTime()); \r\n}\r\n\r\nvoid DUtils::Random::SeedRandOnce()\r\n{\r\n  if(!m_already_seeded)\r\n  {\r\n    DUtils::Random::SeedRand();\r\n    m_already_seeded = true;\r\n  }\r\n}\r\n\r\nvoid DUtils::Random::SeedRand(int seed)\r\n{\r\n\tsrand(seed); \r\n}\r\n\r\nvoid DUtils::Random::SeedRandOnce(int seed)\r\n{\r\n  if(!m_already_seeded)\r\n  {\r\n    DUtils::Random::SeedRand(seed);\r\n    m_already_seeded = true;\r\n  }\r\n}\r\n\r\nint DUtils::Random::RandomInt(int min, int max){\r\n\tint d = max - min + 1;\r\n\treturn int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n// ---------------------------------------------------------------------------\r\n\r\nDUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max)\r\n{\r\n  if(min <= max)\r\n  {\r\n    m_min = min;\r\n    m_max = max;\r\n  }\r\n  else\r\n  {\r\n    m_min = max;\r\n    m_max = min;\r\n  }\r\n\r\n  createValues();\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nDUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer\r\n  (const DUtils::Random::UnrepeatedRandomizer& rnd)\r\n{\r\n  *this = rnd;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nint DUtils::Random::UnrepeatedRandomizer::get()\r\n{\r\n  if(empty()) createValues();\r\n  \r\n  DUtils::Random::SeedRandOnce();\r\n  \r\n  int k = DUtils::Random::RandomInt(0, m_values.size()-1);\r\n  int ret = m_values[k];\r\n  m_values[k] = m_values.back();\r\n  m_values.pop_back();\r\n  \r\n  return ret;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nvoid DUtils::Random::UnrepeatedRandomizer::createValues()\r\n{\r\n  int n = m_max - m_min + 1;\r\n  \r\n  m_values.resize(n);\r\n  for(int i = 0; i < n; ++i) m_values[i] = m_min + i;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nvoid DUtils::Random::UnrepeatedRandomizer::reset()\r\n{\r\n  if((int)m_values.size() != m_max - m_min + 1) createValues();\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\nDUtils::Random::UnrepeatedRandomizer& \r\nDUtils::Random::UnrepeatedRandomizer::operator=\r\n  (const DUtils::Random::UnrepeatedRandomizer& rnd)\r\n{\r\n  if(this != &rnd)\r\n  {\r\n    this->m_min = rnd.m_min;\r\n    this->m_max = rnd.m_max;\r\n    this->m_values = rnd.m_values;\r\n  }\r\n  return *this;\r\n}\r\n\r\n// ---------------------------------------------------------------------------\r\n\r\n\r\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DUtils/Random.h",
    "content": "/*\t\n * File: Random.h\n * Project: DUtils library\n * Author: Dorian Galvez-Lopez\n * Date: April 2010, November 2011\n * Description: manages pseudo-random numbers\n * License: see the LICENSE.txt file\n *\n */\n\n#pragma once\n#ifndef __D_RANDOM__\n#define __D_RANDOM__\n\n#include <cstdlib>\n#include <vector>\n\nnamespace DUtils {\n\n/// Functions to generate pseudo-random numbers\nclass Random\n{\npublic:\n  class UnrepeatedRandomizer;\n  \npublic:\n\t/**\n\t * Sets the random number seed to the current time\n\t */\n\tstatic void SeedRand();\n\t\n\t/**\n\t * Sets the random number seed to the current time only the first\n\t * time this function is called\n\t */\n\tstatic void SeedRandOnce();\n\n\t/** \n\t * Sets the given random number seed\n\t * @param seed\n\t */\n\tstatic void SeedRand(int seed);\n\n\t/** \n\t * Sets the given random number seed only the first time this function \n\t * is called\n\t * @param seed\n\t */\n\tstatic void SeedRandOnce(int seed);\n\n\t/**\n\t * Returns a random number in the range [0..1]\n\t * @return random T number in [0..1]\n\t */\n\ttemplate <class T>\n\tstatic T RandomValue(){\n\t\treturn (T)rand()/(T)RAND_MAX;\n\t}\n\n\t/**\n\t * Returns a random number in the range [min..max]\n\t * @param min\n\t * @param max\n\t * @return random T number in [min..max]\n\t */\n\ttemplate <class T>\n\tstatic T RandomValue(T min, T max){\n\t\treturn Random::RandomValue<T>() * (max - min) + min;\n\t}\n\n\t/**\n\t * Returns a random int in the range [min..max]\n\t * @param min\n\t * @param max\n\t * @return random int in [min..max]\n\t */\n\tstatic int RandomInt(int min, int max);\n\t\n\t/** \n\t * Returns a random number from a gaussian distribution\n\t * @param mean\n\t * @param sigma standard deviation\n\t */\n\ttemplate <class T>\n\tstatic T RandomGaussianValue(T mean, T sigma)\n\t{\n    // Box-Muller transformation\n    T x1, x2, w, y1;\n\n    do {\n      x1 = (T)2. * RandomValue<T>() - (T)1.;\n      x2 = (T)2. * RandomValue<T>() - (T)1.;\n      w = x1 * x1 + x2 * x2;\n    } while ( w >= (T)1. || w == (T)0. );\n\n    w = sqrt( ((T)-2.0 * log( w ) ) / w );\n    y1 = x1 * w;\n\n    return( mean + y1 * sigma );\n\t}\n\nprivate:\n\n  /// If SeedRandOnce() or SeedRandOnce(int) have already been called\n  static bool m_already_seeded;\n  \n};\n\n// ---------------------------------------------------------------------------\n\n/// Provides pseudo-random numbers with no repetitions\nclass Random::UnrepeatedRandomizer\n{\npublic:\n\n  /** \n   * Creates a randomizer that returns numbers in the range [min, max]\n   * @param min\n   * @param max\n   */\n  UnrepeatedRandomizer(int min, int max);\n  ~UnrepeatedRandomizer(){}\n  \n  /**\n   * Copies a randomizer\n   * @param rnd\n   */\n  UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd);\n  \n  /**\n   * Copies a randomizer\n   * @param rnd\n   */\n  UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd);\n  \n  /** \n   * Returns a random number not given before. If all the possible values\n   * were already given, the process starts again\n   * @return unrepeated random number\n   */\n  int get();\n  \n  /**\n   * Returns whether all the possible values between min and max were\n   * already given. If get() is called when empty() is true, the behaviour\n   * is the same than after creating the randomizer\n   * @return true iff all the values were returned\n   */\n  inline bool empty() const { return m_values.empty(); }\n  \n  /**\n   * Returns the number of values still to be returned\n   * @return amount of values to return\n   */\n  inline unsigned int left() const { return m_values.size(); }\n  \n  /**\n   * Resets the randomizer as it were just created\n   */\n  void reset();\n  \nprotected:\n\n  /** \n   * Creates the vector with available values\n   */\n  void createValues();\n\nprotected:\n\n  /// Min of range of values\n  int m_min;\n  /// Max of range of values\n  int m_max;\n\n  /// Available values\n  std::vector<int> m_values;\n\n};\n\n}\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DUtils/Timestamp.cpp",
    "content": "/*\r\n * File: Timestamp.cpp\r\n * Author: Dorian Galvez-Lopez\r\n * Date: March 2009\r\n * Description: timestamping functions\r\n * \r\n * Note: in windows, this class has a 1ms resolution\r\n *\r\n * License: see the LICENSE.txt file\r\n *\r\n */\r\n\r\n#include <cstdio>\r\n#include <cstdlib>\r\n#include <ctime>\r\n#include <cmath>\r\n#include <sstream>\r\n#include <iomanip>\r\n\r\n#ifdef _WIN32\r\n#ifndef WIN32\r\n#define WIN32\r\n#endif\r\n#endif\r\n\r\n#ifdef WIN32\r\n#include <sys/timeb.h>\r\n#define sprintf sprintf_s\r\n#else\r\n#include <sys/time.h>\r\n#endif\r\n\r\n#include \"Timestamp.h\"\r\n\r\nusing namespace std;\r\n\r\nusing namespace DUtils;\r\n\r\nTimestamp::Timestamp(Timestamp::tOptions option)\r\n{\r\n  if(option & CURRENT_TIME)\r\n    setToCurrentTime();\r\n  else if(option & ZERO)\r\n    setTime(0.);\r\n}\r\n\r\nTimestamp::~Timestamp(void)\r\n{\r\n}\r\n\r\nbool Timestamp::empty() const\r\n{\r\n  return m_secs == 0 && m_usecs == 0;\r\n}\r\n\r\nvoid Timestamp::setToCurrentTime(){\r\n\t\r\n#ifdef WIN32\r\n\tstruct __timeb32 timebuffer;\r\n\t_ftime32_s( &timebuffer ); // C4996\r\n\t// Note: _ftime is deprecated; consider using _ftime_s instead\r\n\tm_secs = timebuffer.time;\r\n\tm_usecs = timebuffer.millitm * 1000;\r\n#else\r\n\tstruct timeval now;\r\n\tgettimeofday(&now, NULL);\r\n\tm_secs = now.tv_sec;\r\n\tm_usecs = now.tv_usec;\r\n#endif\r\n\r\n}\r\n\r\nvoid Timestamp::setTime(const string &stime){\r\n\tstring::size_type p = stime.find('.');\r\n\tif(p == string::npos){\r\n\t\tm_secs = atol(stime.c_str());\r\n\t\tm_usecs = 0;\r\n\t}else{\r\n\t\tm_secs = atol(stime.substr(0, p).c_str());\r\n\t\t\r\n\t\tstring s_usecs = stime.substr(p+1, 6);\r\n\t\tm_usecs = atol(stime.substr(p+1).c_str());\r\n\t\tm_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));\r\n\t}\r\n}\r\n\r\nvoid Timestamp::setTime(double s)\r\n{\r\n  m_secs = (unsigned long)s;\r\n  m_usecs = (s - (double)m_secs) * 1e6;\r\n}\r\n\r\ndouble Timestamp::getFloatTime() const {\r\n\treturn double(m_secs) + double(m_usecs)/1000000.0;\r\n}\r\n\r\nstring Timestamp::getStringTime() const {\r\n\tchar buf[32];\r\n\tsprintf(buf, \"%.6lf\", this->getFloatTime());\r\n\treturn string(buf);\r\n}\r\n\r\ndouble Timestamp::operator- (const Timestamp &t) const {\r\n\treturn this->getFloatTime() - t.getFloatTime();\r\n}\r\n\r\nTimestamp& Timestamp::operator+= (double s)\r\n{\r\n  *this = *this + s;\r\n  return *this;\r\n}\r\n\r\nTimestamp& Timestamp::operator-= (double s)\r\n{\r\n  *this = *this - s;\r\n  return *this;\r\n}\r\n\r\nTimestamp Timestamp::operator+ (double s) const\r\n{\r\n\tunsigned long secs = (long)floor(s);\r\n\tunsigned long usecs = (long)((s - (double)secs) * 1e6);\r\n\r\n  return this->plus(secs, usecs);\r\n}\r\n\r\nTimestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const\r\n{\r\n  Timestamp t;\r\n\r\n\tconst unsigned long max = 1000000ul;\r\n\r\n\tif(m_usecs + usecs >= max)\r\n\t\tt.setTime(m_secs + secs + 1, m_usecs + usecs - max);\r\n\telse\r\n\t\tt.setTime(m_secs + secs, m_usecs + usecs);\r\n\t\r\n\treturn t;\r\n}\r\n\r\nTimestamp Timestamp::operator- (double s) const\r\n{\r\n\tunsigned long secs = (long)floor(s);\r\n\tunsigned long usecs = (long)((s - (double)secs) * 1e6);\r\n\r\n\treturn this->minus(secs, usecs);\r\n}\r\n\r\nTimestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const\r\n{\r\n  Timestamp t;\r\n\r\n\tconst unsigned long max = 1000000ul;\r\n\r\n\tif(m_usecs < usecs)\r\n\t\tt.setTime(m_secs - secs - 1, max - (usecs - m_usecs));\r\n\telse\r\n\t\tt.setTime(m_secs - secs, m_usecs - usecs);\r\n\t\r\n\treturn t;\r\n}\r\n\r\nbool Timestamp::operator> (const Timestamp &t) const\r\n{\r\n\tif(m_secs > t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs > t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator>= (const Timestamp &t) const\r\n{\r\n\tif(m_secs > t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator< (const Timestamp &t) const\r\n{\r\n\tif(m_secs < t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs < t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator<= (const Timestamp &t) const\r\n{\r\n\tif(m_secs < t.m_secs) return true;\r\n\telse if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;\r\n\telse return false;\r\n}\r\n\r\nbool Timestamp::operator== (const Timestamp &t) const\r\n{\r\n\treturn(m_secs == t.m_secs && m_usecs == t.m_usecs);\r\n}\r\n\r\n\r\nstring Timestamp::Format(bool machine_friendly) const \r\n{\r\n  struct tm tm_time;\r\n\r\n  time_t t = (time_t)getFloatTime();\r\n\r\n#ifdef WIN32\r\n  localtime_s(&tm_time, &t);\r\n#else\r\n  localtime_r(&t, &tm_time);\r\n#endif\r\n  \r\n  char buffer[128];\r\n  \r\n  if(machine_friendly)\r\n  {\r\n    strftime(buffer, 128, \"%Y%m%d_%H%M%S\", &tm_time);\r\n  }\r\n  else\r\n  {\r\n    strftime(buffer, 128, \"%c\", &tm_time); // Thu Aug 23 14:55:02 2001\r\n  }\r\n  \r\n  return string(buffer);\r\n}\r\n\r\nstring Timestamp::Format(double s) {\r\n\tint days = int(s / (24. * 3600.0));\r\n\ts -= days * (24. * 3600.0);\r\n\tint hours = int(s / 3600.0);\r\n\ts -= hours * 3600;\r\n\tint minutes = int(s / 60.0);\r\n\ts -= minutes * 60;\r\n\tint seconds = int(s);\r\n\tint ms = int((s - seconds)*1e6);\r\n\r\n\tstringstream ss;\r\n\tss.fill('0');\r\n\tbool b;\r\n\tif((b = (days > 0))) ss << days << \"d \";\r\n\tif((b = (b || hours > 0))) ss << setw(2) << hours << \":\";\r\n\tif((b = (b || minutes > 0))) ss << setw(2) << minutes << \":\";\r\n\tif(b) ss << setw(2);\r\n\tss << seconds;\r\n\tif(!b) ss << \".\" << setw(6) << ms;\r\n\r\n\treturn ss.str();\r\n}\r\n\r\n\r\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DUtils/Timestamp.h",
    "content": "/*\n * File: Timestamp.h\n * Author: Dorian Galvez-Lopez\n * Date: March 2009\n * Description: timestamping functions\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_TIMESTAMP__\n#define __D_TIMESTAMP__\n\n#include <iostream>\nusing namespace std;\n\nnamespace DUtils {\n\n/// Timestamp\nclass Timestamp\n{\npublic:\n\n  /// Options to initiate a timestamp\n  enum tOptions\n  {\n    NONE = 0,\n    CURRENT_TIME = 0x1,\n    ZERO = 0x2\n  };\n  \npublic:\n  \n  /**\n   * Creates a timestamp \n   * @param option option to set the initial time stamp\n   */\n\tTimestamp(Timestamp::tOptions option = NONE);\n\t\n\t/**\n\t * Destructor\n\t */\n\tvirtual ~Timestamp(void);\n\n  /**\n   * Says if the timestamp is \"empty\": seconds and usecs are both 0, as \n   * when initiated with the ZERO flag\n   * @return true iif secs == usecs == 0\n   */\n  bool empty() const;\n\n\t/**\n\t * Sets this instance to the current time\n\t */\n\tvoid setToCurrentTime();\n\n\t/**\n\t * Sets the timestamp from seconds and microseconds\n\t * @param secs: seconds\n\t * @param usecs: microseconds\n\t */\n\tinline void setTime(unsigned long secs, unsigned long usecs){\n\t\tm_secs = secs;\n\t\tm_usecs = usecs;\n\t}\n\t\n\t/**\n\t * Returns the timestamp in seconds and microseconds\n\t * @param secs seconds\n\t * @param usecs microseconds\n\t */\n\tinline void getTime(unsigned long &secs, unsigned long &usecs) const\n\t{\n\t  secs = m_secs;\n\t  usecs = m_usecs;\n\t}\n\n\t/**\n\t * Sets the timestamp from a string with the time in seconds\n\t * @param stime: string such as \"1235603336.036609\"\n\t */\n\tvoid setTime(const string &stime);\n\t\n\t/**\n\t * Sets the timestamp from a number of seconds from the epoch\n\t * @param s seconds from the epoch\n\t */\n\tvoid setTime(double s);\n\t\n\t/**\n\t * Returns this timestamp as the number of seconds in (long) float format\n\t */\n\tdouble getFloatTime() const;\n\n\t/**\n\t * Returns this timestamp as the number of seconds in fixed length string format\n\t */\n\tstring getStringTime() const;\n\n\t/**\n\t * Returns the difference in seconds between this timestamp (greater) and t (smaller)\n\t * If the order is swapped, a negative number is returned\n\t * @param t: timestamp to subtract from this timestamp\n\t * @return difference in seconds\n\t */\n\tdouble operator- (const Timestamp &t) const;\n\n\t/** \n\t * Returns a copy of this timestamp + s seconds + us microseconds\n\t * @param s seconds\n\t * @param us microseconds\n\t */\n\tTimestamp plus(unsigned long s, unsigned long us) const;\n\n  /**\n   * Returns a copy of this timestamp - s seconds - us microseconds\n   * @param s seconds\n   * @param us microseconds\n   */\n  Timestamp minus(unsigned long s, unsigned long us) const;\n\n  /**\n   * Adds s seconds to this timestamp and returns a reference to itself\n   * @param s seconds\n   * @return reference to this timestamp\n   */\n  Timestamp& operator+= (double s);\n  \n  /**\n   * Substracts s seconds to this timestamp and returns a reference to itself\n   * @param s seconds\n   * @return reference to this timestamp\n   */\n  Timestamp& operator-= (double s);\n\n\t/**\n\t * Returns a copy of this timestamp + s seconds\n\t * @param s: seconds\n\t */\n\tTimestamp operator+ (double s) const;\n\n\t/**\n\t * Returns a copy of this timestamp - s seconds\n\t * @param s: seconds\n\t */\n\tTimestamp operator- (double s) const;\n\n\t/**\n\t * Returns whether this timestamp is at the future of t\n\t * @param t\n\t */\n\tbool operator> (const Timestamp &t) const;\n\n\t/**\n\t * Returns whether this timestamp is at the future of (or is the same as) t\n\t * @param t\n\t */\n\tbool operator>= (const Timestamp &t) const;\n\n\t/** \n\t * Returns whether this timestamp and t represent the same instant\n\t * @param t\n\t */\n\tbool operator== (const Timestamp &t) const;\n\n\t/**\n\t * Returns whether this timestamp is at the past of t\n\t * @param t\n\t */\n\tbool operator< (const Timestamp &t) const;\n\n\t/**\n\t * Returns whether this timestamp is at the past of (or is the same as) t\n\t * @param t\n\t */\n\tbool operator<= (const Timestamp &t) const;\n\n  /**\n   * Returns the timestamp in a human-readable string\n   * @param machine_friendly if true, the returned string is formatted\n   *   to yyyymmdd_hhmmss, without weekday or spaces\n   * @note This has not been tested under Windows\n   * @note The timestamp is truncated to seconds\n   */\n  string Format(bool machine_friendly = false) const;\n\n\t/**\n\t * Returns a string version of the elapsed time in seconds, with the format\n\t * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us\n\t * @param s: elapsed seconds (given by getFloatTime) to format\n\t */\n\tstatic string Format(double s);\n\t\n\nprotected:\n  /// Seconds\n\tunsigned long m_secs;\t// seconds\n\t/// Microseconds\n\tunsigned long m_usecs;\t// microseconds\n};\n\n}\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DVision/BRIEF.cpp",
    "content": "/**\n * File: BRIEF.cpp\n * Author: Dorian Galvez\n * Date: September 2010\n * Description: implementation of BRIEF (Binary Robust Independent \n *   Elementary Features) descriptor by \n *   Michael Calonder, Vincent Lepetitand Pascal Fua\n *   + close binary tests (by Dorian Galvez-Lopez)\n * License: see the LICENSE.txt file\n *\n */\n\n#include \"BRIEF.h\"\n#include \"../DUtils/DUtils.h\"\n#include <boost/dynamic_bitset.hpp>\n#include <vector>\n\nusing namespace std;\nusing namespace DVision;\n\n// ----------------------------------------------------------------------------\n\nBRIEF::BRIEF(int nbits, int patch_size, Type type):\n  m_bit_length(nbits), m_patch_size(patch_size), m_type(type)\n{\n  assert(patch_size > 1);\n  assert(nbits > 0);\n  generateTestPoints();\n}\n\n// ----------------------------------------------------------------------------\n\nBRIEF::~BRIEF()\n{\n}\n\n// ---------------------------------------------------------------------------\n\nvoid BRIEF::compute(const cv::Mat &image, \n    const std::vector<cv::KeyPoint> &points,\n    vector<bitset> &descriptors,\n    bool treat_image) const\n{\n  const float sigma = 2.f;\n  const cv::Size ksize(9, 9);\n  \n  cv::Mat im;\n  if(treat_image)\n  {\n    cv::Mat aux;\n    if(image.depth() == 3)\n    {\n      cv::cvtColor(image, aux, CV_RGB2GRAY);\n    }\n    else\n    {\n      aux = image;\n    }\n\n    cv::GaussianBlur(aux, im, ksize, sigma, sigma);\n    \n  }\n  else\n  {\n    im = image;\n  }\n  \n  assert(im.type() == CV_8UC1);\n  assert(im.isContinuous());\n  \n  // use im now\n  const int W = im.cols;\n  const int H = im.rows;\n  \n  descriptors.resize(points.size());\n  std::vector<bitset>::iterator dit;\n\n  std::vector<cv::KeyPoint>::const_iterator kit;\n  \n  int x1, y1, x2, y2;\n  \n  dit = descriptors.begin();\n  for(kit = points.begin(); kit != points.end(); ++kit, ++dit)\n  {\n    dit->resize(m_bit_length);\n    dit->reset();\n\n    for(unsigned int i = 0; i < m_x1.size(); ++i)\n    {\n      x1 = (int)(kit->pt.x + m_x1[i]);\n      y1 = (int)(kit->pt.y + m_y1[i]);\n      x2 = (int)(kit->pt.x + m_x2[i]);\n      y2 = (int)(kit->pt.y + m_y2[i]);\n      \n      if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H \n        && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H)\n      {\n        if( im.ptr<unsigned char>(y1)[x1] < im.ptr<unsigned char>(y2)[x2] )\n        {\n          dit->set(i);\n        }        \n      } // if (x,y)_1 and (x,y)_2 are in the image\n            \n    } // for each (x,y)\n  } // for each keypoint\n}\n\n// ---------------------------------------------------------------------------\n\nvoid BRIEF::generateTestPoints()\n{  \n  m_x1.resize(m_bit_length);\n  m_y1.resize(m_bit_length);\n  m_x2.resize(m_bit_length);\n  m_y2.resize(m_bit_length);\n\n  const float g_mean = 0.f;\n  const float g_sigma = 0.2f * (float)m_patch_size;\n  const float c_sigma = 0.08f * (float)m_patch_size;\n  \n  float sigma2;\n  if(m_type == RANDOM)\n    sigma2 = g_sigma;\n  else\n    sigma2 = c_sigma;\n  \n  const int max_v = m_patch_size / 2;\n  \n  DUtils::Random::SeedRandOnce();\n  \n  for(int i = 0; i < m_bit_length; ++i)\n  {\n    int x1, y1, x2, y2;\n    \n    do\n    {\n      x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);\n    } while( x1 > max_v || x1 < -max_v);\n    \n    do\n    {\n      y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);\n    } while( y1 > max_v || y1 < -max_v);\n    \n    float meanx, meany;\n    if(m_type == RANDOM)\n      meanx = meany = g_mean;\n    else\n    {\n      meanx = x1;\n      meany = y1;\n    }\n    \n    do\n    {\n      x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2);\n    } while( x2 > max_v || x2 < -max_v);\n    \n    do\n    {\n      y2 = DUtils::Random::RandomGaussianValue(meany, sigma2);\n    } while( y2 > max_v || y2 < -max_v);\n    \n    m_x1[i] = x1;\n    m_y1[i] = y1;\n    m_x2[i] = x2;\n    m_y2[i] = y2;\n  }\n\n}\n\n// ----------------------------------------------------------------------------\n\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DVision/BRIEF.h",
    "content": "/**\n * File: BRIEF.h\n * Author: Dorian Galvez-Lopez\n * Date: March 2011\n * Description: implementation of BRIEF (Binary Robust Independent \n *   Elementary Features) descriptor by \n *   Michael Calonder, Vincent Lepetit and Pascal Fua\n *   + close binary tests (by Dorian Galvez-Lopez)\n *\n * If you use this code with the RANDOM_CLOSE descriptor version, please cite:\n  @INPROCEEDINGS{GalvezIROS11,\n    author={Galvez-Lopez, Dorian and Tardos, Juan D.},\n    booktitle={Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on},\n    title={Real-time loop detection with bags of binary words},\n    year={2011},\n    month={sept.},\n    volume={},\n    number={},\n    pages={51 -58},\n    keywords={},\n    doi={10.1109/IROS.2011.6094885},\n    ISSN={2153-0858}\n  }\n *\n * License: see the LICENSE.txt file\n *\n */\n\n#ifndef __D_BRIEF__\n#define __D_BRIEF__\n\n#include <opencv2/opencv.hpp>\n#include <vector>\n#include <boost/dynamic_bitset.hpp>\n\nnamespace DVision {\n\n/// BRIEF descriptor\nclass BRIEF\n{\npublic:\n\n  /// Bitset type\n  typedef boost::dynamic_bitset<> bitset;\n\n  /// Type of pairs\n  enum Type\n  {\n    RANDOM, // random pairs (Calonder's original version)\n    RANDOM_CLOSE, // random but close pairs (used in GalvezIROS11)\n  };\n  \npublic:\n\n  /**\n   * Creates the BRIEF a priori data for descriptors of nbits length\n   * @param nbits descriptor length in bits\n   * @param patch_size \n   * @param type type of pairs to generate\n   */\n  BRIEF(int nbits = 256, int patch_size = 48, Type type = RANDOM_CLOSE);\n  virtual ~BRIEF();\n  \n  /**\n   * Returns the descriptor length in bits\n   * @return descriptor length in bits\n   */\n  inline int getDescriptorLengthInBits() const\n  {\n    return m_bit_length;\n  }\n  \n  /**\n   * Returns the type of classifier\n   */\n  inline Type getType() const\n  {\n    return m_type;\n  }\n  \n  /**\n   * Returns the size of the patch\n   */\n  inline int getPatchSize() const\n  {\n    return m_patch_size;\n  }\n  \n  /**\n   * Returns the BRIEF descriptors of the given keypoints in the given image\n   * @param image\n   * @param points\n   * @param descriptors \n   * @param treat_image (default: true) if true, the image is converted to \n   *   grayscale if needed and smoothed. If not, it is assumed the image has\n   *   been treated by the user\n   * @note this function is similar to BRIEF::compute\n   */\n  inline void operator() (const cv::Mat &image, \n    const std::vector<cv::KeyPoint> &points,\n    std::vector<bitset> &descriptors,\n    bool treat_image = true) const\n  {\n    compute(image, points, descriptors, treat_image);\n  }\n  \n  /**\n   * Returns the BRIEF descriptors of the given keypoints in the given image\n   * @param image\n   * @param points\n   * @param descriptors \n   * @param treat_image (default: true) if true, the image is converted to \n   *   grayscale if needed and smoothed. If not, it is assumed the image has\n   *   been treated by the user\n   * @note this function is similar to BRIEF::operator()\n   */ \n  void compute(const cv::Mat &image,\n    const std::vector<cv::KeyPoint> &points,\n    std::vector<bitset> &descriptors,\n    bool treat_image = true) const;\n  \n  /**\n   * Exports the test pattern\n   * @param x1 x1 coordinates of pairs\n   * @param y1 y1 coordinates of pairs\n   * @param x2 x2 coordinates of pairs\n   * @param y2 y2 coordinates of pairs\n   */\n  inline void exportPairs(std::vector<int> &x1, std::vector<int> &y1,\n    std::vector<int> &x2, std::vector<int> &y2) const\n  {\n    x1 = m_x1;\n    y1 = m_y1;\n    x2 = m_x2;\n    y2 = m_y2;\n  }\n  \n  /**\n   * Sets the test pattern\n   * @param x1 x1 coordinates of pairs\n   * @param y1 y1 coordinates of pairs\n   * @param x2 x2 coordinates of pairs\n   * @param y2 y2 coordinates of pairs\n   */\n  inline void importPairs(const std::vector<int> &x1, \n    const std::vector<int> &y1, const std::vector<int> &x2, \n    const std::vector<int> &y2)\n  {\n    m_x1 = x1;\n    m_y1 = y1;\n    m_x2 = x2;\n    m_y2 = y2;\n    m_bit_length = x1.size();\n  }\n  \n  /**\n   * Returns the Hamming distance between two descriptors\n   * @param a first descriptor vector\n   * @param b second descriptor vector\n   * @return hamming distance\n   */\n  inline static int distance(const bitset &a, const bitset &b)\n  {\n    return (a^b).count();\n  }\n\nprotected:\n\n  /**\n   * Generates random points in the patch coordinates, according to \n   * m_patch_size and m_bit_length\n   */\n  void generateTestPoints();\n  \nprotected:\n\n  /// Descriptor length in bits\n  int m_bit_length;\n\n  /// Patch size\n  int m_patch_size;\n  \n  /// Type of pairs\n  Type m_type;\n\n  /// Coordinates of test points relative to the center of the patch\n  std::vector<int> m_x1, m_x2;\n  std::vector<int> m_y1, m_y2;\n\n};\n\n} // namespace DVision\n\n#endif\n\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/DVision/DVision.h",
    "content": "/*\n * File: DVision.h\n * Project: DVision library\n * Author: Dorian Galvez-Lopez\n * Date: October 4, 2010\n * Description: several functions for computer vision\n * License: see the LICENSE.txt file\n *\n */\n\n/*! \\mainpage DVision Library\n *\n * DVision library for C++ and OpenCV:\n * Collection of classes with computer vision functionality\n *\n * Written by Dorian Galvez-Lopez,\n * University of Zaragoza\n * \n * Check my website to obtain updates: http://webdiis.unizar.es/~dorian\n *\n * \\section requirements Requirements\n * This library requires the DUtils and DUtilsCV libraries and the OpenCV library.\n *\n * \\section license License\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU Lesser General Public License (LGPL) as \n * published by the Free Software Foundation, either version 3 of the License, \n * or any later version.\n *\n */\n\n#ifndef __D_VISION__\n#define __D_VISION__\n\n\n/// Computer vision functions\nnamespace DVision\n{\n}\n\n#include \"BRIEF.h\"\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/VocabularyBinary.cpp",
    "content": "//\n//  VocabularyBinary.cpp\n//  VINS_ios\n//\n//  Created by Yang Liu on 3/13/17.\n//  Copyright © 2017 栗大人. All rights reserved.\n//\n\n#include \"VocabularyBinary.hpp\"\n#include <opencv2/core/core.hpp>\n#include <iostream>\nusing namespace std;\n\nVINSLoop::Vocabulary::Vocabulary()\n: nNodes(0), nodes(nullptr), nWords(0), words(nullptr) {\n}\n\nVINSLoop::Vocabulary::~Vocabulary() {\n    if (nodes != nullptr) {\n        delete [] nodes;\n        nodes = nullptr;\n    }\n    \n    if (words != nullptr) {\n        delete [] words;\n        words = nullptr;\n    }\n}\n    \nvoid VINSLoop::Vocabulary::serialize(ofstream& stream) {\n    stream.write((const char *)this, staticDataSize());\n    stream.write((const char *)nodes, sizeof(Node) * nNodes);\n    stream.write((const char *)words, sizeof(Word) * nWords);\n}\n    \nvoid VINSLoop::Vocabulary::deserialize(ifstream& stream) {\n    stream.read((char *)this, staticDataSize());\n    \n    nodes = new Node[nNodes];\n//\tstd::cout << \"+++++++++sizeof(Node):\" << sizeof(Node)  << \"num of nNodes:\" << nNodes << \"staticDataSize:\" << staticDataSize() << std::endl;\n    stream.read((char *)nodes, sizeof(Node) * nNodes);\n//\tstd::cout << \"nodes[0].descriptor:\"  << hex << nodes[0].descriptor[0]  << \"sizeof(nodes[0].descriptor)\" << sizeof(nodes[0].descriptor)<< std::endl;    \n    words = new Word[nWords];\n    stream.read((char *)words, sizeof(Word) * nWords);\n}\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/ThirdParty/VocabularyBinary.hpp",
    "content": "//\n//  VocabularyBinary.hpp\n//  VINS_ios\n//\n//  Created by Yang Liu on 3/13/17.\n//  Copyright © 2017 栗大人. All rights reserved.\n//\n\n#ifndef VocabularyBinary_hpp\n#define VocabularyBinary_hpp\n\n#include <cstdint>\n#include <fstream>\n#include <string>\n\nnamespace VINSLoop {\n    \nstruct Node {\n    int32_t nodeId;\n    int32_t parentId;\n    double weight;\n#if (__SIZEOF_PTRDIFF_T__ == 8)\n  uint64_t descriptor[4];\n#elif (__SIZEOF_PTRDIFF_T__ == 4)\n  uint32_t descriptor[8];\n#endif\n\t//uint8_t descriptor[32]; //solomon modified for debug 32 bit system compatibility issue(boost::dynamic_bitset size()!= rhs.size() issue)\n};\n\nstruct Word {\n    int32_t nodeId;\n    int32_t wordId;\n};\n\nstruct Vocabulary {\n    int32_t k;\n    int32_t L;\n    int32_t scoringType;\n    int32_t weightingType;\n    \n    int32_t nNodes;\n    int32_t nWords;\n    \n    Node* nodes;\n    Word* words;\n    \n    Vocabulary();\n    ~Vocabulary();\n    \n    void serialize(std::ofstream& stream);\n    void deserialize(std::ifstream& stream);\n    \n    inline static size_t staticDataSize() {\n        return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*);\n    }\n};\n\n}\n\n#endif /* VocabularyBinary_hpp */\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/demoDetector.h",
    "content": "/**\n * File: demoDetector.h\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: demo application of DLoopDetector\n * License: see the LICENSE.txt file\n */\n\n#ifndef __DEMO_DETECTOR__\n#define __DEMO_DETECTOR__\n\n#include <iostream>\n#include <vector>\n#include <string>\n\n// OpenCV\n#include <opencv/cv.h>\n#include <opencv/highgui.h>\n\n// DLoopDetector and DBoW2\n#include \"ThirdParty/DBoW/DBoW2.h\"\n#include \"DLoopDetector.h\"\n\n//for time\n#include <cctype>\n#include <iostream>\n#include <stdio.h>\n#include <string.h>\n#include <time.h>\n#include \"dirent.h\"\n#include <unistd.h>\n#include <string>\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n#include \"../utility/tic_toc.h\"\n\nusing namespace DLoopDetector;\nusing namespace DBoW2;\nusing namespace std;\n\n\n/// Generic class to create functors to extract features\ntemplate<class TDescriptor>\nclass FeatureExtractor\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n  /**\n   * Extracts features\n   * @param im image\n   * @param keys keypoints extracted\n   * @param descriptors descriptors extracted\n   */\n  virtual void operator()(const cv::Mat &im, const std::vector<cv::Point2f> window_pts,\n    vector<cv::KeyPoint> &keys, vector<TDescriptor> &descriptors) const = 0;\n};\n\n// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \n\n/// @param TVocabulary vocabulary class (e.g: Surf64Vocabulary)\n/// @param TDetector detector class (e.g: Surf64LoopDetector)\n/// @param TDescriptor descriptor class (e.g: vector<float> for SURF)\ntemplate<class TVocabulary, class TDetector, class TDescriptor>\n/// Class to run the demo \nclass demoDetector\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n  /**\n   * @param vocfile vocabulary file to load\n   * @param imagedir directory to read images from\n   * @param posefile pose file\n   * @param width image width\n   * @param height image height\n   */\n  demoDetector(const std::string &vocfile, int width, int height);\n    \n  ~demoDetector(){}\n\n  void initCameraModel(const std::string &calib_file);\n\n  /**\n   * Runs the demo\n   * @param name demo name\n   * @param extractor functor to extract features\n   */\n  bool run(const std::string &name,\n           const std::vector<cv::KeyPoint> &keys, \n           const std::vector<TDescriptor> &descriptors,\n           std::vector<cv::Point2f> &cur_pts,\n           std::vector<cv::Point2f> &old_pts,\n           int &old_index);\n\n  void eraseIndex(std::vector<int> &erase_index);\n  /*Data*/\n  std::string m_vocfile;\n  int m_width;\n  int m_height;\n  typename TDetector::Parameters params;\n  TVocabulary voc;\n  TDetector detector;\n\n\nprotected:\n\n  /**\n   * Reads the robot poses from a file\n   * @param filename file\n   * @param xs\n   * @param ys\n   */\n  void readPoseFile(const char *filename, std::vector<double> &xs, \n    std::vector<double> &ys) const;\n\nprotected:\n  //std::string m_imagedir;\n  //std::string m_posefile;\n};\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TVocabulary, class TDetector, class TDescriptor>\ndemoDetector<TVocabulary, TDetector, TDescriptor>::demoDetector\n  (const std::string &vocfile, int width, int height)\n  : m_vocfile(vocfile), m_width(width), m_height(height),\n    params(height, width), voc(vocfile), detector(voc, params)\n{\n    //params.use_nss = true; // use normalized similarity score instead of raw score\n    //params.alpha = 0.3; // nss threshold\n    //params.k = 1; // a loop must be consistent with 1 previous matches\n    //params.geom_check = GEOM_FLANN; // use direct index for geometrical checking\n    //params.di_levels = 2; // use two direct index levels\n    //printf(\"load vocfile %s finish\\n\", vocfile);\n    //printf(\"loop image size width: %d height: %d\\n\", params.image_cols,params.image_rows);\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TVocabulary, class TDetector, class TDescriptor>\nvoid demoDetector<TVocabulary, TDetector, TDescriptor>::initCameraModel\n  (const std::string &calib_file)\n{\n    detector.initCameraModel(calib_file);\n}\n\n// ---------------------------------------------------------------------------\ntemplate<class TVocabulary, class TDetector, class TDescriptor>\nvoid demoDetector<TVocabulary, TDetector, TDescriptor>::eraseIndex\n(std::vector<int> &erase_index)\n{\n    detector.eraseIndex(erase_index);\n}\n\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TVocabulary, class TDetector, class TDescriptor>\nbool demoDetector<TVocabulary, TDetector, TDescriptor>::run\n  (const std::string &name, const std::vector<cv::KeyPoint> &keys, \n   const std::vector<TDescriptor> &descriptors,\n   std::vector<cv::Point2f> &cur_pts,\n   std::vector<cv::Point2f> &old_pts,\n   int &old_index)\n{  \n  int count = 0;\n\n  DetectionResult result;\n\n  detector.detectLoop(keys, descriptors, result, cur_pts, old_pts); \n    \n  if(result.detection())\n  {\n      //cout << \"- loop found with image \" << result.match << \"!\"\n      //  << endl;\n      ++count;\n      old_index = result.match;\n      return true;\n  }\n  else\n  {\n      //cout << \"- No loop: \";\n      switch(result.status)\n      {\n        case CLOSE_MATCHES_ONLY:\n          //cout << \"All the images in the database are very recent\" << endl;\n          break;\n          \n        case NO_DB_RESULTS:\n          //cout << \"There are no matches against the database (few features in\"\n          //  \" the image?)\" << endl;\n          break;\n          \n        case LOW_NSS_FACTOR:\n          //cout << \"Little overlap between this image and the previous one\"\n          //  << endl;\n          break;\n            \n        case LOW_SCORES:\n          //cout << \"No match reaches the score threshold (alpha: \" <<\n          //  params.alpha << \")\" << endl;\n          break;\n          \n        case NO_GROUPS:\n          //cout << \"Not enough close matches to create groups. \"\n          //  << \"Best candidate: \" << result.match << endl;\n          break;\n          \n        case NO_TEMPORAL_CONSISTENCY:\n          //cout << \"No temporal consistency (k: \" << params.k << \"). \"\n          //  << \"Best candidate: \" << result.match << endl;\n          break;\n          \n        case NO_GEOMETRICAL_CONSISTENCY:\n          //cout << \"No geometrical consistency. Best candidate: \" \n          //  << result.match << endl;\n          break;\n          \n        default:\n          break;\n      }\n      return false;\n  }\n}\n\n// ---------------------------------------------------------------------------\n\ntemplate<class TVocabulary, class TDetector, class TDescriptor>\nvoid demoDetector<TVocabulary, TDetector, TDescriptor>::readPoseFile\n  (const char *filename, std::vector<double> &xs, std::vector<double> &ys)\n  const\n{\n  xs.clear();\n  ys.clear();\n  \n  fstream f(filename, ios::in);\n  \n  string s;\n  double ts, x, y, t;\n  while(!f.eof())\n  {\n    getline(f, s);\n    if(!f.eof() && !s.empty())\n    {\n      sscanf(s.c_str(), \"%lf, %lf, %lf, %lf\", &ts, &x, &y, &t);\n      xs.push_back(x);\n      ys.push_back(y);\n    }\n  }\n  \n  f.close();\n}\n\n// ---------------------------------------------------------------------------\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/keyframe.cpp",
    "content": "#include \"keyframe.h\"\n\nKeyFrame::KeyFrame(double _header, Eigen::Vector3d _vio_T_w_i, Eigen::Matrix3d _vio_R_w_i, \n                   Eigen::Vector3d _cur_T_w_i, Eigen::Matrix3d _cur_R_w_i, \n                   cv::Mat &_image, const char *_brief_pattern_file, Eigen::Vector3d _relocalize_t, Eigen::Matrix3d _relocalize_r)\n:header{_header}, image{_image}, BRIEF_PATTERN_FILE(_brief_pattern_file)\n{\n    T_w_i = _cur_T_w_i;\n    R_w_i = _cur_R_w_i;\n    COL = image.cols;\n    ROW = image.rows;\n    use_retrive = false;\n    is_looped = 0;\n    has_loop = 0;\n    update_loop_info = 0;\n    vio_T_w_i = _vio_T_w_i;\n    vio_R_w_i = _vio_R_w_i;\n\trelocalize_t = _relocalize_t; // solomon add for draw point cloud \n\trelocalize_r = _relocalize_r;\n}\n\n/*****************************************utility function************************************************/\nbool inBorder(const cv::Point2f &pt, int COL, int ROW)\n{\n    const int BORDER_SIZE = 1;\n    int img_x = cvRound(pt.x);\n    int img_y = cvRound(pt.y);\n    return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;\n}\n\ntemplate <typename Derived>\nstatic void reduceVector(vector<Derived> &v, vector<uchar> status)\n{\n    int j = 0;\n    for (int i = 0; i < int(v.size()); i++)\n        if (status[i])\n            v[j++] = v[i];\n    v.resize(j);\n}\n\nvoid KeyFrame::extractBrief(cv::Mat &image)\n{\n    BriefExtractor extractor(BRIEF_PATTERN_FILE);\n    extractor(image, measurements, keypoints, descriptors);\n    int start = keypoints.size() - measurements.size();\n    for(int i = 0; i< (int)measurements.size(); i++)\n    {\n        window_keypoints.push_back(keypoints[start + i]);\n        window_descriptors.push_back(descriptors[start + i]);\n    }\n}\nvoid KeyFrame::setExtrinsic(Eigen::Vector3d T, Eigen::Matrix3d R)\n{\n    qic = R;\n    tic = T;\n}\n\nvoid KeyFrame::buildKeyFrameFeatures(Estimator &estimator, const camodocal::CameraPtr &m_camera)\n{\n    for (auto &it_per_id : estimator.f_manager.feature)\n    {\n        it_per_id.used_num = it_per_id.feature_per_frame.size();\n        //if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n        //    continue;\n\n        int frame_size = it_per_id.feature_per_frame.size();\n        if(it_per_id.start_frame <= WINDOW_SIZE - 2 && it_per_id.start_frame + frame_size - 1 >= WINDOW_SIZE - 2)\n        {\n            //features current measurements\n            Vector3d point = it_per_id.feature_per_frame[WINDOW_SIZE - 2 - it_per_id. start_frame].point;\n            Vector2d point_uv;\n            m_camera->spaceToPlane(point, point_uv);\n            measurements.push_back(cv::Point2f(point_uv.x(), point_uv.y()));\n            pts_normalize.push_back(cv::Point2f(point.x()/point.z(), point.y()/point.z()));\n            features_id.push_back(it_per_id.feature_id);\n            //features 3D pos from first measurement and inverse depth\n            Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;\n            point_clouds.push_back(estimator.Rs[it_per_id.start_frame] * (qic * pts_i + tic) + estimator.Ps[it_per_id.start_frame]);\n        }\n    }\n}\n\nbool KeyFrame::inAera(cv::Point2f pt, cv::Point2f center, float area_size)\n{\n    if(center.x < 0 || center.x > COL || center.y < 0 || center.y > ROW)\n        return false;\n    if(pt.x > center.x - area_size && pt.x < center.x + area_size &&\n       pt.y > center.y - area_size && pt.y < center.y + area_size)\n        return true;\n    else\n        return false;\n}\n\nbool KeyFrame::searchInAera(cv::Point2f center_cur, float area_size,\n                            const BRIEF::bitset window_descriptor,\n                            const std::vector<BRIEF::bitset> &descriptors_old,\n                            const std::vector<cv::KeyPoint> &keypoints_old,\n                            cv::Point2f &best_match)\n{\n    cv::Point2f best_pt;\n    int bestDist = 128;\n    int bestIndex = -1;\n    for(int i = 0; i < (int)descriptors_old.size(); i++)\n    {\n        if(!inAera(keypoints_old[i].pt, center_cur, area_size))\n            continue;\n\n        int dis = HammingDis(window_descriptor, descriptors_old[i]);\n        if(dis < bestDist)\n        {\n            bestDist = dis;\n            bestIndex = i;\n        }\n    }\n    if (bestIndex != -1)\n    {\n      best_match = keypoints_old[bestIndex].pt;\n      return true;\n    }\n    else\n      return false;\n}\n\nvoid KeyFrame::FundmantalMatrixRANSAC(vector<cv::Point2f> &measurements_old,\n                                      vector<cv::Point2f> &measurements_old_norm,\n                                      const camodocal::CameraPtr &m_camera)\n{\n    if (measurements_old.size() >= 8)\n    {\n        measurements_old_norm.clear();\n\n        vector<cv::Point2f> un_measurements(measurements_matched.size()), un_measurements_old(measurements_old.size());\n        for (int i = 0; i < (int)measurements_matched.size(); i++)\n        {\n            double FOCAL_LENGTH = 460.0;\n            Eigen::Vector3d tmp_p;\n            m_camera->liftProjective(Eigen::Vector2d(measurements_matched[i].x, measurements_matched[i].y), tmp_p);\n            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n            un_measurements[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n\n            m_camera->liftProjective(Eigen::Vector2d(measurements_old[i].x, measurements_old[i].y), tmp_p);\n            measurements_old_norm.push_back(cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z()));\n            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;\n            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;\n            un_measurements_old[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n        }\n        \n        vector<uchar> status;\n        cv::findFundamentalMat(un_measurements, un_measurements_old, cv::FM_RANSAC, 5.0, 0.99, status);\n        reduceVector(measurements_old, status);\n        reduceVector(measurements_old_norm, status);\n        reduceVector(measurements_matched, status);\n        reduceVector(features_id_matched, status);\n        reduceVector(point_clouds_matched, status);\n        \n    }\n}\n\nvoid KeyFrame::searchByDes(std::vector<cv::Point2f> &measurements_old, \n                           std::vector<cv::Point2f> &measurements_old_norm,\n                           const std::vector<BRIEF::bitset> &descriptors_old,\n                           const std::vector<cv::KeyPoint> &keypoints_old,\n                           const camodocal::CameraPtr &m_camera)\n{\n    //ROS_INFO(\"loop_match before cur %d %d, old %d\", (int)window_descriptors.size(), (int)measurements.size(), (int)descriptors_old.size());\n    std::vector<uchar> status;\n    for(int i = 0; i < (int)window_descriptors.size(); i++)\n    {\n        cv::Point2f pt(0.f, 0.f);\n        if (searchInAera(measurements[i], 200, window_descriptors[i], descriptors_old, keypoints_old, pt))\n          status.push_back(1);\n        else\n          status.push_back(0);\n        measurements_old.push_back(pt);\n    }\n    measurements_matched = measurements;\n    features_id_matched = features_id;\n    point_clouds_matched = point_clouds;\n    reduceVector(measurements_old, status);\n    reduceVector(measurements_matched, status);\n    reduceVector(features_id_matched, status);\n    reduceVector(point_clouds_matched, status);\n}\n\nvoid KeyFrame::PnPRANSAC(vector<cv::Point2f> &measurements_old,\n                         std::vector<cv::Point2f> &measurements_old_norm, \n                         Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old)\n{\n    cv::Mat r, rvec, t, D, tmp_r;\n    cv::Mat K = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);\n    Matrix3d R_inital;\n    Vector3d P_inital;\n    Matrix3d R_w_c = vio_R_w_i * qic;\n    Vector3d T_w_c = vio_T_w_i + vio_R_w_i * tic;\n\n    R_inital = R_w_c.inverse();\n    P_inital = -(R_inital * T_w_c);\n    \n    cv::eigen2cv(R_inital, tmp_r);\n    cv::Rodrigues(tmp_r, rvec);\n    cv::eigen2cv(P_inital, t);\n\n    vector<cv::Point3f> pts_3_vector;\n    for(auto &it: point_clouds_matched)\n        pts_3_vector.push_back(cv::Point3f((float)it.x(),(float)it.y(),(float)it.z()));\n\n    cv::Mat inliers;\n    TicToc t_pnp_ransac;\n    if (CV_MAJOR_VERSION < 3)\n        solvePnPRansac(pts_3_vector, measurements_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 100, inliers);\n    else\n    {\n        if (CV_MINOR_VERSION < 2)\n            solvePnPRansac(pts_3_vector, measurements_old_norm, K, D, rvec, t, true, 100, sqrt(10.0 / 460.0), 0.99, inliers);\n        else\n            solvePnPRansac(pts_3_vector, measurements_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 0.99, inliers);\n\n    }\n   // ROS_DEBUG(\"t_pnp_ransac %f ms\", t_pnp_ransac.toc());\n\n    std::vector<uchar> status;\n    for (int i = 0; i < (int)measurements_old_norm.size(); i++)\n        status.push_back(0);\n\n    for( int i = 0; i < inliers.rows; i++)\n    {\n        int n = inliers.at<int>(i);\n        status[n] = 1;\n    } \n\n    cv::Rodrigues(rvec, r);\n    Matrix3d R_pnp, R_w_c_old;\n    cv::cv2eigen(r, R_pnp);\n    R_w_c_old = R_pnp.transpose();\n    Vector3d T_pnp, T_w_c_old;\n    cv::cv2eigen(t, T_pnp);\n    T_w_c_old = R_w_c_old * (-T_pnp);\n\n    PnP_R_old = R_w_c_old * qic.transpose();\n    PnP_T_old = T_w_c_old - PnP_R_old * tic;   \n\n    reduceVector(measurements_old, status);\n    reduceVector(measurements_old_norm, status);\n    reduceVector(measurements_matched, status);\n    reduceVector(features_id_matched, status);\n    reduceVector(point_clouds_matched, status);\n\n\n}\n\nbool KeyFrame::findConnectionWithOldFrame(const KeyFrame* old_kf,\n                                          std::vector<cv::Point2f> &measurements_old, std::vector<cv::Point2f> &measurements_old_norm,\n                                          Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old,\n                                          const camodocal::CameraPtr &m_camera)\n{\n    TicToc t_match;\n    searchByDes(measurements_old, measurements_old_norm, old_kf->descriptors, old_kf->keypoints, m_camera);\n    FundmantalMatrixRANSAC(measurements_old, measurements_old_norm, m_camera);\n    if ((int)measurements_old_norm.size() > MIN_LOOP_NUM)\n    {\n        PnPRANSAC(measurements_old, measurements_old_norm, PnP_T_old, PnP_R_old);\n    }\n   // ROS_DEBUG(\"loop final use num %d %lf---------------\", (int)measurements_old.size(), t_match.toc());\n    return true;\n}\n\nvoid KeyFrame::updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i)\n{\n    unique_lock<mutex> lock(mMutexPose);\n    T_w_i = _T_w_i;\n    R_w_i = _R_w_i;\n}\n\nvoid KeyFrame::updateOriginPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i)\n{\n    unique_lock<mutex> lock(mMutexPose);\n    vio_T_w_i = _T_w_i;\n    vio_R_w_i = _R_w_i;\n}\n\nvoid KeyFrame::getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i)\n{\n    unique_lock<mutex> lock(mMutexPose);\n    _T_w_i = T_w_i;\n    _R_w_i = R_w_i;\n}\n\nvoid KeyFrame::getOriginPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i)\n{\n    unique_lock<mutex> lock(mMutexPose);\n    _T_w_i = vio_T_w_i;\n    _R_w_i = vio_R_w_i;\n}\n\nvoid KeyFrame::updateLoopConnection(Vector3d relative_t, Quaterniond relative_q, double relative_yaw)\n{\n    has_loop = 1;\n    update_loop_info = 1;\n    unique_lock<mutex> lock(mLoopInfo);\n    Eigen::Matrix<double, 8, 1> connected_info;\n    connected_info <<relative_t.x(), relative_t.y(), relative_t.z(),\n                     relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),\n                     relative_yaw;\n    loop_info = connected_info;\n}\n\nEigen::Vector3d KeyFrame::getLoopRelativeT()\n{\n    assert(update_loop_info == 1);\n    unique_lock<mutex> lock(mLoopInfo);\n    return Eigen::Vector3d(loop_info(0), loop_info(1), loop_info(2));\n}\n\ndouble KeyFrame::getLoopRelativeYaw()\n{\n    assert(update_loop_info == 1);\n    unique_lock<mutex> lock(mLoopInfo);\n    return loop_info(7);\n}\n\nvoid KeyFrame::detectLoop(int index)\n{\n    has_loop = true;\n    loop_index = index;\n}\n\nvoid KeyFrame::removeLoop()\n{\n    has_loop = false;\n    update_loop_info = 0;\n}\n\nvoid KeyFrame::getPath(Eigen::Vector3d& path)\n{\n\tpath = T_w_i;\n}\nint KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b)\n{\n    BRIEF::bitset xor_of_bitset = a ^ b;\n    int dis = xor_of_bitset.count();\n    return dis;\n}\n\nBriefExtractor::BriefExtractor(const std::string &pattern_file)\n{\n  // The DVision::BRIEF extractor computes a random pattern by default when\n  // the object is created.\n  // We load the pattern that we used to build the vocabulary, to make\n  // the descriptors compatible with the predefined vocabulary\n  \n  // loads the pattern\n  cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ);\n  if(!fs.isOpened()) throw string(\"Could not open file \") + pattern_file;\n  \n  vector<int> x1, y1, x2, y2;\n  fs[\"x1\"] >> x1;\n  fs[\"x2\"] >> x2;\n  fs[\"y1\"] >> y1;\n  fs[\"y2\"] >> y2;\n  \n  m_brief.importPairs(x1, y1, x2, y2);\n}\n\nvoid BriefExtractor::operator() (const cv::Mat &im, const std::vector<cv::Point2f> window_pts,\n                                 vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const\n{\n  // extract FAST keypoints with opencv\n  const int fast_th = 20; // corner detector response threshold\n  cv::FAST(im, keys, fast_th, true);\n  for(int i = 0; i < (int)window_pts.size(); i++)\n  {\n      cv::KeyPoint key;\n      key.pt = window_pts[i];\n      keys.push_back(key);\n  }\n  // compute their BRIEF descriptor\n  m_brief.compute(im, keys, descriptors);\n}\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/keyframe.h",
    "content": "#ifndef __KEY_FRAME_\n#define __KEY_FRAME_\n\n//#include <ros/ros.h>\n#include <eigen3/Eigen/Dense>\n#include <opencv2/core/eigen.hpp>\n#include <opencv2/opencv.hpp>\n#include \"../utility/utility.h\"\n#include <algorithm>\n#include \"math.h\"\n#include \"../estimator.h\"\n#include \"../parameters.h\"\n#include \"camodocal/camera_models/CameraFactory.h\"\n#include \"camodocal/camera_models/CataCamera.h\"\n#include \"camodocal/camera_models/PinholeCamera.h\"\n#include <mutex>\n#include \"loop_closure.h\"\n\nusing namespace Eigen;\n\n// This functor extracts BRIEF descriptors in the required format\nclass BriefExtractor: public FeatureExtractor<FBrief::TDescriptor>\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n  virtual void operator()(const cv::Mat &im, const std::vector<cv::Point2f> window_pts,\n    vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const;\n  BriefExtractor(const std::string &pattern_file);\n\nprivate:\n  DVision::BRIEF m_brief;\n};\n\nclass KeyFrame\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tKeyFrame(double _header, Eigen::Vector3d _vio_T_w_c, Eigen::Matrix3d _vio_R_w_c, \n\t\t\t\tEigen::Vector3d _cur_T_w_c, Eigen::Matrix3d _cur_R_w_c,cv::Mat &_image, const char *_brief_pattern_file, Eigen::Vector3d _relocalize_t, Eigen::Matrix3d _relocalize_r);\n\tvoid setExtrinsic(Eigen::Vector3d T, Eigen::Matrix3d R);\t\n\tvoid FundmantalMatrixRANSAC(vector<cv::Point2f> &measurements_old, vector<cv::Point2f> &measurements_old_norm,\n                 \t const camodocal::CameraPtr &m_camera);\n\n\tvoid extractBrief(cv::Mat &image);\n\t\n\tvoid buildKeyFrameFeatures(Estimator &estimator, const camodocal::CameraPtr &m_camera);\n\t\n\tbool inAera(cv::Point2f pt, cv::Point2f center, float area_size);\n\n\tbool searchInAera(cv::Point2f center_cur, float area_size,\n                      const BRIEF::bitset window_descriptor,\n                      const std::vector<BRIEF::bitset> &descriptors_old,\n                      const std::vector<cv::KeyPoint> &keypoints_old,\n                      cv::Point2f &best_match);\n\n\tvoid searchByDes(std::vector<cv::Point2f> &measurements_old,\n\t\t\t\t\t std::vector<cv::Point2f> &measurements_old_norm,\n                     const std::vector<BRIEF::bitset> &descriptors_old,\n                     const std::vector<cv::KeyPoint> &keypoints_old,\n                     const camodocal::CameraPtr &m_camera);\n\n\tbool findConnectionWithOldFrame(const KeyFrame* old_kf,\n\t                                std::vector<cv::Point2f> &measurements_old, std::vector<cv::Point2f> &measurements_old_norm,\n\t                                Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old,\n\t                                const camodocal::CameraPtr &m_camera);\n\n\tvoid PnPRANSAC(vector<cv::Point2f> &measurements_old,\n\t               std::vector<cv::Point2f> &measurements_old_norm, \n\t               Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old);\n\n\tvoid updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i);\n\n\tvoid updateOriginPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i);\n\n\tvoid getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i);\n\n\tvoid getOriginPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i);\n\n\tvoid addConnection(int index, KeyFrame* connected_kf);\n\n\tvoid addConnection(int index, KeyFrame* connected_kf, Vector3d relative_t, Quaterniond relative_q, double relative_yaw);\n\n\tvoid updateLoopConnection(Vector3d relative_t, Quaterniond relative_q, double relative_yaw);\n\n\tvoid detectLoop(int index);\n\n\tvoid removeLoop();\n\tvoid getPath(Eigen::Vector3d& path);\n\n\tint HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b);\n\n\tEigen::Vector3d getLoopRelativeT();\n\n\tdouble getLoopRelativeYaw();\n\n\t// data \n\tdouble header;\n\tstd::vector<Eigen::Vector3d> point_clouds, point_clouds_matched;\n\t//feature in origin image plane\n\tstd::vector<cv::Point2f> measurements, measurements_matched;\n\t//feature in normalize image plane\n\tstd::vector<cv::Point2f> pts_normalize;\n\t//feature ID\n\tstd::vector<int> features_id, features_id_matched;\n\t//feature descriptor\n\tstd::vector<BRIEF::bitset> descriptors;\n\t//keypoints\n\tstd::vector<cv::KeyPoint> keypoints;\n\t\n\t//solomon add for point cloud(world)\n\tEigen::Vector3d relocalize_t;\n\tEigen::Matrix3d relocalize_r;\n\n\tint global_index;\n\tcv::Mat image;\n\tMatrix3d qic;\n\tVector3d tic;\n\tint COL, ROW;\n\tbool use_retrive;\n\n\tbool has_loop;\n\tint loop_index;\n\tbool update_loop_info;\n\t// index t_x t_y t_z q_w q_x q_y q_z yaw\n\t// old_R_cur old_T_cur\n\n\t// looped by other frame\n\tbool is_looped;\n\tint resample_index;\n\tconst char *BRIEF_PATTERN_FILE;\n\t// index t_x t_y t_z q_w q_x q_y q_z yaw\n\t\n\nprivate:\n\tEigen::Vector3d T_w_i;\n\tEigen::Matrix3d R_w_i;\n\tEigen::Vector3d vio_T_w_i;\n\tEigen::Matrix3d vio_R_w_i;\n\tstd::mutex mMutexPose;\n\tstd::mutex mLoopInfo;\n\tstd::vector<cv::KeyPoint> window_keypoints;\n\tstd::vector<BRIEF::bitset> window_descriptors;\n\tEigen::Matrix<double, 8, 1 > loop_info;\n\n};\n\n#endif\n\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/keyframe_database.cpp",
    "content": "#include \"keyframe_database.h\"\n#include <pangolin/pangolin.h>\nKeyFrameDatabase::KeyFrameDatabase()\n{\n//\tposegraph_visualization = new CameraPoseVisualization(0.0, 0.0, 1.0, 1.0);\n//\tposegraph_visualization->setScale(0.1);\n //   posegraph_visualization->setLineWidth(0.01);\n    earliest_loop_index = -1;\n    t_drift = Eigen::Vector3d(0, 0, 0);\n    yaw_drift = 0;\n    r_drift = Eigen::Matrix3d::Identity();\n    total_length = 0;\n    last_P = Eigen::Vector3d(0, 0, 0);\n}\nvoid KeyFrameDatabase::viewPointClouds()\n{\n\tlist<KeyFrame*>::iterator iterator_keyframe;\n\tfor(iterator_keyframe = keyFrameList.begin(); iterator_keyframe != keyFrameList.end(); iterator_keyframe++)\n\t{\n\t//\tglPushMatrix();\n    \tglPointSize(1); //设备被渲染点的宽度，以像素位单位，默认为1\n    \tglBegin(GL_POINTS);      //把每一个顶点当做一个独立的点进行处理\n    \t//glColor3f(0.0,0.0,0.0);  //点的颜色为黑色\n    \tglColor3f(1.0,1.0,1.0);  //点的颜色为白色\n\t/*\tcout << \"relocalize_r:\" << (*iterator_keyframe)->relocalize_r(0,0) << \" \" << (*iterator_keyframe)->relocalize_r(0,1) << \" \" << (*iterator_keyframe)->relocalize_r(0,2) << endl\n\t\t\t\t\t\t\t\t<< (*iterator_keyframe)->relocalize_r(1,0) << \" \" << (*iterator_keyframe)->relocalize_r(1,1) << \" \" << (*iterator_keyframe)->relocalize_r(1,2) << endl\n\t\t\t\t\t\t\t\t<< (*iterator_keyframe)->relocalize_r(2,0) << \" \" << (*iterator_keyframe)->relocalize_r(2,1) << \" \" << (*iterator_keyframe)->relocalize_r(2,2) << endl;\n\t\tcout << \"relocalize_t:\" << (*iterator_keyframe)->relocalize_t(0) << \" \" << (*iterator_keyframe)->relocalize_t(1) << \" \" << (*iterator_keyframe)->relocalize_t(2) << endl;\n\t*/\n\tfor(auto it = (*iterator_keyframe)->point_clouds.begin(); it!= (*iterator_keyframe)->point_clouds.end(); it++)\n\t\t{\n\t\t\tVector3d pointCloudWorld;\n\t\t\tpointCloudWorld = (*iterator_keyframe)->relocalize_r * (*it) + (*iterator_keyframe)->relocalize_t;\n\t\t\tglVertex3f((float)(pointCloudWorld.x()), (float)(pointCloudWorld.y()), (float)(pointCloudWorld.z()));\n\t\t}\n\t\tglEnd();\n\t//\tglPopMatrix();\n\t}\n}\nvoid KeyFrameDatabase::viewPath()\n{\n\tEigen::Vector3d tmp_path;\n\tglColor3f(1.0f, 0.0f, 0.0f);\n\tglLineWidth(2);\n\tglBegin(GL_LINE_STRIP);\n\n\tlist<KeyFrame*>::iterator iterator_keyframe;\n\tfor(iterator_keyframe = keyFrameList.begin(); iterator_keyframe != keyFrameList.end(); iterator_keyframe++)\n\t{\n\t\t(*iterator_keyframe)->getPath(tmp_path);\n\t\tglVertex3f(tmp_path.x(), tmp_path.y(), tmp_path.z());\n\t}\n\tglEnd();\n}\nvoid KeyFrameDatabase::add(KeyFrame* pKF)\n{\n\t//ROS_DEBUG(\"add keyframe begin!\");\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\tkeyFrameList.push_back(pKF);\n\tlock.unlock();\n\tVector3d P;\n\tMatrix3d R;\n\tpKF->getPose(P, R);\n\tQuaterniond Q;\n\tQ = R;\n\n\ttotal_length += (P - last_P).norm();\n\tlast_P = P;\n\t//posegraph_visualization->add_pose(P, Q);\n/*\n\t//draw local connection\n\tlist<KeyFrame*>::reverse_iterator rit = keyFrameList.rbegin();\n\tlist<KeyFrame*>::reverse_iterator lrit;\n\tfor (; rit != keyFrameList.rend(); rit++)  \n    {  \n        if ((*rit) == pKF)\n        {\n        \tlrit = rit;\n        \tlrit++;\n        \tfor (int i = 0; i < 4; i++)\n        \t{\n        \t\tif (lrit == keyFrameList.rend())\n        \t\t\tbreak;\n        \t\tVector3d conncected_P;\n        \t\tMatrix3d connected_R;\n        \t\t(*lrit)->getPose(conncected_P, connected_R);\n        \t\tposegraph_visualization->add_edge(P, conncected_P);\n        \t\tlrit++;\n        \t}\n        \tbreak;\n        }\n    } \n*/\n\t// add key frame to path for visualization\n\tnav_msgs::Odometry odometry;\n\todometry.header.stamp = ros::Time(pKF->header);\n\todometry.header.frame_id = \"world\";\n\todometry.pose.pose.position.x = P.x();\n\todometry.pose.pose.position.y = P.y();\n\todometry.pose.pose.position.z = P.z();\n\todometry.pose.pose.orientation.x = Q.x();\n\todometry.pose.pose.orientation.y = Q.y();\n\todometry.pose.pose.orientation.z = Q.z();\n\todometry.pose.pose.orientation.w = Q.w();\n\n\tgeometry_msgs::PoseStamped pose_stamped;\n\tpose_stamped.header = odometry.header;\n\tpose_stamped.pose = odometry.pose.pose;\n\n\t//unique_lock<mutex> mlockPath(mPath);\n\t//refine_path.header = odometry.header;\n\t//refine_path.poses.push_back(pose_stamped);\n\t//mlockPath.unlock();\n\t\n//\tROS_DEBUG(\"add keyframe end!\");\n\n}\n\nvoid KeyFrameDatabase::downsample(vector<int> &erase_index)\n{\n//\tROS_DEBUG(\"resample keyframe begin!\");\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\tint frame_num = (int)keyFrameList.size();\n\tif (mOptimiazationPosegraph.try_lock())\n\t{\n\t\terase_index.clear();\n\t\tdouble min_dis = total_length / (frame_num * 0.7);\n\n\t\tlist<KeyFrame*>::iterator it = keyFrameList.begin();\n\t\tVector3d last_P = Vector3d(0, 0, 0);\n\t\tfor (; it != keyFrameList.end(); )   \n\t\t{\n\t\t\tVector3d tmp_t;\n\t\t\tMatrix3d tmp_r;\n\t\t\t(*it)->getPose(tmp_t, tmp_r);\n\t\t\tdouble dis = (tmp_t - last_P).norm();\n\t\t    if(it == keyFrameList.begin() || dis > min_dis || (*it)->has_loop || (*it)->is_looped)\n\t\t    {\n\t\t    \tlast_P = tmp_t;\n\t\t    \tit++;\n\t\t    }\n\t\t    else\n\t\t    {\n\t\t    \terase_index.push_back((*it)->global_index);\n\t\t    \tdelete (*it);\n\t\t    \tit = keyFrameList.erase(it);\n\t\t    }\n\t\t}\n\t\tmOptimiazationPosegraph.unlock();\n\t}\n\telse\n\t\treturn;\n\n\tlock.unlock();\n\t//ROS_DEBUG(\"resample keyframe end!\");\n}\n\nvoid KeyFrameDatabase::erase(KeyFrame* pKF)\n{\n\tlist<KeyFrame*>::iterator it = find(keyFrameList.begin(), keyFrameList.end(), pKF);\n\tassert(it != keyFrameList.end());\n\tif (it != keyFrameList.end())\n\t    keyFrameList.erase(it);\n}\n\nint KeyFrameDatabase::size()\n{\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\treturn (int)keyFrameList.size();\n}\n\nvoid KeyFrameDatabase::getKeyframeIndexList(vector<int> &keyframe_index_list)\n{\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\tlist<KeyFrame*>::iterator it = keyFrameList.begin();\n\tfor (; it != keyFrameList.end(); it++)   \n\t{\n\t\tkeyframe_index_list.push_back((*it)->global_index);\n\t}\n\treturn;\n}\n\nKeyFrame* KeyFrameDatabase::getKeyframe(int index)\n{\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\tlist<KeyFrame*>::iterator it = keyFrameList.begin();\n\tfor (; it != keyFrameList.end(); it++)   \n\t{\n\t    if((*it)->global_index == index)\n\t    \tbreak;\n\t}\n\tif (it != keyFrameList.end())\n    \treturn *it;\n    else\n    \treturn NULL;\n}\n\nKeyFrame* KeyFrameDatabase::getLastKeyframe()\n{\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\tlist<KeyFrame*>::reverse_iterator rit = keyFrameList.rbegin();\n\tassert(rit != keyFrameList.rend());\n    return *rit;\n}\n\nKeyFrame* KeyFrameDatabase::getLastKeyframe(int last_index)\n{\n\tunique_lock<mutex> lock(mMutexkeyFrameList);\n\tlist<KeyFrame*>::reverse_iterator rit = keyFrameList.rbegin();\n\tfor (int i = 0; i < last_index; i++)  \n    {  \n        rit++;\n        assert(rit != keyFrameList.rend());\n    } \n    return *rit;\n}\n\nvoid KeyFrameDatabase::optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d &loop_correct_t, Eigen::Matrix3d &loop_correct_r)\n{\n\t//ROS_DEBUG(\"optimizae pose graph begin!\");\n\tunique_lock<mutex> lock(mOptimiazationPosegraph);\n\tKeyFrame* cur_kf = getKeyframe(cur_index);\n\tint loop_index = cur_kf->loop_index;\n\tif (earliest_loop_index > loop_index || earliest_loop_index == -1)\n\t\tearliest_loop_index = loop_index;\n\tassert(cur_kf-> update_loop_info == 1);\n\tint max_length = cur_index + 1;\n\n\t// w^t_i   w^q_i\n\tdouble t_array[max_length][3];\n\tQuaterniond q_array[max_length];\n\tdouble euler_array[max_length][3];\n\n\tceres::Problem problem;\n\tceres::Solver::Options options;\n\toptions.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;\n\t//options.minimizer_progress_to_stdout = true;\n\toptions.max_solver_time_in_seconds = SOLVER_TIME * 3;\n\toptions.max_num_iterations = 5;\n\tceres::Solver::Summary summary;\n\tceres::LossFunction *loss_function;\n\tloss_function = new ceres::HuberLoss(1.0);\n\t//loss_function = new ceres::CauchyLoss(1.0);\n\tceres::LocalParameterization* angle_local_parameterization =\n\t    AngleLocalParameterization::Create();\n\n\tlist<KeyFrame*>::iterator it;\n\n\tint i = 0;\n\tfor (it = keyFrameList.begin(); it != keyFrameList.end(); it++)\n\t{\n\t\tif ((*it)->global_index < earliest_loop_index)\n\t\t\tcontinue;\n\t\t(*it)->resample_index = i;\n\t\tQuaterniond tmp_q;\n\t\tMatrix3d tmp_r;\n\t\tVector3d tmp_t;\n\t\t(*it)->getOriginPose(tmp_t, tmp_r);\n\t\ttmp_q = tmp_r;\n\t\tt_array[i][0] = tmp_t(0);\n\t\tt_array[i][1] = tmp_t(1);\n\t\tt_array[i][2] = tmp_t(2);\n\t\tq_array[i] = tmp_q;\n\n\t\tVector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());\n\t\teuler_array[i][0] = euler_angle.x();\n\t\teuler_array[i][1] = euler_angle.y();\n\t\teuler_array[i][2] = euler_angle.z();\n\n\t\tproblem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);\n\t\tproblem.AddParameterBlock(t_array[i], 3);\n\n\t\tif ((*it)->global_index == earliest_loop_index)\n\t\t{\t\n\t\t\tproblem.SetParameterBlockConstant(euler_array[i]);\n\t\t\tproblem.SetParameterBlockConstant(t_array[i]);\n\t\t}\n\n\t\t//add edge\n\t\tfor (int j = 1; j < 5; j++)\n\t\t{\n\t\t  if (i - j > 0)\n\t\t  {\n\t\t    Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());\n\t\t    Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);\n\t\t    relative_t = q_array[i-j].inverse() * relative_t;\n\t\t    double relative_yaw = euler_array[i][0] - euler_array[i-j][0];\n\t\t    ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),\n\t\t                                   relative_yaw, euler_conncected.y(), euler_conncected.z());\n\t\t    problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], \n\t\t                            t_array[i-j], \n\t\t                            euler_array[i], \n\t\t                            t_array[i]);\n\t\t  }\n\t\t}\n\n\t\t//add loop edge\n\t\tif((*it)->update_loop_info)\n\t\t{\n\t\t\tint connected_index = getKeyframe((*it)->loop_index)->resample_index;\n\t\t\tassert((*it)->loop_index >= earliest_loop_index);\n\t\t\tVector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());\n\t\t\tVector3d relative_t;\n\t\t\trelative_t = (*it)->getLoopRelativeT();\n\t\t\tdouble relative_yaw = (*it)->getLoopRelativeYaw();\n\t\t\tceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t   relative_yaw, euler_conncected.y(), euler_conncected.z());\n\t\t\tproblem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], \n\t\t\t\t\t\t\t\t\t\t\t\t\t\t  t_array[connected_index], \n\t\t\t\t\t\t\t\t\t\t\t\t\t\t  euler_array[i], \n\t\t\t\t\t\t\t\t\t\t\t\t\t\t  t_array[i]);\n\t\t\t\n\t\t}\n\t\tif ((*it)->global_index == cur_index)\n\t\t\tbreak;\n\t\ti++;\n\t}\n\n\tceres::Solve(options, &problem, &summary);\n\t//std::cout << summary.BriefReport() << \"\\n\";\n\n\ti = 0;\n\tfor (it = keyFrameList.begin(); it != keyFrameList.end(); it++)\n\t{\n\t\tif ((*it)->global_index < earliest_loop_index)\n\t\t\tcontinue;\n\t\tQuaterniond tmp_q;\n\t\ttmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));\n\t\tVector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);\n\t\tMatrix3d tmp_r = tmp_q.toRotationMatrix();\n\t\t(*it)-> updatePose(tmp_t, tmp_r);\n\n\t\tif ((*it)->global_index == cur_index)\n\t\t\tbreak;\n\t\ti++;\n\t}\n\n\tVector3d cur_t, origin_t;\n\tMatrix3d cur_r, origin_r;\n\tcur_kf->getPose(cur_t, cur_r);\n\tcur_kf->getOriginPose(origin_t, origin_r);\n\tyaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(origin_r).x();\n\tr_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));\n\tt_drift = cur_t - r_drift * origin_t;\n\n\tfor (; it != keyFrameList.end(); it++)\n\t{\n\t\tVector3d P;\n\t\tMatrix3d R;\n\t\t(*it)->getOriginPose(P, R);\n\t\tP = r_drift * P + t_drift;\n\t\tR = r_drift * R;\n\t\t(*it)-> updatePose(P, R);\n\t}\n\tloop_correct_t = t_drift;\n\tloop_correct_r = r_drift;\n\t//ROS_DEBUG(\"optimizae pose graph end!\");\n\n}\n\nvoid KeyFrameDatabase::updateVisualization()\n{\n\t//ROS_DEBUG(\"updateVisualization begin\");\n\tunique_lock<mutex> mlockPath(mPath);\n\tunique_lock<mutex> mlockPosegraph(mPosegraphVisualization);\n\ttotal_length = 0;\n\tlast_P = Vector3d(0, 0, 0);\n\t//update visualization\n\tlist<KeyFrame*>::iterator it;\n//\tposegraph_visualization->reset();\n\trefine_path.poses.clear();\n\n\tfor (it = keyFrameList.begin(); it != keyFrameList.end(); it++)\n\t{\n\n\t\tVector3d P;\n\t\tMatrix3d R;\n\t\t(*it)->getPose(P, R);\n\t\tQuaterniond Q;\n\t\tQ = R;\n\n\t\ttotal_length += (P - last_P).norm();\n\t\tlast_P = P;\n\t\t//posegraph_visualization->add_pose(P, Q);\n\t\t/*\n\t\tlist<KeyFrame*>::iterator lit;\n\t\tlit = it;\n    \tfor (int i = 0; i < 4; i++)\n    \t{\n    \t\tif (lit == keyFrameList.begin())\n    \t\t\tbreak;\n    \t\tlit--;\n    \t\tVector3d conncected_P;\n    \t\tMatrix3d connected_R;\n    \t\t(*lit)->getPose(conncected_P, connected_R);\n    \t\tposegraph_visualization->add_edge(P, conncected_P);\n    \t}\n\t\t*/\n\t\t// draw loop edge\n\n\t\tif ((*it)->update_loop_info)\n\t\t{\n\t\t\t\n\t\t\tKeyFrame* connected_KF = getKeyframe((*it)->loop_index);\n\t\t\tVector3d conncected_P;\n\t\t\tMatrix3d connected_R;\n\t\t\tconnected_KF->getPose(conncected_P, connected_R);\n//\t\t\tposegraph_visualization->add_loopedge(P, conncected_P);\n\t\t\t\n\t\t\t/*\n\t\t\t//supposed edge\n\t\t\tVector3d supposed_P;\n\t\t\tVector3d relative_t;\n\t\t\trelative_t = (*it)->getLoopRelativeT();\n\t\t\tsupposed_P = P - connected_R * relative_t;\n\t\t\tposegraph_visualization->add_edge(P, supposed_P);\n\t\t\t*/\n\t\t\t\n\t\t\tlist<KeyFrame*>::iterator lit;\n\t\t\tlit = it;\n\t\t\tlit--;\n\t\t\tVector3d P_previous;\n\t\t\tMatrix3d R_previous;\n\t\t\t(*lit)->getPose(P_previous, R_previous);\n//\t\t\tposegraph_visualization->add_loopedge(P, P_previous);\n\t\t}\n\n\t\t// add key frame to path for visualization\n\t\tnav_msgs::Odometry odometry;\n\t\todometry.header.stamp = ros::Time((*it)->header);\n\t\todometry.header.frame_id = \"world\";\n\t\todometry.pose.pose.position.x = P.x();\n\t\todometry.pose.pose.position.y = P.y();\n\t\todometry.pose.pose.position.z = P.z();\n\t\todometry.pose.pose.orientation.x = Q.x();\n\t\todometry.pose.pose.orientation.y = Q.y();\n\t\todometry.pose.pose.orientation.z = Q.z();\n\t\todometry.pose.pose.orientation.w = Q.w();\n\n\t\tgeometry_msgs::PoseStamped pose_stamped;\n\t\tpose_stamped.header = odometry.header;\n\t\tpose_stamped.pose = odometry.pose.pose;\n\t\trefine_path.header = odometry.header;\n\t\trefine_path.poses.push_back(pose_stamped);\n\n\t}\n\t//ROS_DEBUG(\"updateVisualization end\");\n}\n\nvoid KeyFrameDatabase::addLoop(int loop_index)\n{\n\tunique_lock<mutex> lock(mPosegraphVisualization);\n\tif (earliest_loop_index > loop_index || earliest_loop_index == -1)\n\t\tearliest_loop_index = loop_index;\n\n\tKeyFrame* cur_KF = getLastKeyframe();\n\tKeyFrame* connected_KF = getKeyframe(loop_index);\n\tVector3d conncected_P, P;\n\tMatrix3d connected_R, R;\n\tcur_KF->getPose(P, R);\n\tconnected_KF->getPose(conncected_P, connected_R);\n//\tposegraph_visualization->add_loopedge(P, conncected_P);\n}\n\nnav_msgs::Path KeyFrameDatabase::getPath()\n{\n\tunique_lock<mutex> lock(mPath);\n\treturn refine_path;\n}\n\n/*\nCameraPoseVisualization* KeyFrameDatabase::getPosegraphVisualization()\n{\n\tunique_lock<mutex> lock(mPosegraphVisualization);\n\treturn posegraph_visualization;\n}\n*/\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/keyframe_database.h",
    "content": "#pragma once\n\n#include <vector>\n#include <list>\n#include \"keyframe.h\"\n#include <assert.h>\n#include <ceres/ceres.h>\n#include <ceres/rotation.h>\n//#include \"../utility/CameraPoseVisualization.h\"\n#include \"../utility/utility.h\"\n//#include <nav_msgs/Path.h>\n//#include <nav_msgs/Odometry.h>\n//#include <geometry_msgs/PointStamped.h>\n#include \"../../include/Path.h\"\n#include \"../../include/Odometry.h\"\n#include \"../../include/PointStamped\"\n\nclass KeyFrameDatabase\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tKeyFrameDatabase();\n\tvoid add(KeyFrame* pKF);\n\tvoid downsample(vector<int> &erase_index);\n\tvoid erase(KeyFrame* pKF);\n\tint size();\n\tvoid optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d &loop_correct_t, Eigen::Matrix3d &loop_correct_r);\n\tKeyFrame* getKeyframe(int index);\n\tKeyFrame* getLastKeyframe();\n\tKeyFrame* getLastKeyframe(int last_index);\n\tvoid getKeyframeIndexList(vector<int> &keyframe_index_list);\n\tvoid updateVisualization();\n\tvoid addLoop(int loop_index);\n\tnav_msgs::Path getPath();\n\tvoid viewPointClouds();\n\tvoid viewPath();\n\n\t//CameraPoseVisualization* getPosegraphVisualization();\n\t\nprivate:\n\tlist<KeyFrame*> keyFrameList;\n\tstd::mutex mMutexkeyFrameList;\n\tstd::mutex mOptimiazationPosegraph;\n\tstd::mutex mPath;\n\tstd::mutex mPosegraphVisualization;\n\tint earliest_loop_index;\n\tVector3d t_drift;\n\tdouble yaw_drift;\n\tMatrix3d r_drift;\n\tdouble total_length;\n\tVector3d last_P;\n\tnav_msgs::Path refine_path;\n\t//CameraPoseVisualization* posegraph_visualization;\n};\n\ntemplate <typename T>\nT NormalizeAngle(const T& angle_degrees) {\n  if (angle_degrees > T(180.0))\n  \treturn angle_degrees - T(360.0);\n  else if (angle_degrees < T(-180.0))\n  \treturn angle_degrees + T(360.0);\n  else\n  \treturn angle_degrees;\n};\n\nclass AngleLocalParameterization {\n public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n  template <typename T>\n  bool operator()(const T* theta_radians, const T* delta_theta_radians,\n                  T* theta_radians_plus_delta) const {\n    *theta_radians_plus_delta =\n        NormalizeAngle(*theta_radians + *delta_theta_radians);\n\n    return true;\n  }\n\n  static ceres::LocalParameterization* Create() {\n    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,\n                                                     1, 1>);\n  }\n};\n\ntemplate <typename T> inline\nvoid QuaternionInverse(const T q[4], T q_inverse[4])\n{\n\tq_inverse[0] = q[0];\n\tq_inverse[1] = -q[1];\n\tq_inverse[2] = -q[2];\n\tq_inverse[3] = -q[3];\n};\n\nstruct RelativeTError\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tRelativeTError(double t_x, double t_y, double t_z)\n\t\t\t\t  :t_x(t_x), t_y(t_y), t_z(t_z){}\n\n\ttemplate <typename T>\n\tbool operator()(const T* const w_q_i, const T* ti, const T* tj, T* residuals) const\n\t{\n\t\tT t_w_ij[3];\n\t\tt_w_ij[0] = tj[0] - ti[0];\n\t\tt_w_ij[1] = tj[1] - ti[1];\n\t\tt_w_ij[2] = tj[2] - ti[2];\n\n\t\tT i_q_w[4];\n\t\tQuaternionInverse(w_q_i, i_q_w);\n\n\t\tT t_i_ij[3];\n\t\tceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);\n\n\t\tresiduals[0] = t_i_ij[0] - T(t_x);\n\t\tresiduals[1] = t_i_ij[1] - T(t_y);\n\t\tresiduals[2] = t_i_ij[2] - T(t_z);\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z) \n\t{\n\t  return (new ceres::AutoDiffCostFunction<\n\t          RelativeTError, 3, 4, 3, 3>(\n\t          \tnew RelativeTError(t_x, t_y, t_z)));\n\t}\n\n\tdouble t_x, t_y, t_z;\n\n};\n\n\nstruct TError\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tTError(double t_x, double t_y, double t_z)\n\t\t\t\t  :t_x(t_x), t_y(t_y), t_z(t_z){}\n\n\ttemplate <typename T>\n\tbool operator()(const T* tj, T* residuals) const\n\t{\n\t\tresiduals[0] = tj[0] - T(t_x);\n\t\tresiduals[1] = tj[1] - T(t_y);\n\t\tresiduals[2] = tj[2] - T(t_z);\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z) \n\t{\n\t  return (new ceres::AutoDiffCostFunction<\n\t          TError, 3, 3>(\n\t          \tnew TError(t_x, t_y, t_z)));\n\t}\n\n\tdouble t_x, t_y, t_z;\n\n};\n\nstruct RelativeRTError\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tRelativeRTError(double t_x, double t_y, double t_z, double q_w, double q_x, double q_y, double q_z)\n\t\t\t\t  :t_x(t_x), t_y(t_y), t_z(t_z), q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z)\n\t\t\t\t  {\n\t\t\t\t  \tt_norm = sqrt(t_x * t_x + t_y * t_y + t_z * t_z);\n\t\t\t\t  }\n\n\ttemplate <typename T>\n\tbool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const\n\t{\n\t\tT t_w_ij[3];\n\t\tt_w_ij[0] = tj[0] - ti[0];\n\t\tt_w_ij[1] = tj[1] - ti[1];\n\t\tt_w_ij[2] = tj[2] - ti[2];\n\n\t\tT i_q_w[4];\n\t\tQuaternionInverse(w_q_i, i_q_w);\n\n\t\tT t_i_ij[3];\n\t\tceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);\n\n\t\t//residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_norm);\n\t\t//residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_norm);\n\t\t//residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_norm);\n\t\tresiduals[0] = (t_i_ij[0] - T(t_x));\n\t\tresiduals[1] = (t_i_ij[1] - T(t_y));\n\t\tresiduals[2] = (t_i_ij[2] - T(t_z));\n\n\t\tT relative_q[4];\n\t\trelative_q[0] = T(q_w);\n\t\trelative_q[1] = T(q_x);\n\t\trelative_q[2] = T(q_y);\n\t\trelative_q[3] = T(q_z);\n\n\t\tT q_i_j[4];\n\t\tceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);\n\n\t\tT relative_q_inv[4];\n\t\tQuaternionInverse(relative_q, relative_q_inv);\n\n\t\tT error_q[4];\n\t\tceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); \n\n\t\tresiduals[3] = T(2) * error_q[1];\n\t\tresiduals[4] = T(2) * error_q[2];\n\t\tresiduals[5] = T(2) * error_q[3];\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,\n\t\t\t\t\t\t\t\t\t   const double q_w, const double q_x, const double q_y, const double q_z) \n\t{\n\t  return (new ceres::AutoDiffCostFunction<\n\t          RelativeRTError, 6, 4, 3, 4, 3>(\n\t          \tnew RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z)));\n\t}\n\n\tdouble t_x, t_y, t_z, t_norm;\n\tdouble q_w, q_x, q_y, q_z;\n\n};\n\ntemplate <typename T> \nvoid YawPitchRollToRotationMatrix(const T yaw, const T pitch, const T roll, T R[9])\n{\n\n\tT y = yaw / T(180.0) * T(M_PI);\n\tT p = pitch / T(180.0) * T(M_PI);\n\tT r = roll / T(180.0) * T(M_PI);\n\n\n\tR[0] = cos(y) * cos(p);\n\tR[1] = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r);\n\tR[2] = sin(y) * sin(r) + cos(y) * sin(p) * cos(r);\n\tR[3] = sin(y) * cos(p);\n\tR[4] = cos(y) * cos(r) + sin(y) * sin(p) * sin(r);\n\tR[5] = -cos(y) * sin(r) + sin(y) * sin(p) * cos(r);\n\tR[6] = -sin(p);\n\tR[7] = cos(p) * sin(r);\n\tR[8] = cos(p) * cos(r);\n};\n\ntemplate <typename T> \nvoid RotationMatrixTranspose(const T R[9], T inv_R[9])\n{\n\tinv_R[0] = R[0];\n\tinv_R[1] = R[3];\n\tinv_R[2] = R[6];\n\tinv_R[3] = R[1];\n\tinv_R[4] = R[4];\n\tinv_R[5] = R[7];\n\tinv_R[6] = R[2];\n\tinv_R[7] = R[5];\n\tinv_R[8] = R[8];\n};\n\ntemplate <typename T> \nvoid RotationMatrixRotatePoint(const T R[9], const T t[3], T r_t[3])\n{\n\tr_t[0] = R[0] * t[0] + R[1] * t[1] + R[2] * t[2];\n\tr_t[1] = R[3] * t[0] + R[4] * t[1] + R[5] * t[2];\n\tr_t[2] = R[6] * t[0] + R[7] * t[1] + R[8] * t[2];\n};\n\nstruct FourDOFError\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tFourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)\n\t\t\t\t  :t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}\n\n\ttemplate <typename T>\n\tbool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const\n\t{\n\t\tT t_w_ij[3];\n\t\tt_w_ij[0] = tj[0] - ti[0];\n\t\tt_w_ij[1] = tj[1] - ti[1];\n\t\tt_w_ij[2] = tj[2] - ti[2];\n\n\t\t// euler to rotation\n\t\tT w_R_i[9];\n\t\tYawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);\n\t\t// rotation transpose\n\t\tT i_R_w[9];\n\t\tRotationMatrixTranspose(w_R_i, i_R_w);\n\t\t// rotation matrix rotate point\n\t\tT t_i_ij[3];\n\t\tRotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);\n\n\t\tresiduals[0] = (t_i_ij[0] - T(t_x));\n\t\tresiduals[1] = (t_i_ij[1] - T(t_y));\n\t\tresiduals[2] = (t_i_ij[2] - T(t_z));\n\t\tresiduals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw)) / T(10.0);\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,\n\t\t\t\t\t\t\t\t\t   const double relative_yaw, const double pitch_i, const double roll_i) \n\t{\n\t  return (new ceres::AutoDiffCostFunction<\n\t          FourDOFError, 4, 1, 3, 1, 3>(\n\t          \tnew FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));\n\t}\n\n\tdouble t_x, t_y, t_z;\n\tdouble relative_yaw, pitch_i, roll_i;\n\n};\n\nstruct FourDOFWeightError\n{\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tFourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)\n\t\t\t\t  :t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){\n\t\t\t\t  \tweight = 5;\n\t\t\t\t  }\n\n\ttemplate <typename T>\n\tbool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const\n\t{\n\t\tT t_w_ij[3];\n\t\tt_w_ij[0] = tj[0] - ti[0];\n\t\tt_w_ij[1] = tj[1] - ti[1];\n\t\tt_w_ij[2] = tj[2] - ti[2];\n\n\t\t// euler to rotation\n\t\tT w_R_i[9];\n\t\tYawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);\n\t\t// rotation transpose\n\t\tT i_R_w[9];\n\t\tRotationMatrixTranspose(w_R_i, i_R_w);\n\t\t// rotation matrix rotate point\n\t\tT t_i_ij[3];\n\t\tRotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);\n\n\t\tresiduals[0] = (t_i_ij[0] - T(t_x)) * T(weight);\n\t\tresiduals[1] = (t_i_ij[1] - T(t_y)) * T(weight);\n\t\tresiduals[2] = (t_i_ij[2] - T(t_z)) * T(weight);\n\t\tresiduals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,\n\t\t\t\t\t\t\t\t\t   const double relative_yaw, const double pitch_i, const double roll_i) \n\t{\n\t  return (new ceres::AutoDiffCostFunction<\n\t          FourDOFWeightError, 4, 1, 3, 1, 3>(\n\t          \tnew FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));\n\t}\n\n\tdouble t_x, t_y, t_z;\n\tdouble relative_yaw, pitch_i, roll_i;\n\tdouble weight;\n\n};\n"
  },
  {
    "path": "vins_estimator/src/loop-closure/loop_closure.cpp",
    "content": "/**\n * File: demo_brief.cpp\n * Date: November 2011\n * Author: Dorian Galvez-Lopez\n * Description: demo application of DLoopDetector\n * License: see the LICENSE.txt file\n */\n\n// ----------------------------------------------------------------------------\n#include \"loop_closure.h\"\n\nLoopClosure::LoopClosure(const char *_voc_file, int _image_w, int _image_h)\n:demo(_voc_file,_image_w, _image_h), IMAGE_W(_image_w), IMAGE_H(_image_h)\n{\n    printf(\" loop closure init finish\\n\");\n}\n\nvoid LoopClosure::initCameraModel(const std::string &calib_file)\n{\n      demo.initCameraModel(calib_file);\n}\n\nbool LoopClosure::startLoopClosure(std::vector<cv::KeyPoint> &keys, std::vector<BRIEF::bitset> &descriptors,\n                                   std::vector<cv::Point2f> &cur_pts,\n                                   std::vector<cv::Point2f> &old_pts,\n                                   int &old_index)\n{\n  try \n  {\n    bool loop_succ = false;\n    loop_succ = demo.run(\"BRIEF\", keys, descriptors, cur_pts, old_pts, old_index);\n    return loop_succ;\n  }\n  catch(const std::string &ex)\n  {\n    cout << \"Error: \" << ex << endl;\n    return false;\n  }\n}\n\nvoid LoopClosure::eraseIndex(std::vector<int> &erase_index)\n{\n  demo.eraseIndex(erase_index);\n}"
  },
  {
    "path": "vins_estimator/src/loop-closure/loop_closure.h",
    "content": "#ifndef __LOOP_CLOSURE__\n#define __LOOP_CLOSURE__\n\n#include <iostream>\n#include <vector>\n#include <string>\n\n// DLoopDetector and DBoW2\n#include \"ThirdParty/DBoW/DBoW2.h\" // defines BriefVocabulary\n#include \"DLoopDetector.h\" // defines BriefLoopDetector\n#include \"ThirdParty/DVision/DVision.h\" // Brief\n\n// OpenCV\n#include <opencv/cv.h>\n#include <opencv/highgui.h>\n\n#include \"demoDetector.h\"\n//#include \"brief_extractor.h\"\n\nusing namespace DLoopDetector;\nusing namespace DBoW2;\nusing namespace DVision;\nusing namespace std;\n\n\nclass LoopClosure\n{\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tLoopClosure(const char *voc_file, int _image_w, int _image_h);\n\n\tbool startLoopClosure(std::vector<cv::KeyPoint> &keys, std::vector<BRIEF::bitset> &descriptors,\n                                   std::vector<cv::Point2f> &cur_pts,\n                                   std::vector<cv::Point2f> &old_pts,\n                                   int &old_index);\n\tvoid initCameraModel(const std::string &calib_file);\n\n\tvoid eraseIndex(std::vector<int> &erase_index);\n\t/* data */\n\tdemoDetector<BriefVocabulary, BriefLoopDetector, FBrief::TDescriptor> demo;\n\tint IMAGE_W;\n\tint IMAGE_H;\n};\n\n#endif\n"
  },
  {
    "path": "vins_estimator/src/parameters.cpp",
    "content": "#include \"parameters.h\"\n#include <string>\n#include <unistd.h>\n#include <iostream>\n\n#define FILENAMEPATH_MAX 80\nusing namespace std;\n\ndouble INIT_DEPTH;\ndouble MIN_PARALLAX;\ndouble ACC_N, ACC_W;\ndouble GYR_N, GYR_W;\n\nstd::vector<Eigen::Matrix3d> RIC;\nstd::vector<Eigen::Vector3d> TIC;\n\nEigen::Vector3d G{0.0, 0.0, 9.8};\n\ndouble BIAS_ACC_THRESHOLD;\ndouble BIAS_GYR_THRESHOLD;\ndouble SOLVER_TIME;\nint NUM_ITERATIONS;\nint ESTIMATE_EXTRINSIC;\nstd::string EX_CALIB_RESULT_PATH;\nstd::string VINS_RESULT_PATH;\nint LOOP_CLOSURE = 0;\nint MIN_LOOP_NUM;\nstd::string CAM_NAMES_ESTIMATOR;   //add\nstd::string PATTERN_FILE;\nstd::string VOC_FILE;\nstd::string IMAGE_TOPIC;\nstd::string IMU_TOPIC;\nint IMAGE_ROW, IMAGE_COL;\nstd::string VINS_FOLDER_PATH;\nint MAX_KEYFRAME_NUM;\n\n//feature tracker section\nstd::vector<std::string> CAM_NAMES;\nstd::string FISHEYE_MASK;\nint MAX_CNT;\nint MIN_DIST;\nint WINDOW_SIZE_FEATURE_TRACKER;\nint FREQ;\ndouble F_THRESHOLD;\nint SHOW_TRACK;\nint STEREO_TRACK;\nint EQUALIZE;\nint ROW;\nint COL;\nint FOCAL_LENGTH;\nint FISHEYE;\nbool PUB_THIS_FRAME;\nfloat VISUALLOOKATX;\nfloat VISUALLOOKATY;\nfloat VISUALLOOKATZ;\n\nvoid readParameters(const string & config_file)\n{\n\n\n    cv::FileStorage fsSettings(config_file.c_str(), cv::FileStorage::READ);\n    if(!fsSettings.isOpened())\n    {\n        std::cerr << \"ERROR: Wrong path to settings \" << config_file << std::endl;\n    }\n\n\n    VINS_FOLDER_PATH = getcwd(NULL,FILENAMEPATH_MAX);\n //   fsSettings[\"output_path\"] >> VINS_RESULT_PATH;\n\n   \n    fsSettings[\"image_topic\"] >> IMAGE_TOPIC;\n    fsSettings[\"imu_topic\"] >> IMU_TOPIC;\n    fsSettings[\"visualLookAtX\"] >> VISUALLOOKATX;\n    fsSettings[\"visualLookAtY\"] >> VISUALLOOKATY;\n    fsSettings[\"visualLookAtZ\"] >> VISUALLOOKATZ;\n\n    IMAGE_COL = fsSettings[\"image_width\"];\n    IMAGE_ROW = fsSettings[\"image_height\"];\n\n    SOLVER_TIME = fsSettings[\"max_solver_time\"];\n    NUM_ITERATIONS = fsSettings[\"max_num_iterations\"];\n    MIN_PARALLAX = fsSettings[\"keyframe_parallax\"];\n\n    fsSettings[\"output_path\"] >> VINS_RESULT_PATH;\n    VINS_RESULT_PATH = VINS_FOLDER_PATH + VINS_RESULT_PATH;\n    cout << VINS_RESULT_PATH << endl;\n    std::ofstream foutC(VINS_RESULT_PATH, std::ios::out);\n    foutC.close();\n\n    ACC_N = fsSettings[\"acc_n\"];\n    ACC_W = fsSettings[\"acc_w\"];\n    GYR_N = fsSettings[\"gyr_n\"];\n    GYR_W = fsSettings[\"gyr_w\"];\n    G.z() = fsSettings[\"g_norm\"];\n\n    ESTIMATE_EXTRINSIC = fsSettings[\"estimate_extrinsic\"];\n    if (ESTIMATE_EXTRINSIC == 2)\n    {\n      //  ROS_WARN(\"have no prior about extrinsic param, calibrate extrinsic param\");\n\tcout << \"WARN: have no prior about extrinsic param, calibrate extrinsic param\" << endl;\n        RIC.push_back(Eigen::Matrix3d::Identity());\n        TIC.push_back(Eigen::Vector3d::Zero());\n        fsSettings[\"ex_calib_result_path\"] >> EX_CALIB_RESULT_PATH;\n        EX_CALIB_RESULT_PATH = VINS_FOLDER_PATH + EX_CALIB_RESULT_PATH;\n\n    }\n    else \n    {\n        if ( ESTIMATE_EXTRINSIC == 1)\n        {\n          //  ROS_WARN(\" Optimize extrinsic param around initial guess!\");\n\t  cout << \"WARN: Optimize extrinsic param around initial guess!\" << endl;\n            fsSettings[\"ex_calib_result_path\"] >> EX_CALIB_RESULT_PATH;\n            EX_CALIB_RESULT_PATH = VINS_FOLDER_PATH + EX_CALIB_RESULT_PATH;\n        }\n        if (ESTIMATE_EXTRINSIC == 0)\n          //  ROS_WARN(\" fix extrinsic param \");\n\t  cout << \"WARN: fix extrinsic param \"<< endl;\n\n        cv::Mat cv_R, cv_T;\n        fsSettings[\"extrinsicRotation\"] >> cv_R;\n        fsSettings[\"extrinsicTranslation\"] >> cv_T;\n        Eigen::Matrix3d eigen_R;\n        Eigen::Vector3d eigen_T;\n        cv::cv2eigen(cv_R, eigen_R);\n        cv::cv2eigen(cv_T, eigen_T);\n        Eigen::Quaterniond Q(eigen_R);\n        eigen_R = Q.normalized();\n        RIC.push_back(eigen_R);\n        TIC.push_back(eigen_T);\n     //   ROS_INFO_STREAM(\"Extrinsic_R : \" << std::endl << RIC[0]);\n    //    ROS_INFO_STREAM(\"Extrinsic_T : \" << std::endl << TIC[0].transpose());\n\tcout << \"Extrinsic_R : \" << std::endl << RIC[0] << endl;\n\tcout << \"Extrinsic_T : \" << std::endl << TIC[0].transpose() << endl;\n        \n    } \n\n\n\n    LOOP_CLOSURE = fsSettings[\"loop_closure\"];\n    if (LOOP_CLOSURE == 1)\n    {\n        fsSettings[\"voc_file\"] >> VOC_FILE;;\n        fsSettings[\"pattern_file\"] >> PATTERN_FILE;\n        VOC_FILE = VINS_FOLDER_PATH + VOC_FILE;\n        PATTERN_FILE = VINS_FOLDER_PATH + PATTERN_FILE;\n        MIN_LOOP_NUM = fsSettings[\"min_loop_num\"];\n        CAM_NAMES_ESTIMATOR = config_file;   //add\n    }\n\n\n    INIT_DEPTH = 5.0;\n    BIAS_ACC_THRESHOLD = 0.1;\n    BIAS_GYR_THRESHOLD = 0.1;\n    MAX_KEYFRAME_NUM = 1000;\n    \n    // feature tracker\n    fsSettings[\"image_topic\"] >> IMAGE_TOPIC;\n    fsSettings[\"imu_topic\"] >> IMU_TOPIC;\n    MAX_CNT = fsSettings[\"max_cnt\"];\n    MIN_DIST = fsSettings[\"min_dist\"];\n    ROW = fsSettings[\"image_height\"];\n    COL = fsSettings[\"image_width\"];\n   FREQ = fsSettings[\"freq\"];\n    F_THRESHOLD = fsSettings[\"F_threshold\"];\n    SHOW_TRACK = fsSettings[\"show_track\"];\n   EQUALIZE = fsSettings[\"equalize\"];\n    FISHEYE = fsSettings[\"fisheye\"];\n    if (FISHEYE == 1)\n        FISHEYE_MASK = VINS_FOLDER_PATH + \"/src/config/fisheye_mask.jpg\";\n    CAM_NAMES.push_back(config_file);\n\t//ROS_INFO_STREAM(\"CAM_NAMES\" << CAM_NAMES.front());\n    WINDOW_SIZE_FEATURE_TRACKER = 20;\n    STEREO_TRACK = false;\n    FOCAL_LENGTH = 460;\n    PUB_THIS_FRAME = false; \n\n    MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;\n    if (FREQ == 0)\n        FREQ = 100;\n   \n    fsSettings.release();\n}\n"
  },
  {
    "path": "vins_estimator/src/parameters.h",
    "content": "#pragma once\n\n//#include <ros/ros.h>\n#include <vector>\n#include <eigen3/Eigen/Dense>\n#include \"utility/utility.h\"\n#include <opencv2/opencv.hpp>\n#include <opencv2/core/eigen.hpp>\n#include <fstream>\n#include <string>\nusing namespace std;\n\n//const double FOCAL_LENGTH = 460.0;\nextern int FOCAL_LENGTH; //add\nconst int WINDOW_SIZE = 10;\nconst int NUM_OF_CAM = 1;\nconst int NUM_OF_F = 1000;\nconst double LOOP_INFO_VALUE = 50.0;\n//#define DEPTH_PRIOR\n//#define GT\n#define UNIT_SPHERE_ERROR\n\nextern float VISUALLOOKATX;\nextern float VISUALLOOKATY;\nextern float VISUALLOOKATZ;\nextern double INIT_DEPTH;\nextern double MIN_PARALLAX;\nextern int ESTIMATE_EXTRINSIC;\n\nextern double ACC_N, ACC_W;\nextern double GYR_N, GYR_W;\n\nextern std::vector<Eigen::Matrix3d> RIC;\nextern std::vector<Eigen::Vector3d> TIC;\nextern Eigen::Vector3d G;\n\nextern double BIAS_ACC_THRESHOLD;\nextern double BIAS_GYR_THRESHOLD;\nextern double SOLVER_TIME;\nextern int NUM_ITERATIONS;\nextern std::string EX_CALIB_RESULT_PATH;\nextern std::string VINS_RESULT_PATH;\nextern std::string VINS_FOLDER_PATH;\nextern int IMAGE_ROW, IMAGE_COL;\n\nextern int LOOP_CLOSURE;\nextern int MIN_LOOP_NUM;\nextern int MAX_KEYFRAME_NUM;\nextern std::string PATTERN_FILE;\nextern std::string VOC_FILE;\nextern std::string CAM_NAMES_ESTIMATOR;\nextern std::string IMAGE_TOPIC;\nextern std::string IMU_TOPIC;\n\n//feature tracker section\nextern int ROW;\nextern int COL;\nextern int FOCAL_LENGTH;\n//const int NUM_OF_CAM = 1;\n\n\nextern std::string IMAGE_TOPIC;\nextern std::string IMU_TOPIC;\nextern std::string FISHEYE_MASK;\nextern std::vector<std::string> CAM_NAMES;\nextern int MAX_CNT;\nextern int MIN_DIST;\nextern  int WINDOW_SIZE_FEATURE_TRACKER;\nextern int FREQ;\nextern double F_THRESHOLD;\nextern int SHOW_TRACK;\nextern int STEREO_TRACK;\nextern int EQUALIZE;\nextern int FISHEYE;\nextern bool PUB_THIS_FRAME;\n\nvoid readParameters(const string & );\n\nenum SIZE_PARAMETERIZATION\n{\n    SIZE_POSE = 7,\n    SIZE_SPEEDBIAS = 9,\n    SIZE_FEATURE = 1\n};\n\nenum StateOrder\n{\n    O_P = 0,\n    O_R = 3,\n    O_V = 6,\n    O_BA = 9,\n    O_BG = 12\n};\n\nenum NoiseOrder\n{\n    O_AN = 0,\n    O_GN = 3,\n    O_AW = 6,\n    O_GW = 9\n};\n"
  },
  {
    "path": "vins_estimator/src/utility/CameraPoseVisualization.cpp",
    "content": "#include \"CameraPoseVisualization.h\"\n\nconst Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0,  0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0,  0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);\nconst Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);\n\nvoid Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) {\n    p.x = v.x();\n    p.y = v.y();\n    p.z = v.z();\n}\n\nCameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a)\n    : m_marker_ns(\"CameraPoseVisualization\"), m_scale(0.2), m_line_width(0.01) {\n    m_image_boundary_color.r = r;\n    m_image_boundary_color.g = g;\n    m_image_boundary_color.b = b;\n    m_image_boundary_color.a = a;\n    m_optical_center_connector_color.r = r;\n    m_optical_center_connector_color.g = g;\n    m_optical_center_connector_color.b = b;\n    m_optical_center_connector_color.a = a;\n}\n\nvoid CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) {\n    m_image_boundary_color.r = r;\n    m_image_boundary_color.g = g;\n    m_image_boundary_color.b = b;\n    m_image_boundary_color.a = a;\n}\n\nvoid CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {\n    m_optical_center_connector_color.r = r;\n    m_optical_center_connector_color.g = g;\n    m_optical_center_connector_color.b = b;\n    m_optical_center_connector_color.a = a;\n}\n\nvoid CameraPoseVisualization::setScale(double s) {\n    m_scale = s;\n}\nvoid CameraPoseVisualization::setLineWidth(double width) {\n    m_line_width = width;\n}\nvoid CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){\n    visualization_msgs::Marker marker;\n\n    marker.ns = m_marker_ns;\n    marker.id = m_markers.size() + 1;\n    marker.type = visualization_msgs::Marker::LINE_LIST;\n    marker.action = visualization_msgs::Marker::ADD;\n    marker.scale.x = 0.005;\n\n    marker.color.g = 1.0f;\n    marker.color.a = 1.0;\n\n    geometry_msgs::Point point0, point1;\n\n    Eigen2Point(p0, point0);\n    Eigen2Point(p1, point1);\n\n    marker.points.push_back(point0);\n    marker.points.push_back(point1);\n\n    m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){\n    visualization_msgs::Marker marker;\n\n    marker.ns = m_marker_ns;\n    marker.id = m_markers.size() + 1;\n    marker.type = visualization_msgs::Marker::LINE_LIST;\n    marker.action = visualization_msgs::Marker::ADD;\n    marker.scale.x = 0.02;\n    //marker.scale.x = 0.3;\n\n    marker.color.r = 1.0f;\n    marker.color.b = 1.0f;\n    marker.color.a = 1.0;\n\n    geometry_msgs::Point point0, point1;\n\n    Eigen2Point(p0, point0);\n    Eigen2Point(p1, point1);\n\n    marker.points.push_back(point0);\n    marker.points.push_back(point1);\n\n    m_markers.push_back(marker);\n}\n\n\nvoid CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {\n    visualization_msgs::Marker marker;\n\n    marker.ns = m_marker_ns;\n    marker.id = m_markers.size() + 1;\n    marker.type = visualization_msgs::Marker::LINE_STRIP;\n    marker.action = visualization_msgs::Marker::ADD;\n    marker.scale.x = m_line_width;\n\n    marker.pose.position.x = 0.0;\n    marker.pose.position.y = 0.0;\n    marker.pose.position.z = 0.0;\n    marker.pose.orientation.w = 1.0;\n    marker.pose.orientation.x = 0.0;\n    marker.pose.orientation.y = 0.0;\n    marker.pose.orientation.z = 0.0;\n\n\n    geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;\n\n    Eigen2Point(q * (m_scale *imlt) + p, pt_lt);\n    Eigen2Point(q * (m_scale *imlb) + p, pt_lb);\n    Eigen2Point(q * (m_scale *imrt) + p, pt_rt);\n    Eigen2Point(q * (m_scale *imrb) + p, pt_rb);\n    Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0);\n    Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1);\n    Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2);\n    Eigen2Point(q * (m_scale *oc  ) + p, pt_oc);\n\n    // image boundaries\n    marker.points.push_back(pt_lt);\n    marker.points.push_back(pt_lb);\n    marker.colors.push_back(m_image_boundary_color);\n    marker.colors.push_back(m_image_boundary_color);\n\n    marker.points.push_back(pt_lb);\n    marker.points.push_back(pt_rb);\n    marker.colors.push_back(m_image_boundary_color);\n    marker.colors.push_back(m_image_boundary_color);\n\n    marker.points.push_back(pt_rb);\n    marker.points.push_back(pt_rt);\n    marker.colors.push_back(m_image_boundary_color);\n    marker.colors.push_back(m_image_boundary_color);\n\n    marker.points.push_back(pt_rt);\n    marker.points.push_back(pt_lt);\n    marker.colors.push_back(m_image_boundary_color);\n    marker.colors.push_back(m_image_boundary_color);\n\n    // top-left indicator\n    marker.points.push_back(pt_lt0);\n    marker.points.push_back(pt_lt1);\n    marker.colors.push_back(m_image_boundary_color);\n    marker.colors.push_back(m_image_boundary_color);\n\n    marker.points.push_back(pt_lt1);\n    marker.points.push_back(pt_lt2);\n    marker.colors.push_back(m_image_boundary_color);\n    marker.colors.push_back(m_image_boundary_color);\n\n    // optical center connector\n    marker.points.push_back(pt_lt);\n    marker.points.push_back(pt_oc);\n    marker.colors.push_back(m_optical_center_connector_color);\n    marker.colors.push_back(m_optical_center_connector_color);\n\n\n    marker.points.push_back(pt_lb);\n    marker.points.push_back(pt_oc);\n    marker.colors.push_back(m_optical_center_connector_color);\n    marker.colors.push_back(m_optical_center_connector_color);\n\n    marker.points.push_back(pt_rt);\n    marker.points.push_back(pt_oc);\n    marker.colors.push_back(m_optical_center_connector_color);\n    marker.colors.push_back(m_optical_center_connector_color);\n\n    marker.points.push_back(pt_rb);\n    marker.points.push_back(pt_oc);\n    marker.colors.push_back(m_optical_center_connector_color);\n    marker.colors.push_back(m_optical_center_connector_color);\n\n    m_markers.push_back(marker);\n}\n\nvoid CameraPoseVisualization::reset() {\n\tm_markers.clear();\n}\n\nvoid CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) {\n\tvisualization_msgs::MarkerArray markerArray_msg;\n\t\n\tfor(auto& marker : m_markers) {\n\t\tmarker.header = header;\n\t\tmarkerArray_msg.markers.push_back(marker);\n\t}\n\n\tpub.publish(markerArray_msg);\n}"
  },
  {
    "path": "vins_estimator/src/utility/CameraPoseVisualization.h",
    "content": "#pragma once\n\n//#include <ros/ros.h>\n#include <std_msgs/ColorRGBA.h>\n#include <visualization_msgs/Marker.h>\n#include <visualization_msgs/MarkerArray.h>\n#include <Eigen/Dense>\n#include <Eigen/Geometry>\n\nclass CameraPoseVisualization {\npublic:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\tstd::string m_marker_ns;\n\n\tCameraPoseVisualization(float r, float g, float b, float a);\n\t\n\tvoid setImageBoundaryColor(float r, float g, float b, float a=1.0);\n\tvoid setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0);\n\tvoid setScale(double s);\n\tvoid setLineWidth(double width);\n\n\tvoid add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);\n\tvoid reset();\n\n\tvoid publish_by(ros::Publisher& pub, const std_msgs::Header& header);\n\tvoid add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);\n\tvoid add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);\nprivate:\n\tstd::vector<visualization_msgs::Marker> m_markers;\n\tstd_msgs::ColorRGBA m_image_boundary_color;\n\tstd_msgs::ColorRGBA m_optical_center_connector_color;\n\tdouble m_scale;\n\tdouble m_line_width;\n\n\tstatic const Eigen::Vector3d imlt;\n\tstatic const Eigen::Vector3d imlb;\n\tstatic const Eigen::Vector3d imrt;\n\tstatic const Eigen::Vector3d imrb;\n\tstatic const Eigen::Vector3d oc  ;\n\tstatic const Eigen::Vector3d lt0 ;\n\tstatic const Eigen::Vector3d lt1 ;\n\tstatic const Eigen::Vector3d lt2 ;\n};\n"
  },
  {
    "path": "vins_estimator/src/utility/tic_toc.h",
    "content": "#pragma once\n\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n\nclass TicToc\n{\n  public:\n    TicToc()\n    {\n        tic();\n    }\n\n    void tic()\n    {\n        start = std::chrono::system_clock::now();\n    }\n\n    double toc()\n    {\n        end = std::chrono::system_clock::now();\n        std::chrono::duration<double> elapsed_seconds = end - start;\n        return elapsed_seconds.count() * 1000;\n    }\n\n  private:\n    std::chrono::time_point<std::chrono::system_clock> start, end;\n};\n"
  },
  {
    "path": "vins_estimator/src/utility/utility.cpp",
    "content": "#include \"utility.h\"\n\nEigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)\n{\n    Eigen::Matrix3d R0;\n    Eigen::Vector3d ng1 = g.normalized();\n    Eigen::Vector3d ng2{0, 0, 1.0};\n    R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();\n    double yaw = Utility::R2ypr(R0).x();\n    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;\n    // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;\n    return R0;\n}\n"
  },
  {
    "path": "vins_estimator/src/utility/utility.h",
    "content": "#pragma once\n\n#include <cmath>\n#include <cassert>\n#include <cstring>\n#include <eigen3/Eigen/Dense>\n\nclass Utility\n{\n  public:\n\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n    template <typename Derived>\n    static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)\n    {\n        typedef typename Derived::Scalar Scalar_t;\n\n        Eigen::Quaternion<Scalar_t> dq;\n        Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;\n        half_theta /= static_cast<Scalar_t>(2.0);\n        dq.w() = static_cast<Scalar_t>(1.0);\n        dq.x() = half_theta.x();\n        dq.y() = half_theta.y();\n        dq.z() = half_theta.z();\n        return dq;\n    }\n\n    template <typename Derived>\n    static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)\n    {\n        Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;\n        ans << typename Derived::Scalar(0), -q(2), q(1),\n            q(2), typename Derived::Scalar(0), -q(0),\n            -q(1), q(0), typename Derived::Scalar(0);\n        return ans;\n    }\n\n    template <typename Derived>\n    static Eigen::Quaternion<typename Derived::Scalar> positify(const Eigen::QuaternionBase<Derived> &q)\n    {\n        //printf(\"a: %f %f %f %f\", q.w(), q.x(), q.y(), q.z());\n        //Eigen::Quaternion<typename Derived::Scalar> p(-q.w(), -q.x(), -q.y(), -q.z());\n        //printf(\"b: %f %f %f %f\", p.w(), p.x(), p.y(), p.z());\n        //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion<typename Derived::Scalar>(-q.w(), -q.x(), -q.y(), -q.z());\n        return q;\n    }\n\n    template <typename Derived>\n    static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qleft(const Eigen::QuaternionBase<Derived> &q)\n    {\n        Eigen::Quaternion<typename Derived::Scalar> qq = positify(q);\n        Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;\n        ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose();\n        ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() + skewSymmetric(qq.vec());\n        return ans;\n    }\n\n    template <typename Derived>\n    static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qright(const Eigen::QuaternionBase<Derived> &p)\n    {\n        Eigen::Quaternion<typename Derived::Scalar> pp = positify(p);\n        Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;\n        ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose();\n        ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() - skewSymmetric(pp.vec());\n        return ans;\n    }\n\n    static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)\n    {\n        Eigen::Vector3d n = R.col(0);\n        Eigen::Vector3d o = R.col(1);\n        Eigen::Vector3d a = R.col(2);\n\n        Eigen::Vector3d ypr(3);\n        double y = atan2(n(1), n(0));\n        double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));\n        double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));\n        ypr(0) = y;\n        ypr(1) = p;\n        ypr(2) = r;\n\n        return ypr / M_PI * 180.0;\n    }\n\n    template <typename Derived>\n    static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived> &ypr)\n    {\n        typedef typename Derived::Scalar Scalar_t;\n\n        Scalar_t y = ypr(0) / 180.0 * M_PI;\n        Scalar_t p = ypr(1) / 180.0 * M_PI;\n        Scalar_t r = ypr(2) / 180.0 * M_PI;\n\n        Eigen::Matrix<Scalar_t, 3, 3> Rz;\n        Rz << cos(y), -sin(y), 0,\n            sin(y), cos(y), 0,\n            0, 0, 1;\n\n        Eigen::Matrix<Scalar_t, 3, 3> Ry;\n        Ry << cos(p), 0., sin(p),\n            0., 1., 0.,\n            -sin(p), 0., cos(p);\n\n        Eigen::Matrix<Scalar_t, 3, 3> Rx;\n        Rx << 1., 0., 0.,\n            0., cos(r), -sin(r),\n            0., sin(r), cos(r);\n\n        return Rz * Ry * Rx;\n    }\n\n    static Eigen::Matrix3d g2R(const Eigen::Vector3d &g);\n\n    template <size_t N>\n    struct uint_\n    {\n    };\n\n    template <size_t N, typename Lambda, typename IterT>\n    void unroller(const Lambda &f, const IterT &iter, uint_<N>)\n    {\n        unroller(f, iter, uint_<N - 1>());\n        f(iter + N);\n    }\n\n    template <typename Lambda, typename IterT>\n    void unroller(const Lambda &f, const IterT &iter, uint_<0>)\n    {\n        f(iter);\n    }\n\n    template <typename T>\n    static T normalizeAngle(const T& angle_degrees) {\n      T two_pi(2.0 * 180);\n      if (angle_degrees > 0)\n      return angle_degrees -\n          two_pi * std::floor((angle_degrees + T(180)) / two_pi);\n      else\n        return angle_degrees +\n            two_pi * std::floor((-angle_degrees + T(180)) / two_pi);\n    };\n};\n"
  },
  {
    "path": "vins_estimator/src/utility/visualization.cpp",
    "content": "#include \"visualization.h\"\n\nusing namespace ros;\nusing namespace Eigen;\n#if 0\nros::Publisher pub_odometry, pub_latest_odometry;\nros::Publisher pub_path, pub_loop_path;\nros::Publisher pub_point_cloud, pub_margin_cloud;\nros::Publisher pub_key_poses;\n\nros::Publisher pub_camera_pose;\nros::Publisher pub_camera_pose_visual, pub_pose_graph;\nnav_msgs::Path path, loop_path;\nCameraPoseVisualization cameraposevisual(0, 0, 1, 1);\nCameraPoseVisualization keyframebasevisual(0.0, 0.0, 1.0, 1.0);\nstatic double sum_of_path = 0;\nstatic Vector3d last_path(0.0, 0.0, 0.0);\n\nvoid registerPub(ros::NodeHandle &n)\n{\n    pub_latest_odometry = n.advertise<nav_msgs::Odometry>(\"imu_propagate\", 1000);\n    pub_path = n.advertise<nav_msgs::Path>(\"path_no_loop\", 1000);\n    pub_loop_path = n.advertise<nav_msgs::Path>(\"path\", 1000);\n    pub_odometry = n.advertise<nav_msgs::Odometry>(\"odometry\", 1000);\n    pub_point_cloud = n.advertise<sensor_msgs::PointCloud>(\"point_cloud\", 1000);\n    pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>(\"history_cloud\", 1000);\n    pub_key_poses = n.advertise<visualization_msgs::Marker>(\"key_poses\", 1000);\n    pub_camera_pose = n.advertise<geometry_msgs::PoseStamped>(\"camera_pose\", 1000);\n    pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>(\"camera_pose_visual\", 1000);\n    pub_pose_graph = n.advertise<visualization_msgs::MarkerArray>(\"pose_graph\", 1000);\n\n    cameraposevisual.setScale(0.2);\n    cameraposevisual.setLineWidth(0.01);\n    keyframebasevisual.setScale(0.1);\n    keyframebasevisual.setLineWidth(0.01);\n}\n\nvoid pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header)\n{\n    Eigen::Quaterniond quadrotor_Q = Q ;\n\n    nav_msgs::Odometry odometry;\n    odometry.header = header;\n    odometry.header.frame_id = \"world\";\n    odometry.pose.pose.position.x = P.x();\n    odometry.pose.pose.position.y = P.y();\n    odometry.pose.pose.position.z = P.z();\n    odometry.pose.pose.orientation.x = quadrotor_Q.x();\n    odometry.pose.pose.orientation.y = quadrotor_Q.y();\n    odometry.pose.pose.orientation.z = quadrotor_Q.z();\n    odometry.pose.pose.orientation.w = quadrotor_Q.w();\n    odometry.twist.twist.linear.x = V.x();\n    odometry.twist.twist.linear.y = V.y();\n    odometry.twist.twist.linear.z = V.z();\n    pub_latest_odometry.publish(odometry);\n}\n\nvoid printStatistics(const Estimator &estimator, double t)\n{\n    if (estimator.solver_flag != Estimator::SolverFlag::NON_LINEAR)\n        return;\n    ROS_INFO_STREAM(\"position: \" << estimator.Ps[WINDOW_SIZE].transpose());\n    ROS_DEBUG_STREAM(\"orientation: \" << estimator.Vs[WINDOW_SIZE].transpose());\n    for (int i = 0; i < NUM_OF_CAM; i++)\n    {\n        //ROS_DEBUG(\"calibration result for camera %d\", i);\n        ROS_DEBUG_STREAM(\"extirnsic tic: \" << estimator.tic[i].transpose());\n        ROS_DEBUG_STREAM(\"extrinsic ric: \" << Utility::R2ypr(estimator.ric[i]).transpose());\n        if (ESTIMATE_EXTRINSIC)\n        {\n            cv::FileStorage fs(EX_CALIB_RESULT_PATH, cv::FileStorage::WRITE);\n            Eigen::Matrix3d eigen_R;\n            Eigen::Vector3d eigen_T;\n            eigen_R = estimator.ric[i];\n            eigen_T = estimator.tic[i];\n            cv::Mat cv_R, cv_T;\n            cv::eigen2cv(eigen_R, cv_R);\n            cv::eigen2cv(eigen_T, cv_T);\n            fs << \"extrinsicRotation\" << cv_R << \"extrinsicTranslation\" << cv_T;\n            fs.release();\n        }\n    }\n\n    static double sum_of_time = 0;\n    static int sum_of_calculation = 0;\n    sum_of_time += t;\n    sum_of_calculation++;\n    ROS_DEBUG(\"vo solver costs: %f ms\", t);\n    ROS_DEBUG(\"average of time %f ms\", sum_of_time / sum_of_calculation);\n\n    sum_of_path += (estimator.Ps[WINDOW_SIZE] - last_path).norm();\n    last_path = estimator.Ps[WINDOW_SIZE];\n    ROS_DEBUG(\"sum of path %f\", sum_of_path);\n}\n\nvoid pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                Eigen::Matrix3d loop_correct_r)\n{\n    if (estimator.key_poses.size() == 0)\n        return;\n    visualization_msgs::Marker key_poses;\n    key_poses.header = header;\n    key_poses.header.frame_id = \"world\";\n    key_poses.ns = \"key_poses\";\n    key_poses.type = visualization_msgs::Marker::SPHERE_LIST;\n    key_poses.action = visualization_msgs::Marker::ADD;\n    key_poses.pose.orientation.w = 1.0;\n    key_poses.lifetime = ros::Duration();\n\n    //static int key_poses_id = 0;\n    key_poses.id = 0; //key_poses_id++;\n    key_poses.scale.x = 0.05;\n    key_poses.scale.y = 0.05;\n    key_poses.scale.z = 0.05;\n    key_poses.color.r = 1.0;\n    key_poses.color.a = 1.0;\n\n    for (int i = 0; i <= WINDOW_SIZE; i++)\n    {\n        geometry_msgs::Point pose_marker;\n        Vector3d correct_pose;\n        correct_pose = loop_correct_r * estimator.key_poses[i] + loop_correct_t;\n        pose_marker.x = correct_pose.x();\n        pose_marker.y = correct_pose.y();\n        pose_marker.z = correct_pose.z();\n        key_poses.points.push_back(pose_marker);\n    }\n    pub_key_poses.publish(key_poses);\n}\n\nvoid pubCameraPose(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                   Eigen::Matrix3d loop_correct_r)\n{\n    int idx2 = WINDOW_SIZE - 1;\n    if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n    {\n        int i = idx2;\n        geometry_msgs::PoseStamped camera_pose;\n        camera_pose.header = header;\n        camera_pose.header.frame_id = std::to_string(estimator.Headers[i].stamp.toNSec());\n        Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0];\n        Quaterniond R = Quaterniond(estimator.Rs[i] * estimator.ric[0]);\n        P = (loop_correct_r * estimator.Ps[i] + loop_correct_t) + (loop_correct_r * estimator.Rs[i]) * estimator.tic[0];\n        R = Quaterniond((loop_correct_r * estimator.Rs[i]) * estimator.ric[0]);\n        camera_pose.pose.position.x = P.x();\n        camera_pose.pose.position.y = P.y();\n        camera_pose.pose.position.z = P.z();\n        camera_pose.pose.orientation.w = R.w();\n        camera_pose.pose.orientation.x = R.x();\n        camera_pose.pose.orientation.y = R.y();\n        camera_pose.pose.orientation.z = R.z();\n\n        pub_camera_pose.publish(camera_pose);\n\n        cameraposevisual.reset();\n        cameraposevisual.add_pose(P, R);\n        camera_pose.header.frame_id = \"world\";\n        cameraposevisual.publish_by(pub_camera_pose_visual, camera_pose.header);\n    }\n}\n\n\nvoid pubPointCloud(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                   Eigen::Matrix3d loop_correct_r)\n{\n    sensor_msgs::PointCloud point_cloud, loop_point_cloud;\n    point_cloud.header = header;\n    loop_point_cloud.header = header;\n\n    for (auto &it_per_id : estimator.f_manager.feature)\n    {\n        int used_num;\n        used_num = it_per_id.feature_per_frame.size();\n        if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n        if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)\n            continue;\n        int imu_i = it_per_id.start_frame;\n        Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;\n        Vector3d w_pts_i = loop_correct_r * estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0])\n                              + loop_correct_r * estimator.Ps[imu_i] + loop_correct_t;\n\n        geometry_msgs::Point32 p;\n        p.x = w_pts_i(0);\n        p.y = w_pts_i(1);\n        p.z = w_pts_i(2);\n        point_cloud.points.push_back(p);\n    }\n    pub_point_cloud.publish(point_cloud);\n\n\n    // pub margined potin\n    sensor_msgs::PointCloud margin_cloud, loop_margin_cloud;\n    margin_cloud.header = header;\n    loop_margin_cloud.header = header;\n\n    for (auto &it_per_id : estimator.f_manager.feature)\n    { \n        int used_num;\n        used_num = it_per_id.feature_per_frame.size();\n        if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))\n            continue;\n        //if (it_per_id->start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id->solve_flag != 1)\n        //        continue;\n\n        if (it_per_id.start_frame == 0 && it_per_id.feature_per_frame.size() <= 2 \n            && it_per_id.solve_flag == 1 )\n        {\n            int imu_i = it_per_id.start_frame;\n            Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;\n            Vector3d w_pts_i = loop_correct_r * estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0])\n                             + loop_correct_r * estimator.Ps[imu_i] + loop_correct_t;\n\n            geometry_msgs::Point32 p;\n            p.x = w_pts_i(0);\n            p.y = w_pts_i(1);\n            p.z = w_pts_i(2);\n            margin_cloud.points.push_back(p);\n        }\n    }\n    pub_margin_cloud.publish(margin_cloud);\n}\n\nvoid pubPoseGraph(CameraPoseVisualization* posegraph, const std_msgs::Header &header)\n{\n    posegraph->publish_by(pub_pose_graph, header);\n    \n}\n\nvoid updateLoopPath(nav_msgs::Path _loop_path)\n{\n    loop_path = _loop_path;\n}\n\nvoid pubTF(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                   Eigen::Matrix3d loop_correct_r)\n{\n    if( estimator.solver_flag != Estimator::SolverFlag::NON_LINEAR)\n        return;\n    static tf::TransformBroadcaster br;\n    tf::Transform transform;\n    tf::Quaternion q;\n    // body frame\n    Vector3d correct_t;\n    Quaterniond correct_q;\n    correct_t = loop_correct_r * estimator.Ps[WINDOW_SIZE] + loop_correct_t;\n    correct_q = loop_correct_r * estimator.Rs[WINDOW_SIZE];\n\n    transform.setOrigin(tf::Vector3(correct_t(0),\n                                    correct_t(1),\n                                    correct_t(2)));\n    q.setW(correct_q.w());\n    q.setX(correct_q.x());\n    q.setY(correct_q.y());\n    q.setZ(correct_q.z());\n    transform.setRotation(q);\n    br.sendTransform(tf::StampedTransform(transform, header.stamp, \"world\", \"body\"));\n\n    // camera frame\n    transform.setOrigin(tf::Vector3(estimator.tic[0].x(),\n                                    estimator.tic[0].y(),\n                                    estimator.tic[0].z()));\n    q.setW(Quaterniond(estimator.ric[0]).w());\n    q.setX(Quaterniond(estimator.ric[0]).x());\n    q.setY(Quaterniond(estimator.ric[0]).y());\n    q.setZ(Quaterniond(estimator.ric[0]).z());\n    transform.setRotation(q);\n    br.sendTransform(tf::StampedTransform(transform, header.stamp, \"body\", \"camera\"));\n}\n#endif\nvoid pubOdometry(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                Eigen::Matrix3d loop_correct_r)\n{\n    if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)\n    {\n  /*      nav_msgs::Odometry odometry;\n        odometry.header = header;\n        odometry.header.frame_id = \"world\";\n        odometry.child_frame_id = \"world\";\n        odometry.pose.pose.position.x = estimator.Ps[WINDOW_SIZE].x();\n        odometry.pose.pose.position.y = estimator.Ps[WINDOW_SIZE].y();\n        odometry.pose.pose.position.z = estimator.Ps[WINDOW_SIZE].z();\n        odometry.pose.pose.orientation.x = Quaterniond(estimator.Rs[WINDOW_SIZE]).x();\n        odometry.pose.pose.orientation.y = Quaterniond(estimator.Rs[WINDOW_SIZE]).y();\n        odometry.pose.pose.orientation.z = Quaterniond(estimator.Rs[WINDOW_SIZE]).z();\n        odometry.pose.pose.orientation.w = Quaterniond(estimator.Rs[WINDOW_SIZE]).w();\n        \n        geometry_msgs::PoseStamped pose_stamped;\n        pose_stamped.header = header;\n        pose_stamped.header.frame_id = \"world\";\n        pose_stamped.pose = odometry.pose.pose;\n        path.header = header;\n        path.header.frame_id = \"world\";\n        path.poses.push_back(pose_stamped);\n        pub_path.publish(path);*/\n\n        Vector3d correct_t;\n        Vector3d correct_v;\n        Quaterniond correct_q;\n        correct_t = loop_correct_r * estimator.Ps[WINDOW_SIZE] + loop_correct_t;\n        correct_q = loop_correct_r * estimator.Rs[WINDOW_SIZE];\n        correct_v = loop_correct_r * estimator.Vs[WINDOW_SIZE];\n /*       odometry.pose.pose.position.x = correct_t.x();\n        odometry.pose.pose.position.y = correct_t.y();\n        odometry.pose.pose.position.z = correct_t.z();\n        odometry.pose.pose.orientation.x = correct_q.x();\n        odometry.pose.pose.orientation.y = correct_q.y();\n        odometry.pose.pose.orientation.z = correct_q.z();\n        odometry.pose.pose.orientation.w = correct_q.w();\n        odometry.twist.twist.linear.x = correct_v(0);\n        odometry.twist.twist.linear.y = correct_v(1);\n        odometry.twist.twist.linear.z = correct_v(2);\n        pub_odometry.publish(odometry);\n\n        pose_stamped.pose = odometry.pose.pose;\n        loop_path.header = header;\n        loop_path.header.frame_id = \"world\";\n        loop_path.poses.push_back(pose_stamped);\n        pub_loop_path.publish(loop_path);\n*/\n        // write result to file\n        ofstream foutC(VINS_RESULT_PATH, ios::app);\n        foutC.setf(ios::fixed, ios::floatfield);\n        foutC.precision(0);\n        foutC << header.stamp.toSec() * 1e9 << \",\";\n        foutC.precision(5);\n        foutC << correct_t.x() << \",\"\n              << correct_t.y() << \",\"\n              << correct_t.z() << \",\"\n              << correct_q.w() << \",\"\n              << correct_q.x() << \",\"\n              << correct_q.y() << \",\"\n              << correct_q.z() << \",\"\n              << correct_v(0) << \",\"\n              << correct_v(1) << \",\"\n              << correct_v(2) << \",\" << endl;\n        foutC.close();\n    }\n}\n"
  },
  {
    "path": "vins_estimator/src/utility/visualization.h",
    "content": "#pragma once\n\n#include <eigen3/Eigen/Dense>\n#include \"../estimator.h\"\n#include \"../parameters.h\"\n#include \"../../../include/Vector3.h\"\n#include \"../../../include/Quaternion.h\"\n#include <fstream>\n\n\nvoid pubOdometry(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                Eigen::Matrix3d loop_correct_r);\n//#include <ros/ros.h>\n#if 0\n#include <std_msgs/Header.h>\n#include <std_msgs/Float32.h>\n#include <sensor_msgs/Imu.h>\n#include <sensor_msgs/PointCloud.h>\n#include <sensor_msgs/Image.h>\n#include <sensor_msgs/image_encodings.h>\n#include <nav_msgs/Path.h>\n#include <nav_msgs/Odometry.h>\n#include <geometry_msgs/PointStamped.h>\n#include <visualization_msgs/Marker.h>\n#include <tf/transform_broadcaster.h>\n#include \"CameraPoseVisualization.h\"\n#include <eigen3/Eigen/Dense>\n#include \"../estimator.h\"\n#include \"../parameters.h\"\n#include <fstream>\n\nextern ros::Publisher pub_odometry;\nextern ros::Publisher pub_path, pub_pose;\nextern ros::Publisher pub_cloud, pub_map;\nextern ros::Publisher pub_key_poses;\n\nextern ros::Publisher pub_ref_pose, pub_cur_pose;\n\nextern ros::Publisher pub_key;\n\nextern nav_msgs::Path path;\n\nextern ros::Publisher pub_pose_graph;\n\nextern int IMAGE_ROW, IMAGE_COL;\n\nvoid registerPub(ros::NodeHandle &n);\n\nvoid pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header);\n\nvoid printStatistics(const Estimator &estimator, double t);\n\nvoid pubOdometry(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                Eigen::Matrix3d loop_correct_r);\n\nvoid pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header);\n\nvoid pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n\t\t\t\tEigen::Matrix3d loop_correct_r);\n\nvoid pubCameraPose(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                   Eigen::Matrix3d loop_correct_r);\n\nvoid pubPointCloud(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                   Eigen::Matrix3d loop_correct_r);\n\nvoid pubPoseGraph(CameraPoseVisualization* posegraph, const std_msgs::Header &header);\n\nvoid updateLoopPath(nav_msgs::Path _loop_path);\n\nvoid pubTF(const Estimator &estimator, const std_msgs::Header &header, Eigen::Vector3d loop_correct_t,\n                   Eigen::Matrix3d loop_correct_r);\n#endif\n"
  }
]