Repository: heguixiang/Remove_ROS_VINS Branch: master Commit: 9b654de20f18 Files: 156 Total size: 1.1 MB Directory structure: gitextract__17qxa0t/ ├── .gitignore ├── .kdev4/ │ └── src.kdev4 ├── LICENCE ├── README.md ├── ar_demo/ │ ├── CMakeLists.txt │ ├── CMakeLists.txt.back │ ├── launch/ │ │ ├── 3dm_bag.launch │ │ ├── ar_A3.launch │ │ └── ar_rviz.launch │ ├── package.xml │ ├── package.xml.back │ └── src/ │ └── ar_demo_node.cpp ├── benchmark_publisher/ │ ├── CMakeLists.txt │ ├── CMakeLists.txt.back │ ├── launch/ │ │ └── publish.launch │ ├── package.xml │ ├── package.xml.back │ └── src/ │ └── benchmark_publisher_node.cpp ├── camera_model/ │ ├── CMakeLists.txt │ ├── include/ │ │ └── camodocal/ │ │ ├── calib/ │ │ │ └── CameraCalibration.h │ │ ├── camera_models/ │ │ │ ├── Camera.h │ │ │ ├── CameraFactory.h │ │ │ ├── CataCamera.h │ │ │ ├── CostFunctionFactory.h │ │ │ ├── EquidistantCamera.h │ │ │ ├── PinholeCamera.h │ │ │ └── ScaramuzzaCamera.h │ │ ├── chessboard/ │ │ │ ├── Chessboard.h │ │ │ ├── ChessboardCorner.h │ │ │ ├── ChessboardQuad.h │ │ │ └── Spline.h │ │ ├── gpl/ │ │ │ ├── EigenQuaternionParameterization.h │ │ │ ├── EigenUtils.h │ │ │ └── gpl.h │ │ └── sparse_graph/ │ │ └── Transform.h │ ├── instruction │ ├── package.xml │ ├── readme.md │ └── src/ │ ├── calib/ │ │ └── CameraCalibration.cc │ ├── camera_models/ │ │ ├── Camera.cc │ │ ├── CameraFactory.cc │ │ ├── CataCamera.cc │ │ ├── CostFunctionFactory.cc │ │ ├── EquidistantCamera.cc │ │ ├── PinholeCamera.cc │ │ └── ScaramuzzaCamera.cc │ ├── chessboard/ │ │ └── Chessboard.cc │ ├── gpl/ │ │ ├── EigenQuaternionParameterization.cc │ │ └── gpl.cc │ ├── intrinsic_calib.cc │ └── sparse_graph/ │ └── Transform.cc ├── config/ │ ├── 3dm/ │ │ └── 3dm_config.yaml │ ├── A3/ │ │ └── A3_config.yaml │ ├── AR_demo.rviz │ ├── euroc/ │ │ ├── euroc_config.yaml │ │ ├── euroc_config_no_extrinsic.yaml │ │ ├── ex_calib_result.yaml │ │ └── vins_result.csv │ └── vins_rviz_config.rviz ├── generate.sh ├── include/ │ ├── ChannelFloat32.h │ ├── Float32.h │ ├── Header.h │ ├── Imu.h │ ├── Odometry.h │ ├── Path.h │ ├── Point.h │ ├── Point32.h │ ├── PointCloud.h │ ├── PointStamped │ ├── Pose.h │ ├── PoseStamped.h │ ├── PoseWithCovariance.h │ ├── Quaternion.h │ ├── Time.h │ ├── Twist.h │ ├── TwistWithCovariance.h │ └── Vector3.h ├── src.kdev4 ├── support_files/ │ ├── brief_pattern.yml │ └── cmake/ │ └── FindEigen.cmake └── vins_estimator/ ├── .kdev4/ │ └── vins_estimator.kdev4 ├── CMakeLists.txt ├── launch/ │ ├── 3dm.launch │ ├── A3.launch │ ├── euroc.launch │ ├── euroc_no_extrinsic_param.launch │ └── vins_rviz.launch ├── package.xml └── src/ ├── estimator.cpp ├── estimator.h ├── estimator_node.cpp ├── factor/ │ ├── imu_factor.h │ ├── integration_base.h │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── pose_local_parameterization.cpp │ ├── pose_local_parameterization.h │ ├── projection_factor.cpp │ └── projection_factor.h ├── feature_manager.cpp ├── feature_manager.h ├── feature_tracker/ │ ├── feature_tracker.cpp │ ├── feature_tracker.h │ └── tic_toc.h ├── initial/ │ ├── initial_aligment.cpp │ ├── initial_alignment.h │ ├── initial_ex_rotation.cpp │ ├── initial_ex_rotation.h │ ├── initial_sfm.cpp │ ├── initial_sfm.h │ ├── solve_5pts.cpp │ └── solve_5pts.h ├── loop-closure/ │ ├── DLoopDetector.h │ ├── TemplatedLoopDetector.h │ ├── ThirdParty/ │ │ ├── DBoW/ │ │ │ ├── BowVector.cpp │ │ │ ├── BowVector.h │ │ │ ├── DBoW2.h │ │ │ ├── FBrief.cpp │ │ │ ├── FBrief.h │ │ │ ├── FClass.h │ │ │ ├── FeatureVector.cpp │ │ │ ├── FeatureVector.h │ │ │ ├── QueryResults.cpp │ │ │ ├── QueryResults.h │ │ │ ├── ScoringObject.cpp │ │ │ ├── ScoringObject.h │ │ │ ├── TemplatedDatabase.h │ │ │ └── TemplatedVocabulary.h │ │ ├── DUtils/ │ │ │ ├── DException.h │ │ │ ├── DUtils.h │ │ │ ├── Random.cpp │ │ │ ├── Random.h │ │ │ ├── Timestamp.cpp │ │ │ └── Timestamp.h │ │ ├── DVision/ │ │ │ ├── BRIEF.cpp │ │ │ ├── BRIEF.h │ │ │ └── DVision.h │ │ ├── VocabularyBinary.cpp │ │ └── VocabularyBinary.hpp │ ├── demoDetector.h │ ├── keyframe.cpp │ ├── keyframe.h │ ├── keyframe_database.cpp │ ├── keyframe_database.h │ ├── loop_closure.cpp │ └── loop_closure.h ├── parameters.cpp ├── parameters.h └── utility/ ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ data.csv /.git/ camera_model/build/ vins_estimator/build/ ================================================ FILE: .kdev4/src.kdev4 ================================================ [Buildset] BuildItems=@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) [CMake] Build Directory Count=1 Current Build Directory Index=0 ProjectRootRelative=./ [CMake][CMake Build Directory 0] Build Directory Path=file:///home/solomon/merge_vins_version/build Build Type= CMake Binary=file:///usr/bin/cmake Environment Profile= Extra Arguments= Install Directory=file:///home/solomon/merge_vins_version/install [Launch] Launch Configurations=Launch Configuration 0 [Launch][Launch Configuration 0] Configured Launch Modes=execute Configured Launchers=nativeAppLauncher Name=New Application Launcher Type=Native Application [Launch][Launch Configuration 0][Data] Arguments= Debugger Shell= Dependencies=@Variant(\x00\x00\x00\t\x00\x00\x00\x00\x00) Dependency Action=Nothing Display Demangle Names=true Display Static Members=false EnvironmentGroup=default Executable=file:///home/solomon/workspace/slam/VINS-MONO/catkin_ws/src/CMakeLists.txt External Terminal=konsole --noclose --workdir %workdir -e %exe GDB Path=file:///home/solomon/merge_vins_version/devel/lib/vins_estimator/vins_estimator Project Target=Remove_ROS_VINS,benchmark_publisher,benchmark_publisher Remote GDB Config Script= Remote GDB Run Script= Remote GDB Shell Script= Start With=ApplicationOutput Use External Terminal=false Working Directory=file:///home/solomon/workspace/slam/VINS-MONO/catkin_ws/src isExecutable=true [Project] VersionControlSupport=kdevgit ================================================ FILE: LICENCE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. {one line to give the program's name and a brief idea of what it does.} Copyright (C) {year} {name of author} This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: {project} Copyright (C) {year} {fullname} This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . ================================================ FILE: README.md ================================================ # 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. # thanks to the reference program： # [HKUST-Aerial-Robotics/VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) # What need to be installed ``` 1. Ceres Solver 2. Opencv 3.1 3. Eigen 3.2.0 4. boost 5. Pangolin ``` # How to build: ``` 1. mkdir VINS_Workspace 2. cd VINS_Workspace 3. git clone git@github.com:heguixiang/Remove_ROS_VINS.git 4. mv Remove_ROS_VINS src 5. ./generate.sh ``` # How to run: ``` 1. cd VINS_Workspace 2. mkdir -p data/image/MH_01_easy 3. mkdir -p data/imu/ 4. cd VINS_Workspace/data & git clone git@github.com:heguixiang/EuRoc-Timestamps.git 5. download the EuRoc dataset(e.g. Machine Hall 01) 6. cp the MH_01_easy/mav0/cam0/data to data/image/MH_01_easy 7. cp the MH_01_easy/mav0/imu0/data.csv to data/imu/ 8. cd VINS_Workspace 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 ``` ================================================ FILE: ar_demo/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(ar_demo) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE") #-DEIGEN_USE_MKL_ALL") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs image_transport sensor_msgs cv_bridge message_filters camera_model ) find_package(OpenCV REQUIRED) catkin_package( ) include_directories( ${catkin_INCLUDE_DIRS} ) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3 REQUIRED) include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) add_executable(ar_demo_node src/ar_demo_node.cpp) target_link_libraries(ar_demo_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ) ================================================ FILE: ar_demo/CMakeLists.txt.back ================================================ cmake_minimum_required(VERSION 2.8.3) project(ar_demo) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE") #-DEIGEN_USE_MKL_ALL") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs image_transport sensor_msgs cv_bridge message_filters camera_model ) find_package(OpenCV REQUIRED) catkin_package( ) include_directories( ${catkin_INCLUDE_DIRS} ) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3 REQUIRED) include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) add_executable(ar_demo_node src/ar_demo_node.cpp) target_link_libraries(ar_demo_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ) ================================================ FILE: ar_demo/launch/3dm_bag.launch ================================================ ================================================ FILE: ar_demo/launch/ar_A3.launch ================================================ ================================================ FILE: ar_demo/launch/ar_rviz.launch ================================================ ================================================ FILE: ar_demo/package.xml ================================================ ar_demo 0.0.0 The ar_demo package tony-ws TODO catkin roscpp rospy std_msgs camera_model roscpp rospy std_msgs camera_model ================================================ FILE: ar_demo/package.xml.back ================================================ ar_demo 0.0.0 The ar_demo package tony-ws TODO catkin roscpp rospy std_msgs camera_model roscpp rospy std_msgs camera_model ================================================ FILE: ar_demo/src/ar_demo_node.cpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include #include #include using namespace std; using namespace Eigen; using namespace sensor_msgs; using namespace message_filters; using namespace camodocal; int ROW; int COL; double FOCAL_LENGTH; const int axis_num = 0; const int cube_num = 1; const double box_length = 0.8; bool USE_UNDISTORED_IMG; bool pose_init = false; int img_cnt = 0; ros::Publisher object_pub; image_transport::Publisher pub_ARimage; Vector3d Axis[6]; Vector3d Cube_center[3]; vector Cube_corner[3]; vector output_Axis[6]; vector output_Cube[3]; vector output_corner_dis[3]; double Cube_center_depth[3]; queue img_buf; camodocal::CameraPtr m_camera; bool look_ground = 0; std_msgs::ColorRGBA line_color_r; std_msgs::ColorRGBA line_color_g; std_msgs::ColorRGBA line_color_b; void axis_generate(visualization_msgs::Marker &line_list, Vector3d &origin, int id) { line_list.id = id; line_list.header.frame_id = "world"; line_list.header.stamp = ros::Time::now(); line_list.action = visualization_msgs::Marker::ADD; line_list.type = visualization_msgs::Marker::LINE_LIST; line_list.scale.x = 0.1; line_list.color.a = 1.0; line_list.lifetime = ros::Duration(); line_list.pose.orientation.w = 1.0; line_list.color.b = 1.0; geometry_msgs::Point p; p.x = origin.x(); p.y = origin.y(); p.z = origin.z(); line_list.points.push_back(p); line_list.colors.push_back(line_color_r); p.x += 1.0; line_list.points.push_back(p); line_list.colors.push_back(line_color_r); p.x -= 1.0; line_list.points.push_back(p); line_list.colors.push_back(line_color_g); p.y += 1.0; line_list.points.push_back(p); line_list.colors.push_back(line_color_g); p.y -= 1.0; line_list.points.push_back(p); line_list.colors.push_back(line_color_b); p.z += 1.0; line_list.points.push_back(p); line_list.colors.push_back(line_color_b); } void cube_generate(visualization_msgs::Marker &marker, Vector3d &origin, int id) { //uint32_t shape = visualization_msgs::Marker::CUBE; marker.header.frame_id = "world"; marker.header.stamp = ros::Time::now(); marker.ns = "basic_shapes"; marker.id = 0; //marker.type = shape; marker.action = visualization_msgs::Marker::ADD; marker.type = visualization_msgs::Marker::CUBE_LIST; /* marker.pose.position.x = origin.x(); marker.pose.position.y = origin.y(); marker.pose.position.z = origin.z(); marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; */ marker.scale.x = box_length; marker.scale.y = box_length; marker.scale.z = box_length; marker.color.r = 0.0f; marker.color.g = 1.0f; marker.color.b = 0.0f; marker.color.a = 1.0; marker.lifetime = ros::Duration(); geometry_msgs::Point p; p.x = origin.x(); p.y = origin.y(); p.z = origin.z(); marker.points.push_back(p); marker.colors.push_back(line_color_r); Cube_corner[id].clear(); Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() - box_length / 2, origin.z() - box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() - box_length / 2, origin.z() - box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() + box_length / 2, origin.z() - box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() + box_length / 2, origin.z() - box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() - box_length / 2, origin.z() + box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() - box_length / 2, origin.z() + box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() + box_length / 2, origin.z() + box_length / 2)); Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() + box_length / 2, origin.z() + box_length / 2)); } void add_object() { visualization_msgs::MarkerArray markerArray_msg; visualization_msgs::Marker line_list; visualization_msgs::Marker cube_list; for (int i = 0; i < axis_num; i++) { axis_generate(line_list, Axis[i], i); markerArray_msg.markers.push_back(line_list); } for (int i = 0; i spaceToPlane(local_point, local_uv); if (local_point.z() > 0) //&& 0 <= local_uv.x() && local_uv.x() <= COL - 1 && 0 <= local_uv.y() && local_uv.y() <= ROW -1) { output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1)); local_point = camera_q.inverse() * (Axis[i] + Vector3d(1, 0, 0) - camera_p); m_camera->spaceToPlane(local_point, local_uv); output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1)); local_point = camera_q.inverse() * (Axis[i] + Vector3d(0, 1, 0) - camera_p); m_camera->spaceToPlane(local_point, local_uv); output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1)); local_point = camera_q.inverse() * (Axis[i] + Vector3d(0, 0, 1) - camera_p); m_camera->spaceToPlane(local_point, local_uv); output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1)); } } for (int i = 0; i < cube_num; i++) { output_Cube[i].clear(); output_corner_dis[i].clear(); Vector3d local_point; Vector2d local_uv; local_point = camera_q.inverse() * (Cube_center[i] - camera_p); if (USE_UNDISTORED_IMG) { local_uv.x() = local_point(0) / local_point(2) * FOCAL_LENGTH + COL / 2; local_uv.y() = local_point(1) / local_point(2) * FOCAL_LENGTH + ROW / 2; } else m_camera->spaceToPlane(local_point, local_uv); if (local_point.z() > box_length / 2) //&& 0 <= local_uv.x() && local_uv.x() <= COL - 1 && 0 <= local_uv.y() && local_uv.y() <= ROW -1) { Cube_center_depth[i] = local_point.z(); for (int j = 0; j < 8; j++) { local_point = camera_q.inverse() * (Cube_corner[i][j] - camera_p); output_corner_dis[i].push_back(local_point.norm()); if (USE_UNDISTORED_IMG) { //ROS_INFO("directly project!"); local_uv.x() = local_point(0) / local_point(2) * FOCAL_LENGTH + COL / 2; local_uv.y() = local_point(1) / local_point(2) * FOCAL_LENGTH + ROW / 2; } else { //ROS_INFO("camera model project!"); m_camera->spaceToPlane(local_point, local_uv); local_uv.x() = std::min(std::max(-5000.0, local_uv.x()),5000.0); local_uv.y() = std::min(std::max(-5000.0, local_uv.y()),5000.0); } output_Cube[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1)); } } else { Cube_center_depth[i] = -1; } } } void draw_object(cv::Mat &AR_image) { for (int i = 0; i < axis_num; i++) { if(output_Axis[i].empty()) continue; cv::Point2d origin(output_Axis[i][0].x(), output_Axis[i][0].y()); cv::Point2d axis_x(output_Axis[i][1].x(), output_Axis[i][1].y()); cv::Point2d axis_y(output_Axis[i][2].x(), output_Axis[i][2].y()); cv::Point2d axis_z(output_Axis[i][3].x(), output_Axis[i][3].y()); cv::line(AR_image, origin, axis_x, cv::Scalar(0, 0, 255), 2, 8, 0); cv::line(AR_image, origin, axis_y, cv::Scalar(0, 255, 0), 2, 8, 0); cv::line(AR_image, origin, axis_z, cv::Scalar(255, 0, 0), 2, 8, 0); } //depth sort big---->small int index[cube_num]; for (int i = 0; i < cube_num; i++) { index[i] = i; //cout << "i " << i << " init depth" << Cube_center_depth[i] << endl; } for (int i = 0; i < cube_num; i++) for (int j = 0; j < cube_num - i - 1; j++) { if (Cube_center_depth[j] < Cube_center_depth[j + 1]) { double tmp = Cube_center_depth[j]; Cube_center_depth[j] = Cube_center_depth[j + 1]; Cube_center_depth[j + 1] = tmp; int tmp_index = index[j]; index[j] = index[j + 1]; index[j + 1] = tmp_index; } } for (int k = 0; k < cube_num; k++) { int i = index[k]; //cout << "draw " << i << "depth " << Cube_center_depth[i] << endl; if (output_Cube[i].empty()) continue; //draw color cv::Point* p = new cv::Point[8]; p[0] = cv::Point(output_Cube[i][0].x(), output_Cube[i][0].y()); p[1] = cv::Point(output_Cube[i][1].x(), output_Cube[i][1].y()); p[2] = cv::Point(output_Cube[i][2].x(), output_Cube[i][2].y()); p[3] = cv::Point(output_Cube[i][3].x(), output_Cube[i][3].y()); p[4] = cv::Point(output_Cube[i][4].x(), output_Cube[i][4].y()); p[5] = cv::Point(output_Cube[i][5].x(), output_Cube[i][5].y()); p[6] = cv::Point(output_Cube[i][6].x(), output_Cube[i][6].y()); p[7] = cv::Point(output_Cube[i][7].x(), output_Cube[i][7].y()); int npts[1] = {4}; float min_depth = 100000; int min_index = 5; for(int j= 0; j < (int)output_corner_dis[i].size(); j++) { if(output_corner_dis[i][j] < min_depth) { min_depth = output_corner_dis[i][j]; min_index = j; } } cv::Point plain[1][4]; const cv::Point* ppt[1] = {plain[0]}; //first draw large depth plane int point_group[8][12] = {{0,1,5,4, 0,4,6,2, 0,1,3,2}, {0,1,5,4, 1,5,7,3, 0,1,3,2}, {2,3,7,6, 0,4,6,2, 0,1,3,2}, {2,3,7,6, 1,5,7,3, 0,1,3,2}, {0,1,5,4, 0,4,6,2, 4,5,7,6}, {0,1,5,4, 1,5,7,3, 4,5,7,6}, {2,3,7,6, 0,4,6,2, 4,5,7,6}, {2,3,7,6, 1,5,7,3, 4,5,7,6}}; plain[0][0] = p[point_group[min_index][4]]; plain[0][1] = p[point_group[min_index][5]]; plain[0][2] = p[point_group[min_index][6]]; plain[0][3] = p[point_group[min_index][7]]; cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 200, 0)); plain[0][0] = p[point_group[min_index][0]]; plain[0][1] = p[point_group[min_index][1]]; plain[0][2] = p[point_group[min_index][2]]; plain[0][3] = p[point_group[min_index][3]]; cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(200, 0, 0)); if(output_corner_dis[i][point_group[min_index][2]] + output_corner_dis[i][point_group[min_index][3]] > output_corner_dis[i][point_group[min_index][5]] + output_corner_dis[i][point_group[min_index][6]]) { plain[0][0] = p[point_group[min_index][4]]; plain[0][1] = p[point_group[min_index][5]]; plain[0][2] = p[point_group[min_index][6]]; plain[0][3] = p[point_group[min_index][7]]; cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 200, 0)); } plain[0][0] = p[point_group[min_index][8]]; plain[0][1] = p[point_group[min_index][9]]; plain[0][2] = p[point_group[min_index][10]]; plain[0][3] = p[point_group[min_index][11]]; cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 0, 200)); delete p; } } void callback(const ImageConstPtr& img_msg, const geometry_msgs::PoseStamped::ConstPtr& pose_msg) { //throw the first few unstable pose if(img_cnt < 20) { img_cnt ++; return; } //ROS_INFO("sync callback!"); Vector3d camera_p(pose_msg->pose.position.x, pose_msg->pose.position.y, pose_msg->pose.position.z); Quaterniond camera_q(pose_msg->pose.orientation.w, pose_msg->pose.orientation.x, pose_msg->pose.orientation.y, pose_msg->pose.orientation.z); //test plane Vector3d cam_z(0, 0, -1); Vector3d w_cam_z = camera_q * cam_z; //cout << "angle " << acos(w_cam_z.dot(Vector3d(0, 0, 1))) * 180.0 / M_PI << endl; if (acos(w_cam_z.dot(Vector3d(0, 0, 1))) * 180.0 / M_PI < 90) { //ROS_WARN(" look down"); look_ground = 1; } else look_ground = 0; project_object(camera_p, camera_q); cv_bridge::CvImagePtr bridge_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); cv::Mat AR_image; AR_image = bridge_ptr->image.clone(); cv::cvtColor(AR_image, AR_image, cv::COLOR_GRAY2RGB); draw_object(AR_image); sensor_msgs::ImagePtr AR_msg = cv_bridge::CvImage(img_msg->header, "bgr8", AR_image).toImageMsg(); pub_ARimage.publish(AR_msg); } void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg) { if (!look_ground) return; int height_range[30]; double height_sum[30]; for (int i = 0; i < 30; i++) { height_range[i] = 0; height_sum[i] = 0; } for (unsigned int i = 0; i < point_msg->points.size(); i++) { //double x = point_msg->points[i].x; //double y = point_msg->points[i].y; double z = point_msg->points[i].z; int index = (z + 2.0) / 0.1; if (0 <= index && index < 30) { height_range[index]++; height_sum[index] += z; } //cout << "point " << " z " << z << endl; } int max_num = 0; int max_index = -1; for (int i = 1; i < 29; i++) { if (max_num < height_range[i]) { max_num = height_range[i]; max_index = i; } } if (max_index == -1) return; int neigh_num = height_range[max_index - 1] + height_range[max_index] + height_range[max_index + 1]; double neigh_height = (height_sum[max_index - 1] + height_sum[max_index] + height_sum[max_index + 1]) / neigh_num; //ROS_WARN("detect ground plain, height %f", neigh_height); if (neigh_num < (int)point_msg->points.size() / 2) { //ROS_INFO("points not enough"); return; } //update height for (int i = 0; i < cube_num; i++) { Cube_center[i].z() = neigh_height + box_length / 2.0; } add_object(); } void img_callback(const ImageConstPtr& img_msg) { if(pose_init) { img_buf.push(img_msg); } else return; } void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg) { if(!pose_init) { pose_init = true; return; } if (img_buf.empty()) { //ROS_WARN("image coming late"); return; } while (img_buf.front()->header.stamp < pose_msg->header.stamp && !img_buf.empty()) { img_buf.pop(); } if (!img_buf.empty()) { callback(img_buf.front(), pose_msg); img_buf.pop(); } //else // ROS_WARN("image coming late"); } int main( int argc, char** argv ) { ros::init(argc, argv, "points_and_lines"); ros::NodeHandle n("~"); object_pub = n.advertise("AR_object", 10); n.getParam("use_undistored_img", USE_UNDISTORED_IMG); ros::Subscriber sub_img; if (USE_UNDISTORED_IMG) { // the same as image crop ROW = 600; COL = 480; FOCAL_LENGTH = 320.0; sub_img = n.subscribe("image_undistored", 100, img_callback); } else { ROW = 752; COL = 480; FOCAL_LENGTH = 460.0; sub_img = n.subscribe("image_raw", 100, img_callback); } Axis[0] = Vector3d(0, 2, -1.2); Axis[1]= Vector3d(-10, 5, 0); Axis[2] = Vector3d(3, 3, 3); Axis[3] = Vector3d(-2, 2, 0); Axis[4]= Vector3d(5, 10, -5); Axis[5] = Vector3d(0, 10, -1); Cube_center[0] = Vector3d(-2, 0, -1.2 + box_length / 2.0); //Cube_center[0] = Vector3d(0, 3, -1.2 + box_length / 2.0); Cube_center[1] = Vector3d(4, -2, -1.2 + box_length / 2.0); Cube_center[2] = Vector3d(0, -2, -1.2 + box_length / 2.0); ros::Subscriber pose_img = n.subscribe("camera_pose", 100, pose_callback); ros::Subscriber sub_point = n.subscribe("pointcloud", 2000, point_callback); image_transport::ImageTransport it(n); pub_ARimage = it.advertise("AR_image", 1000); line_color_r.r = 1.0; line_color_r.a = 1.0; line_color_g.g = 1.0; line_color_g.a = 1.0; line_color_b.b = 1.0; line_color_b.a = 1.0; string calib_file; n.getParam("calib_file", calib_file); ROS_INFO("reading paramerter of camera %s", calib_file.c_str()); m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); ros::Rate r(100); ros::Duration(1).sleep(); add_object(); add_object(); ros::spin(); } ================================================ FILE: benchmark_publisher/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(benchmark_publisher) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -rdynamic") find_package(catkin REQUIRED COMPONENTS roscpp tf ) catkin_package() include_directories(${catkin_INCLUDE_DIRS}) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3 REQUIRED) include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) add_executable(benchmark_publisher src/benchmark_publisher_node.cpp ) target_link_libraries(benchmark_publisher ${catkin_LIBRARIES}) ================================================ FILE: benchmark_publisher/CMakeLists.txt.back ================================================ cmake_minimum_required(VERSION 2.8.3) project(benchmark_publisher) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -rdynamic") find_package(catkin REQUIRED COMPONENTS roscpp tf ) catkin_package() include_directories(${catkin_INCLUDE_DIRS}) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3 REQUIRED) include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) add_executable(benchmark_publisher src/benchmark_publisher_node.cpp ) target_link_libraries(benchmark_publisher ${catkin_LIBRARIES}) ================================================ FILE: benchmark_publisher/launch/publish.launch ================================================ ================================================ FILE: benchmark_publisher/package.xml ================================================ benchmark_publisher 0.0.0 The benchmark_publisher package dvorak TODO catkin roscpp roscpp ================================================ FILE: benchmark_publisher/package.xml.back ================================================ benchmark_publisher 0.0.0 The benchmark_publisher package dvorak TODO catkin roscpp roscpp ================================================ FILE: benchmark_publisher/src/benchmark_publisher_node.cpp ================================================ #include #include #include #include #include #include #include #include #include using namespace std; using namespace Eigen; const int SKIP = 50; string benchmark_output_path; string estimate_output_path; template T readParam(ros::NodeHandle &n, std::string name) { T ans; if (n.getParam(name, ans)) { ROS_INFO_STREAM("Loaded " << name << ": " << ans); } else { ROS_ERROR_STREAM("Failed to load " << name); n.shutdown(); } return ans; } struct Data { Data(FILE *f) { fscanf(f, " %lf,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", &t, &px, &py, &pz, &qw, &qx, &qy, &qz, &vx, &vy, &vz, &wx, &wy, &wz, &ax, &ay, &az); t /= 1e9; } double t; float px, py, pz; float qw, qx, qy, qz; float vx, vy, vz; float wx, wy, wz; float ax, ay, az; }; int idx = 1; vector benchmark; ros::Publisher pub_odom; ros::Publisher pub_path; nav_msgs::Path path; int init = 0; Quaterniond baseRgt; Vector3d baseTgt; tf::Transform trans; void odom_callback(const nav_msgs::OdometryConstPtr &odom_msg) { //ROS_INFO("odom callback!"); if (odom_msg->header.stamp.toSec() > benchmark.back().t) return; for (; idx < static_cast(benchmark.size()) && benchmark[idx].t <= odom_msg->header.stamp.toSec(); idx++) ; if (init++ < SKIP) { baseRgt = Quaterniond(odom_msg->pose.pose.orientation.w, odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z) * Quaterniond(benchmark[idx - 1].qw, benchmark[idx - 1].qx, benchmark[idx - 1].qy, benchmark[idx - 1].qz).inverse(); baseTgt = Vector3d{odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z} - baseRgt * Vector3d{benchmark[idx - 1].px, benchmark[idx - 1].py, benchmark[idx - 1].pz}; return; } nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time(benchmark[idx - 1].t); odometry.header.frame_id = "world"; odometry.child_frame_id = "world"; Vector3d tmp_T = baseTgt + baseRgt * Vector3d{benchmark[idx - 1].px, benchmark[idx - 1].py, benchmark[idx - 1].pz}; odometry.pose.pose.position.x = tmp_T.x(); odometry.pose.pose.position.y = tmp_T.y(); odometry.pose.pose.position.z = tmp_T.z(); Quaterniond tmp_R = baseRgt * Quaterniond{benchmark[idx - 1].qw, benchmark[idx - 1].qx, benchmark[idx - 1].qy, benchmark[idx - 1].qz}; odometry.pose.pose.orientation.w = tmp_R.w(); odometry.pose.pose.orientation.x = tmp_R.x(); odometry.pose.pose.orientation.y = tmp_R.y(); odometry.pose.pose.orientation.z = tmp_R.z(); Vector3d tmp_V = baseRgt * Vector3d{benchmark[idx - 1].vx, benchmark[idx - 1].vy, benchmark[idx - 1].vz}; odometry.twist.twist.linear.x = tmp_V.x(); odometry.twist.twist.linear.y = tmp_V.y(); odometry.twist.twist.linear.z = tmp_V.z(); pub_odom.publish(odometry); geometry_msgs::PoseStamped pose_stamped; pose_stamped.header = odometry.header; pose_stamped.pose = odometry.pose.pose; path.header = odometry.header; path.poses.push_back(pose_stamped); pub_path.publish(path); } int main(int argc, char **argv) { ros::init(argc, argv, "benchmark_publisher"); ros::NodeHandle n("~"); string csv_file = readParam(n, "data_name"); std::cout << "load ground truth " << csv_file << std::endl; FILE *f = fopen(csv_file.c_str(), "r"); if (f==NULL) { ROS_WARN("can't load ground truth; wrong path"); //std::cerr << "can't load ground truth; wrong path " << csv_file << std::endl; return 0; } char tmp[10000]; fgets(tmp, 10000, f); while (!feof(f)) benchmark.emplace_back(f); fclose(f); benchmark.pop_back(); ROS_INFO("Data loaded: %d", (int)benchmark.size()); pub_odom = n.advertise("odometry", 1000); pub_path = n.advertise("path", 1000); ros::Subscriber sub_odom = n.subscribe("estimated_odometry", 1000, odom_callback); ros::Rate r(20); ros::spin(); } ================================================ FILE: camera_model/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(camera_model) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC") #find_package(catkin REQUIRED COMPONENTS # roscpp # std_msgs # ) find_package(Boost REQUIRED COMPONENTS filesystem program_options system) include_directories(${Boost_INCLUDE_DIRS}) find_package(OpenCV REQUIRED) # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") find_package(Ceres REQUIRED) include_directories(${CERES_INCLUDE_DIRS}) #catkin_package( # INCLUDE_DIRS include # LIBRARIES camera_model # CATKIN_DEPENDS roscpp std_msgs # DEPENDS system_lib # ) #include_directories( # ${catkin_INCLUDE_DIRS} # ) include_directories("include") # add_executable(Calibration # src/intrinsic_calib.cc # src/chessboard/Chessboard.cc # src/calib/CameraCalibration.cc # src/camera_models/Camera.cc # src/camera_models/CameraFactory.cc # src/camera_models/CostFunctionFactory.cc # src/camera_models/PinholeCamera.cc # src/camera_models/CataCamera.cc # src/camera_models/EquidistantCamera.cc # src/camera_models/ScaramuzzaCamera.cc # src/sparse_graph/Transform.cc # src/gpl/gpl.cc # src/gpl/EigenQuaternionParameterization.cc) add_library(camera_model STATIC src/chessboard/Chessboard.cc src/calib/CameraCalibration.cc src/camera_models/Camera.cc src/camera_models/CameraFactory.cc src/camera_models/CostFunctionFactory.cc src/camera_models/PinholeCamera.cc src/camera_models/CataCamera.cc src/camera_models/EquidistantCamera.cc src/camera_models/ScaramuzzaCamera.cc src/sparse_graph/Transform.cc src/gpl/gpl.cc src/gpl/EigenQuaternionParameterization.cc) # target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) ================================================ FILE: camera_model/include/camodocal/calib/CameraCalibration.h ================================================ #ifndef CAMERACALIBRATION_H #define CAMERACALIBRATION_H #include #include "camodocal/camera_models/Camera.h" namespace camodocal { class CameraCalibration { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CameraCalibration(); CameraCalibration(Camera::ModelType modelType, const std::string& cameraName, const cv::Size& imageSize, const cv::Size& boardSize, float squareSize); void clear(void); void addChessboardData(const std::vector& corners); bool calibrate(void); int sampleCount(void) const; std::vector >& imagePoints(void); const std::vector >& imagePoints(void) const; std::vector >& scenePoints(void); const std::vector >& scenePoints(void) const; CameraPtr& camera(void); const CameraConstPtr camera(void) const; Eigen::Matrix2d& measurementCovariance(void); const Eigen::Matrix2d& measurementCovariance(void) const; cv::Mat& cameraPoses(void); const cv::Mat& cameraPoses(void) const; void drawResults(std::vector& images) const; void writeParams(const std::string& filename) const; bool writeChessboardData(const std::string& filename) const; bool readChessboardData(const std::string& filename); void setVerbose(bool verbose); private: bool calibrateHelper(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const; void optimize(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const; template void readData(std::ifstream& ifs, T& data) const; template void writeData(std::ofstream& ofs, T data) const; cv::Size m_boardSize; float m_squareSize; CameraPtr m_camera; cv::Mat m_cameraPoses; std::vector > m_imagePoints; std::vector > m_scenePoints; Eigen::Matrix2d m_measurementCovariance; bool m_verbose; }; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/Camera.h ================================================ #ifndef CAMERA_H #define CAMERA_H #include #include #include #include namespace camodocal { class Camera { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW enum ModelType { KANNALA_BRANDT, MEI, PINHOLE, SCARAMUZZA }; class Parameters { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Parameters(ModelType modelType); Parameters(ModelType modelType, const std::string& cameraName, int w, int h); ModelType& modelType(void); std::string& cameraName(void); int& imageWidth(void); int& imageHeight(void); ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; int nIntrinsics(void) const; virtual bool readFromYamlFile(const std::string& filename) = 0; virtual void writeToYamlFile(const std::string& filename) const = 0; protected: ModelType m_modelType; int m_nIntrinsics; std::string m_cameraName; int m_imageWidth; int m_imageHeight; }; virtual ModelType modelType(void) const = 0; virtual const std::string& cameraName(void) const = 0; virtual int imageWidth(void) const = 0; virtual int imageHeight(void) const = 0; virtual cv::Mat& mask(void); virtual const cv::Mat& mask(void) const; virtual void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) = 0; virtual void estimateExtrinsics(const std::vector& objectPoints, const std::vector& imagePoints, cv::Mat& rvec, cv::Mat& tvec) const; // Lift points from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; //%output P // Lift points from the image plane to the projective space virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; //%output P // Projects 3D points to the image plane (Pi function) virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, // Eigen::Matrix& J) const = 0; //%output p //%output J virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; //%output p //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; virtual int parameterCount(void) const = 0; virtual void readParameters(const std::vector& parameters) = 0; virtual void writeParameters(std::vector& parameters) const = 0; virtual void writeParametersToYamlFile(const std::string& filename) const = 0; virtual std::string parametersToString(void) const = 0; /** * \brief Calculates the reprojection distance between points * * \param P1 first 3D point coordinates * \param P2 second 3D point coordinates * \return euclidean distance in the plane */ double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; double reprojectionError(const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints, const std::vector& rvecs, const std::vector& tvecs, cv::OutputArray perViewErrors = cv::noArray()) const; double reprojectionError(const Eigen::Vector3d& P, const Eigen::Quaterniond& camera_q, const Eigen::Vector3d& camera_t, const Eigen::Vector2d& observed_p) const; void projectPoints(const std::vector& objectPoints, const cv::Mat& rvec, const cv::Mat& tvec, std::vector& imagePoints) const; protected: cv::Mat m_mask; }; typedef boost::shared_ptr CameraPtr; typedef boost::shared_ptr CameraConstPtr; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/CameraFactory.h ================================================ #ifndef CAMERAFACTORY_H #define CAMERAFACTORY_H #include #include #include "camodocal/camera_models/Camera.h" namespace camodocal { class CameraFactory { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CameraFactory(); static boost::shared_ptr instance(void); CameraPtr generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const; CameraPtr generateCameraFromYamlFile(const std::string& filename); private: static boost::shared_ptr m_instance; }; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/CataCamera.h ================================================ #ifndef CATACAMERA_H #define CATACAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { /** * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration * from Planar Grids, ICRA 2007 */ class CataCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0); double& xi(void); double& k1(void); double& k2(void); double& p1(void); double& p2(void); double& gamma1(void); double& gamma2(void); double& u0(void); double& v0(void); double xi(void) const; double k1(void) const; double k2(void) const; double p1(void) const; double p2(void) const; double gamma1(void) const; double gamma2(void) const; double u0(void) const; double v0(void) const; bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_xi; double m_k1; double m_k2; double m_p1; double m_p2; double m_gamma1; double m_gamma2; double m_u0; double m_v0; }; CataCamera(); /** * \brief Constructor from the projection model parameters */ CataCamera(const std::string& cameraName, int imageWidth, int imageHeight, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0); /** * \brief Constructor from the projection model parameters */ CataCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const; void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; bool m_noDistortion; }; typedef boost::shared_ptr CataCameraPtr; typedef boost::shared_ptr CataCameraConstPtr; template void CataCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; T P_c[3]; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; // project 3D object point to the image plane T xi = params[0]; T k1 = params[1]; T k2 = params[2]; T p1 = params[3]; T p2 = params[4]; T gamma1 = params[5]; T gamma2 = params[6]; T alpha = T(0); //cameraParams.alpha(); T u0 = params[7]; T v0 = params[8]; // Transform to model plane T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); P_c[0] /= len; P_c[1] /= len; P_c[2] /= len; T u = P_c[0] / (P_c[2] + xi); T v = P_c[1] / (P_c[2] + xi); T rho_sqr = u * u + v * v; T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; u = L * u + du; v = L * v + dv; p(0) = gamma1 * (u + alpha * v) + u0; p(1) = gamma2 * v + v0; } } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/CostFunctionFactory.h ================================================ #ifndef COSTFUNCTIONFACTORY_H #define COSTFUNCTIONFACTORY_H #include #include #include "camodocal/camera_models/Camera.h" namespace ceres { class CostFunction; } namespace camodocal { enum { CAMERA_INTRINSICS = 1 << 0, CAMERA_POSE = 1 << 1, POINT_3D = 1 << 2, ODOMETRY_INTRINSICS = 1 << 3, ODOMETRY_3D_POSE = 1 << 4, ODOMETRY_6D_POSE = 1 << 5, CAMERA_ODOMETRY_TRANSFORM = 1 << 6 }; class CostFunctionFactory { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CostFunctionFactory(); static boost::shared_ptr instance(void); ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, int flags) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z = true) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags, bool optimize_cam_odo_z = true) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z = true) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Quaterniond& cam_odo_q, const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags) const; ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, const CameraConstPtr& cameraRight, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_left, const Eigen::Vector2d& observed_p_right) const; private: static boost::shared_ptr m_instance; }; } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/EquidistantCamera.h ================================================ #ifndef EQUIDISTANTCAMERA_H #define EQUIDISTANTCAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { /** * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 */ class EquidistantCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0); double& k2(void); double& k3(void); double& k4(void); double& k5(void); double& mu(void); double& mv(void); double& u0(void); double& v0(void); double k2(void) const; double k3(void) const; double k4(void) const; double k5(void) const; double mu(void) const; double mv(void) const; double u0(void) const; double v0(void) const; bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: // projection double m_k2; double m_k3; double m_k4; double m_k5; double m_mu; double m_mv; double m_u0; double m_v0; }; EquidistantCamera(); /** * \brief Constructor from the projection model parameters */ EquidistantCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0); /** * \brief Constructor from the projection model parameters */ EquidistantCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: template static T r(T k2, T k3, T k4, T k5, T theta); void fitOddPoly(const std::vector& x, const std::vector& y, int n, std::vector& coeffs) const; void backprojectSymmetric(const Eigen::Vector2d& p_u, double& theta, double& phi) const; Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; }; typedef boost::shared_ptr EquidistantCameraPtr; typedef boost::shared_ptr EquidistantCameraConstPtr; template T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) { // k1 = 1 return theta + k2 * theta * theta * theta + k3 * theta * theta * theta * theta * theta + k4 * theta * theta * theta * theta * theta * theta * theta + k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; } template void EquidistantCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; T P_c[3]; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; // project 3D object point to the image plane; T k2 = params[0]; T k3 = params[1]; T k4 = params[2]; T k5 = params[3]; T mu = params[4]; T mv = params[5]; T u0 = params[6]; T v0 = params[7]; T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); T theta = acos(P_c[2] / len); T phi = atan2(P_c[1], P_c[0]); Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); p(0) = mu * p_u(0) + u0; p(1) = mv * p_u(1) + v0; } } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/PinholeCamera.h ================================================ #ifndef PINHOLECAMERA_H #define PINHOLECAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { class PinholeCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy); double& k1(void); double& k2(void); double& p1(void); double& p2(void); double& fx(void); double& fy(void); double& cx(void); double& cy(void); double xi(void) const; double k1(void) const; double k2(void) const; double p1(void) const; double p2(void) const; double fx(void) const; double fy(void) const; double cx(void) const; double cy(void) const; bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_k1; double m_k2; double m_p1; double m_p2; double m_fx; double m_fy; double m_cx; double m_cy; }; PinholeCamera(); /** * \brief Constructor from the projection model parameters */ PinholeCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy); /** * \brief Constructor from the projection model parameters */ PinholeCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const; void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; bool m_noDistortion; }; typedef boost::shared_ptr PinholeCameraPtr; typedef boost::shared_ptr PinholeCameraConstPtr; template void PinholeCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; T P_c[3]; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; // project 3D object point to the image plane T k1 = params[0]; T k2 = params[1]; T p1 = params[2]; T p2 = params[3]; T fx = params[4]; T fy = params[5]; T alpha = T(0); //cameraParams.alpha(); T cx = params[6]; T cy = params[7]; // Transform to model plane T u = P_c[0] / P_c[2]; T v = P_c[1] / P_c[2]; T rho_sqr = u * u + v * v; T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; u = L * u + du; v = L * v + dv; p(0) = fx * (u + alpha * v) + cx; p(1) = fy * v + cy; } } #endif ================================================ FILE: camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h ================================================ #ifndef SCARAMUZZACAMERA_H #define SCARAMUZZACAMERA_H #include #include #include "ceres/rotation.h" #include "Camera.h" namespace camodocal { #define SCARAMUZZA_POLY_SIZE 5 #define SCARAMUZZA_INV_POLY_SIZE 20 #define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/) /** * Scaramuzza Camera (Omnidirectional) * https://sites.google.com/site/scarabotix/ocamcalib-toolbox */ class OCAMCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); double& C(void) { return m_C; } double& D(void) { return m_D; } double& E(void) { return m_E; } double& center_x(void) { return m_center_x; } double& center_y(void) { return m_center_y; } double& poly(int idx) { return m_poly[idx]; } double& inv_poly(int idx) { return m_inv_poly[idx]; } double C(void) const { return m_C; } double D(void) const { return m_D; } double E(void) const { return m_E; } double center_x(void) const { return m_center_x; } double center_y(void) const { return m_center_y; } double poly(int idx) const { return m_poly[idx]; } double inv_poly(int idx) const { return m_inv_poly[idx]; } bool readFromYamlFile(const std::string& filename); void writeToYamlFile(const std::string& filename) const; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_poly[SCARAMUZZA_POLY_SIZE]; double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE]; double m_C; double m_D; double m_E; double m_center_x; double m_center_y; }; OCAMCamera(); /** * \brief Constructor from the projection model parameters */ OCAMCamera(const Parameters& params); Camera::ModelType modelType(void) const; const std::string& cameraName(void) const; int imageWidth(void) const; int imageHeight(void) const; void estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints); // Lift points from the image plane to the sphere void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Lift points from the image plane to the projective space void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; //%output P // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; //%output p // Projects 3D points to the image plane (Pi function) // and calculates jacobian //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, // Eigen::Matrix& J) const; //%output p //%output J void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; //%output p template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); template static void spaceToSphere(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& P_s); template static void LiftToSphere(const T* const params, const Eigen::Matrix& p, Eigen::Matrix& P); template static void SphereToPlane(const T* const params, const Eigen::Matrix& P, Eigen::Matrix& p); void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx = -1.0f, float fy = -1.0f, cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; int parameterCount(void) const; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec); void writeParameters(std::vector& parameterVec) const; void writeParametersToYamlFile(const std::string& filename) const; std::string parametersToString(void) const; private: Parameters mParameters; double m_inv_scale; }; typedef boost::shared_ptr OCAMCameraPtr; typedef boost::shared_ptr OCAMCameraConstPtr; template void OCAMCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_c[3]; { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; } T c = params[0]; T d = params[1]; T e = params[2]; T xc[2] = { params[3], params[4] }; //T poly[SCARAMUZZA_POLY_SIZE]; //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) // poly[i] = params[5+i]; T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; T norm = T(0.0); if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); T theta = atan2(-P_c[2], norm); T rho = T(0.0); T theta_i = T(1.0); for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { rho += theta_i * inv_poly[i]; theta_i *= theta; } T invNorm = T(1.0) / norm; T xn[2] = { P_c[0] * invNorm * rho, P_c[1] * invNorm * rho }; p(0) = xn[0] * c + xn[1] * d + xc[0]; p(1) = xn[0] * e + xn[1] + xc[1]; } template void OCAMCamera::spaceToSphere(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& P_s) { T P_c[3]; { T P_w[3]; P_w[0] = T(P(0)); P_w[1] = T(P(1)); P_w[2] = T(P(2)); // Convert quaternion from Eigen convention (x, y, z, w) // to Ceres convention (w, x, y, z) T q_ceres[4] = {q[3], q[0], q[1], q[2]}; ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); P_c[0] += t[0]; P_c[1] += t[1]; P_c[2] += t[2]; } //T poly[SCARAMUZZA_POLY_SIZE]; //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) // poly[i] = params[5+i]; T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]; T norm = T(0.0); if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); P_s(0) = P_c[0] / norm; P_s(1) = P_c[1] / norm; P_s(2) = P_c[2] / norm; } template void OCAMCamera::LiftToSphere(const T* const params, const Eigen::Matrix& p, Eigen::Matrix& P) { T c = params[0]; T d = params[1]; T e = params[2]; T cc[2] = { params[3], params[4] }; T poly[SCARAMUZZA_POLY_SIZE]; for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) poly[i] = params[5+i]; // Relative to Center T p_2d[2]; p_2d[0] = T(p(0)); p_2d[1] = T(p(1)); T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]}; T inv_scale = T(1.0) / (c - d * e); // Affine Transformation T xc_a[2]; xc_a[0] = inv_scale * (xc[0] - d * xc[1]); xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]); T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]; T phi = sqrt(norm_sqr); T phi_i = T(1.0); T z = T(0.0); for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) { if (i!=1) { z += phi_i * poly[i]; } phi_i *= phi; } T p_3d[3]; p_3d[0] = xc[0]; p_3d[1] = xc[1]; p_3d[2] = -z; T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2]; T p_3d_norm = sqrt(p_3d_norm_sqr); P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm; } template void OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P, Eigen::Matrix& p) { T P_c[3]; { P_c[0] = T(P(0)); P_c[1] = T(P(1)); P_c[2] = T(P(2)); } T c = params[0]; T d = params[1]; T e = params[2]; T xc[2] = {params[3], params[4]}; T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; T norm = T(0.0); if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); T theta = atan2(-P_c[2], norm); T rho = T(0.0); T theta_i = T(1.0); for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { rho += theta_i * inv_poly[i]; theta_i *= theta; } T invNorm = T(1.0) / norm; T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho}; p(0) = xn[0] * c + xn[1] * d + xc[0]; p(1) = xn[0] * e + xn[1] + xc[1]; } } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/Chessboard.h ================================================ #ifndef CHESSBOARD_H #define CHESSBOARD_H #include #include namespace camodocal { // forward declarations class ChessboardCorner; typedef boost::shared_ptr ChessboardCornerPtr; class ChessboardQuad; typedef boost::shared_ptr ChessboardQuadPtr; class Chessboard { public: Chessboard(cv::Size boardSize, cv::Mat& image); void findCorners(bool useOpenCV = false); const std::vector& getCorners(void) const; bool cornersFound(void) const; const cv::Mat& getImage(void) const; const cv::Mat& getSketch(void) const; private: bool findChessboardCorners(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags, bool useOpenCV); bool findChessboardCornersImproved(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags); void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); void findConnectedQuads(std::vector& quads, std::vector& group, int group_idx, int dilation); // int checkQuadGroup(std::vector& quadGroup, // std::vector& outCorners, // cv::Size patternSize); void labelQuadGroup(std::vector& quad_group, cv::Size patternSize, bool firstRun); void findQuadNeighbors(std::vector& quads, int dilation); int augmentBestRun(std::vector& candidateQuads, int candidateDilation, std::vector& existingQuads, int existingDilation); void generateQuads(std::vector& quads, cv::Mat& image, int flags, int dilation, bool firstRun); bool checkQuadGroup(std::vector& quads, std::vector& corners, cv::Size patternSize); void getQuadrangleHypotheses(const std::vector< std::vector >& contours, std::vector< std::pair >& quads, int classId) const; bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; bool checkBoardMonotony(std::vector& corners, cv::Size patternSize); bool matchCorners(ChessboardQuadPtr& quad1, int corner1, ChessboardQuadPtr& quad2, int corner2) const; cv::Mat mImage; cv::Mat mSketch; std::vector mCorners; cv::Size mBoardSize; bool mCornersFound; }; } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/ChessboardCorner.h ================================================ #ifndef CHESSBOARDCORNER_H #define CHESSBOARDCORNER_H #include #include namespace camodocal { class ChessboardCorner; typedef boost::shared_ptr ChessboardCornerPtr; class ChessboardCorner { public: ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} float meanDist(int &n) const { float sum = 0; n = 0; for (int i = 0; i < 4; ++i) { if (neighbors[i].get()) { float dx = neighbors[i]->pt.x - pt.x; float dy = neighbors[i]->pt.y - pt.y; sum += sqrt(dx*dx + dy*dy); n++; } } return sum / std::max(n, 1); } cv::Point2f pt; // X and y coordinates int row; // Row and column of the corner int column; // in the found pattern bool needsNeighbor; // Does the corner require a neighbor? int count; // number of corner neighbors ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors }; } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/ChessboardQuad.h ================================================ #ifndef CHESSBOARDQUAD_H #define CHESSBOARDQUAD_H #include #include "camodocal/chessboard/ChessboardCorner.h" namespace camodocal { class ChessboardQuad; typedef boost::shared_ptr ChessboardQuadPtr; class ChessboardQuad { public: ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} int count; // Number of quad neighbors int group_idx; // Quad group ID float edge_len; // Smallest side length^2 ChessboardCornerPtr corners[4]; // Coordinates of quad corners ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors bool labeled; // Has this corner been labeled? }; } #endif ================================================ FILE: camera_model/include/camodocal/chessboard/Spline.h ================================================ /* dynamo:- Event driven molecular dynamics simulator http://www.marcusbannerman.co.uk/dynamo Copyright (C) 2011 Marcus N Campbell Bannerman This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License version 3 as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #pragma once #include #include #include #include #include #include namespace ublas = boost::numeric::ublas; class Spline : private std::vector > { public: //The boundary conditions available enum BC_type { FIXED_1ST_DERIV_BC, FIXED_2ND_DERIV_BC, PARABOLIC_RUNOUT_BC }; enum Spline_type { LINEAR, CUBIC }; //Constructor takes the boundary conditions as arguments, this //sets the first derivative (gradient) at the lower and upper //end points Spline(): _valid(false), _BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC), _BCLowVal(0), _BCHighVal(0), _type(CUBIC) {} typedef std::vector > base; typedef base::const_iterator const_iterator; //Standard STL read-only container stuff const_iterator begin() const { return base::begin(); } const_iterator end() const { return base::end(); } void clear() { _valid = false; base::clear(); _data.clear(); } size_t size() const { return base::size(); } size_t max_size() const { return base::max_size(); } size_t capacity() const { return base::capacity(); } bool empty() const { return base::empty(); } //Add a point to the spline, and invalidate it so its //recalculated on the next access inline void addPoint(double x, double y) { _valid = false; base::push_back(std::pair(x,y)); } //Reset the boundary conditions inline void setLowBC(BC_type BC, double val = 0) { _BCLow = BC; _BCLowVal = val; _valid = false; } inline void setHighBC(BC_type BC, double val = 0) { _BCHigh = BC; _BCHighVal = val; _valid = false; } void setType(Spline_type type) { _type = type; _valid = false; } //Check if the spline has been calculated, then generate the //spline interpolated value double operator()(double xval) { if (!_valid) generate(); //Special cases when we're outside the range of the spline points if (xval <= x(0)) return lowCalc(xval); if (xval >= x(size()-1)) return highCalc(xval); //Check all intervals except the last one for (std::vector::const_iterator iPtr = _data.begin(); iPtr != _data.end()-1; ++iPtr) if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x)) return splineCalc(iPtr, xval); return splineCalc(_data.end() - 1, xval); } private: ///////PRIVATE DATA MEMBERS struct SplineData { double x,a,b,c,d; }; //vector of calculated spline data std::vector _data; //Second derivative at each point ublas::vector _ddy; //Tracks whether the spline parameters have been calculated for //the current set of points bool _valid; //The boundary conditions BC_type _BCLow, _BCHigh; //The values of the boundary conditions double _BCLowVal, _BCHighVal; Spline_type _type; ///////PRIVATE FUNCTIONS //Function to calculate the value of a given spline at a point xval inline double splineCalc(std::vector::const_iterator i, double xval) { const double lx = xval - i->x; return ((i->a * lx + i->b) * lx + i->c) * lx + i->d; } inline double lowCalc(double xval) { const double lx = xval - x(0); if (_type == LINEAR) return lx * _BCHighVal + y(0); const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6; switch(_BCLow) { case FIXED_1ST_DERIV_BC: return lx * _BCLowVal + y(0); case FIXED_2ND_DERIV_BC: return lx * lx * _BCLowVal + firstDeriv * lx + y(0); case PARABOLIC_RUNOUT_BC: return lx * lx * _ddy[0] + lx * firstDeriv + y(0); } throw std::runtime_error("Unknown BC"); } inline double highCalc(double xval) { const double lx = xval - x(size() - 1); if (_type == LINEAR) return lx * _BCHighVal + y(size() - 1); const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2); switch(_BCHigh) { case FIXED_1ST_DERIV_BC: return lx * _BCHighVal + y(size() - 1); case FIXED_2ND_DERIV_BC: return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1); case PARABOLIC_RUNOUT_BC: return lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1); } throw std::runtime_error("Unknown BC"); } //These just provide access to the point data in a clean way inline double x(size_t i) const { return operator[](i).first; } inline double y(size_t i) const { return operator[](i).second; } inline double h(size_t i) const { return x(i+1) - x(i); } //Invert a arbitrary matrix using the boost ublas library template bool InvertMatrix(ublas::matrix A, ublas::matrix& inverse) { using namespace ublas; // create a permutation matrix for the LU-factorization permutation_matrix pm(A.size1()); // perform LU-factorization int res = lu_factorize(A,pm); if( res != 0 ) return false; // create identity matrix of "inverse" inverse.assign(ublas::identity_matrix(A.size1())); // backsubstitute to get the inverse lu_substitute(A, pm, inverse); return true; } //This function will recalculate the spline parameters and store //them in _data, ready for spline interpolation void generate() { if (size() < 2) throw std::runtime_error("Spline requires at least 2 points"); //If any spline points are at the same x location, we have to //just slightly seperate them { bool testPassed(false); while (!testPassed) { testPassed = true; std::sort(base::begin(), base::end()); for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr) if (iPtr->first == (iPtr+1)->first) { if ((iPtr+1)->first != 0) (iPtr+1)->first += (iPtr+1)->first * std::numeric_limits::epsilon() * 10; else (iPtr+1)->first = std::numeric_limits::epsilon() * 10; testPassed = false; break; } } } const size_t e = size() - 1; switch (_type) { case LINEAR: { _data.resize(e); for (size_t i(0); i < e; ++i) { _data[i].x = x(i); _data[i].a = 0; _data[i].b = 0; _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i)); _data[i].d = y(i); } break; } case CUBIC: { ublas::matrix A(size(), size()); for (size_t yv(0); yv <= e; ++yv) for (size_t xv(0); xv <= e; ++xv) A(xv,yv) = 0; for (size_t i(1); i < e; ++i) { A(i-1,i) = h(i-1); A(i,i) = 2 * (h(i-1) + h(i)); A(i+1,i) = h(i); } ublas::vector C(size()); for (size_t xv(0); xv <= e; ++xv) C(xv) = 0; for (size_t i(1); i < e; ++i) C(i) = 6 * ((y(i+1) - y(i)) / h(i) - (y(i) - y(i-1)) / h(i-1)); //Boundary conditions switch(_BCLow) { case FIXED_1ST_DERIV_BC: C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal); A(0,0) = 2 * h(0); A(1,0) = h(0); break; case FIXED_2ND_DERIV_BC: C(0) = _BCLowVal; A(0,0) = 1; break; case PARABOLIC_RUNOUT_BC: C(0) = 0; A(0,0) = 1; A(1,0) = -1; break; } switch(_BCHigh) { case FIXED_1ST_DERIV_BC: C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1)); A(e,e) = 2 * h(e - 1); A(e-1,e) = h(e - 1); break; case FIXED_2ND_DERIV_BC: C(e) = _BCHighVal; A(e,e) = 1; break; case PARABOLIC_RUNOUT_BC: C(e) = 0; A(e,e) = 1; A(e-1,e) = -1; break; } ublas::matrix AInv(size(), size()); InvertMatrix(A,AInv); _ddy = ublas::prod(C, AInv); _data.resize(size()-1); for (size_t i(0); i < e; ++i) { _data[i].x = x(i); _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i)); _data[i].b = _ddy(i) / 2; _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3; _data[i].d = y(i); } } } _valid = true; } }; ================================================ FILE: camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h ================================================ #ifndef EIGENQUATERNIONPARAMETERIZATION_H #define EIGENQUATERNIONPARAMETERIZATION_H #include "ceres/local_parameterization.h" namespace camodocal { class EigenQuaternionParameterization : public ceres::LocalParameterization { public: virtual ~EigenQuaternionParameterization() {} virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; virtual bool ComputeJacobian(const double* x, double* jacobian) const; virtual int GlobalSize() const { return 4; } virtual int LocalSize() const { return 3; } private: template void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; }; template void EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const { zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; } } #endif ================================================ FILE: camera_model/include/camodocal/gpl/EigenUtils.h ================================================ #ifndef EIGENUTILS_H #define EIGENUTILS_H #include #include "ceres/rotation.h" #include "camodocal/gpl/gpl.h" namespace camodocal { // Returns the 3D cross product skew symmetric matrix of a given 3D vector template Eigen::Matrix skew(const Eigen::Matrix& vec) { return (Eigen::Matrix() << T(0), -vec(2), vec(1), vec(2), T(0), -vec(0), -vec(1), vec(0), T(0)).finished(); } template typename Eigen::MatrixBase::PlainObject sqrtm(const Eigen::MatrixBase& A) { Eigen::SelfAdjointEigenSolver es(A); return es.operatorSqrt(); } template Eigen::Matrix AngleAxisToRotationMatrix(const Eigen::Matrix& rvec) { T angle = rvec.norm(); if (angle == T(0)) { return Eigen::Matrix::Identity(); } Eigen::Matrix axis; axis = rvec.normalized(); Eigen::Matrix rmat; rmat = Eigen::AngleAxis(angle, axis); return rmat; } template Eigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix& rvec) { Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec); return Eigen::Quaternion(rmat); } template void AngleAxisToQuaternion(const Eigen::Matrix& rvec, T* q) { Eigen::Quaternion quat = AngleAxisToQuaternion(rvec); q[0] = quat.x(); q[1] = quat.y(); q[2] = quat.z(); q[3] = quat.w(); } template Eigen::Matrix RotationToAngleAxis(const Eigen::Matrix & rmat) { Eigen::AngleAxis angleaxis; angleaxis.fromRotationMatrix(rmat); return angleaxis.angle() * angleaxis.axis(); } template void QuaternionToAngleAxis(const T* const q, Eigen::Matrix& rvec) { Eigen::Quaternion quat(q[3], q[0], q[1], q[2]); Eigen::Matrix rmat = quat.toRotationMatrix(); Eigen::AngleAxis angleaxis; angleaxis.fromRotationMatrix(rmat); rvec = angleaxis.angle() * angleaxis.axis(); } template Eigen::Matrix QuaternionToRotation(const T* const q) { T R[9]; ceres::QuaternionToRotation(q, R); Eigen::Matrix rmat; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { rmat(i,j) = R[i * 3 + j]; } } return rmat; } template void QuaternionToRotation(const T* const q, T* rot) { ceres::QuaternionToRotation(q, rot); } template Eigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion& q) { return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(), q.z(), q.w(), -q.x(), q.y(), -q.y(), q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(), q.w()).finished(); } template Eigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion& q) { return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(), -q.z(), q.w(), q.x(), q.y(), q.y(), -q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(), q.w()).finished(); } /// @param theta - rotation about screw axis /// @param d - projection of tvec on the rotation axis /// @param l - screw axis direction /// @param m - screw axis moment template void AngleAxisAndTranslationToScrew(const Eigen::Matrix& rvec, const Eigen::Matrix& tvec, T& theta, T& d, Eigen::Matrix& l, Eigen::Matrix& m) { theta = rvec.norm(); if (theta == 0) { l.setZero(); m.setZero(); std::cout << "Warning: Undefined screw! Returned 0. " << std::endl; } l = rvec.normalized(); Eigen::Matrix t = tvec; d = t.transpose() * l; // point on screw axis - projection of origin on screw axis Eigen::Matrix c; c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t)); // c and hence the screw axis is not defined if theta is either 0 or M_PI m = c.cross(l); } template Eigen::Matrix RPY2mat(T roll, T pitch, T yaw) { Eigen::Matrix m; T cr = cos(roll); T sr = sin(roll); T cp = cos(pitch); T sp = sin(pitch); T cy = cos(yaw); T sy = sin(yaw); m(0,0) = cy * cp; m(0,1) = cy * sp * sr - sy * cr; m(0,2) = cy * sp * cr + sy * sr; m(1,0) = sy * cp; m(1,1) = sy * sp * sr + cy * cr; m(1,2) = sy * sp * cr - cy * sr; m(2,0) = - sp; m(2,1) = cp * sr; m(2,2) = cp * cr; return m; } template void mat2RPY(const Eigen::Matrix& m, T& roll, T& pitch, T& yaw) { roll = atan2(m(2,1), m(2,2)); pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2))); yaw = atan2(m(1,0), m(0,0)); } template Eigen::Matrix homogeneousTransform(const Eigen::Matrix& R, const Eigen::Matrix& t) { Eigen::Matrix H; H.setIdentity(); H.block(0,0,3,3) = R; H.block(0,3,3,1) = t; return H; } template Eigen::Matrix poseWithCartesianTranslation(const T* const q, const T* const p) { Eigen::Matrix pose = Eigen::Matrix::Identity(); T rotation[9]; ceres::QuaternionToRotation(q, rotation); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { pose(i,j) = rotation[i * 3 + j]; } } pose(0,3) = p[0]; pose(1,3) = p[1]; pose(2,3) = p[2]; return pose; } template Eigen::Matrix poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0)) { Eigen::Matrix pose = Eigen::Matrix::Identity(); T rotation[9]; ceres::QuaternionToRotation(q, rotation); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { pose(i,j) = rotation[i * 3 + j]; } } T theta = p[0]; T phi = p[1]; pose(0,3) = sin(theta) * cos(phi) * scale; pose(1,3) = sin(theta) * sin(phi) * scale; pose(2,3) = cos(theta) * scale; return pose; } // Returns the Sampson error of a given essential matrix and 2 image points template T sampsonError(const Eigen::Matrix& E, const Eigen::Matrix& p1, const Eigen::Matrix& p2) { Eigen::Matrix Ex1 = E * p1; Eigen::Matrix Etx2 = E.transpose() * p2; T x2tEx1 = p2.dot(Ex1); // compute Sampson error T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0))); return err; } // Returns the Sampson error of a given rotation/translation and 2 image points template T sampsonError(const Eigen::Matrix& R, const Eigen::Matrix& t, const Eigen::Matrix& p1, const Eigen::Matrix& p2) { // construct essential matrix Eigen::Matrix E = skew(t) * R; Eigen::Matrix Ex1 = E * p1; Eigen::Matrix Etx2 = E.transpose() * p2; T x2tEx1 = p2.dot(Ex1); // compute Sampson error T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0))); return err; } // Returns the Sampson error of a given rotation/translation and 2 image points template T sampsonError(const Eigen::Matrix& H, const Eigen::Matrix& p1, const Eigen::Matrix& p2) { Eigen::Matrix R = H.block(0, 0, 3, 3); Eigen::Matrix t = H.block(0, 3, 3, 1); return sampsonError(R, t, p1, p2); } template Eigen::Matrix transformPoint(const Eigen::Matrix& H, const Eigen::Matrix& P) { Eigen::Matrix P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1); return P_trans; } template Eigen::Matrix estimate3DRigidTransform(const std::vector, Eigen::aligned_allocator > >& points1, const std::vector, Eigen::aligned_allocator > >& points2) { // compute centroids Eigen::Matrix c1, c2; c1.setZero(); c2.setZero(); for (size_t i = 0; i < points1.size(); ++i) { c1 += points1.at(i); c2 += points2.at(i); } c1 /= points1.size(); c2 /= points1.size(); Eigen::Matrix X(3, points1.size()); Eigen::Matrix Y(3, points1.size()); for (size_t i = 0; i < points1.size(); ++i) { X.col(i) = points1.at(i) - c1; Y.col(i) = points2.at(i) - c2; } Eigen::Matrix H = X * Y.transpose(); Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix U = svd.matrixU(); Eigen::Matrix V = svd.matrixV(); if (U.determinant() * V.determinant() < 0.0) { V.col(2) *= -1.0; } Eigen::Matrix R = V * U.transpose(); Eigen::Matrix t = c2 - R * c1; return homogeneousTransform(R, t); } template Eigen::Matrix estimate3DRigidSimilarityTransform(const std::vector, Eigen::aligned_allocator > >& points1, const std::vector, Eigen::aligned_allocator > >& points2) { // compute centroids Eigen::Matrix c1, c2; c1.setZero(); c2.setZero(); for (size_t i = 0; i < points1.size(); ++i) { c1 += points1.at(i); c2 += points2.at(i); } c1 /= points1.size(); c2 /= points1.size(); Eigen::Matrix X(3, points1.size()); Eigen::Matrix Y(3, points1.size()); for (size_t i = 0; i < points1.size(); ++i) { X.col(i) = points1.at(i) - c1; Y.col(i) = points2.at(i) - c2; } Eigen::Matrix H = X * Y.transpose(); Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix U = svd.matrixU(); Eigen::Matrix V = svd.matrixV(); if (U.determinant() * V.determinant() < 0.0) { V.col(2) *= -1.0; } Eigen::Matrix R = V * U.transpose(); std::vector, Eigen::aligned_allocator > > rotatedPoints1(points1.size()); for (size_t i = 0; i < points1.size(); ++i) { rotatedPoints1.at(i) = R * (points1.at(i) - c1); } double sum_ss = 0.0, sum_tt = 0.0; for (size_t i = 0; i < points1.size(); ++i) { sum_ss += (points1.at(i) - c1).squaredNorm(); sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i)); } double scale = sum_tt / sum_ss; Eigen::Matrix sR = scale * R; Eigen::Matrix t = c2 - sR * c1; return homogeneousTransform(sR, t); } } #endif ================================================ FILE: camera_model/include/camodocal/gpl/gpl.h ================================================ #ifndef GPL_H #define GPL_H #include #include #include namespace camodocal { template const T clamp(const T& v, const T& a, const T& b) { return std::min(b, std::max(a, v)); } double hypot3(double x, double y, double z); float hypot3f(float x, float y, float z); template const T normalizeTheta(const T& theta) { T normTheta = theta; while (normTheta < - M_PI) { normTheta += 2.0 * M_PI; } while (normTheta > M_PI) { normTheta -= 2.0 * M_PI; } return normTheta; } double d2r(double deg); float d2r(float deg); double r2d(double rad); float r2d(float rad); double sinc(double theta); template const T square(const T& x) { return x * x; } template const T cube(const T& x) { return x * x * x; } template const T random(const T& a, const T& b) { return static_cast(rand()) / RAND_MAX * (b - a) + a; } template const T randomNormal(const T& sigma) { T x1, x2, w; do { x1 = 2.0 * random(0.0, 1.0) - 1.0; x2 = 2.0 * random(0.0, 1.0) - 1.0; w = x1 * x1 + x2 * x2; } while (w >= 1.0 || w == 0.0); w = sqrt((-2.0 * log(w)) / w); return x1 * w * sigma; } unsigned long long timeInMicroseconds(void); double timeInSeconds(void); void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth, float minRange, float maxRange); bool colormap(const std::string& name, unsigned char idx, float& r, float& g, float& b); std::vector bresLine(int x0, int y0, int x1, int y1); std::vector bresCircle(int x0, int y0, int r); void fitCircle(const std::vector& points, double& centerX, double& centerY, double& radius); std::vector intersectCircles(double x1, double y1, double r1, double x2, double y2, double r2); void LLtoUTM(double latitude, double longitude, double& utmNorthing, double& utmEasting, std::string& utmZone); void UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, double& latitude, double& longitude); long int timestampDiff(uint64_t t1, uint64_t t2); } #endif ================================================ FILE: camera_model/include/camodocal/sparse_graph/Transform.h ================================================ #ifndef TRANSFORM_H #define TRANSFORM_H #include #include #include namespace camodocal { class Transform { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Transform(); Transform(const Eigen::Matrix4d& H); Eigen::Quaterniond& rotation(void); const Eigen::Quaterniond& rotation(void) const; double* rotationData(void); const double* const rotationData(void) const; Eigen::Vector3d& translation(void); const Eigen::Vector3d& translation(void) const; double* translationData(void); const double* const translationData(void) const; Eigen::Matrix4d toMatrix(void) const; private: Eigen::Quaterniond m_q; Eigen::Vector3d m_t; }; } #endif ================================================ FILE: camera_model/instruction ================================================ rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ ================================================ FILE: camera_model/package.xml ================================================ camera_model 0.0.0 The camera_model package dvorak TODO catkin ================================================ FILE: camera_model/readme.md ================================================ part of [camodocal](https://github.com/hengli/camodocal) [Google Ceres](http://ceres-solver.org) is needed. # Calibration: Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. # Undistortion: See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: - liftProjective: Lift points from the image plane to the projective space. - spaceToPlane: Projects 3D points to the image plane (Pi function) ================================================ FILE: camera_model/src/calib/CameraCalibration.cc ================================================ #include "camodocal/calib/CameraCalibration.h" #include #include #include #include #include #include #include #include #include #include #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/sparse_graph/Transform.h" #include "camodocal/gpl/EigenQuaternionParameterization.h" #include "camodocal/gpl/EigenUtils.h" #include "camodocal/camera_models/CostFunctionFactory.h" #include "ceres/ceres.h" namespace camodocal { CameraCalibration::CameraCalibration() : m_boardSize(cv::Size(0,0)) , m_squareSize(0.0f) , m_verbose(false) { } CameraCalibration::CameraCalibration(const Camera::ModelType modelType, const std::string& cameraName, const cv::Size& imageSize, const cv::Size& boardSize, float squareSize) : m_boardSize(boardSize) , m_squareSize(squareSize) , m_verbose(false) { m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize); } void CameraCalibration::clear(void) { m_imagePoints.clear(); m_scenePoints.clear(); } void CameraCalibration::addChessboardData(const std::vector& corners) { m_imagePoints.push_back(corners); std::vector scenePointsInView; for (int i = 0; i < m_boardSize.height; ++i) { for (int j = 0; j < m_boardSize.width; ++j) { scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0)); } } m_scenePoints.push_back(scenePointsInView); } bool CameraCalibration::calibrate(void) { int imageCount = m_imagePoints.size(); // compute intrinsic camera parameters and extrinsic parameters for each of the views std::vector rvecs; std::vector tvecs; bool ret = calibrateHelper(m_camera, rvecs, tvecs); m_cameraPoses = cv::Mat(imageCount, 6, CV_64F); for (int i = 0; i < imageCount; ++i) { m_cameraPoses.at(i,0) = rvecs.at(i).at(0); m_cameraPoses.at(i,1) = rvecs.at(i).at(1); m_cameraPoses.at(i,2) = rvecs.at(i).at(2); m_cameraPoses.at(i,3) = tvecs.at(i).at(0); m_cameraPoses.at(i,4) = tvecs.at(i).at(1); m_cameraPoses.at(i,5) = tvecs.at(i).at(2); } // Compute measurement covariance. std::vector > errVec(m_imagePoints.size()); Eigen::Vector2d errSum = Eigen::Vector2d::Zero(); size_t errCount = 0; for (size_t i = 0; i < m_imagePoints.size(); ++i) { std::vector estImagePoints; m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { cv::Point2f pObs = m_imagePoints.at(i).at(j); cv::Point2f pEst = estImagePoints.at(j); cv::Point2f err = pObs - pEst; errVec.at(i).push_back(err); errSum += Eigen::Vector2d(err.x, err.y); } errCount += m_imagePoints.at(i).size(); } Eigen::Vector2d errMean = errSum / static_cast(errCount); Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero(); for (size_t i = 0; i < errVec.size(); ++i) { for (size_t j = 0; j < errVec.at(i).size(); ++j) { cv::Point2f err = errVec.at(i).at(j); double d0 = err.x - errMean(0); double d1 = err.y - errMean(1); measurementCovariance(0,0) += d0 * d0; measurementCovariance(0,1) += d0 * d1; measurementCovariance(1,1) += d1 * d1; } } measurementCovariance /= static_cast(errCount); measurementCovariance(1,0) = measurementCovariance(0,1); m_measurementCovariance = measurementCovariance; return ret; } int CameraCalibration::sampleCount(void) const { return m_imagePoints.size(); } std::vector >& CameraCalibration::imagePoints(void) { return m_imagePoints; } const std::vector >& CameraCalibration::imagePoints(void) const { return m_imagePoints; } std::vector >& CameraCalibration::scenePoints(void) { return m_scenePoints; } const std::vector >& CameraCalibration::scenePoints(void) const { return m_scenePoints; } CameraPtr& CameraCalibration::camera(void) { return m_camera; } const CameraConstPtr CameraCalibration::camera(void) const { return m_camera; } Eigen::Matrix2d& CameraCalibration::measurementCovariance(void) { return m_measurementCovariance; } const Eigen::Matrix2d& CameraCalibration::measurementCovariance(void) const { return m_measurementCovariance; } cv::Mat& CameraCalibration::cameraPoses(void) { return m_cameraPoses; } const cv::Mat& CameraCalibration::cameraPoses(void) const { return m_cameraPoses; } void CameraCalibration::drawResults(std::vector& images) const { std::vector rvecs, tvecs; for (size_t i = 0; i < images.size(); ++i) { cv::Mat rvec(3, 1, CV_64F); rvec.at(0) = m_cameraPoses.at(i,0); rvec.at(1) = m_cameraPoses.at(i,1); rvec.at(2) = m_cameraPoses.at(i,2); cv::Mat tvec(3, 1, CV_64F); tvec.at(0) = m_cameraPoses.at(i,3); tvec.at(1) = m_cameraPoses.at(i,4); tvec.at(2) = m_cameraPoses.at(i,5); rvecs.push_back(rvec); tvecs.push_back(tvec); } int drawShiftBits = 4; int drawMultiplier = 1 << drawShiftBits; cv::Scalar green(0, 255, 0); cv::Scalar red(0, 0, 255); for (size_t i = 0; i < images.size(); ++i) { cv::Mat& image = images.at(i); if (image.channels() == 1) { cv::cvtColor(image, image, CV_GRAY2RGB); } std::vector estImagePoints; m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); float errorSum = 0.0f; float errorMax = std::numeric_limits::min(); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { cv::Point2f pObs = m_imagePoints.at(i).at(j); cv::Point2f pEst = estImagePoints.at(j); cv::circle(image, cv::Point(cvRound(pObs.x * drawMultiplier), cvRound(pObs.y * drawMultiplier)), 5, green, 2, CV_AA, drawShiftBits); cv::circle(image, cv::Point(cvRound(pEst.x * drawMultiplier), cvRound(pEst.y * drawMultiplier)), 5, red, 2, CV_AA, drawShiftBits); float error = cv::norm(pObs - pEst); errorSum += error; if (error > errorMax) { errorMax = error; } } std::ostringstream oss; oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size() << " max = " << errorMax; cv::putText(image, oss.str(), cv::Point(10, image.rows - 10), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, CV_AA); } } void CameraCalibration::writeParams(const std::string& filename) const { m_camera->writeParametersToYamlFile(filename); } bool CameraCalibration::writeChessboardData(const std::string& filename) const { std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary); if (!ofs.is_open()) { return false; } writeData(ofs, m_boardSize.width); writeData(ofs, m_boardSize.height); writeData(ofs, m_squareSize); writeData(ofs, m_measurementCovariance(0,0)); writeData(ofs, m_measurementCovariance(0,1)); writeData(ofs, m_measurementCovariance(1,0)); writeData(ofs, m_measurementCovariance(1,1)); writeData(ofs, m_cameraPoses.rows); writeData(ofs, m_cameraPoses.cols); writeData(ofs, m_cameraPoses.type()); for (int i = 0; i < m_cameraPoses.rows; ++i) { for (int j = 0; j < m_cameraPoses.cols; ++j) { writeData(ofs, m_cameraPoses.at(i,j)); } } writeData(ofs, m_imagePoints.size()); for (size_t i = 0; i < m_imagePoints.size(); ++i) { writeData(ofs, m_imagePoints.at(i).size()); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { const cv::Point2f& ipt = m_imagePoints.at(i).at(j); writeData(ofs, ipt.x); writeData(ofs, ipt.y); } } writeData(ofs, m_scenePoints.size()); for (size_t i = 0; i < m_scenePoints.size(); ++i) { writeData(ofs, m_scenePoints.at(i).size()); for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { const cv::Point3f& spt = m_scenePoints.at(i).at(j); writeData(ofs, spt.x); writeData(ofs, spt.y); writeData(ofs, spt.z); } } return true; } bool CameraCalibration::readChessboardData(const std::string& filename) { std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary); if (!ifs.is_open()) { return false; } readData(ifs, m_boardSize.width); readData(ifs, m_boardSize.height); readData(ifs, m_squareSize); readData(ifs, m_measurementCovariance(0,0)); readData(ifs, m_measurementCovariance(0,1)); readData(ifs, m_measurementCovariance(1,0)); readData(ifs, m_measurementCovariance(1,1)); int rows, cols, type; readData(ifs, rows); readData(ifs, cols); readData(ifs, type); m_cameraPoses = cv::Mat(rows, cols, type); for (int i = 0; i < m_cameraPoses.rows; ++i) { for (int j = 0; j < m_cameraPoses.cols; ++j) { readData(ifs, m_cameraPoses.at(i,j)); } } size_t nImagePointSets; readData(ifs, nImagePointSets); m_imagePoints.clear(); m_imagePoints.resize(nImagePointSets); for (size_t i = 0; i < m_imagePoints.size(); ++i) { size_t nImagePoints; readData(ifs, nImagePoints); m_imagePoints.at(i).resize(nImagePoints); for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { cv::Point2f& ipt = m_imagePoints.at(i).at(j); readData(ifs, ipt.x); readData(ifs, ipt.y); } } size_t nScenePointSets; readData(ifs, nScenePointSets); m_scenePoints.clear(); m_scenePoints.resize(nScenePointSets); for (size_t i = 0; i < m_scenePoints.size(); ++i) { size_t nScenePoints; readData(ifs, nScenePoints); m_scenePoints.at(i).resize(nScenePoints); for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { cv::Point3f& spt = m_scenePoints.at(i).at(j); readData(ifs, spt.x); readData(ifs, spt.y); readData(ifs, spt.z); } } return true; } void CameraCalibration::setVerbose(bool verbose) { m_verbose = verbose; } bool CameraCalibration::calibrateHelper(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const { rvecs.assign(m_scenePoints.size(), cv::Mat()); tvecs.assign(m_scenePoints.size(), cv::Mat()); // STEP 1: Estimate intrinsics camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints); // STEP 2: Estimate extrinsics for (size_t i = 0; i < m_scenePoints.size(); ++i) { camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i)); } if (m_verbose) { std::cout << "[" << camera->cameraName() << "] " << "# INFO: " << "Initial reprojection error: " << std::fixed << std::setprecision(3) << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs) << " pixels" << std::endl; } // STEP 3: optimization using ceres optimize(camera, rvecs, tvecs); if (m_verbose) { double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs); std::cout << "[" << camera->cameraName() << "] " << "# INFO: Final reprojection error: " << err << " pixels" << std::endl; std::cout << "[" << camera->cameraName() << "] " << "# INFO: " << camera->parametersToString() << std::endl; } return true; } void CameraCalibration::optimize(CameraPtr& camera, std::vector& rvecs, std::vector& tvecs) const { // Use ceres to do optimization ceres::Problem problem; std::vector > transformVec(rvecs.size()); for (size_t i = 0; i < rvecs.size(); ++i) { Eigen::Vector3d rvec; cv::cv2eigen(rvecs.at(i), rvec); transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized()); transformVec.at(i).translation() << tvecs[i].at(0), tvecs[i].at(1), tvecs[i].at(2); } std::vector intrinsicCameraParams; m_camera->writeParameters(intrinsicCameraParams); // create residuals for each observation for (size_t i = 0; i < m_imagePoints.size(); ++i) { for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { const cv::Point3f& spt = m_scenePoints.at(i).at(j); const cv::Point2f& ipt = m_imagePoints.at(i).at(j); ceres::CostFunction* costFunction = CostFunctionFactory::instance()->generateCostFunction(camera, Eigen::Vector3d(spt.x, spt.y, spt.z), Eigen::Vector2d(ipt.x, ipt.y), CAMERA_INTRINSICS | CAMERA_POSE); ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0); problem.AddResidualBlock(costFunction, lossFunction, intrinsicCameraParams.data(), transformVec.at(i).rotationData(), transformVec.at(i).translationData()); } ceres::LocalParameterization* quaternionParameterization = new EigenQuaternionParameterization; problem.SetParameterization(transformVec.at(i).rotationData(), quaternionParameterization); } std::cout << "begin ceres" << std::endl; ceres::Solver::Options options; options.max_num_iterations = 1000; options.num_threads = 1; if (m_verbose) { options.minimizer_progress_to_stdout = true; } ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << "end ceres" << std::endl; if (m_verbose) { std::cout << summary.FullReport() << std::endl; } camera->readParameters(intrinsicCameraParams); for (size_t i = 0; i < rvecs.size(); ++i) { Eigen::AngleAxisd aa(transformVec.at(i).rotation()); Eigen::Vector3d rvec = aa.angle() * aa.axis(); cv::eigen2cv(rvec, rvecs.at(i)); cv::Mat& tvec = tvecs.at(i); tvec.at(0) = transformVec.at(i).translation()(0); tvec.at(1) = transformVec.at(i).translation()(1); tvec.at(2) = transformVec.at(i).translation()(2); } } template void CameraCalibration::readData(std::ifstream& ifs, T& data) const { char* buffer = new char[sizeof(T)]; ifs.read(buffer, sizeof(T)); data = *(reinterpret_cast(buffer)); delete[] buffer; } template void CameraCalibration::writeData(std::ofstream& ofs, T data) const { char* pData = reinterpret_cast(&data); ofs.write(pData, sizeof(T)); } } ================================================ FILE: camera_model/src/camera_models/Camera.cc ================================================ #include "camodocal/camera_models/Camera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" #include namespace camodocal { Camera::Parameters::Parameters(ModelType modelType) : m_modelType(modelType) , m_imageWidth(0) , m_imageHeight(0) { switch (modelType) { case KANNALA_BRANDT: m_nIntrinsics = 8; break; case PINHOLE: m_nIntrinsics = 8; break; case SCARAMUZZA: m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; break; case MEI: default: m_nIntrinsics = 9; } } Camera::Parameters::Parameters(ModelType modelType, const std::string& cameraName, int w, int h) : m_modelType(modelType) , m_cameraName(cameraName) , m_imageWidth(w) , m_imageHeight(h) { switch (modelType) { case KANNALA_BRANDT: m_nIntrinsics = 8; break; case PINHOLE: m_nIntrinsics = 8; break; case SCARAMUZZA: m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; break; case MEI: default: m_nIntrinsics = 9; } } Camera::ModelType& Camera::Parameters::modelType(void) { return m_modelType; } std::string& Camera::Parameters::cameraName(void) { return m_cameraName; } int& Camera::Parameters::imageWidth(void) { return m_imageWidth; } int& Camera::Parameters::imageHeight(void) { return m_imageHeight; } Camera::ModelType Camera::Parameters::modelType(void) const { return m_modelType; } const std::string& Camera::Parameters::cameraName(void) const { return m_cameraName; } int Camera::Parameters::imageWidth(void) const { return m_imageWidth; } int Camera::Parameters::imageHeight(void) const { return m_imageHeight; } int Camera::Parameters::nIntrinsics(void) const { return m_nIntrinsics; } cv::Mat& Camera::mask(void) { return m_mask; } const cv::Mat& Camera::mask(void) const { return m_mask; } void Camera::estimateExtrinsics(const std::vector& objectPoints, const std::vector& imagePoints, cv::Mat& rvec, cv::Mat& tvec) const { std::vector Ms(imagePoints.size()); for (size_t i = 0; i < Ms.size(); ++i) { Eigen::Vector3d P; liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); P /= P(2); Ms.at(i).x = P(0); Ms.at(i).y = P(1); } // assume unit focal length, zero principal point, and zero distortion cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); } double Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const { Eigen::Vector2d p1, p2; spaceToPlane(P1, p1); spaceToPlane(P2, p2); return (p1 - p2).norm(); } double Camera::reprojectionError(const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints, const std::vector& rvecs, const std::vector& tvecs, cv::OutputArray _perViewErrors) const { int imageCount = objectPoints.size(); size_t pointsSoFar = 0; double totalErr = 0.0; bool computePerViewErrors = _perViewErrors.needed(); cv::Mat perViewErrors; if (computePerViewErrors) { _perViewErrors.create(imageCount, 1, CV_64F); perViewErrors = _perViewErrors.getMat(); } for (int i = 0; i < imageCount; ++i) { size_t pointCount = imagePoints.at(i).size(); pointsSoFar += pointCount; std::vector estImagePoints; projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); double err = 0.0; for (size_t j = 0; j < imagePoints.at(i).size(); ++j) { err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); } if (computePerViewErrors) { perViewErrors.at(i) = err / pointCount; } totalErr += err; } return totalErr / pointsSoFar; } double Camera::reprojectionError(const Eigen::Vector3d& P, const Eigen::Quaterniond& camera_q, const Eigen::Vector3d& camera_t, const Eigen::Vector2d& observed_p) const { Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; Eigen::Vector2d p; spaceToPlane(P_cam, p); return (p - observed_p).norm(); } void Camera::projectPoints(const std::vector& objectPoints, const cv::Mat& rvec, const cv::Mat& tvec, std::vector& imagePoints) const { // project 3D object points to the image plane imagePoints.reserve(objectPoints.size()); //double cv::Mat R0; cv::Rodrigues(rvec, R0); Eigen::MatrixXd R(3,3); R << R0.at(0,0), R0.at(0,1), R0.at(0,2), R0.at(1,0), R0.at(1,1), R0.at(1,2), R0.at(2,0), R0.at(2,1), R0.at(2,2); Eigen::Vector3d t; t << tvec.at(0), tvec.at(1), tvec.at(2); for (size_t i = 0; i < objectPoints.size(); ++i) { const cv::Point3f& objectPoint = objectPoints.at(i); // Rotate and translate Eigen::Vector3d P; P << objectPoint.x, objectPoint.y, objectPoint.z; P = R * P + t; Eigen::Vector2d p; spaceToPlane(P, p); imagePoints.push_back(cv::Point2f(p(0), p(1))); } } } ================================================ FILE: camera_model/src/camera_models/CameraFactory.cc ================================================ #include "camodocal/camera_models/CameraFactory.h" #include #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/EquidistantCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" #include "ceres/ceres.h" namespace camodocal { boost::shared_ptr CameraFactory::m_instance; CameraFactory::CameraFactory() { } boost::shared_ptr CameraFactory::instance(void) { if (m_instance.get() == 0) { m_instance.reset(new CameraFactory); } return m_instance; } CameraPtr CameraFactory::generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const { switch (modelType) { case Camera::KANNALA_BRANDT: { EquidistantCameraPtr camera(new EquidistantCamera); EquidistantCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::PINHOLE: { PinholeCameraPtr camera(new PinholeCamera); PinholeCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::SCARAMUZZA: { OCAMCameraPtr camera(new OCAMCamera); OCAMCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::MEI: default: { CataCameraPtr camera(new CataCamera); CataCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } } } CameraPtr CameraFactory::generateCameraFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return CameraPtr(); } Camera::ModelType modelType = Camera::MEI; if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (boost::iequals(sModelType, "kannala_brandt")) { modelType = Camera::KANNALA_BRANDT; } else if (boost::iequals(sModelType, "mei")) { modelType = Camera::MEI; } else if (boost::iequals(sModelType, "scaramuzza")) { modelType = Camera::SCARAMUZZA; } else if (boost::iequals(sModelType, "pinhole")) { modelType = Camera::PINHOLE; } else { std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; return CameraPtr(); } } switch (modelType) { case Camera::KANNALA_BRANDT: { EquidistantCameraPtr camera(new EquidistantCamera); EquidistantCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::PINHOLE: { PinholeCameraPtr camera(new PinholeCamera); PinholeCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::SCARAMUZZA: { OCAMCameraPtr camera(new OCAMCamera); OCAMCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::MEI: default: { CataCameraPtr camera(new CataCamera); CataCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } } return CameraPtr(); } } ================================================ FILE: camera_model/src/camera_models/CataCamera.cc ================================================ #include "camodocal/camera_models/CataCamera.h" #include #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" namespace camodocal { CataCamera::Parameters::Parameters() : Camera::Parameters(MEI) , m_xi(0.0) , m_k1(0.0) , m_k2(0.0) , m_p1(0.0) , m_p2(0.0) , m_gamma1(0.0) , m_gamma2(0.0) , m_u0(0.0) , m_v0(0.0) { } CataCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0) : Camera::Parameters(MEI, cameraName, w, h) , m_xi(xi) , m_k1(k1) , m_k2(k2) , m_p1(p1) , m_p2(p2) , m_gamma1(gamma1) , m_gamma2(gamma2) , m_u0(u0) , m_v0(v0) { } double& CataCamera::Parameters::xi(void) { return m_xi; } double& CataCamera::Parameters::k1(void) { return m_k1; } double& CataCamera::Parameters::k2(void) { return m_k2; } double& CataCamera::Parameters::p1(void) { return m_p1; } double& CataCamera::Parameters::p2(void) { return m_p2; } double& CataCamera::Parameters::gamma1(void) { return m_gamma1; } double& CataCamera::Parameters::gamma2(void) { return m_gamma2; } double& CataCamera::Parameters::u0(void) { return m_u0; } double& CataCamera::Parameters::v0(void) { return m_v0; } double CataCamera::Parameters::xi(void) const { return m_xi; } double CataCamera::Parameters::k1(void) const { return m_k1; } double CataCamera::Parameters::k2(void) const { return m_k2; } double CataCamera::Parameters::p1(void) const { return m_p1; } double CataCamera::Parameters::p2(void) const { return m_p2; } double CataCamera::Parameters::gamma1(void) const { return m_gamma1; } double CataCamera::Parameters::gamma2(void) const { return m_gamma2; } double CataCamera::Parameters::u0(void) const { return m_u0; } double CataCamera::Parameters::v0(void) const { return m_v0; } bool CataCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("MEI") != 0) { return false; } } m_modelType = MEI; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["mirror_parameters"]; m_xi = static_cast(n["xi"]); n = fs["distortion_parameters"]; m_k1 = static_cast(n["k1"]); m_k2 = static_cast(n["k2"]); m_p1 = static_cast(n["p1"]); m_p2 = static_cast(n["p2"]); n = fs["projection_parameters"]; m_gamma1 = static_cast(n["gamma1"]); m_gamma2 = static_cast(n["gamma2"]); m_u0 = static_cast(n["u0"]); m_v0 = static_cast(n["v0"]); return true; } void CataCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "MEI"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; // mirror: xi fs << "mirror_parameters"; fs << "{" << "xi" << m_xi << "}"; // radial distortion: k1, k2 // tangential distortion: p1, p2 fs << "distortion_parameters"; fs << "{" << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; // projection: gamma1, gamma2, u0, v0 fs << "projection_parameters"; fs << "{" << "gamma1" << m_gamma1 << "gamma2" << m_gamma2 << "u0" << m_u0 << "v0" << m_v0 << "}"; fs.release(); } CataCamera::Parameters& CataCamera::Parameters::operator=(const CataCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_xi = other.m_xi; m_k1 = other.m_k1; m_k2 = other.m_k2; m_p1 = other.m_p1; m_p2 = other.m_p2; m_gamma1 = other.m_gamma1; m_gamma2 = other.m_gamma2; m_u0 = other.m_u0; m_v0 = other.m_v0; } return *this; } std::ostream& operator<< (std::ostream& out, const CataCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "MEI" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; out << "Mirror Parameters" << std::endl; out << std::fixed << std::setprecision(10); out << " xi " << params.m_xi << std::endl; // radial distortion: k1, k2 // tangential distortion: p1, p2 out << "Distortion Parameters" << std::endl; out << " k1 " << params.m_k1 << std::endl << " k2 " << params.m_k2 << std::endl << " p1 " << params.m_p1 << std::endl << " p2 " << params.m_p2 << std::endl; // projection: gamma1, gamma2, u0, v0 out << "Projection Parameters" << std::endl; out << " gamma1 " << params.m_gamma1 << std::endl << " gamma2 " << params.m_gamma2 << std::endl << " u0 " << params.m_u0 << std::endl << " v0 " << params.m_v0 << std::endl; return out; } CataCamera::CataCamera() : m_inv_K11(1.0) , m_inv_K13(0.0) , m_inv_K22(1.0) , m_inv_K23(0.0) , m_noDistortion(true) { } CataCamera::CataCamera(const std::string& cameraName, int imageWidth, int imageHeight, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0) : mParameters(cameraName, imageWidth, imageHeight, xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } CataCamera::CataCamera(const CataCamera::Parameters& params) : mParameters(params) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } Camera::ModelType CataCamera::modelType(void) const { return mParameters.modelType(); } const std::string& CataCamera::cameraName(void) const { return mParameters.cameraName(); } int CataCamera::imageWidth(void) const { return mParameters.imageWidth(); } int CataCamera::imageHeight(void) const { return mParameters.imageHeight(); } void CataCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { Parameters params = getParameters(); double u0 = params.imageWidth() / 2.0; double v0 = params.imageHeight() / 2.0; double gamma0 = 0.0; double minReprojErr = std::numeric_limits::max(); std::vector rvecs, tvecs; rvecs.assign(objectPoints.size(), cv::Mat()); tvecs.assign(objectPoints.size(), cv::Mat()); params.xi() = 1.0; params.k1() = 0.0; params.k2() = 0.0; params.p1() = 0.0; params.p2() = 0.0; params.u0() = u0; params.v0() = v0; // Initialize gamma (focal length) // Use non-radial line image and xi = 1 for (size_t i = 0; i < imagePoints.size(); ++i) { for (int r = 0; r < boardSize.height; ++r) { cv::Mat P(boardSize.width, 4, CV_64F); for (int c = 0; c < boardSize.width; ++c) { const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c); double u = imagePoint.x - u0; double v = imagePoint.y - v0; P.at(c, 0) = u; P.at(c, 1) = v; P.at(c, 2) = 0.5; P.at(c, 3) = -0.5 * (square(u) + square(v)); } cv::Mat C; cv::SVD::solveZ(P, C); double t = square(C.at(0)) + square(C.at(1)) + C.at(2) * C.at(3); if (t < 0.0) { continue; } // check that line image is not radial double d = sqrt(1.0 / t); double nx = C.at(0) * d; double ny = C.at(1) * d; if (hypot(nx, ny) > 0.95) { continue; } double gamma = sqrt(C.at(2) / C.at(3)); params.gamma1() = gamma; params.gamma2() = gamma; setParameters(params); for (size_t j = 0; j < objectPoints.size(); ++j) { estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j)); } double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); if (reprojErr < minReprojErr) { minReprojErr = reprojErr; gamma0 = gamma; } } } if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { std::cout << "[" << params.cameraName() << "] " << "# INFO: CataCamera model fails with given data. " << std::endl; return; } params.gamma1() = gamma0; params.gamma2() = gamma0; setParameters(params); } /** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ void CataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; double lambda; // Lift points to normalised plane mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { // Apply inverse distortion model if (0) { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); // Inverse distortion model // proposed by Heikkila mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } else { // Recursive distortion model int n = 6; Eigen::Vector2d d_u; distortion(Eigen::Vector2d(mx_d, my_d), d_u); // Approximate value mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); for (int i = 1; i < n; ++i) { distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Lift normalised points to the sphere (inv_hslash) double xi = mParameters.xi(); if (xi == 1.0) { lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0); P << lambda * mx_u, lambda * my_u, lambda - 1.0; } else { 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); P << lambda * mx_u, lambda * my_u, lambda - xi; } } /** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ void CataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; //double lambda; // Lift points to normalised plane mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { if (0) { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); // Apply inverse distortion model // proposed by Heikkila mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } else { // Recursive distortion model int n = 8; Eigen::Vector2d d_u; distortion(Eigen::Vector2d(mx_d, my_d), d_u); // Approximate value mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); for (int i = 1; i < n; ++i) { distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Obtain a projective ray double xi = mParameters.xi(); if (xi == 1.0) { P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0; } else { // Reuse variable rho2_d = mx_u * mx_u + my_u * my_u; P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d)); } } /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { Eigen::Vector2d p_u, p_d; // Project points to the normalised plane double z = P(2) + mParameters.xi() * P.norm(); p_u << P(0) / z, P(1) / z; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.gamma1() * p_d(0) + mParameters.u0(), mParameters.gamma2() * p_d(1) + mParameters.v0(); } #if 0 /** * \brief Project a 3D point to the image plane and calculate Jacobian * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const { double xi = mParameters.xi(); Eigen::Vector2d p_u, p_d; double norm, inv_denom; double dxdmx, dydmx, dxdmy, dydmy; norm = P.norm(); // Project points to the normalised plane inv_denom = 1.0 / (P(2) + xi * norm); p_u << inv_denom * P(0), inv_denom * P(1); // Calculate jacobian inv_denom = inv_denom * inv_denom / norm; double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2))); double dvdx = -inv_denom * xi * P(0) * P(1); double dudy = dvdx; double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2))); inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable double dudz = P(0) * inv_denom; double dvdz = P(1) * inv_denom; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } double gamma1 = mParameters.gamma1(); double gamma2 = mParameters.gamma2(); // Make the product of the jacobians // and add projection matrix jacobian inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy); dudx = inv_denom; inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy); dudy = inv_denom; inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy); dudz = inv_denom; // Apply generalised projection matrix p << gamma1 * p_d(0) + mParameters.u0(), gamma2 * p_d(1) + mParameters.v0(); J << dudx, dudy, dudz, dvdx, dvdy, dvdz; } #endif /** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ void CataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { Eigen::Vector2d p_d; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.gamma1() * p_d(0) + mParameters.u0(), mParameters.gamma2() * p_d(1) + mParameters.v0(); } /** * \brief Apply distortion to input point (from the normalised plane) * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } /** * \brief Apply distortion to input point (from the normalised plane) * and calculate Jacobian * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); 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); 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); double dxdmy = dydmx; 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); J << dxdmx, dxdmy, dydmx, dydmy; } void CataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; double xi = mParameters.xi(); double d2 = mx_u * mx_u + my_u * my_u; Eigen::Vector3d P; P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2)); Eigen::Vector2d p; spaceToPlane(P, p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } cv::Mat CataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f K_rect; if (cx == -1.0f && cy == -1.0f) { K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; } else { K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; } if (fx == -1.0f || fy == -1.0f) { K_rect(0,0) = mParameters.gamma1(); K_rect(1,1) = mParameters.gamma2(); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int CataCamera::parameterCount(void) const { return 9; } const CataCamera::Parameters& CataCamera::getParameters(void) const { return mParameters; } void CataCamera::setParameters(const CataCamera::Parameters& parameters) { mParameters = parameters; if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } void CataCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.xi() = parameterVec.at(0); params.k1() = parameterVec.at(1); params.k2() = parameterVec.at(2); params.p1() = parameterVec.at(3); params.p2() = parameterVec.at(4); params.gamma1() = parameterVec.at(5); params.gamma2() = parameterVec.at(6); params.u0() = parameterVec.at(7); params.v0() = parameterVec.at(8); setParameters(params); } void CataCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.xi(); parameterVec.at(1) = mParameters.k1(); parameterVec.at(2) = mParameters.k2(); parameterVec.at(3) = mParameters.p1(); parameterVec.at(4) = mParameters.p2(); parameterVec.at(5) = mParameters.gamma1(); parameterVec.at(6) = mParameters.gamma2(); parameterVec.at(7) = mParameters.u0(); parameterVec.at(8) = mParameters.v0(); } void CataCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string CataCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/camera_models/CostFunctionFactory.cc ================================================ #include "camodocal/camera_models/CostFunctionFactory.h" #include "ceres/ceres.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/EquidistantCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" namespace camodocal { template void worldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, T* q, T* t, bool optimize_cam_odo_z = true) { Eigen::Quaternion q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2))); Eigen::Quaternion q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0)); Eigen::Quaternion q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0)); Eigen::Quaternion q_zyx_inv = q_x_inv * q_y_inv * q_z_inv; T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()}; T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]}; T q0[4]; ceres::QuaternionProduct(q_odo_cam, q_odo, q0); T t0[3]; T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]}; ceres::QuaternionRotatePoint(q_odo, t_odo, t0); t0[0] += t_cam_odo[0]; t0[1] += t_cam_odo[1]; if (optimize_cam_odo_z) { t0[2] += t_cam_odo[2]; } ceres::QuaternionRotatePoint(q_odo_cam, t0, t); t[0] = -t[0]; t[1] = -t[1]; t[2] = -t[2]; // Convert quaternion from Ceres convention (w, x, y, z) // to Eigen convention (x, y, z, w) q[0] = q0[1]; q[1] = q0[2]; q[2] = q0[3]; q[3] = q0[0]; } template class ReprojectionError1 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReprojectionError1(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p) : m_observed_P(observed_P), m_observed_p(observed_p) , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {} ReprojectionError1(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat) : m_observed_P(observed_P), m_observed_p(observed_p) , m_sqrtPrecisionMat(sqrtPrecisionMat) {} ReprojectionError1(const std::vector& intrinsic_params, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params) , m_observed_P(observed_P), m_observed_p(observed_p) {} // variables: camera intrinsics and camera extrinsics template bool operator()(const T* const intrinsic_params, const T* const q, const T* const t, T* residuals) const { Eigen::Matrix P = m_observed_P.cast(); Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); Eigen::Matrix e = predicted_p - m_observed_p.cast(); Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; residuals[0] = e_weighted(0); residuals[1] = e_weighted(1); return true; } // variables: camera-odometry transforms and odometry poses template bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, T* residuals) const { T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t); Eigen::Matrix P = m_observed_P.cast(); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } //private: // camera intrinsics std::vector m_intrinsic_params; // observed 3D point Eigen::Vector3d m_observed_P; // observed 2D point Eigen::Vector2d m_observed_p; // square root of precision matrix Eigen::Matrix2d m_sqrtPrecisionMat; }; // variables: camera extrinsics, 3D point template class ReprojectionError2 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReprojectionError2(const std::vector& intrinsic_params, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {} template bool operator()(const T* const q, const T* const t, const T* const point, T* residuals) const { Eigen::Matrix P; P(0) = T(point[0]); P(1) = T(point[1]); P(2) = T(point[2]); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } private: // camera intrinsics std::vector m_intrinsic_params; // observed 2D point Eigen::Vector2d m_observed_p; }; template class ReprojectionError3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReprojectionError3(const Eigen::Vector2d& observed_p) : m_observed_p(observed_p) , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat) : m_observed_p(observed_p) , m_sqrtPrecisionMat(sqrtPrecisionMat) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params) , m_observed_p(observed_p) , m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat) : m_intrinsic_params(intrinsic_params) , m_observed_p(observed_p) , m_sqrtPrecisionMat(sqrtPrecisionMat) , m_optimize_cam_odo_z(true) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, bool optimize_cam_odo_z) : m_intrinsic_params(intrinsic_params) , m_odo_pos(odo_pos), m_odo_att(odo_att) , m_observed_p(observed_p) , m_optimize_cam_odo_z(optimize_cam_odo_z) {} ReprojectionError3(const std::vector& intrinsic_params, const Eigen::Quaterniond& cam_odo_q, const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p) : m_intrinsic_params(intrinsic_params) , m_cam_odo_q(cam_odo_q), m_cam_odo_t(cam_odo_t) , m_odo_pos(odo_pos), m_odo_att(odo_att) , m_observed_p(observed_p) , m_optimize_cam_odo_z(true) {} // variables: camera intrinsics, camera-to-odometry transform, // odometry extrinsics, 3D point template bool operator()(const T* const intrinsic_params, const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, const T* const point, T* residuals) const { T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); Eigen::Matrix err = predicted_p - m_observed_p.cast(); Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; residuals[0] = err_weighted(0); residuals[1] = err_weighted(1); return true; } // variables: camera-to-odometry transform, 3D point template bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const point, T* residuals) const { T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } // variables: camera-to-odometry transform, odometry extrinsics, 3D point template bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const p_odo, const T* const att_odo, const T* const point, T* residuals) const { T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); Eigen::Matrix err = predicted_p - m_observed_p.cast(); Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; residuals[0] = err_weighted(0); residuals[1] = err_weighted(1); return true; } // variables: 3D point template bool operator()(const T* const point, T* residuals) const { 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))}; T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))}; T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; T q[4], t[3]; worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); std::vector intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end()); Eigen::Matrix P(point[0], point[1], point[2]); // project 3D object point to the image plane Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); residuals[0] = predicted_p(0) - T(m_observed_p(0)); residuals[1] = predicted_p(1) - T(m_observed_p(1)); return true; } private: // camera intrinsics std::vector m_intrinsic_params; // observed camera-odometry transform Eigen::Quaterniond m_cam_odo_q; Eigen::Vector3d m_cam_odo_t; // observed odometry Eigen::Vector3d m_odo_pos; Eigen::Vector3d m_odo_att; // observed 2D point Eigen::Vector2d m_observed_p; Eigen::Matrix2d m_sqrtPrecisionMat; bool m_optimize_cam_odo_z; }; // variables: camera intrinsics and camera extrinsics template class StereoReprojectionError { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW StereoReprojectionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_l, const Eigen::Vector2d& observed_p_r) : m_observed_P(observed_P) , m_observed_p_l(observed_p_l) , m_observed_p_r(observed_p_r) { } template bool operator()(const T* const intrinsic_params_l, const T* const intrinsic_params_r, const T* const q_l, const T* const t_l, const T* const q_l_r, const T* const t_l_r, T* residuals) const { Eigen::Matrix P; P(0) = T(m_observed_P(0)); P(1) = T(m_observed_P(1)); P(2) = T(m_observed_P(2)); Eigen::Matrix predicted_p_l; CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l); Eigen::Quaternion q_r = Eigen::Quaternion(q_l_r) * Eigen::Quaternion(q_l); Eigen::Matrix t_r; t_r(0) = t_l[0]; t_r(1) = t_l[1]; t_r(2) = t_l[2]; t_r = Eigen::Quaternion(q_l_r) * t_r; t_r(0) += t_l_r[0]; t_r(1) += t_l_r[1]; t_r(2) += t_l_r[2]; Eigen::Matrix predicted_p_r; CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r); residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0)); residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1)); residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0)); residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1)); return true; } private: // observed 3D point Eigen::Vector3d m_observed_P; // observed 2D point Eigen::Vector2d m_observed_p_l; Eigen::Vector2d m_observed_p_r; }; template class ComprehensionError { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p) : m_observed_P(observed_P), m_observed_p(observed_p), m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) { } template bool operator()(const T* const intrinsic_params, const T* const q, const T* const t, T* residuals) const { { Eigen::Matrix p = m_observed_p.cast(); Eigen::Matrix predicted_img_P; CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P); Eigen::Matrix predicted_p; CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p); Eigen::Matrix e = predicted_p - m_observed_p.cast(); Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; residuals[0] = e_weighted(0); residuals[1] = e_weighted(1); } { Eigen::Matrix P = m_observed_P.cast(); Eigen::Matrix predicted_p; CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); Eigen::Matrix e = predicted_p - m_observed_p.cast(); Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; residuals[2] = e_weighted(0); residuals[3] = e_weighted(1); } return true; } // private: // camera intrinsics // std::vector m_intrinsic_params; // observed 3D point Eigen::Vector3d m_observed_P; // observed 2D point Eigen::Vector2d m_observed_p; // square root of precision matrix Eigen::Matrix2d m_sqrtPrecisionMat; }; boost::shared_ptr CostFunctionFactory::m_instance; CostFunctionFactory::CostFunctionFactory() { } boost::shared_ptr CostFunctionFactory::instance(void) { if (m_instance.get() == 0) { m_instance.reset(new CostFunctionFactory); } return m_instance; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, int flags) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_INTRINSICS | CAMERA_POSE: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3>( new ReprojectionError1(observed_P, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new ComprehensionError(observed_P, observed_p)); // new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( // new ReprojectionError1(observed_P, observed_p)); break; } break; case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3>( new ReprojectionError1(intrinsic_params, observed_P, observed_p)); break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_INTRINSICS | CAMERA_POSE: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new ReprojectionError1(observed_P, observed_p, sqrtPrecisionMat)); break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError2(intrinsic_params, observed_p)); break; } break; case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 2, 1, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; } break; case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p)); } break; } break; case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3>( new ReprojectionError3(observed_p)); } break; } break; case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p)); } break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, int flags, bool optimize_cam_odo_z) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3, 3, 3>( new ReprojectionError3(intrinsic_params, observed_p, sqrtPrecisionMat)); } break; } break; case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 8, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 9, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>( new ReprojectionError3(observed_p, sqrtPrecisionMat)); } break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags, bool optimize_cam_odo_z) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case CAMERA_ODOMETRY_TRANSFORM | POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; case Camera::PINHOLE: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; case Camera::MEI: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; case Camera::SCARAMUZZA: if (optimize_cam_odo_z) { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 3, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } else { costFunction = new ceres::AutoDiffCostFunction, 2, 4, 2, 3>( new ReprojectionError3(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z)); } break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera, const Eigen::Quaterniond& cam_odo_q, const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, int flags) const { ceres::CostFunction* costFunction = 0; std::vector intrinsic_params; camera->writeParameters(intrinsic_params); switch (flags) { case POINT_3D: switch (camera->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 2, 3>( new ReprojectionError3(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p)); break; } break; } return costFunction; } ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL, const CameraConstPtr& cameraR, const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_l, const Eigen::Vector2d& observed_p_r) const { ceres::CostFunction* costFunction = 0; if (cameraL->modelType() != cameraR->modelType()) { return costFunction; } switch (cameraL->modelType()) { case Camera::KANNALA_BRANDT: costFunction = new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; case Camera::PINHOLE: costFunction = new ceres::AutoDiffCostFunction, 4, 8, 8, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; case Camera::MEI: costFunction = new ceres::AutoDiffCostFunction, 4, 9, 9, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; case Camera::SCARAMUZZA: costFunction = new ceres::AutoDiffCostFunction, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>( new StereoReprojectionError(observed_P, observed_p_l, observed_p_r)); break; } return costFunction; } } ================================================ FILE: camera_model/src/camera_models/EquidistantCamera.cc ================================================ #include "camodocal/camera_models/EquidistantCamera.h" #include #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" namespace camodocal { EquidistantCamera::Parameters::Parameters() : Camera::Parameters(KANNALA_BRANDT) , m_k2(0.0) , m_k3(0.0) , m_k4(0.0) , m_k5(0.0) , m_mu(0.0) , m_mv(0.0) , m_u0(0.0) , m_v0(0.0) { } EquidistantCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0) : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h) , m_k2(k2) , m_k3(k3) , m_k4(k4) , m_k5(k5) , m_mu(mu) , m_mv(mv) , m_u0(u0) , m_v0(v0) { } double& EquidistantCamera::Parameters::k2(void) { return m_k2; } double& EquidistantCamera::Parameters::k3(void) { return m_k3; } double& EquidistantCamera::Parameters::k4(void) { return m_k4; } double& EquidistantCamera::Parameters::k5(void) { return m_k5; } double& EquidistantCamera::Parameters::mu(void) { return m_mu; } double& EquidistantCamera::Parameters::mv(void) { return m_mv; } double& EquidistantCamera::Parameters::u0(void) { return m_u0; } double& EquidistantCamera::Parameters::v0(void) { return m_v0; } double EquidistantCamera::Parameters::k2(void) const { return m_k2; } double EquidistantCamera::Parameters::k3(void) const { return m_k3; } double EquidistantCamera::Parameters::k4(void) const { return m_k4; } double EquidistantCamera::Parameters::k5(void) const { return m_k5; } double EquidistantCamera::Parameters::mu(void) const { return m_mu; } double EquidistantCamera::Parameters::mv(void) const { return m_mv; } double EquidistantCamera::Parameters::u0(void) const { return m_u0; } double EquidistantCamera::Parameters::v0(void) const { return m_v0; } bool EquidistantCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("KANNALA_BRANDT") != 0) { return false; } } m_modelType = KANNALA_BRANDT; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["projection_parameters"]; m_k2 = static_cast(n["k2"]); m_k3 = static_cast(n["k3"]); m_k4 = static_cast(n["k4"]); m_k5 = static_cast(n["k5"]); m_mu = static_cast(n["mu"]); m_mv = static_cast(n["mv"]); m_u0 = static_cast(n["u0"]); m_v0 = static_cast(n["v0"]); return true; } void EquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "KANNALA_BRANDT"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; // projection: k2, k3, k4, k5, mu, mv, u0, v0 fs << "projection_parameters"; fs << "{" << "k2" << m_k2 << "k3" << m_k3 << "k4" << m_k4 << "k5" << m_k5 << "mu" << m_mu << "mv" << m_mv << "u0" << m_u0 << "v0" << m_v0 << "}"; fs.release(); } EquidistantCamera::Parameters& EquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_k2 = other.m_k2; m_k3 = other.m_k3; m_k4 = other.m_k4; m_k5 = other.m_k5; m_mu = other.m_mu; m_mv = other.m_mv; m_u0 = other.m_u0; m_v0 = other.m_v0; } return *this; } std::ostream& operator<< (std::ostream& out, const EquidistantCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "KANNALA_BRANDT" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; // projection: k2, k3, k4, k5, mu, mv, u0, v0 out << "Projection Parameters" << std::endl; out << " k2 " << params.m_k2 << std::endl << " k3 " << params.m_k3 << std::endl << " k4 " << params.m_k4 << std::endl << " k5 " << params.m_k5 << std::endl << " mu " << params.m_mu << std::endl << " mv " << params.m_mv << std::endl << " u0 " << params.m_u0 << std::endl << " v0 " << params.m_v0 << std::endl; return out; } EquidistantCamera::EquidistantCamera() : m_inv_K11(1.0) , m_inv_K13(0.0) , m_inv_K22(1.0) , m_inv_K23(0.0) { } EquidistantCamera::EquidistantCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k2, double k3, double k4, double k5, double mu, double mv, double u0, double v0) : mParameters(cameraName, imageWidth, imageHeight, k2, k3, k4, k5, mu, mv, u0, v0) { // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.mu(); m_inv_K13 = -mParameters.u0() / mParameters.mu(); m_inv_K22 = 1.0 / mParameters.mv(); m_inv_K23 = -mParameters.v0() / mParameters.mv(); } EquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params) : mParameters(params) { // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.mu(); m_inv_K13 = -mParameters.u0() / mParameters.mu(); m_inv_K22 = 1.0 / mParameters.mv(); m_inv_K23 = -mParameters.v0() / mParameters.mv(); } Camera::ModelType EquidistantCamera::modelType(void) const { return mParameters.modelType(); } const std::string& EquidistantCamera::cameraName(void) const { return mParameters.cameraName(); } int EquidistantCamera::imageWidth(void) const { return mParameters.imageWidth(); } int EquidistantCamera::imageHeight(void) const { return mParameters.imageHeight(); } void EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { Parameters params = getParameters(); double u0 = params.imageWidth() / 2.0; double v0 = params.imageHeight() / 2.0; double minReprojErr = std::numeric_limits::max(); std::vector rvecs, tvecs; rvecs.assign(objectPoints.size(), cv::Mat()); tvecs.assign(objectPoints.size(), cv::Mat()); params.k2() = 0.0; params.k3() = 0.0; params.k4() = 0.0; params.k5() = 0.0; params.u0() = u0; params.v0() = v0; // Initialize focal length // C. Hughes, P. Denny, M. Glavin, and E. Jones, // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point // Extraction, PAMI 2010 // Find circles from rows of chessboard corners, and for each pair // of circles, find vanishing points: v1 and v2. // f = ||v1 - v2|| / PI; double f0 = 0.0; for (size_t i = 0; i < imagePoints.size(); ++i) { std::vector center(boardSize.height); double radius[boardSize.height]; for (int r = 0; r < boardSize.height; ++r) { std::vector circle; for (int c = 0; c < boardSize.width; ++c) { circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); } fitCircle(circle, center[r](0), center[r](1), radius[r]); } for (int j = 0; j < boardSize.height; ++j) { for (int k = j + 1; k < boardSize.height; ++k) { // find distance between pair of vanishing points which // correspond to intersection points of 2 circles std::vector ipts; ipts = intersectCircles(center[j](0), center[j](1), radius[j], center[k](0), center[k](1), radius[k]); if (ipts.size() < 2) { continue; } double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; params.mu() = f; params.mv() = f; setParameters(params); for (size_t l = 0; l < objectPoints.size(); ++l) { estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l)); } double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); if (reprojErr < minReprojErr) { minReprojErr = reprojErr; f0 = f; } } } } if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { std::cout << "[" << params.cameraName() << "] " << "# INFO: kannala-Brandt model fails with given data. " << std::endl; return; } params.mu() = f0; params.mv() = f0; setParameters(params); } /** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ void EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { liftProjective(p, P); } /** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ void EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { // Lift points to normalised plane Eigen::Vector2d p_u; p_u << m_inv_K11 * p(0) + m_inv_K13, m_inv_K22 * p(1) + m_inv_K23; // Obtain a projective ray double theta, phi; backprojectSymmetric(p_u, theta, phi); P(0) = sin(theta) * cos(phi); P(1) = sin(theta) * sin(phi); P(2) = cos(theta); } /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { double theta = acos(P(2) / P.norm()); double phi = atan2(P(1), P(0)); Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); // Apply generalised projection matrix p << mParameters.mu() * p_u(0) + mParameters.u0(), mParameters.mv() * p_u(1) + mParameters.v0(); } /** * \brief Project a 3D point to the image plane and calculate Jacobian * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const { double theta = acos(P(2) / P.norm()); double phi = atan2(P(1), P(0)); Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); // Apply generalised projection matrix p << mParameters.mu() * p_u(0) + mParameters.u0(), mParameters.mv() * p_u(1) + mParameters.v0(); } /** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ void EquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { // Eigen::Vector2d p_d; // // if (m_noDistortion) // { // p_d = p_u; // } // else // { // // Apply distortion // Eigen::Vector2d d_u; // distortion(p_u, d_u); // p_d = p_u + d_u; // } // // // Apply generalised projection matrix // p << mParameters.gamma1() * p_d(0) + mParameters.u0(), // mParameters.gamma2() * p_d(1) + mParameters.v0(); } void EquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; double theta, phi; backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); Eigen::Vector3d P; P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta); Eigen::Vector2d p; spaceToPlane(P, p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } cv::Mat EquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f K_rect; if (cx == -1.0f && cy == -1.0f) { K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; } else { K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; } if (fx == -1.0f || fy == -1.0f) { K_rect(0,0) = mParameters.mu(); K_rect(1,1) = mParameters.mv(); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int EquidistantCamera::parameterCount(void) const { return 8; } const EquidistantCamera::Parameters& EquidistantCamera::getParameters(void) const { return mParameters; } void EquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters) { mParameters = parameters; // Inverse camera projection matrix parameters m_inv_K11 = 1.0 / mParameters.mu(); m_inv_K13 = -mParameters.u0() / mParameters.mu(); m_inv_K22 = 1.0 / mParameters.mv(); m_inv_K23 = -mParameters.v0() / mParameters.mv(); } void EquidistantCamera::readParameters(const std::vector& parameterVec) { if (parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.k2() = parameterVec.at(0); params.k3() = parameterVec.at(1); params.k4() = parameterVec.at(2); params.k5() = parameterVec.at(3); params.mu() = parameterVec.at(4); params.mv() = parameterVec.at(5); params.u0() = parameterVec.at(6); params.v0() = parameterVec.at(7); setParameters(params); } void EquidistantCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.k2(); parameterVec.at(1) = mParameters.k3(); parameterVec.at(2) = mParameters.k4(); parameterVec.at(3) = mParameters.k5(); parameterVec.at(4) = mParameters.mu(); parameterVec.at(5) = mParameters.mv(); parameterVec.at(6) = mParameters.u0(); parameterVec.at(7) = mParameters.v0(); } void EquidistantCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string EquidistantCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } void EquidistantCamera::fitOddPoly(const std::vector& x, const std::vector& y, int n, std::vector& coeffs) const { std::vector pows; for (int i = 1; i <= n; i += 2) { pows.push_back(i); } Eigen::MatrixXd X(x.size(), pows.size()); Eigen::MatrixXd Y(y.size(), 1); for (size_t i = 0; i < x.size(); ++i) { for (size_t j = 0; j < pows.size(); ++j) { X(i,j) = pow(x.at(i), pows.at(j)); } Y(i,0) = y.at(i); } Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y; coeffs.resize(A.rows()); for (int i = 0; i < A.rows(); ++i) { coeffs.at(i) = A(i,0); } } void EquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u, double& theta, double& phi) const { double tol = 1e-10; double p_u_norm = p_u.norm(); if (p_u_norm < 1e-10) { phi = 0.0; } else { phi = atan2(p_u(1), p_u(0)); } int npow = 9; if (mParameters.k5() == 0.0) { npow -= 2; } if (mParameters.k4() == 0.0) { npow -= 2; } if (mParameters.k3() == 0.0) { npow -= 2; } if (mParameters.k2() == 0.0) { npow -= 2; } Eigen::MatrixXd coeffs(npow + 1, 1); coeffs.setZero(); coeffs(0) = -p_u_norm; coeffs(1) = 1.0; if (npow >= 3) { coeffs(3) = mParameters.k2(); } if (npow >= 5) { coeffs(5) = mParameters.k3(); } if (npow >= 7) { coeffs(7) = mParameters.k4(); } if (npow >= 9) { coeffs(9) = mParameters.k5(); } if (npow == 1) { theta = p_u_norm; } else { // Get eigenvalues of companion matrix corresponding to polynomial. // Eigenvalues correspond to roots of polynomial. Eigen::MatrixXd A(npow, npow); A.setZero(); A.block(1, 0, npow - 1, npow - 1).setIdentity(); A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow); Eigen::EigenSolver es(A); Eigen::MatrixXcd eigval = es.eigenvalues(); std::vector thetas; for (int i = 0; i < eigval.rows(); ++i) { if (fabs(eigval(i).imag()) > tol) { continue; } double t = eigval(i).real(); if (t < -tol) { continue; } else if (t < 0.0) { t = 0.0; } thetas.push_back(t); } if (thetas.empty()) { theta = p_u_norm; } else { theta = *std::min_element(thetas.begin(), thetas.end()); } } } } ================================================ FILE: camera_model/src/camera_models/PinholeCamera.cc ================================================ #include "camodocal/camera_models/PinholeCamera.h" #include #include #include #include #include #include #include #include using namespace std; #include "camodocal/gpl/gpl.h" namespace camodocal { PinholeCamera::Parameters::Parameters() : Camera::Parameters(PINHOLE) , m_k1(0.0) , m_k2(0.0) , m_p1(0.0) , m_p2(0.0) , m_fx(0.0) , m_fy(0.0) , m_cx(0.0) , m_cy(0.0) { } PinholeCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy) : Camera::Parameters(PINHOLE, cameraName, w, h) , m_k1(k1) , m_k2(k2) , m_p1(p1) , m_p2(p2) , m_fx(fx) , m_fy(fy) , m_cx(cx) , m_cy(cy) { } double& PinholeCamera::Parameters::k1(void) { return m_k1; } double& PinholeCamera::Parameters::k2(void) { return m_k2; } double& PinholeCamera::Parameters::p1(void) { return m_p1; } double& PinholeCamera::Parameters::p2(void) { return m_p2; } double& PinholeCamera::Parameters::fx(void) { return m_fx; } double& PinholeCamera::Parameters::fy(void) { return m_fy; } double& PinholeCamera::Parameters::cx(void) { return m_cx; } double& PinholeCamera::Parameters::cy(void) { return m_cy; } double PinholeCamera::Parameters::k1(void) const { return m_k1; } double PinholeCamera::Parameters::k2(void) const { return m_k2; } double PinholeCamera::Parameters::p1(void) const { return m_p1; } double PinholeCamera::Parameters::p2(void) const { return m_p2; } double PinholeCamera::Parameters::fx(void) const { return m_fx; } double PinholeCamera::Parameters::fy(void) const { return m_fy; } double PinholeCamera::Parameters::cx(void) const { return m_cx; } double PinholeCamera::Parameters::cy(void) const { return m_cy; } bool PinholeCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("PINHOLE") != 0) { return false; } } m_modelType = PINHOLE; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["distortion_parameters"]; m_k1 = static_cast(n["k1"]); m_k2 = static_cast(n["k2"]); m_p1 = static_cast(n["p1"]); m_p2 = static_cast(n["p2"]); n = fs["projection_parameters"]; m_fx = static_cast(n["fx"]); m_fy = static_cast(n["fy"]); m_cx = static_cast(n["cx"]); m_cy = static_cast(n["cy"]); cout << "m_k1:" << m_k1 << " m_k2" << m_k2 << " m_p1:"< >& objectPoints, const std::vector< std::vector >& imagePoints) { // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 Parameters params = getParameters(); params.k1() = 0.0; params.k2() = 0.0; params.p1() = 0.0; params.p2() = 0.0; double cx = params.imageWidth() / 2.0; double cy = params.imageHeight() / 2.0; params.cx() = cx; params.cy() = cy; size_t nImages = imagePoints.size(); cv::Mat A(nImages * 2, 2, CV_64F); cv::Mat b(nImages * 2, 1, CV_64F); for (size_t i = 0; i < nImages; ++i) { const std::vector& oPoints = objectPoints.at(i); std::vector M(oPoints.size()); for (size_t j = 0; j < M.size(); ++j) { M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y); } cv::Mat H = cv::findHomography(M, imagePoints.at(i)); H.at(0,0) -= H.at(2,0) * cx; H.at(0,1) -= H.at(2,1) * cx; H.at(0,2) -= H.at(2,2) * cx; H.at(1,0) -= H.at(2,0) * cy; H.at(1,1) -= H.at(2,1) * cy; H.at(1,2) -= H.at(2,2) * cy; double h[3], v[3], d1[3], d2[3]; double n[4] = {0,0,0,0}; for (int j = 0; j < 3; ++j) { double t0 = H.at(j,0); double t1 = H.at(j,1); h[j] = t0; v[j] = t1; d1[j] = (t0 + t1) * 0.5; d2[j] = (t0 - t1) * 0.5; n[0] += t0 * t0; n[1] += t1 * t1; n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j]; } for (int j = 0; j < 4; ++j) { n[j] = 1.0 / sqrt(n[j]); } for (int j = 0; j < 3; ++j) { h[j] *= n[0]; v[j] *= n[1]; d1[j] *= n[2]; d2[j] *= n[3]; } A.at(i * 2, 0) = h[0] * v[0]; A.at(i * 2, 1) = h[1] * v[1]; A.at(i * 2 + 1, 0) = d1[0] * d2[0]; A.at(i * 2 + 1, 1) = d1[1] * d2[1]; b.at(i * 2, 0) = -h[2] * v[2]; b.at(i * 2 + 1, 0) = -d1[2] * d2[2]; } cv::Mat f(2, 1, CV_64F); cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU); params.fx() = sqrt(fabs(1.0 / f.at(0))); params.fy() = sqrt(fabs(1.0 / f.at(1))); setParameters(params); } /** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ void PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { liftProjective(p, P); P.normalize(); } /** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; //double lambda; // Lift points to normalised plane mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { if (0) { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); // Apply inverse distortion model // proposed by Heikkila mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } else { // Recursive distortion model int n = 8; Eigen::Vector2d d_u; distortion(Eigen::Vector2d(mx_d, my_d), d_u); // Approximate value mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); for (int i = 1; i < n; ++i) { distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Obtain a projective ray //cout<<"lift:mx_u:"<& J) const { Eigen::Vector2d p_u, p_d; double norm, inv_denom; double dxdmx, dydmx, dxdmy, dydmy; norm = P.norm(); // Project points to the normalised plane inv_denom = 1.0 / P(2); p_u << inv_denom * P(0), inv_denom * P(1); // Calculate jacobian double dudx = inv_denom; double dvdx = 0.0; double dudy = 0.0; double dvdy = inv_denom; inv_denom = - inv_denom * inv_denom; double dudz = P(0) * inv_denom; double dvdz = P(1) * inv_denom; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } double fx = mParameters.fx(); double fy = mParameters.fy(); // Make the product of the jacobians // and add projection matrix jacobian inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse dvdx = fy * (dudx * dydmx + dvdx * dydmy); dudx = inv_denom; inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse dvdy = fy * (dudy * dydmx + dvdy * dydmy); dudy = inv_denom; inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse dvdz = fy * (dudz * dydmx + dvdz * dydmy); dudz = inv_denom; // Apply generalised projection matrix p << fx * p_d(0) + mParameters.cx(), fy * p_d(1) + mParameters.cy(); J << dudx, dudy, dudz, dvdx, dvdy, dvdz; } #endif /** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ void PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { Eigen::Vector2d p_d; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.fx() * p_d(0) + mParameters.cx(), mParameters.fy() * p_d(1) + mParameters.cy(); } /** * \brief Apply distortion to input point (from the normalised plane) * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } /** * \brief Apply distortion to input point (from the normalised plane) * and calculate Jacobian * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); 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); 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); double dxdmy = dydmx; 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); J << dxdmx, dxdmy, dydmx, dydmy; } void PinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; Eigen::Vector3d P; P << mx_u, my_u, 1.0; Eigen::Vector2d p; spaceToPlane(P, p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } cv::Mat PinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); // assume no skew Eigen::Matrix3f K_rect; if (cx == -1.0f || cy == -1.0f) { K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; } else { K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; } if (fx == -1.0f || fy == -1.0f) { K_rect(0,0) = mParameters.fx(); K_rect(1,1) = mParameters.fy(); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int PinholeCamera::parameterCount(void) const { return 8; } const PinholeCamera::Parameters& PinholeCamera::getParameters(void) const { return mParameters; } void PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters) { mParameters = parameters; if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } void PinholeCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.k1() = parameterVec.at(0); params.k2() = parameterVec.at(1); params.p1() = parameterVec.at(2); params.p2() = parameterVec.at(3); params.fx() = parameterVec.at(4); params.fy() = parameterVec.at(5); params.cx() = parameterVec.at(6); params.cy() = parameterVec.at(7); setParameters(params); } void PinholeCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.k1(); parameterVec.at(1) = mParameters.k2(); parameterVec.at(2) = mParameters.p1(); parameterVec.at(3) = mParameters.p2(); parameterVec.at(4) = mParameters.fx(); parameterVec.at(5) = mParameters.fy(); parameterVec.at(6) = mParameters.cx(); parameterVec.at(7) = mParameters.cy(); } void PinholeCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string PinholeCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/camera_models/ScaramuzzaCamera.cc ================================================ #include "camodocal/camera_models/ScaramuzzaCamera.h" #include #include #include #include #include #include #include #include #include #include #include #include "camodocal/gpl/gpl.h" Eigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order) { assert(poly_order > 0); assert(xVec.size() > poly_order); assert(xVec.size() == yVec.size()); Eigen::MatrixXd A(xVec.size(), poly_order+1); Eigen::VectorXd B(xVec.size()); for(int i = 0; i < xVec.size(); ++i) { const double x = xVec(i); const double y = yVec(i); double x_pow_k = 1.0; for(int k=0; k<=poly_order; ++k) { A(i,k) = x_pow_k; x_pow_k *= x; } B(i) = y; } Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::VectorXd x = svd.solve(B); return x; } namespace camodocal { OCAMCamera::Parameters::Parameters() : Camera::Parameters(SCARAMUZZA) , m_C(0.0) , m_D(0.0) , m_E(0.0) , m_center_x(0.0) , m_center_y(0.0) { memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE); memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); } bool OCAMCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (!boost::iequals(sModelType, "scaramuzza")) { return false; } } m_modelType = SCARAMUZZA; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["poly_parameters"]; for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) m_poly[i] = static_cast(n[std::string("p") + boost::lexical_cast(i)]); n = fs["inv_poly_parameters"]; for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) m_inv_poly[i] = static_cast(n[std::string("p") + boost::lexical_cast(i)]); n = fs["affine_parameters"]; m_C = static_cast(n["ac"]); m_D = static_cast(n["ad"]); m_E = static_cast(n["ae"]); m_center_x = static_cast(n["cx"]); m_center_y = static_cast(n["cy"]); return true; } void OCAMCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "scaramuzza"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; fs << "poly_parameters"; fs << "{"; for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) fs << std::string("p") + boost::lexical_cast(i) << m_poly[i]; fs << "}"; fs << "inv_poly_parameters"; fs << "{"; for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) fs << std::string("p") + boost::lexical_cast(i) << m_inv_poly[i]; fs << "}"; fs << "affine_parameters"; fs << "{" << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy" << m_center_y << "}"; fs.release(); } OCAMCamera::Parameters& OCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_C = other.m_C; m_D = other.m_D; m_E = other.m_E; m_center_x = other.m_center_x; m_center_y = other.m_center_y; memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE); memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); } return *this; } std::ostream& operator<< (std::ostream& out, const OCAMCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "scaramuzza" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; out << std::fixed << std::setprecision(10); out << "Poly Parameters" << std::endl; for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) out << std::string("p") + boost::lexical_cast(i) << ": " << params.m_poly[i] << std::endl; out << "Inverse Poly Parameters" << std::endl; for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) out << std::string("p") + boost::lexical_cast(i) << ": " << params.m_inv_poly[i] << std::endl; out << "Affine Parameters" << std::endl; out << " ac " << params.m_C << std::endl << " ad " << params.m_D << std::endl << " ae " << params.m_E << std::endl; out << " cx " << params.m_center_x << std::endl << " cy " << params.m_center_y << std::endl; return out; } OCAMCamera::OCAMCamera() : m_inv_scale(0.0) { } OCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params) : mParameters(params) { m_inv_scale = 1.0 / (params.C() - params.D() * params.E()); } Camera::ModelType OCAMCamera::modelType(void) const { return mParameters.modelType(); } const std::string& OCAMCamera::cameraName(void) const { return mParameters.cameraName(); } int OCAMCamera::imageWidth(void) const { return mParameters.imageWidth(); } int OCAMCamera::imageHeight(void) const { return mParameters.imageHeight(); } void OCAMCamera::estimateIntrinsics(const cv::Size& boardSize, const std::vector< std::vector >& objectPoints, const std::vector< std::vector >& imagePoints) { // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" << std::endl; // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED"); // Reference: Page 30 of // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635." // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf // Matlab code: calibrate.m // First, estimate every image's extrinsics parameters std::vector< Eigen::Matrix3d > RList; std::vector< Eigen::Vector3d > TList; RList.reserve(imagePoints.size()); TList.reserve(imagePoints.size()); // i-th image for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index) { const std::vector& objPts = objectPoints.at(image_index); const std::vector& imgPts = imagePoints.at(image_index); assert(objPts.size() == imgPts.size()); assert(objPts.size() == static_cast(boardSize.width * boardSize.height)); Eigen::MatrixXd M(objPts.size(), 6); for(size_t corner_index = 0; corner_index < objPts.size(); ++corner_index) { double X = objPts.at(corner_index).x; double Y = objPts.at(corner_index).y; assert(objPts.at(corner_index).z==0.0); double u = imgPts.at(corner_index).x; double v = imgPts.at(corner_index).y; M(corner_index, 0) = -v * X; M(corner_index, 1) = -v * Y; M(corner_index, 2) = u * X; M(corner_index, 3) = u * Y; M(corner_index, 4) = -v; M(corner_index, 5) = u; } Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); assert(svd.matrixV().cols() == 6); Eigen::VectorXd h = -svd.matrixV().col(5); // scaled version of R and T const double sr11 = h(0); const double sr12 = h(1); const double sr21 = h(2); const double sr22 = h(3); const double st1 = h(4); const double st2 = h(5); const double AA = square(sr11*sr12 + sr21*sr22); const double BB = square(sr11) + square(sr21); const double CC = square(sr12) + square(sr22); const double sr32_squared_1 = (- (CC-BB) + sqrt(square(CC-BB) + 4.0 * AA)) / 2.0; const double sr32_squared_2 = (- (CC-BB) - sqrt(square(CC-BB) + 4.0 * AA)) / 2.0; // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 + (CC-BB)*sr32_squared_1 - AA); std::vector sr32_squared_values; if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1); if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2); assert(!sr32_squared_values.empty()); std::vector sr32_values; std::vector sr31_values; for (auto sr32_squared : sr32_squared_values) { for(int sign = -1; sign <= 1; sign += 2) { const double sr32 = static_cast(sign) * std::sqrt(sr32_squared); sr32_values.push_back( sr32 ); if (sr32_squared == 0.0) { // sr31 can be calculated through norm equality, // but it has positive and negative posibilities // positive one sr31_values.push_back(std::sqrt(CC-BB)); // negative one sr32_values.push_back( sr32 ); sr31_values.push_back(-std::sqrt(CC-BB)); break; // skip the same situation } else { // sr31 can be calculated throught dot product == 0 sr31_values.push_back(- (sr11*sr12 + sr21*sr22) / sr32); } } } // std::cout << "h= " << std::setprecision(12) << h.transpose() << std::endl; // std::cout << "length: " << sr32_values.size() << " & " << sr31_values.size() << std::endl; assert(!sr31_values.empty()); assert(sr31_values.size() == sr32_values.size()); std::vector H_values; for(size_t i=0;i(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } #endif cv::Mat OCAMCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Eigen::Matrix3f K_rect; K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy, cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1; if (fx < 0 || fy < 0) { throw std::string(std::string(__FUNCTION__) + ": Focal length must be specified"); } Eigen::Matrix3f K_rect_inv = K_rect.inverse(); Eigen::Matrix3f R, R_inv; cv::cv2eigen(rmat, R); R_inv = R.inverse(); for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Eigen::Vector3f xo; xo << u, v, 1; Eigen::Vector3f uo = R_inv * K_rect_inv * xo; Eigen::Vector2d p; spaceToPlane(uo.cast(), p); mapX.at(v,u) = p(0); mapY.at(v,u) = p(1); } } cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); cv::Mat K_rect_cv; cv::eigen2cv(K_rect, K_rect_cv); return K_rect_cv; } int OCAMCamera::parameterCount(void) const { return SCARAMUZZA_CAMERA_NUM_PARAMS; } const OCAMCamera::Parameters& OCAMCamera::getParameters(void) const { return mParameters; } void OCAMCamera::setParameters(const OCAMCamera::Parameters& parameters) { mParameters = parameters; m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E()); } void OCAMCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.C() = parameterVec.at(0); params.D() = parameterVec.at(1); params.E() = parameterVec.at(2); params.center_x() = parameterVec.at(3); params.center_y() = parameterVec.at(4); for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) params.poly(i) = parameterVec.at(5+i); for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i); setParameters(params); } void OCAMCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.C(); parameterVec.at(1) = mParameters.D(); parameterVec.at(2) = mParameters.E(); parameterVec.at(3) = mParameters.center_x(); parameterVec.at(4) = mParameters.center_y(); for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) parameterVec.at(5+i) = mParameters.poly(i); for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i); } void OCAMCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string OCAMCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/chessboard/Chessboard.cc ================================================ #include "camodocal/chessboard/Chessboard.h" #include #include #include "camodocal/chessboard/ChessboardQuad.h" #include "camodocal/chessboard/Spline.h" #define MAX_CONTOUR_APPROX 7 namespace camodocal { Chessboard::Chessboard(cv::Size boardSize, cv::Mat& image) : mBoardSize(boardSize) , mCornersFound(false) { if (image.channels() == 1) { cv::cvtColor(image, mSketch, CV_GRAY2BGR); image.copyTo(mImage); } else { image.copyTo(mSketch); cv::cvtColor(image, mImage, CV_BGR2GRAY); } } void Chessboard::findCorners(bool useOpenCV) { mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners, CV_CALIB_CB_ADAPTIVE_THRESH + CV_CALIB_CB_NORMALIZE_IMAGE + CV_CALIB_CB_FILTER_QUADS + CV_CALIB_CB_FAST_CHECK, useOpenCV); if (mCornersFound) { // draw chessboard corners cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound); } } const std::vector& Chessboard::getCorners(void) const { return mCorners; } bool Chessboard::cornersFound(void) const { return mCornersFound; } const cv::Mat& Chessboard::getImage(void) const { return mImage; } const cv::Mat& Chessboard::getSketch(void) const { return mSketch; } bool Chessboard::findChessboardCorners(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags, bool useOpenCV) { if (useOpenCV) { return cv::findChessboardCorners(image, patternSize, corners, flags); } else { return findChessboardCornersImproved(image, patternSize, corners, flags); } } bool Chessboard::findChessboardCornersImproved(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, int flags) { /************************************************************************************\ This is improved variant of chessboard corner detection algorithm that uses a graph of connected quads. It is based on the code contributed by Vladimir Vezhnevets and Philip Gruebele. Here is the copyright notice from the original Vladimir's code: =============================================================== The algorithms developed and implemented by Vezhnevets Vldimir aka Dead Moroz (vvp@graphics.cs.msu.ru) See http://graphics.cs.msu.su/en/research/calibration/opencv.html for detailed information. Reliability additions and modifications made by Philip Gruebele. pgruebele@cox.net Some improvements were made by: 1) Martin Rufli - increased chance of correct corner matching Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), Automatic Detection of Checkerboards on Blurred and Distorted Images, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008. 2) Lionel Heng - post-detection checks and better thresholding \************************************************************************************/ //int bestDilation = -1; const int minDilations = 0; const int maxDilations = 7; std::vector outputQuadGroup; if (image.depth() != CV_8U || image.channels() == 2) { return false; } if (patternSize.width < 2 || patternSize.height < 2) { return false; } if (patternSize.width > 127 || patternSize.height > 127) { return false; } cv::Mat img = image; // Image histogram normalization and // BGR to Grayscale image conversion (if applicable) // MARTIN: Set to "false" if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE)) { cv::Mat norm_img(image.rows, image.cols, CV_8UC1); if (image.channels() != 1) { cv::cvtColor(image, norm_img, CV_BGR2GRAY); img = norm_img; } if (flags & CV_CALIB_CB_NORMALIZE_IMAGE) { cv::equalizeHist(image, norm_img); img = norm_img; } } if (flags & CV_CALIB_CB_FAST_CHECK) { if (!checkChessboard(img, patternSize)) { return false; } } // PART 1: FIND LARGEST PATTERN //----------------------------------------------------------------------- // Checker patterns are tried to be found by dilating the background and // then applying a canny edge finder on the closed contours (checkers). // Try one dilation run, but if the pattern is not found, repeat until // max_dilations is reached. int prevSqrSize = 0; bool found = false; std::vector outputCorners; for (int k = 0; k < 6; ++k) { for (int dilations = minDilations; dilations <= maxDilations; ++dilations) { if (found) { break; } cv::Mat thresh_img; // convert the input grayscale image to binary (black-n-white) if (flags & CV_CALIB_CB_ADAPTIVE_THRESH) { int blockSize = lround(prevSqrSize == 0 ? std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1; // convert to binary cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k/2)*5); } else { // empiric threshold level double mean = (cv::mean(img))[0]; int thresh_level = lround(mean - 10); thresh_level = std::max(thresh_level, 10); cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY); } // MARTIN's Code // Use both a rectangular and a cross kernel. In this way, a more // homogeneous dilation is performed, which is crucial for small, // distorted checkers. Use the CROSS kernel first, since its action // on the image is more subtle cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3,3), cv::Point(1,1)); cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3,3), cv::Point(1,1)); if (dilations >= 1) cv::dilate(thresh_img, thresh_img, kernel1); if (dilations >= 2) cv::dilate(thresh_img, thresh_img, kernel2); if (dilations >= 3) cv::dilate(thresh_img, thresh_img, kernel1); if (dilations >= 4) cv::dilate(thresh_img, thresh_img, kernel2); if (dilations >= 5) cv::dilate(thresh_img, thresh_img, kernel1); if (dilations >= 6) cv::dilate(thresh_img, thresh_img, kernel2); // In order to find rectangles that go to the edge, we draw a white // line around the image edge. Otherwise FindContours will miss those // clipped rectangle contours. The border color will be the image mean, // because otherwise we risk screwing up filters like cvSmooth() cv::rectangle(thresh_img, cv::Point(0,0), cv::Point(thresh_img.cols - 1, thresh_img.rows - 1), CV_RGB(255,255,255), 3, 8); // Generate quadrangles in the following function std::vector quads; generateQuads(quads, thresh_img, flags, dilations, true); if (quads.empty()) { continue; } // The following function finds and assigns neighbor quads to every // quadrangle in the immediate vicinity fulfilling certain // prerequisites findQuadNeighbors(quads, dilations); // The connected quads will be organized in groups. The following loop // increases a "group_idx" identifier. // The function "findConnectedQuads assigns all connected quads // a unique group ID. // If more quadrangles were assigned to a given group (i.e. connected) // than are expected by the input variable "patternSize", the // function "cleanFoundConnectedQuads" erases the surplus // quadrangles by minimizing the convex hull of the remaining pattern. for (int group_idx = 0; ; ++group_idx) { std::vector quadGroup; findConnectedQuads(quads, quadGroup, group_idx, dilations); if (quadGroup.empty()) { break; } cleanFoundConnectedQuads(quadGroup, patternSize); // The following function labels all corners of every quad // with a row and column entry. // "count" specifies the number of found quads in "quad_group" // with group identifier "group_idx" // The last parameter is set to "true", because this is the // first function call and some initializations need to be // made. labelQuadGroup(quadGroup, patternSize, true); found = checkQuadGroup(quadGroup, outputCorners, patternSize); float sumDist = 0; int total = 0; for (int i = 0; i < (int)outputCorners.size(); ++i) { int ni = 0; float avgi = outputCorners.at(i)->meanDist(ni); sumDist += avgi * ni; total += ni; } prevSqrSize = lround(sumDist / std::max(total, 1)); if (found && !checkBoardMonotony(outputCorners, patternSize)) { found = false; } } } } if (!found) { return false; } else { corners.clear(); corners.reserve(outputCorners.size()); for (size_t i = 0; i < outputCorners.size(); ++i) { corners.push_back(outputCorners.at(i)->pt); } cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); return true; } } //=========================================================================== // ERASE OVERHEAD //=========================================================================== // If we found too many connected quads, remove those which probably do not // belong. void Chessboard::cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize) { cv::Point2f center(0.0f, 0.0f); // Number of quads this pattern should contain int count = ((patternSize.width + 1)*(patternSize.height + 1) + 1)/2; // If we have more quadrangles than we should, try to eliminate duplicates // or ones which don't belong to the pattern rectangle. Else go to the end // of the function if ((int)quadGroup.size() <= count) { return; } // Create an array of quadrangle centers std::vector centers; centers.resize(quadGroup.size()); for (size_t i = 0; i < quadGroup.size(); ++i) { cv::Point2f ci(0.0f, 0.0f); ChessboardQuadPtr& q = quadGroup[i]; for (int j = 0; j < 4; ++j) { ci += q->corners[j]->pt; } ci *= 0.25f; // Centers(i), is the geometric center of quad(i) // Center, is the center of all found quads centers[i] = ci; center += ci; } center *= 1.0f / quadGroup.size(); // If we have more quadrangles than we should, we try to eliminate bad // ones based on minimizing the bounding box. We iteratively remove the // point which reduces the size of the bounding box of the blobs the most // (since we want the rectangle to be as small as possible) remove the // quadrange that causes the biggest reduction in pattern size until we // have the correct number while ((int)quadGroup.size() > count) { double minBoxArea = DBL_MAX; int minBoxAreaIndex = -1; // For each point, calculate box area without that point for (size_t skip = 0; skip < quadGroup.size(); ++skip) { // get bounding rectangle cv::Point2f temp = centers[skip]; centers[skip] = center; std::vector hull; cv::convexHull(centers, hull, true, true); centers[skip] = temp; double hull_area = fabs(cv::contourArea(hull)); // remember smallest box area if (hull_area < minBoxArea) { minBoxArea = hull_area; minBoxAreaIndex = skip; } } ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex]; // remove any references to this quad as a neighbor for (size_t i = 0; i < quadGroup.size(); ++i) { ChessboardQuadPtr& q = quadGroup.at(i); for (int j = 0; j < 4; ++j) { if (q->neighbors[j] == q0) { q->neighbors[j].reset(); q->count--; for (int k = 0; k < 4; ++k) { if (q0->neighbors[k] == q) { q0->neighbors[k].reset(); q0->count--; break; } } break; } } } // remove the quad quadGroup.at(minBoxAreaIndex) = quadGroup.back(); centers.at(minBoxAreaIndex) = centers.back(); quadGroup.pop_back(); centers.pop_back(); } } //=========================================================================== // FIND COONECTED QUADS //=========================================================================== void Chessboard::findConnectedQuads(std::vector& quads, std::vector& group, int group_idx, int dilation) { ChessboardQuadPtr q; // Scan the array for a first unlabeled quad for (size_t i = 0; i < quads.size(); ++i) { ChessboardQuadPtr& quad = quads.at(i); if (quad->count > 0 && quad->group_idx < 0) { q = quad; break; } } if (q.get() == 0) { return; } // Recursively find a group of connected quads starting from the seed quad std::vector stack; stack.push_back(q); group.push_back(q); q->group_idx = group_idx; while (!stack.empty()) { q = stack.back(); stack.pop_back(); for (int i = 0; i < 4; ++i) { ChessboardQuadPtr& neighbor = q->neighbors[i]; // If he neighbor exists and the neighbor has more than 0 // neighbors and the neighbor has not been classified yet. if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0) { stack.push_back(neighbor); group.push_back(neighbor); neighbor->group_idx = group_idx; } } } } void Chessboard::labelQuadGroup(std::vector& quadGroup, cv::Size patternSize, bool firstRun) { // If this is the first function call, a seed quad needs to be selected if (firstRun) { // Search for the (first) quad with the maximum number of neighbors // (usually 4). This will be our starting point. int mark = -1; int maxNeighborCount = 0; for (size_t i = 0; i < quadGroup.size(); ++i) { ChessboardQuadPtr& q = quadGroup.at(i); if (q->count > maxNeighborCount) { mark = i; maxNeighborCount = q->count; if (maxNeighborCount == 4) { break; } } } // Mark the starting quad's (per definition) upper left corner with //(0,0) and then proceed clockwise // The following labeling sequence enshures a "right coordinate system" ChessboardQuadPtr& q = quadGroup.at(mark); q->labeled = true; q->corners[0]->row = 0; q->corners[0]->column = 0; q->corners[1]->row = 0; q->corners[1]->column = 1; q->corners[2]->row = 1; q->corners[2]->column = 1; q->corners[3]->row = 1; q->corners[3]->column = 0; } // Mark all other corners with their respective row and column bool flagChanged = true; while (flagChanged) { // First reset the flag to "false" flagChanged = false; // Go through all quads top down is faster, since unlabeled quads will // be inserted at the end of the list for (int i = quadGroup.size() - 1; i >= 0; --i) { ChessboardQuadPtr& quad = quadGroup.at(i); // Check whether quad "i" has been labeled already if (!quad->labeled) { // Check its neighbors, whether some of them have been labeled // already for (int j = 0; j < 4; j++) { // Check whether the neighbor exists (i.e. is not the NULL // pointer) if (quad->neighbors[j]) { ChessboardQuadPtr& quadNeighbor = quad->neighbors[j]; // Only proceed, if neighbor "j" was labeled if (quadNeighbor->labeled) { // For every quad it could happen to pass here // multiple times. We therefore "break" later. // Check whitch of the neighbors corners is // connected to the current quad int connectedNeighborCornerId = -1; for (int k = 0; k < 4; ++k) { if (quadNeighbor->neighbors[k] == quad) { connectedNeighborCornerId = k; // there is only one, therefore break; } } // For the following calculations we need the row // and column of the connected neighbor corner and // all other corners of the connected quad "j", // clockwise (CW) ChessboardCornerPtr& conCorner = quadNeighbor->corners[connectedNeighborCornerId]; ChessboardCornerPtr& conCornerCW1 = quadNeighbor->corners[(connectedNeighborCornerId+1)%4]; ChessboardCornerPtr& conCornerCW2 = quadNeighbor->corners[(connectedNeighborCornerId+2)%4]; ChessboardCornerPtr& conCornerCW3 = quadNeighbor->corners[(connectedNeighborCornerId+3)%4]; quad->corners[j]->row = conCorner->row; quad->corners[j]->column = conCorner->column; quad->corners[(j+1)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW3->row; quad->corners[(j+1)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW3->column; quad->corners[(j+2)%4]->row = conCorner->row + conCorner->row - conCornerCW2->row; quad->corners[(j+2)%4]->column = conCorner->column + conCorner->column - conCornerCW2->column; quad->corners[(j+3)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW1->row; quad->corners[(j+3)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW1->column; // Mark this quad as labeled quad->labeled = true; // Changes have taken place, set the flag flagChanged = true; // once is enough! break; } } } } } } // All corners are marked with row and column // Record the minimal and maximal row and column indices // It is unlikely that more than 8bit checkers are used per dimension, if there are // an error would have been thrown at the beginning of "cvFindChessboardCorners2" int min_row = 127; int max_row = -127; int min_column = 127; int max_column = -127; for (int i = 0; i < (int)quadGroup.size(); ++i) { ChessboardQuadPtr& q = quadGroup.at(i); for (int j = 0; j < 4; ++j) { ChessboardCornerPtr& c = q->corners[j]; if (c->row > max_row) { max_row = c->row; } if (c->row < min_row) { min_row = c->row; } if (c->column > max_column) { max_column = c->column; } if (c->column < min_column) { min_column = c->column; } } } // Label all internal corners with "needsNeighbor" = false // Label all external corners with "needsNeighbor" = true, // except if in a given dimension the pattern size is reached for (int i = min_row; i <= max_row; ++i) { for (int j = min_column; j <= max_column; ++j) { // A flag that indicates, whether a row/column combination is // executed multiple times bool flag = false; // Remember corner and quad int cornerID; int quadID; for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { if (flag) { // Passed at least twice through here q->corners[l]->needsNeighbor = false; quadGroup[quadID]->corners[cornerID]->needsNeighbor = false; } else { // Mark with needs a neighbor, but note the // address q->corners[l]->needsNeighbor = true; cornerID = l; quadID = k; } // set the flag to true flag = true; } } } } } // Complete Linking: // sometimes not all corners were properly linked in "findQuadNeighbors", // but after labeling each corner with its respective row and column, it is // possible to match them anyway. for (int i = min_row; i <= max_row; ++i) { for (int j = min_column; j <= max_column; ++j) { // the following "number" indicates the number of corners which // correspond to the given (i,j) value // 1 is a border corner or a conrer which still needs a neighbor // 2 is a fully connected internal corner // >2 something went wrong during labeling, report a warning int number = 1; // remember corner and quad int cornerID; int quadID; for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { if (number == 1) { // First corner, note its ID cornerID = l; quadID = k; } else if (number == 2) { // Second corner, check wheter this and the // first one have equal coordinates, else // interpolate cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt; if (delta.x != 0.0f || delta.y != 0.0f) { // Interpolate q->corners[l]->pt -= delta * 0.5f; quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f; } } else if (number > 2) { // Something went wrong during row/column labeling // Report a Warning // ->Implemented in the function "mrWriteCorners" } // increase the number by one ++number; } } } } } // Bordercorners don't need any neighbors, if the pattern size in the // respective direction is reached // The only time we can make shure that the target pattern size is reached in a given // dimension, is when the larger side has reached the target size in the maximal // direction, or if the larger side is larger than the smaller target size and the // smaller side equals the smaller target size int largerDimPattern = std::max(patternSize.height,patternSize.width); int smallerDimPattern = std::min(patternSize.height,patternSize.width); bool flagSmallerDim1 = false; bool flagSmallerDim2 = false; if ((largerDimPattern + 1) == max_column - min_column) { flagSmallerDim1 = true; // We found out that in the column direction the target pattern size is reached // Therefore border column corners do not need a neighbor anymore // Go through all corners for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->column == min_column || c->column == max_column) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } if ((largerDimPattern + 1) == max_row - min_row) { flagSmallerDim2 = true; // We found out that in the column direction the target pattern size is reached // Therefore border column corners do not need a neighbor anymore // Go through all corners for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->row == min_row || c->row == max_row) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } // Check the two flags: // - If one is true and the other false, then the pattern target // size was reached in in one direction -> We can check, whether the target // pattern size is also reached in the other direction // - If both are set to true, then we deal with a square board -> do nothing // - If both are set to false -> There is a possibility that the larger side is // larger than the smaller target size -> Check and if true, then check whether // the other side has the same size as the smaller target size if ((flagSmallerDim1 == false && flagSmallerDim2 == true)) { // Larger target pattern size is in row direction, check wheter smaller target // pattern size is reached in column direction if ((smallerDimPattern + 1) == max_column - min_column) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->column == min_column || c->column == max_column) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } if ((flagSmallerDim1 == true && flagSmallerDim2 == false)) { // Larger target pattern size is in column direction, check wheter smaller target // pattern size is reached in row direction if ((smallerDimPattern + 1) == max_row - min_row) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->row == min_row || c->row == max_row) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column) { // Larger target pattern size is in column direction, check wheter smaller target // pattern size is reached in row direction if ((smallerDimPattern + 1) == max_row - min_row) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->row == min_row || c->row == max_row) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row) { // Larger target pattern size is in row direction, check wheter smaller target // pattern size is reached in column direction if ((smallerDimPattern + 1) == max_column - min_column) { for (int k = 0; k < (int)quadGroup.size(); ++k) { ChessboardQuadPtr& q = quadGroup.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = q->corners[l]; if (c->column == min_column || c->column == max_column) { // Needs no neighbor anymore c->needsNeighbor = false; } } } } } } //=========================================================================== // GIVE A GROUP IDX //=========================================================================== void Chessboard::findQuadNeighbors(std::vector& quads, int dilation) { // Thresh dilation is used to counter the effect of dilation on the // distance between 2 neighboring corners. Since the distance below is // computed as its square, we do here the same. Additionally, we take the // conservative assumption that dilation was performed using the 3x3 CROSS // kernel, which coresponds to the 4-neighborhood. const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2; // the "*2" is for the x and y component // the "3" is for initial corner mismatch // Find quad neighbors for (size_t idx = 0; idx < quads.size(); ++idx) { ChessboardQuadPtr& curQuad = quads.at(idx); // Go through all quadrangles and label them in groups // For each corner of this quadrangle for (int i = 0; i < 4; ++i) { float minDist = FLT_MAX; int closestCornerIdx = -1; ChessboardQuadPtr closestQuad; if (curQuad->neighbors[i]) { continue; } cv::Point2f pt = curQuad->corners[i]->pt; // Find the closest corner in all other quadrangles for (size_t k = 0; k < quads.size(); ++k) { if (k == idx) { continue; } ChessboardQuadPtr& quad = quads.at(k); for (int j = 0; j < 4; ++j) { // If it already has a neighbor if (quad->neighbors[j]) { continue; } cv::Point2f dp = pt - quad->corners[j]->pt; float dist = dp.dot(dp); // The following "if" checks, whether "dist" is the // shortest so far and smaller than the smallest // edge length of the current and target quads if (dist < minDist && dist <= (curQuad->edge_len + thresh_dilation) && dist <= (quad->edge_len + thresh_dilation) ) { // Check whether conditions are fulfilled if (matchCorners(curQuad, i, quad, j)) { closestCornerIdx = j; closestQuad = quad; minDist = dist; } } } } // Have we found a matching corner point? if (closestCornerIdx >= 0 && minDist < FLT_MAX) { ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx]; // Make sure that the closest quad does not have the current // quad as neighbor already bool valid = true; for (int j = 0; j < 4; ++j) { if (closestQuad->neighbors[j] == curQuad) { valid = false; break; } } if (!valid) { continue; } // We've found one more corner - remember it closestCorner->pt = (pt + closestCorner->pt) * 0.5f; curQuad->count++; curQuad->neighbors[i] = closestQuad; curQuad->corners[i] = closestCorner; closestQuad->count++; closestQuad->neighbors[closestCornerIdx] = curQuad; closestQuad->corners[closestCornerIdx] = closestCorner; } } } } //=========================================================================== // AUGMENT PATTERN WITH ADDITIONAL QUADS //=========================================================================== // The first part of the function is basically a copy of // "findQuadNeighbors" // The comparisons between two points and two lines could be computed in their // own function int Chessboard::augmentBestRun(std::vector& candidateQuads, int candidateDilation, std::vector& existingQuads, int existingDilation) { // thresh dilation is used to counter the effect of dilation on the // distance between 2 neighboring corners. Since the distance below is // computed as its square, we do here the same. Additionally, we take the // conservative assumption that dilation was performed using the 3x3 CROSS // kernel, which coresponds to the 4-neighborhood. const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2; // the "*2" is for the x and y component // Search all old quads which have a neighbor that needs to be linked for (size_t idx = 0; idx < existingQuads.size(); ++idx) { ChessboardQuadPtr& curQuad = existingQuads.at(idx); // For each corner of this quadrangle for (int i = 0; i < 4; ++i) { float minDist = FLT_MAX; int closestCornerIdx = -1; ChessboardQuadPtr closestQuad; // If curQuad corner[i] is already linked, continue if (!curQuad->corners[i]->needsNeighbor) { continue; } cv::Point2f pt = curQuad->corners[i]->pt; // Look for a match in all candidateQuads' corners for (size_t k = 0; k < candidateQuads.size(); ++k) { ChessboardQuadPtr& candidateQuad = candidateQuads.at(k); // Only look at unlabeled new quads if (candidateQuad->labeled) { continue; } for (int j = 0; j < 4; ++j) { // Only proceed if they are less than dist away from each // other cv::Point2f dp = pt - candidateQuad->corners[j]->pt; float dist = dp.dot(dp); if ((dist < minDist) && dist <= (curQuad->edge_len + thresh_dilation) && dist <= (candidateQuad->edge_len + thresh_dilation)) { if (matchCorners(curQuad, i, candidateQuad, j)) { closestCornerIdx = j; closestQuad = candidateQuad; minDist = dist; } } } } // Have we found a matching corner point? if (closestCornerIdx >= 0 && minDist < FLT_MAX) { ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx]; closestCorner->pt = (pt + closestCorner->pt) * 0.5f; // We've found one more corner - remember it // ATTENTION: write the corner x and y coordinates separately, // else the crucial row/column entries will be overwritten !!! curQuad->corners[i]->pt = closestCorner->pt; curQuad->neighbors[i] = closestQuad; closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt; // Label closest quad as labeled. In this way we exclude it // being considered again during the next loop iteration closestQuad->labeled = true; // We have a new member of the final pattern, copy it over ChessboardQuadPtr newQuad(new ChessboardQuad); newQuad->count = 1; newQuad->edge_len = closestQuad->edge_len; newQuad->group_idx = curQuad->group_idx; //the same as the current quad newQuad->labeled = false; //do it right afterwards curQuad->neighbors[i] = newQuad; // We only know one neighbor for sure newQuad->neighbors[closestCornerIdx] = curQuad; for (int j = 0; j < 4; j++) { newQuad->corners[j].reset(new ChessboardCorner); newQuad->corners[j]->pt = closestQuad->corners[j]->pt; } existingQuads.push_back(newQuad); // Start the function again return -1; } } } // Finished, don't start the function again return 1; } //=========================================================================== // GENERATE QUADRANGLES //=========================================================================== void Chessboard::generateQuads(std::vector& quads, cv::Mat& image, int flags, int dilation, bool firstRun) { // Empirical lower bound for the size of allowable quadrangles. // MARTIN, modified: Added "*0.1" in order to find smaller quads. int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1); std::vector< std::vector > contours; std::vector hierarchy; // Initialize contour retrieving routine cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); std::vector< std::vector > quadContours; for (size_t i = 0; i < contours.size(); ++i) { // Reject contours with a too small perimeter and contours which are // completely surrounded by another contour // MARTIN: If this function is called during PART 1, then the parameter "first run" // is set to "true". This guarantees, that only "nice" squares are detected. // During PART 2, we allow the polygonal approximation function below to // approximate more freely, which can result in recognized "squares" that are in // reality multiple blurred and sticked together squares. std::vector& contour = contours.at(i); if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize) { continue; } int min_approx_level = 1, max_approx_level; if (firstRun) { max_approx_level = 3; } else { max_approx_level = MAX_CONTOUR_APPROX; } std::vector approxContour; for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++) { cv::approxPolyDP(contour, approxContour, approx_level, true); if (approxContour.size() == 4) { break; } } // Reject non-quadrangles if (approxContour.size() == 4 && cv::isContourConvex(approxContour)) { double p = cv::arcLength(approxContour, true); double area = cv::contourArea(approxContour); cv::Point pt[4]; for (int i = 0; i < 4; i++) { pt[i] = approxContour[i]; } cv::Point dp = pt[0] - pt[2]; double d1 = sqrt(dp.dot(dp)); dp = pt[1] - pt[3]; double d2 = sqrt(dp.dot(dp)); // PHILIPG: Only accept those quadrangles which are more // square than rectangular and which are big enough dp = pt[0] - pt[1]; double d3 = sqrt(dp.dot(dp)); dp = pt[1] - pt[2]; double d4 = sqrt(dp.dot(dp)); if (!(flags & CV_CALIB_CB_FILTER_QUADS) || (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize && d1 >= 0.15 * p && d2 >= 0.15 * p)) { quadContours.push_back(approxContour); } } } // Allocate quad & corner buffers quads.resize(quadContours.size()); // Create array of quads structures for (size_t idx = 0; idx < quadContours.size(); ++idx) { ChessboardQuadPtr& q = quads.at(idx); std::vector& contour = quadContours.at(idx); // Reset group ID q.reset(new ChessboardQuad); assert(contour.size() == 4); for (int i = 0; i < 4; ++i) { cv::Point2f pt = contour.at(i); ChessboardCornerPtr corner(new ChessboardCorner); corner->pt = pt; q->corners[i] = corner; } for (int i = 0; i < 4; ++i) { cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt; float d = dp.dot(dp); if (q->edge_len > d) { q->edge_len = d; } } } } bool Chessboard::checkQuadGroup(std::vector& quads, std::vector& corners, cv::Size patternSize) { // Initialize bool flagRow = false; bool flagColumn = false; int height = -1; int width = -1; // Compute the minimum and maximum row / column ID // (it is unlikely that more than 8bit checkers are used per dimension) int min_row = 127; int max_row = -127; int min_col = 127; int max_col = -127; for (size_t i = 0; i < quads.size(); ++i) { ChessboardQuadPtr& q = quads.at(i); for (int j = 0; j < 4; ++j) { ChessboardCornerPtr& c = q->corners[j]; if (c->row > max_row) { max_row = c->row; } if (c->row < min_row) { min_row = c->row; } if (c->column > max_col) { max_col = c->column; } if (c->column < min_col) { min_col = c->column; } } } // If in a given direction the target pattern size is reached, we know exactly how // the checkerboard is oriented. // Else we need to prepare enough "dummy" corners for the worst case. for (size_t i = 0; i < quads.size(); ++i) { ChessboardQuadPtr& q = quads.at(i); for (int j = 0; j < 4; ++j) { ChessboardCornerPtr& c = q->corners[j]; if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor) { flagColumn = true; } if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor) { flagRow = true; } } } if (flagColumn) { if (max_col - min_col == patternSize.width + 1) { width = patternSize.width; height = patternSize.height; } else { width = patternSize.height; height = patternSize.width; } } else if (flagRow) { if (max_row - min_row == patternSize.width + 1) { height = patternSize.width; width = patternSize.height; } else { height = patternSize.height; width = patternSize.width; } } else { // If target pattern size is not reached in at least one of the two // directions, then we do not know where the remaining corners are // located. Account for this. width = std::max(patternSize.width, patternSize.height); height = std::max(patternSize.width, patternSize.height); } ++min_row; ++min_col; max_row = min_row + height - 1; max_col = min_col + width - 1; corners.clear(); int linkedBorderCorners = 0; // Write the corners in increasing order to the output file for (int i = min_row; i <= max_row; ++i) { for (int j = min_col; j <= max_col; ++j) { // Reset the iterator int iter = 1; for (int k = 0; k < (int)quads.size(); ++k) { ChessboardQuadPtr& quad = quads.at(k); for (int l = 0; l < 4; ++l) { ChessboardCornerPtr& c = quad->corners[l]; if (c->row == i && c->column == j) { bool boardEdge = false; if (i == min_row || i == max_row || j == min_col || j == max_col) { boardEdge = true; } if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge)) { // The respective row and column have been found corners.push_back(quad->corners[l]); } if (iter == 2 && boardEdge) { ++linkedBorderCorners; } // If the iterator is larger than two, this means that more than // two corners have the same row / column entries. Then some // linking errors must have occured and we should not use the found // pattern if (iter > 2) { return false; } iter++; } } } } } if ((int)corners.size() != patternSize.width * patternSize.height || linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f) { return false; } // check that no corners lie at image boundary float border = 5.0f; for (int i = 0; i < (int)corners.size(); ++i) { ChessboardCornerPtr& c = corners.at(i); if (c->pt.x < border || c->pt.x > mImage.cols - border || c->pt.y < border || c->pt.y > mImage.rows - border) { return false; } } // check if we need to transpose the board if (width != patternSize.width) { std::swap(width, height); std::vector outputCorners; outputCorners.resize(corners.size()); for (int i = 0; i < height; ++i) { for (int j = 0; j < width; ++j) { outputCorners.at(i * width + j) = corners.at(j * height + i); } } corners = outputCorners; } // check if we need to revert the order in each row cv::Point2f p0 = corners.at(0)->pt; cv::Point2f p1 = corners.at(width-1)->pt; cv::Point2f p2 = corners.at(width)->pt; if ((p1 - p0).cross(p2 - p0) < 0.0f) { for (int i = 0; i < height; ++i) { for (int j = 0; j < width / 2; ++j) { std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1)); } } } p0 = corners.at(0)->pt; p2 = corners.at(width)->pt; // check if we need to rotate the board if (p2.y < p0.y) { std::vector outputCorners; outputCorners.resize(corners.size()); for (int i = 0; i < height; ++i) { for (int j = 0; j < width; ++j) { outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1); } } corners = outputCorners; } return true; } void Chessboard::getQuadrangleHypotheses(const std::vector< std::vector >& contours, std::vector< std::pair >& quads, int classId) const { const float minAspectRatio = 0.2f; const float maxAspectRatio = 5.0f; const float minBoxSize = 10.0f; for (size_t i = 0; i < contours.size(); ++i) { cv::RotatedRect box = cv::minAreaRect(contours.at(i)); float boxSize = std::max(box.size.width, box.size.height); if (boxSize < minBoxSize) { continue; } float aspectRatio = box.size.width / std::max(box.size.height, 1.0f); if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio) { continue; } quads.push_back(std::pair(boxSize, classId)); } } bool less_pred(const std::pair& p1, const std::pair& p2) { return p1.first < p2.first; } void countClasses(const std::vector >& pairs, size_t idx1, size_t idx2, std::vector& counts) { counts.assign(2, 0); for (size_t i = idx1; i != idx2; ++i) { counts[pairs[i].second]++; } } bool Chessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const { const int erosionCount = 1; const float blackLevel = 20.f; const float whiteLevel = 130.f; const float blackWhiteGap = 70.f; cv::Mat white = image.clone(); cv::Mat black = image.clone(); cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount); cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount); cv::Mat thresh(image.rows, image.cols, CV_8UC1); bool result = false; for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f) { cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY); std::vector< std::vector > contours; std::vector hierarchy; // Initialize contour retrieving routine cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); std::vector > quads; getQuadrangleHypotheses(contours, quads, 1); cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV); cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); getQuadrangleHypotheses(contours, quads, 0); const size_t min_quads_count = patternSize.width * patternSize.height / 2; std::sort(quads.begin(), quads.end(), less_pred); // now check if there are many hypotheses with similar sizes // do this by floodfill-style algorithm const float sizeRelDev = 0.4f; for (size_t i = 0; i < quads.size(); ++i) { size_t j = i + 1; for (; j < quads.size(); ++j) { if (quads[j].first / quads[i].first > 1.0f + sizeRelDev) { break; } } if (j + 1 > min_quads_count + i) { // check the number of black and white squares std::vector counts; countClasses(quads, i, j, counts); const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f)); const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f)); if (counts[0] < blackCount * 0.75f || counts[1] < whiteCount * 0.75f) { continue; } result = true; break; } } } return result; } bool Chessboard::checkBoardMonotony(std::vector& corners, cv::Size patternSize) { const float threshFactor = 0.2f; Spline splineXY, splineYX; splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC); splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC); splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC); splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC); // check if each row of corners approximates a cubic spline for (int i = 0; i < patternSize.height; ++i) { splineXY.clear(); splineYX.clear(); cv::Point2f p[3]; p[0] = corners.at(i * patternSize.width)->pt; p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt; p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt; for (int j = 0; j < 3; ++j) { splineXY.addPoint(p[j].x, p[j].y); splineYX.addPoint(p[j].y, p[j].x); } for (int j = 1; j < patternSize.width - 1; ++j) { cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt; float thresh = std::numeric_limits::max(); // up-neighbor if (i > 0) { cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } // down-neighbor if (i < patternSize.height - 1) { cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } // left-neighbor { cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } // right-neighbor { cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_j)); } thresh *= threshFactor; if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh) { return false; } } } // check if each column of corners approximates a cubic spline for (int j = 0; j < patternSize.width; ++j) { splineXY.clear(); splineYX.clear(); cv::Point2f p[3]; p[0] = corners.at(j)->pt; p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt; p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt; for (int i = 0; i < 3; ++i) { splineXY.addPoint(p[i].x, p[i].y); splineYX.addPoint(p[i].y, p[i].x); } for (int i = 1; i < patternSize.height - 1; ++i) { cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt; float thresh = std::numeric_limits::max(); // up-neighbor { cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } // down-neighbor { cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } // left-neighbor if (j > 0) { cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } // right-neighbor if (j < patternSize.width - 1) { cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; thresh = fminf(thresh, cv::norm(neighbor - p_i)); } thresh *= threshFactor; if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh) { return false; } } } return true; } bool Chessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1, ChessboardQuadPtr& quad2, int corner2) const { // First Check everything from the viewpoint of the // current quad compute midpoints of "parallel" quad // sides 1 float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2; float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2; float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2; float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2; // compute midpoints of "parallel" quad sides 2 float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2; float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2; float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2; float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the same side of the two lines as // corner1. This is given, if the cross product has // the same sign for both computations below: float a1 = x1 - x2; float b1 = y1 - y2; // the current corner float c11 = quad1->corners[corner1]->pt.x - x2; float d11 = quad1->corners[corner1]->pt.y - y2; // the candidate corner float c12 = quad2->corners[corner2]->pt.x - x2; float d12 = quad2->corners[corner2]->pt.y - y2; float sign11 = a1*d11 - c11*b1; float sign12 = a1*d12 - c12*b1; float a2 = x3 - x4; float b2 = y3 - y4; // the current corner float c21 = quad1->corners[corner1]->pt.x - x4; float d21 = quad1->corners[corner1]->pt.y - y4; // the candidate corner float c22 = quad2->corners[corner2]->pt.x - x4; float d22 = quad2->corners[corner2]->pt.y - y4; float sign21 = a2*d21 - c21*b2; float sign22 = a2*d22 - c22*b2; // Also make shure that two border quads of the same row or // column don't link. Check from the current corner's view, // whether the corner diagonal from the candidate corner // is also on the same side of the two lines as the current // corner and the candidate corner. float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2; float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2; float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4; float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4; float sign13 = a1*d13 - c13*b1; float sign23 = a2*d23 - c23*b2; // Second: Then check everything from the viewpoint of // the candidate quad. Compute midpoints of "parallel" // quad sides 1 float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2; float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2; float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2; float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2; // compute midpoints of "parallel" quad sides 2 float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2; float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2; float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2; float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the same side of the two lines as // corner1. This is given, if the cross product has // the same sign for both computations below: float a3 = u1 - u2; float b3 = v1 - v2; // the current corner float c31 = quad1->corners[corner1]->pt.x - u2; float d31 = quad1->corners[corner1]->pt.y - v2; // the candidate corner float c32 = quad2->corners[corner2]->pt.x - u2; float d32 = quad2->corners[corner2]->pt.y - v2; float sign31 = a3*d31-c31*b3; float sign32 = a3*d32-c32*b3; float a4 = u3 - u4; float b4 = v3 - v4; // the current corner float c41 = quad1->corners[corner1]->pt.x - u4; float d41 = quad1->corners[corner1]->pt.y - v4; // the candidate corner float c42 = quad2->corners[corner2]->pt.x - u4; float d42 = quad2->corners[corner2]->pt.y - v4; float sign41 = a4*d41-c41*b4; float sign42 = a4*d42-c42*b4; // Also make sure that two border quads of the same row or // column don't link. Check from the candidate corner's view, // whether the corner diagonal from the current corner // is also on the same side of the two lines as the current // corner and the candidate corner. float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2; float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2; float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4; float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4; float sign33 = a3*d33-c33*b3; float sign43 = a4*d43-c43*b4; // This time we also need to make shure, that no quad // is linked to a quad of another dilation run which // may lie INSIDE it!!! // Third: Therefore check everything from the viewpoint // of the current quad compute midpoints of "parallel" // quad sides 1 float x5 = quad1->corners[corner1]->pt.x; float y5 = quad1->corners[corner1]->pt.y; float x6 = quad1->corners[(corner1+1)%4]->pt.x; float y6 = quad1->corners[(corner1+1)%4]->pt.y; // compute midpoints of "parallel" quad sides 2 float x7 = x5; float y7 = y5; float x8 = quad1->corners[(corner1+3)%4]->pt.x; float y8 = quad1->corners[(corner1+3)%4]->pt.y; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the other side of the two lines than // corner1. This is given, if the cross product has // a different sign for both computations below: float a5 = x6 - x5; float b5 = y6 - y5; // the current corner float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5; float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5; // the candidate corner float c52 = quad2->corners[corner2]->pt.x - x5; float d52 = quad2->corners[corner2]->pt.y - y5; float sign51 = a5*d51 - c51*b5; float sign52 = a5*d52 - c52*b5; float a6 = x8 - x7; float b6 = y8 - y7; // the current corner float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7; float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7; // the candidate corner float c62 = quad2->corners[corner2]->pt.x - x7; float d62 = quad2->corners[corner2]->pt.y - y7; float sign61 = a6*d61 - c61*b6; float sign62 = a6*d62 - c62*b6; // Fourth: Then check everything from the viewpoint of // the candidate quad compute midpoints of "parallel" // quad sides 1 float u5 = quad2->corners[corner2]->pt.x; float v5 = quad2->corners[corner2]->pt.y; float u6 = quad2->corners[(corner2+1)%4]->pt.x; float v6 = quad2->corners[(corner2+1)%4]->pt.y; // compute midpoints of "parallel" quad sides 2 float u7 = u5; float v7 = v5; float u8 = quad2->corners[(corner2+3)%4]->pt.x; float v8 = quad2->corners[(corner2+3)%4]->pt.y; // MARTIN: Heuristic // For corner2 of quad2 to be considered, // it needs to be on the other side of the two lines than // corner1. This is given, if the cross product has // a different sign for both computations below: float a7 = u6 - u5; float b7 = v6 - v5; // the current corner float c71 = quad1->corners[corner1]->pt.x - u5; float d71 = quad1->corners[corner1]->pt.y - v5; // the candidate corner float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5; float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5; float sign71 = a7*d71-c71*b7; float sign72 = a7*d72-c72*b7; float a8 = u8 - u7; float b8 = v8 - v7; // the current corner float c81 = quad1->corners[corner1]->pt.x - u7; float d81 = quad1->corners[corner1]->pt.y - v7; // the candidate corner float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7; float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7; float sign81 = a8*d81-c81*b8; float sign82 = a8*d82-c82*b8; // Check whether conditions are fulfilled if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) && ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) && ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) && ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) && ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) && ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) && ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) && ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) && ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) && ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) && ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) && ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0))) { return true; } else { return false; } } } ================================================ FILE: camera_model/src/gpl/EigenQuaternionParameterization.cc ================================================ #include "camodocal/gpl/EigenQuaternionParameterization.h" #include namespace camodocal { bool EigenQuaternionParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const { const double norm_delta = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); if (norm_delta > 0.0) { const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); double q_delta[4]; q_delta[0] = sin_delta_by_delta * delta[0]; q_delta[1] = sin_delta_by_delta * delta[1]; q_delta[2] = sin_delta_by_delta * delta[2]; q_delta[3] = cos(norm_delta); EigenQuaternionProduct(q_delta, x, x_plus_delta); } else { for (int i = 0; i < 4; ++i) { x_plus_delta[i] = x[i]; } } return true; } bool EigenQuaternionParameterization::ComputeJacobian(const double* x, double* jacobian) const { jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT return true; } } ================================================ FILE: camera_model/src/gpl/gpl.cc ================================================ #include "camodocal/gpl/gpl.h" #include #ifdef _WIN32 #include #else #include #endif // source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x #ifdef __APPLE__ #include #define ORWL_NANO (+1.0E-9) #define ORWL_GIGA UINT64_C(1000000000) static double orwl_timebase = 0.0; static uint64_t orwl_timestart = 0; struct timespec orwl_gettime(void) { // be more careful in a multithreaded environement if (!orwl_timestart) { mach_timebase_info_data_t tb = { 0 }; mach_timebase_info(&tb); orwl_timebase = tb.numer; orwl_timebase /= tb.denom; orwl_timestart = mach_absolute_time(); } struct timespec t; double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase; t.tv_sec = diff * ORWL_NANO; t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA); return t; } #endif // __APPLE__ const double WGS84_A = 6378137.0; const double WGS84_ECCSQ = 0.00669437999013; // Windows lacks fminf #ifndef fminf #define fminf(x, y) (((x) < (y)) ? (x) : (y)) #endif namespace camodocal { double hypot3(double x, double y, double z) { return sqrt(square(x) + square(y) + square(z)); } float hypot3f(float x, float y, float z) { return sqrtf(square(x) + square(y) + square(z)); } double d2r(double deg) { return deg / 180.0 * M_PI; } float d2r(float deg) { return deg / 180.0f * M_PI; } double r2d(double rad) { return rad / M_PI * 180.0; } float r2d(float rad) { return rad / M_PI * 180.0f; } double sinc(double theta) { return sin(theta) / theta; } #ifdef _WIN32 #include #include #include LARGE_INTEGER getFILETIMEoffset() { SYSTEMTIME s; FILETIME f; LARGE_INTEGER t; s.wYear = 1970; s.wMonth = 1; s.wDay = 1; s.wHour = 0; s.wMinute = 0; s.wSecond = 0; s.wMilliseconds = 0; SystemTimeToFileTime(&s, &f); t.QuadPart = f.dwHighDateTime; t.QuadPart <<= 32; t.QuadPart |= f.dwLowDateTime; return (t); } int clock_gettime(int X, struct timespec *tp) { LARGE_INTEGER t; FILETIME f; double microseconds; static LARGE_INTEGER offset; static double frequencyToMicroseconds; static int initialized = 0; static BOOL usePerformanceCounter = 0; if (!initialized) { LARGE_INTEGER performanceFrequency; initialized = 1; usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency); if (usePerformanceCounter) { QueryPerformanceCounter(&offset); frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.; } else { offset = getFILETIMEoffset(); frequencyToMicroseconds = 10.; } } if (usePerformanceCounter) QueryPerformanceCounter(&t); else { GetSystemTimeAsFileTime(&f); t.QuadPart = f.dwHighDateTime; t.QuadPart <<= 32; t.QuadPart |= f.dwLowDateTime; } t.QuadPart -= offset.QuadPart; microseconds = (double)t.QuadPart / frequencyToMicroseconds; t.QuadPart = microseconds; tp->tv_sec = t.QuadPart / 1000000; tp->tv_nsec = (t.QuadPart % 1000000) * 1000; return (0); } #endif unsigned long long timeInMicroseconds(void) { struct timespec tp; #ifdef __APPLE__ tp = orwl_gettime(); #else clock_gettime(CLOCK_REALTIME, &tp); #endif return tp.tv_sec * 1000000 + tp.tv_nsec / 1000; } double timeInSeconds(void) { struct timespec tp; #ifdef __APPLE__ tp = orwl_gettime(); #else clock_gettime(CLOCK_REALTIME, &tp); #endif return static_cast(tp.tv_sec) + static_cast(tp.tv_nsec) / 1000000000.0; } float colormapAutumn[128][3] = { {1.0f,0.f,0.f}, {1.0f,0.007874f,0.f}, {1.0f,0.015748f,0.f}, {1.0f,0.023622f,0.f}, {1.0f,0.031496f,0.f}, {1.0f,0.03937f,0.f}, {1.0f,0.047244f,0.f}, {1.0f,0.055118f,0.f}, {1.0f,0.062992f,0.f}, {1.0f,0.070866f,0.f}, {1.0f,0.07874f,0.f}, {1.0f,0.086614f,0.f}, {1.0f,0.094488f,0.f}, {1.0f,0.10236f,0.f}, {1.0f,0.11024f,0.f}, {1.0f,0.11811f,0.f}, {1.0f,0.12598f,0.f}, {1.0f,0.13386f,0.f}, {1.0f,0.14173f,0.f}, {1.0f,0.14961f,0.f}, {1.0f,0.15748f,0.f}, {1.0f,0.16535f,0.f}, {1.0f,0.17323f,0.f}, {1.0f,0.1811f,0.f}, {1.0f,0.18898f,0.f}, {1.0f,0.19685f,0.f}, {1.0f,0.20472f,0.f}, {1.0f,0.2126f,0.f}, {1.0f,0.22047f,0.f}, {1.0f,0.22835f,0.f}, {1.0f,0.23622f,0.f}, {1.0f,0.24409f,0.f}, {1.0f,0.25197f,0.f}, {1.0f,0.25984f,0.f}, {1.0f,0.26772f,0.f}, {1.0f,0.27559f,0.f}, {1.0f,0.28346f,0.f}, {1.0f,0.29134f,0.f}, {1.0f,0.29921f,0.f}, {1.0f,0.30709f,0.f}, {1.0f,0.31496f,0.f}, {1.0f,0.32283f,0.f}, {1.0f,0.33071f,0.f}, {1.0f,0.33858f,0.f}, {1.0f,0.34646f,0.f}, {1.0f,0.35433f,0.f}, {1.0f,0.3622f,0.f}, {1.0f,0.37008f,0.f}, {1.0f,0.37795f,0.f}, {1.0f,0.38583f,0.f}, {1.0f,0.3937f,0.f}, {1.0f,0.40157f,0.f}, {1.0f,0.40945f,0.f}, {1.0f,0.41732f,0.f}, {1.0f,0.4252f,0.f}, {1.0f,0.43307f,0.f}, {1.0f,0.44094f,0.f}, {1.0f,0.44882f,0.f}, {1.0f,0.45669f,0.f}, {1.0f,0.46457f,0.f}, {1.0f,0.47244f,0.f}, {1.0f,0.48031f,0.f}, {1.0f,0.48819f,0.f}, {1.0f,0.49606f,0.f}, {1.0f,0.50394f,0.f}, {1.0f,0.51181f,0.f}, {1.0f,0.51969f,0.f}, {1.0f,0.52756f,0.f}, {1.0f,0.53543f,0.f}, {1.0f,0.54331f,0.f}, {1.0f,0.55118f,0.f}, {1.0f,0.55906f,0.f}, {1.0f,0.56693f,0.f}, {1.0f,0.5748f,0.f}, {1.0f,0.58268f,0.f}, {1.0f,0.59055f,0.f}, {1.0f,0.59843f,0.f}, {1.0f,0.6063f,0.f}, {1.0f,0.61417f,0.f}, {1.0f,0.62205f,0.f}, {1.0f,0.62992f,0.f}, {1.0f,0.6378f,0.f}, {1.0f,0.64567f,0.f}, {1.0f,0.65354f,0.f}, {1.0f,0.66142f,0.f}, {1.0f,0.66929f,0.f}, {1.0f,0.67717f,0.f}, {1.0f,0.68504f,0.f}, {1.0f,0.69291f,0.f}, {1.0f,0.70079f,0.f}, {1.0f,0.70866f,0.f}, {1.0f,0.71654f,0.f}, {1.0f,0.72441f,0.f}, {1.0f,0.73228f,0.f}, {1.0f,0.74016f,0.f}, {1.0f,0.74803f,0.f}, {1.0f,0.75591f,0.f}, {1.0f,0.76378f,0.f}, {1.0f,0.77165f,0.f}, {1.0f,0.77953f,0.f}, {1.0f,0.7874f,0.f}, {1.0f,0.79528f,0.f}, {1.0f,0.80315f,0.f}, {1.0f,0.81102f,0.f}, {1.0f,0.8189f,0.f}, {1.0f,0.82677f,0.f}, {1.0f,0.83465f,0.f}, {1.0f,0.84252f,0.f}, {1.0f,0.85039f,0.f}, {1.0f,0.85827f,0.f}, {1.0f,0.86614f,0.f}, {1.0f,0.87402f,0.f}, {1.0f,0.88189f,0.f}, {1.0f,0.88976f,0.f}, {1.0f,0.89764f,0.f}, {1.0f,0.90551f,0.f}, {1.0f,0.91339f,0.f}, {1.0f,0.92126f,0.f}, {1.0f,0.92913f,0.f}, {1.0f,0.93701f,0.f}, {1.0f,0.94488f,0.f}, {1.0f,0.95276f,0.f}, {1.0f,0.96063f,0.f}, {1.0f,0.9685f,0.f}, {1.0f,0.97638f,0.f}, {1.0f,0.98425f,0.f}, {1.0f,0.99213f,0.f}, {1.0f,1.0f,0.0f} }; float colormapJet[128][3] = { {0.0f,0.0f,0.53125f}, {0.0f,0.0f,0.5625f}, {0.0f,0.0f,0.59375f}, {0.0f,0.0f,0.625f}, {0.0f,0.0f,0.65625f}, {0.0f,0.0f,0.6875f}, {0.0f,0.0f,0.71875f}, {0.0f,0.0f,0.75f}, {0.0f,0.0f,0.78125f}, {0.0f,0.0f,0.8125f}, {0.0f,0.0f,0.84375f}, {0.0f,0.0f,0.875f}, {0.0f,0.0f,0.90625f}, {0.0f,0.0f,0.9375f}, {0.0f,0.0f,0.96875f}, {0.0f,0.0f,1.0f}, {0.0f,0.03125f,1.0f}, {0.0f,0.0625f,1.0f}, {0.0f,0.09375f,1.0f}, {0.0f,0.125f,1.0f}, {0.0f,0.15625f,1.0f}, {0.0f,0.1875f,1.0f}, {0.0f,0.21875f,1.0f}, {0.0f,0.25f,1.0f}, {0.0f,0.28125f,1.0f}, {0.0f,0.3125f,1.0f}, {0.0f,0.34375f,1.0f}, {0.0f,0.375f,1.0f}, {0.0f,0.40625f,1.0f}, {0.0f,0.4375f,1.0f}, {0.0f,0.46875f,1.0f}, {0.0f,0.5f,1.0f}, {0.0f,0.53125f,1.0f}, {0.0f,0.5625f,1.0f}, {0.0f,0.59375f,1.0f}, {0.0f,0.625f,1.0f}, {0.0f,0.65625f,1.0f}, {0.0f,0.6875f,1.0f}, {0.0f,0.71875f,1.0f}, {0.0f,0.75f,1.0f}, {0.0f,0.78125f,1.0f}, {0.0f,0.8125f,1.0f}, {0.0f,0.84375f,1.0f}, {0.0f,0.875f,1.0f}, {0.0f,0.90625f,1.0f}, {0.0f,0.9375f,1.0f}, {0.0f,0.96875f,1.0f}, {0.0f,1.0f,1.0f}, {0.03125f,1.0f,0.96875f}, {0.0625f,1.0f,0.9375f}, {0.09375f,1.0f,0.90625f}, {0.125f,1.0f,0.875f}, {0.15625f,1.0f,0.84375f}, {0.1875f,1.0f,0.8125f}, {0.21875f,1.0f,0.78125f}, {0.25f,1.0f,0.75f}, {0.28125f,1.0f,0.71875f}, {0.3125f,1.0f,0.6875f}, {0.34375f,1.0f,0.65625f}, {0.375f,1.0f,0.625f}, {0.40625f,1.0f,0.59375f}, {0.4375f,1.0f,0.5625f}, {0.46875f,1.0f,0.53125f}, {0.5f,1.0f,0.5f}, {0.53125f,1.0f,0.46875f}, {0.5625f,1.0f,0.4375f}, {0.59375f,1.0f,0.40625f}, {0.625f,1.0f,0.375f}, {0.65625f,1.0f,0.34375f}, {0.6875f,1.0f,0.3125f}, {0.71875f,1.0f,0.28125f}, {0.75f,1.0f,0.25f}, {0.78125f,1.0f,0.21875f}, {0.8125f,1.0f,0.1875f}, {0.84375f,1.0f,0.15625f}, {0.875f,1.0f,0.125f}, {0.90625f,1.0f,0.09375f}, {0.9375f,1.0f,0.0625f}, {0.96875f,1.0f,0.03125f}, {1.0f,1.0f,0.0f}, {1.0f,0.96875f,0.0f}, {1.0f,0.9375f,0.0f}, {1.0f,0.90625f,0.0f}, {1.0f,0.875f,0.0f}, {1.0f,0.84375f,0.0f}, {1.0f,0.8125f,0.0f}, {1.0f,0.78125f,0.0f}, {1.0f,0.75f,0.0f}, {1.0f,0.71875f,0.0f}, {1.0f,0.6875f,0.0f}, {1.0f,0.65625f,0.0f}, {1.0f,0.625f,0.0f}, {1.0f,0.59375f,0.0f}, {1.0f,0.5625f,0.0f}, {1.0f,0.53125f,0.0f}, {1.0f,0.5f,0.0f}, {1.0f,0.46875f,0.0f}, {1.0f,0.4375f,0.0f}, {1.0f,0.40625f,0.0f}, {1.0f,0.375f,0.0f}, {1.0f,0.34375f,0.0f}, {1.0f,0.3125f,0.0f}, {1.0f,0.28125f,0.0f}, {1.0f,0.25f,0.0f}, {1.0f,0.21875f,0.0f}, {1.0f,0.1875f,0.0f}, {1.0f,0.15625f,0.0f}, {1.0f,0.125f,0.0f}, {1.0f,0.09375f,0.0f}, {1.0f,0.0625f,0.0f}, {1.0f,0.03125f,0.0f}, {1.0f,0.0f,0.0f}, {0.96875f,0.0f,0.0f}, {0.9375f,0.0f,0.0f}, {0.90625f,0.0f,0.0f}, {0.875f,0.0f,0.0f}, {0.84375f,0.0f,0.0f}, {0.8125f,0.0f,0.0f}, {0.78125f,0.0f,0.0f}, {0.75f,0.0f,0.0f}, {0.71875f,0.0f,0.0f}, {0.6875f,0.0f,0.0f}, {0.65625f,0.0f,0.0f}, {0.625f,0.0f,0.0f}, {0.59375f,0.0f,0.0f}, {0.5625f,0.0f,0.0f}, {0.53125f,0.0f,0.0f}, {0.5f,0.0f,0.0f} }; void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth, float minRange, float maxRange) { imgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3); for (int i = 0; i < imgColoredDepth.rows; ++i) { const float* depth = imgDepth.ptr(i); unsigned char* pixel = imgColoredDepth.ptr(i); for (int j = 0; j < imgColoredDepth.cols; ++j) { if (depth[j] != 0) { int idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f; idx = 127 - idx; pixel[0] = colormapJet[idx][2] * 255.0f; pixel[1] = colormapJet[idx][1] * 255.0f; pixel[2] = colormapJet[idx][0] * 255.0f; } pixel += 3; } } } bool colormap(const std::string& name, unsigned char idx, float& r, float& g, float& b) { if (name.compare("jet") == 0) { float* color = colormapJet[idx]; r = color[0]; g = color[1]; b = color[2]; return true; } else if (name.compare("autumn") == 0) { float* color = colormapAutumn[idx]; r = color[0]; g = color[1]; b = color[2]; return true; } return false; } std::vector bresLine(int x0, int y0, int x1, int y1) { // Bresenham's line algorithm // Find cells intersected by line between (x0,y0) and (x1,y1) std::vector cells; int dx = std::abs(x1 - x0); int dy = std::abs(y1 - y0); int sx = (x0 < x1) ? 1 : -1; int sy = (y0 < y1) ? 1 : -1; int err = dx - dy; while (1) { cells.push_back(cv::Point2i(x0, y0)); if (x0 == x1 && y0 == y1) { break; } int e2 = 2 * err; if (e2 > -dy) { err -= dy; x0 += sx; } if (e2 < dx) { err += dx; y0 += sy; } } return cells; } std::vector bresCircle(int x0, int y0, int r) { // Bresenham's circle algorithm // Find cells intersected by circle with center (x0,y0) and radius r std::vector< std::vector > mask(2 * r + 1); for (int i = 0; i < 2 * r + 1; ++i) { mask[i].resize(2 * r + 1); for (int j = 0; j < 2 * r + 1; ++j) { mask[i][j] = false; } } int f = 1 - r; int ddF_x = 1; int ddF_y = -2 * r; int x = 0; int y = r; std::vector line; line = bresLine(x0, y0 - r, x0, y0 + r); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - r, y0, x0 + r, y0); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } while (x < y) { if (f >= 0) { y--; ddF_y += 2; f += ddF_y; } x++; ddF_x += 2; f += ddF_x; line = bresLine(x0 - x, y0 + y, x0 + x, y0 + y); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - x, y0 - y, x0 + x, y0 - y); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - y, y0 + x, x0 + y, y0 + x); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } line = bresLine(x0 - y, y0 - x, x0 + y, y0 - x); for (std::vector::iterator it = line.begin(); it != line.end(); ++it) { mask[it->x - x0 + r][it->y - y0 + r] = true; } } std::vector cells; for (int i = 0; i < 2 * r + 1; ++i) { for (int j = 0; j < 2 * r + 1; ++j) { if (mask[i][j]) { cells.push_back(cv::Point2i(i - r + x0, j - r + y0)); } } } return cells; } void fitCircle(const std::vector& points, double& centerX, double& centerY, double& radius) { // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data, // IEEE Transactions on Instrumentation and Measurement, 2000 // We use the modified least squares method. double sum_x = 0.0; double sum_y = 0.0; double sum_xx = 0.0; double sum_xy = 0.0; double sum_yy = 0.0; double sum_xxx = 0.0; double sum_xxy = 0.0; double sum_xyy = 0.0; double sum_yyy = 0.0; int n = points.size(); for (int i = 0; i < n; ++i) { double x = points.at(i).x; double y = points.at(i).y; sum_x += x; sum_y += y; sum_xx += x * x; sum_xy += x * y; sum_yy += y * y; sum_xxx += x * x * x; sum_xxy += x * x * y; sum_xyy += x * y * y; sum_yyy += y * y * y; } double A = n * sum_xx - square(sum_x); double B = n * sum_xy - sum_x * sum_y; double C = n * sum_yy - square(sum_y); double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx); double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy); centerX = (D * C - B * E) / (A * C - square(B)); centerY = (A * E - B * D) / (A * C - square(B)); double sum_r = 0.0; for (int i = 0; i < n; ++i) { double x = points.at(i).x; double y = points.at(i).y; sum_r += hypot(x - centerX, y - centerY); } radius = sum_r / n; } std::vector intersectCircles(double x1, double y1, double r1, double x2, double y2, double r2) { std::vector ipts; double d = hypot(x1 - x2, y1 - y2); if (d > r1 + r2) { // circles are separate return ipts; } if (d < fabs(r1 - r2)) { // one circle is contained within the other return ipts; } double a = (square(r1) - square(r2) + square(d)) / (2.0 * d); double h = sqrt(square(r1) - square(a)); double x3 = x1 + a * (x2 - x1) / d; double y3 = y1 + a * (y2 - y1) / d; if (h < 1e-10) { // two circles touch at one point ipts.push_back(cv::Point2d(x3, y3)); return ipts; } ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d, y3 - h * (x2 - x1) / d)); ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d, y3 + h * (x2 - x1) / d)); return ipts; } char UTMLetterDesignator(double latitude) { // This routine determines the correct UTM letter designator for the given latitude // returns 'Z' if latitude is outside the UTM limits of 84N to 80S // Written by Chuck Gantz- chuck.gantz@globalstar.com char letterDesignator; if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X'; else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W'; else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V'; else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U'; else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T'; else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S'; else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R'; else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q'; else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P'; else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N'; else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M'; else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L'; else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K'; else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J'; else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H'; else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G'; else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F'; else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E'; else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D'; else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C'; else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits return letterDesignator; } void LLtoUTM(double latitude, double longitude, double& utmNorthing, double& utmEasting, std::string& utmZone) { // converts lat/long to UTM coords. Equations from USGS Bulletin 1532 // East Longitudes are positive, West longitudes are negative. // North latitudes are positive, South latitudes are negative // Lat and Long are in decimal degrees // Written by Chuck Gantz- chuck.gantz@globalstar.com double k0 = 0.9996; double LongOrigin; double eccPrimeSquared; double N, T, C, A, M; double LatRad = latitude * M_PI / 180.0; double LongRad = longitude * M_PI / 180.0; double LongOriginRad; int ZoneNumber = static_cast((longitude + 180.0) / 6.0) + 1; if (latitude >= 56.0 && latitude < 64.0 && longitude >= 3.0 && longitude < 12.0) { ZoneNumber = 32; } // Special zones for Svalbard if (latitude >= 72.0 && latitude < 84.0) { if ( longitude >= 0.0 && longitude < 9.0) ZoneNumber = 31; else if (longitude >= 9.0 && longitude < 21.0) ZoneNumber = 33; else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35; else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37; } LongOrigin = static_cast((ZoneNumber - 1) * 6 - 180 + 3); //+3 puts origin in middle of zone LongOriginRad = LongOrigin * M_PI / 180.0; // compute the UTM Zone from the latitude and longitude std::ostringstream oss; oss << ZoneNumber << UTMLetterDesignator(latitude); utmZone = oss.str(); eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad)); T = tan(LatRad) * tan(LatRad); C = eccPrimeSquared * cos(LatRad) * cos(LatRad); A = cos(LatRad) * (LongRad - LongOriginRad); M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0) * LatRad - (3.0 * WGS84_ECCSQ / 8.0 + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0 + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * sin(2.0 * LatRad) + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0 + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * sin(4.0 * LatRad) - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0) * sin(6.0 * LatRad)); utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * eccPrimeSquared) * A * A * A * A * A / 120.0) + 500000.0; utmNorthing = k0 * (M + N * tan(LatRad) * (A * A / 2.0 + (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * eccPrimeSquared) * A * A * A * A * A * A / 720.0)); if (latitude < 0.0) { utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere } } void UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, double& latitude, double& longitude) { // converts UTM coords to lat/long. Equations from USGS Bulletin 1532 // East Longitudes are positive, West longitudes are negative. // North latitudes are positive, South latitudes are negative // Lat and Long are in decimal degrees. // Written by Chuck Gantz- chuck.gantz@globalstar.com double k0 = 0.9996; double eccPrimeSquared; double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ)); double N1, T1, C1, R1, D, M; double LongOrigin; double mu, phi1, phi1Rad; double x, y; int ZoneNumber; char ZoneLetter; bool NorthernHemisphere; x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude y = utmNorthing; std::istringstream iss(utmZone); iss >> ZoneNumber >> ZoneLetter; if ((static_cast(ZoneLetter) - static_cast('N')) >= 0) { NorthernHemisphere = true;//point is in northern hemisphere } else { NorthernHemisphere = false;//point is in southern hemisphere y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere } LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0; //+3 puts origin in middle of zone eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); M = y / k0; mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)); phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu) + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0) * sin(4.0 * mu) + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu); phi1 = phi1Rad / M_PI * 180.0; N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad)); T1 = tan(phi1Rad) * tan(phi1Rad); C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); R1 = WGS84_A * (1.0 - WGS84_ECCSQ) / pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5); D = x / (N1 * k0); latitude = phi1Rad - (N1 * tan(phi1Rad) / R1) * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0 + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - 252.0 * eccPrimeSquared - 3.0 * C1 * C1) * D * D * D * D * D * D / 720.0); latitude *= 180.0 / M_PI; longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0 + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 + 8.0 * eccPrimeSquared + 24.0 * T1 * T1) * D * D * D * D * D / 120.0) / cos(phi1Rad); longitude = LongOrigin + longitude / M_PI * 180.0; } long int timestampDiff(uint64_t t1, uint64_t t2) { if (t2 > t1) { uint64_t d = t2 - t1; if (d > std::numeric_limits::max()) { return std::numeric_limits::max(); } else { return d; } } else { uint64_t d = t1 - t2; if (d > std::numeric_limits::max()) { return std::numeric_limits::min(); } else { return - static_cast(d); } } } } ================================================ FILE: camera_model/src/intrinsic_calib.cc ================================================ #include #include #include #include #include #include #include #include #include #include "camodocal/chessboard/Chessboard.h" #include "camodocal/calib/CameraCalibration.h" #include "camodocal/gpl/gpl.h" int main(int argc, char** argv) { cv::Size boardSize; float squareSize; std::string inputDir; std::string cameraModel; std::string cameraName; std::string prefix; std::string fileExtension; bool useOpenCV; bool viewResults; bool verbose; //========= Handling Program options ========= boost::program_options::options_description desc("Allowed options"); desc.add_options() ("help", "produce help message") ("width,w", boost::program_options::value(&boardSize.width)->default_value(8), "Number of inner corners on the chessboard pattern in x direction") ("height,h", boost::program_options::value(&boardSize.height)->default_value(12), "Number of inner corners on the chessboard pattern in y direction") ("size,s", boost::program_options::value(&squareSize)->default_value(7.f), "Size of one square in mm") ("input,i", boost::program_options::value(&inputDir)->default_value("calibrationdata"), "Input directory containing chessboard images") ("prefix,p", boost::program_options::value(&prefix)->default_value("left-"), "Prefix of images") ("file-extension,e", boost::program_options::value(&fileExtension)->default_value(".png"), "File extension of images") ("camera-model", boost::program_options::value(&cameraModel)->default_value("mei"), "Camera model: kannala-brandt | mei | pinhole") ("camera-name", boost::program_options::value(&cameraName)->default_value("camera"), "Name of camera") ("opencv", boost::program_options::bool_switch(&useOpenCV)->default_value(true), "Use OpenCV to detect corners") ("view-results", boost::program_options::bool_switch(&viewResults)->default_value(false), "View results") ("verbose,v", boost::program_options::bool_switch(&verbose)->default_value(true), "Verbose output") ; boost::program_options::positional_options_description pdesc; pdesc.add("input", 1); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm); boost::program_options::notify(vm); if (vm.count("help")) { std::cout << desc << std::endl; return 1; } if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir)) { std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl; return 1; } camodocal::Camera::ModelType modelType; if (boost::iequals(cameraModel, "kannala-brandt")) { modelType = camodocal::Camera::KANNALA_BRANDT; } else if (boost::iequals(cameraModel, "mei")) { modelType = camodocal::Camera::MEI; } else if (boost::iequals(cameraModel, "pinhole")) { modelType = camodocal::Camera::PINHOLE; } else if (boost::iequals(cameraModel, "scaramuzza")) { modelType = camodocal::Camera::SCARAMUZZA; } else { std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl; return 1; } switch (modelType) { case camodocal::Camera::KANNALA_BRANDT: std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl; break; case camodocal::Camera::MEI: std::cout << "# INFO: Camera model: Mei" << std::endl; break; case camodocal::Camera::PINHOLE: std::cout << "# INFO: Camera model: Pinhole" << std::endl; break; case camodocal::Camera::SCARAMUZZA: std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl; break; } // look for images in input directory std::vector imageFilenames; boost::filesystem::directory_iterator itr; for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr) { if (!boost::filesystem::is_regular_file(itr->status())) { continue; } std::string filename = itr->path().filename().string(); // check if prefix matches if (!prefix.empty()) { if (filename.compare(0, prefix.length(), prefix) != 0) { continue; } } // check if file extension matches if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0) { continue; } imageFilenames.push_back(itr->path().string()); if (verbose) { std::cerr << "# INFO: Adding " << imageFilenames.back() << std::endl; } } if (imageFilenames.empty()) { std::cerr << "# ERROR: No chessboard images found." << std::endl; return 1; } if (verbose) { std::cerr << "# INFO: # images: " << imageFilenames.size() << std::endl; } std::sort(imageFilenames.begin(), imageFilenames.end()); cv::Mat image = cv::imread(imageFilenames.front(), -1); const cv::Size frameSize = image.size(); camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize); calibration.setVerbose(verbose); std::vector chessboardFound(imageFilenames.size(), false); for (size_t i = 0; i < imageFilenames.size(); ++i) { image = cv::imread(imageFilenames.at(i), -1); camodocal::Chessboard chessboard(boardSize, image); chessboard.findCorners(useOpenCV); if (chessboard.cornersFound()) { if (verbose) { std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " << imageFilenames.at(i) << std::endl; } calibration.addChessboardData(chessboard.getCorners()); cv::Mat sketch; chessboard.getSketch().copyTo(sketch); cv::imshow("Image", sketch); cv::waitKey(50); } else if (verbose) { std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl; } chessboardFound.at(i) = chessboard.cornersFound(); } cv::destroyWindow("Image"); if (calibration.sampleCount() < 10) { std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl; return 1; } if (verbose) { std::cerr << "# INFO: Calibrating..." << std::endl; } double startTime = camodocal::timeInSeconds(); calibration.calibrate(); calibration.writeParams(cameraName + "_camera_calib.yaml"); calibration.writeChessboardData(cameraName + "_chessboard_data.dat"); if (verbose) { std::cout << "# INFO: Calibration took a total time of " << std::fixed << std::setprecision(3) << camodocal::timeInSeconds() - startTime << " sec.\n"; } if (verbose) { std::cerr << "# INFO: Wrote calibration file to " << cameraName + "_camera_calib.yaml" << std::endl; } if (viewResults) { std::vector cbImages; std::vector cbImageFilenames; for (size_t i = 0; i < imageFilenames.size(); ++i) { if (!chessboardFound.at(i)) { continue; } cbImages.push_back(cv::imread(imageFilenames.at(i), -1)); cbImageFilenames.push_back(imageFilenames.at(i)); } // visualize observed and reprojected points calibration.drawResults(cbImages); for (size_t i = 0; i < cbImages.size(); ++i) { cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, CV_AA); cv::imshow("Image", cbImages.at(i)); cv::waitKey(0); } } return 0; } ================================================ FILE: camera_model/src/sparse_graph/Transform.cc ================================================ #include namespace camodocal { Transform::Transform() { m_q.setIdentity(); m_t.setZero(); } Transform::Transform(const Eigen::Matrix4d& H) { m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); m_t = H.block<3,1>(0,3); } Eigen::Quaterniond& Transform::rotation(void) { return m_q; } const Eigen::Quaterniond& Transform::rotation(void) const { return m_q; } double* Transform::rotationData(void) { return m_q.coeffs().data(); } const double* const Transform::rotationData(void) const { return m_q.coeffs().data(); } Eigen::Vector3d& Transform::translation(void) { return m_t; } const Eigen::Vector3d& Transform::translation(void) const { return m_t; } double* Transform::translationData(void) { return m_t.data(); } const double* const Transform::translationData(void) const { return m_t.data(); } Eigen::Matrix4d Transform::toMatrix(void) const { Eigen::Matrix4d H; H.setIdentity(); H.block<3,3>(0,0) = m_q.toRotationMatrix(); H.block<3,1>(0,3) = m_t; return H; } } ================================================ FILE: config/3dm/3dm_config.yaml ================================================ %YAML:1.0 #common parameters imu_topic: "/imu_3dm_gx4/imu" image_topic: "/mv_25001498/image_raw" output_path: "/config/3dm/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path # MEI is better than PINHOLE for large FOV camera model_type: MEI camera_name: camera image_width: 752 image_height: 480 mirror_parameters: xi: 2.057e+00 distortion_parameters: k1: 7.145e-02 k2: 5.059e-01 p1: 4.727e-05 p2: -5.492e-04 projection_parameters: gamma1: 1.115e+03 gamma2: 1.114e+03 u0: 3.672e+02 v0: 2.385e+02 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 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. ex_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. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 0, 0, -1, -1, 0, 0, 0, 1, 0] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [ 0, 0, 0.02] #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 20 # min distance between two features freq: 20 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic equalize: 0 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time max_num_iterations: 10 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.2 # accelerometer measurement noise standard deviation. gyr_n: 0.05 # gyroscope measurement noise standard deviation. acc_w: 0.002 # accelerometer bias random work noise standard deviation. gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. g_norm: 9.805 # #loop closure parameters loop_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; #also give the camera calibration file same as feature_tracker node pattern_file: "/support_files/brief_pattern.yml" voc_file: "/support_files/brief_k10L6.bin" min_loop_num: 20 ================================================ FILE: config/A3/A3_config.yaml ================================================ %YAML:1.0 #common parameters imu_topic: "/djiros/imu" image_topic: "/djiros/image" output_path: "/config/A3/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path #camera calibration model_type: MEI camera_name: camera image_width: 752 image_height: 480 mirror_parameters: xi: 2.5826025650890592e+00 distortion_parameters: k1: 3.9389417936108251e-01 k2: 2.2073276594959599e+00 p1: -1.9253757033376501e-03 p2: -6.1503758445210599e-04 projection_parameters: gamma1: 1.3068921176079280e+03 gamma2: 1.3065892394640514e+03 u0: 3.6959543908262725e+02 v0: 2.1891731132428231e+02 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 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. ex_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. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 0, -1, 0, 1, 0, 0, 0, 0, 1] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [ 0, 0.05, 0] #feature traker paprameters max_cnt: 250 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic equalize: 1 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.2 # accelerometer measurement noise standard deviation. gyr_n: 0.05 # gyroscope measurement noise standard deviation. acc_w: 0.002 # accelerometer bias random work noise standard deviation. gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. g_norm: 9.805 # #loop closure parameters loop_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; #also give the camera calibration file same as feature_tracker node pattern_file: "/support_files/brief_pattern.yml" voc_file: "/support_files/brief_k10L6.bin" min_loop_num: 30 ================================================ FILE: config/AR_demo.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /AR_image1/Status1 - /raw image1/Status1 - /Path2/Status1 Splitter Ratio: 0.465116 Tree Height: 729 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: PointCloud Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 130; 130; 130 Enabled: true Line Style: Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: -1.2 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 170; 0 Enabled: true Head Diameter: 0.3 Head Length: 0.2 Length: 0.3 Line Style: Lines Line Width: 0.01 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Style: None Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 Topic: /vins_estimator/path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: true - Class: rviz/Image Enabled: true Image Topic: /ar_demo_node/AR_image Max Value: 1 Median window: 5 Min Value: 0 Name: AR_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image Enabled: true Image Topic: /mv_25001498/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: MarkerArray Namespaces: CameraPoseVisualization: true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /ar_demo_node/AR_object Name: MarkerArray Namespaces: basic_shapes: true Queue Size: 100 Value: true - Class: rviz/Axes Enabled: true Length: 1 Name: Axes Radius: 0.1 Reference Frame: Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 85; 255 Enabled: true Head Diameter: 0.3 Head Length: 0.2 Length: 0.3 Line Style: Billboards Line Width: 0.3 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Style: None Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 Topic: /odom_translation/vins Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 33.293 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: -3.42082 Y: -4.08465 Z: -8.071 Name: Current View Near Clip Distance: 0.01 Pitch: 1.5698 Target Frame: Value: Orbit (rviz) Yaw: 0.0571614 Saved: ~ Window Geometry: AR_image: collapsed: false Displays: collapsed: true Height: 959 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 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 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1572 X: 286 Y: 43 raw image: collapsed: false ================================================ FILE: config/euroc/euroc_config.yaml ================================================ %YAML:1.0 #common parameters imu_topic: "/imu0" image_topic: "/cam0/image_raw" output_path: "/src/config/euroc/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path #camera calibration model_type: PINHOLE camera_name: camera image_width: 752 image_height: 480 distortion_parameters: k1: -2.917e-01 k2: 8.228e-02 p1: 5.333e-05 p2: -1.578e-04 projection_parameters: fx: 4.616e+02 fy: 4.603e+02 cx: 3.630e+02 cy: 2.481e+02 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 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. ex_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. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [0, -1, 0, 1, 0, 0, 0, 0, 1] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [-0.02,-0.06, 0.01] #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 0 # publish tracking image as topic equalize: 1 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02 gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.81007 # gravity magnitude #loop closure parameters loop_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; #also give the camera calibration file same as feature_tracker node pattern_file: "/src/support_files/brief_pattern.yml" voc_file: "/src/support_files/brief_k10L6.bin" min_loop_num: 25 visualLookAtX: 1 visualLookAtY: 0 visualLookAtZ: 1 ================================================ FILE: config/euroc/euroc_config_no_extrinsic.yaml ================================================ %YAML:1.0 #common parameters imu_topic: "/imu0" image_topic: "/cam0/image_raw" output_path: "/config/euroc/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path #camera calibration model_type: PINHOLE camera_name: camera image_width: 752 image_height: 480 distortion_parameters: k1: -2.917e-01 k2: 8.228e-02 p1: 5.333e-05 p2: -1.578e-04 projection_parameters: fx: 4.616e+02 fy: 4.603e+02 cx: 3.630e+02 cy: 2.481e+02 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 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. ex_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. #If you choose 0 or 1, you should write down the following matrix. #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic equalize: 1 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05 acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02 gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.81007 # gravity magnitude #loop closure parameters loop_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; #also give the camera calibration file same as feature_tracker node pattern_file: "/support_files/brief_pattern.yml" voc_file: "/support_files/brief_k10L6.bin" min_loop_num: 25 ================================================ FILE: config/euroc/ex_calib_result.yaml ================================================ %YAML:1.0 extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 1.5942328773486114e-02, -9.9987084186029263e-01, 2.0351292016634716e-03, 9.9974794303804226e-01, 1.5972482669662336e-02, 1.5777521623200924e-02, -1.5807989893762975e-02, 1.7830857962318550e-03, 9.9987345602359201e-01 ] extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [ -1.3988442922766306e-02, -6.9759508958340238e-02, -5.3892141087174272e-03 ] ================================================ FILE: config/euroc/vins_result.csv ================================================ 1403636580863555328,-0.00682,0.01361,-0.10559,-0.03014,0.83605,0.03655,0.54660,0.08347,-0.00007,0.78361, 1403636580963555584,0.00063,0.01544,-0.02783,-0.02147,0.82770,0.03682,0.55955,0.06403,0.02669,0.74688, 1403636581063555584,0.00615,0.01809,0.04191,-0.02227,0.81983,0.03778,0.57093,0.05168,0.01699,0.65374, 1403636581163555328,0.01111,0.01832,0.10225,-0.02462,0.81161,0.03404,0.58269,0.03790,-0.00546,0.54246, 1403636581263555584,0.01280,0.01693,0.15032,-0.02759,0.80171,0.02989,0.59633,-0.00011,-0.01448,0.39584, 1403636581363555328,0.01139,0.01555,0.17670,-0.02755,0.79400,0.03025,0.60654,-0.02140,-0.01516,0.08846, 1403636581463555584,0.00908,0.01307,0.17128,-0.02164,0.79456,0.03262,0.60593,-0.02207,-0.02356,-0.26511, 1403636581563555584,0.00549,0.00999,0.13216,-0.01908,0.80127,0.03309,0.59708,-0.00963,-0.01141,-0.53333, 1403636581663555328,0.00425,0.00813,0.07301,-0.02201,0.80807,0.03159,0.58783,-0.00746,-0.02882,-0.63866, 1403636581763555584,0.00224,0.00486,0.00733,-0.02349,0.81096,0.03284,0.58371,-0.02661,-0.02463,-0.66817, 1403636581863555328,-0.00138,0.00367,-0.06040,-0.02713,0.81300,0.03475,0.58059,-0.05044,-0.00807,-0.67626, 1403636581963555584,-0.00818,0.00224,-0.12536,-0.02876,0.81965,0.03211,0.57125,-0.05573,0.00336,-0.61788, 1403636582063555584,-0.01213,0.00277,-0.18242,-0.02620,0.82924,0.02980,0.55748,-0.04760,0.01219,-0.48735, 1403636582163555328,-0.01622,0.00515,-0.22037,-0.02718,0.83795,0.03162,0.54414,-0.04203,0.02805,-0.23760, 1403636582263555584,-0.01772,0.00836,-0.22837,-0.02825,0.84310,0.03385,0.53594,0.00988,0.02488,0.10118, 1403636582363555328,-0.01587,0.01170,-0.20023,-0.03057,0.84240,0.03443,0.53689,0.02482,0.03975,0.43828, 1403636582463555584,-0.01297,0.01503,-0.14738,-0.03151,0.83480,0.03368,0.54862,0.04004,0.02633,0.59434, 1403636582563555584,-0.00857,0.01659,-0.08374,-0.02784,0.82851,0.03327,0.55829,0.04105,0.00830,0.65474, 1403636582663555328,-0.00481,0.01614,-0.01704,-0.02201,0.82558,0.03171,0.56297,0.02795,-0.00798,0.66133, 1403636582763555584,-0.00073,0.01393,0.04884,-0.02107,0.82246,0.02766,0.56777,0.04748,-0.03126,0.64921, 1403636582863555328,0.00401,0.00921,0.11337,-0.01812,0.81602,0.01894,0.57742,0.03672,-0.04482,0.62942, 1403636582963555584,0.00621,0.00464,0.16994,-0.01667,0.80631,0.01348,0.59110,0.00175,-0.03304,0.48289, 1403636583063555584,0.00546,0.00173,0.20403,-0.01539,0.79817,0.01292,0.60210,-0.01204,-0.01868,0.19227, 1403636583163555328,0.00458,-0.00081,0.20675,-0.01137,0.79670,0.01378,0.60411,-0.01422,-0.01915,-0.15242, 1403636583263555584,0.00508,-0.00164,0.17817,-0.01010,0.79854,0.01637,0.60164,0.00180,-0.00825,-0.42100, 1403636583363555328,0.00693,-0.00265,0.12937,-0.01123,0.80119,0.01817,0.59803,0.00130,-0.02156,-0.59314, 1403636583463555584,0.00645,-0.00530,0.06614,-0.01431,0.80447,0.02210,0.59342,-0.01723,-0.01765,-0.67679, 1403636583563555584,0.00484,-0.00848,-0.00345,-0.01442,0.80886,0.02554,0.58727,-0.01692,-0.02377,-0.70251, 1403636583663555328,0.00311,-0.00947,-0.07431,-0.01476,0.81695,0.02628,0.57591,-0.04006,0.00836,-0.68957, 1403636583763555584,-0.00078,-0.00980,-0.13802,-0.02199,0.82965,0.02314,0.55737,-0.04540,-0.00183,-0.58660, 1403636583863555328,-0.00448,-0.00961,-0.19031,-0.02375,0.83956,0.02172,0.54232,-0.04907,-0.00471,-0.46805, 1403636583963555584,-0.00885,-0.00900,-0.22914,-0.02092,0.84388,0.02786,0.53540,-0.05500,0.01272,-0.28167, 1403636584063555584,-0.01361,-0.00641,-0.24448,-0.02514,0.84441,0.03415,0.53401,-0.02981,0.02875,-0.02367, 1403636584163555328,-0.01307,-0.00382,-0.23146,-0.02957,0.84435,0.03741,0.53366,0.02839,0.01821,0.27895, 1403636584263555584,-0.00937,-0.00188,-0.19180,-0.02999,0.84341,0.03856,0.53505,0.04755,0.01772,0.49891, 1403636584363555328,-0.00327,-0.00054,-0.13544,-0.02825,0.84128,0.03930,0.53842,0.08527,-0.00339,0.63773, 1403636584463555584,0.00570,-0.00134,-0.06794,-0.01914,0.83835,0.03999,0.54332,0.09626,-0.00778,0.70873, 1403636584563555584,0.01455,-0.00120,0.00492,-0.01673,0.83187,0.03785,0.55343,0.09200,0.00124,0.74241, 1403636584663555328,0.02252,-0.00084,0.07846,-0.01370,0.82133,0.03199,0.56940,0.06049,0.00450,0.71021, 1403636584763555584,0.02661,-0.00030,0.14443,-0.00954,0.80942,0.02684,0.58654,0.02434,0.00644,0.60608, 1403636584863555328,0.02660,0.00043,0.19703,-0.00966,0.80061,0.02284,0.59868,-0.01410,0.01794,0.43076, 1403636584963555584,0.02523,0.00151,0.22624,-0.01281,0.79647,0.01868,0.60425,-0.00257,0.00904,0.14903, 1403636585063555584,0.02616,0.00122,0.22568,-0.01164,0.79779,0.01475,0.60264,0.01033,-0.00187,-0.14730, 1403636585163555328,0.02695,0.00066,0.20140,-0.01014,0.80085,0.01266,0.59865,0.01872,-0.01485,-0.37597, 1403636585263555584,0.02864,-0.00284,0.15758,-0.00327,0.80455,0.01256,0.59374,0.02415,-0.03724,-0.49994, 1403636585363555328,0.02988,-0.00751,0.10519,0.00403,0.80779,0.01678,0.58921,0.00758,-0.04032,-0.54607, 1403636585463555584,0.02981,-0.01061,0.05146,0.00427,0.81249,0.02212,0.58254,-0.03808,-0.02216,-0.53485, 1403636585563555584,0.02496,-0.01199,0.00102,-0.00124,0.81883,0.02935,0.57328,-0.10143,-0.01583,-0.45602, 1403636585663555328,0.01371,-0.01370,-0.03851,-0.01312,0.82613,0.03561,0.56221,-0.12840,-0.00625,-0.30761, 1403636585763555584,0.00059,-0.01433,-0.06270,-0.02456,0.83456,0.03742,0.54910,-0.11288,-0.00365,-0.15807, 1403636585863555328,-0.01119,-0.01407,-0.07323,-0.03499,0.84394,0.03648,0.53406,-0.10343,0.00023,-0.03933, 1403636585963555584,-0.02096,-0.01383,-0.07419,-0.03917,0.85465,0.03402,0.51660,-0.07020,-0.00846,0.00663, 1403636586063555584,-0.02792,-0.01432,-0.07235,-0.03164,0.86754,0.02674,0.49563,-0.05282,-0.00646,0.03034, 1403636586163555328,-0.03457,-0.01190,-0.06826,-0.02351,0.87785,0.01733,0.47805,-0.06373,0.03524,0.05472, 1403636586263555584,-0.04310,-0.00816,-0.06367,-0.02241,0.88658,0.01297,0.46186,-0.08928,0.02216,0.03913, 1403636586363555328,-0.05323,-0.00721,-0.06067,-0.02313,0.89605,0.00884,0.44326,-0.10886,-0.00328,0.02318, 1403636586463555584,-0.06517,-0.00872,-0.05883,-0.02388,0.90491,0.00698,0.42487,-0.11631,-0.03065,0.02336, 1403636586563555584,-0.07493,-0.01369,-0.05573,-0.02413,0.91132,0.00988,0.41088,-0.07317,-0.06445,0.04026, 1403636586663555328,-0.07811,-0.02196,-0.05045,-0.02568,0.91324,0.01098,0.40646,-0.00491,-0.09759,0.05111, 1403636586763555584,-0.07581,-0.03102,-0.04673,-0.02967,0.91068,0.01224,0.41186,0.03783,-0.08282,0.02462, 1403636586863555328,-0.07068,-0.03886,-0.04569,-0.03244,0.90736,0.01465,0.41883,0.04341,-0.04604,-0.00889, 1403636586963555584,-0.06558,-0.04120,-0.04663,-0.03432,0.90516,0.01977,0.42321,0.05356,-0.01656,-0.00643, 1403636587063555584,-0.05950,-0.04229,-0.04677,-0.03563,0.90496,0.02578,0.42322,0.05596,-0.01597,0.00264, 1403636587163555328,-0.05341,-0.04581,-0.04670,-0.03508,0.90704,0.03001,0.41851,0.06335,-0.06018,-0.00411, 1403636587263555584,-0.04627,-0.05463,-0.04767,-0.03593,0.90814,0.03038,0.41601,0.07638,-0.12152,-0.00936, 1403636587363555328,-0.03736,-0.07088,-0.04863,-0.03581,0.90890,0.02802,0.41453,0.09566,-0.19419,-0.00491, 1403636587463555584,-0.02808,-0.09346,-0.04964,-0.03621,0.90985,0.02434,0.41264,0.08926,-0.26001,-0.00517, 1403636587563555584,-0.01936,-0.12289,-0.05033,-0.03725,0.91064,0.02028,0.41101,0.08643,-0.32509,-0.00006, 1403636587663555328,-0.00990,-0.15754,-0.04991,-0.03641,0.90967,0.01470,0.41346,0.09966,-0.36416,0.01376, 1403636587763555584,0.00005,-0.19409,-0.04795,-0.03462,0.90688,0.00668,0.41991,0.09540,-0.37206,0.01780, 1403636587863555328,0.00942,-0.23144,-0.04776,-0.03139,0.90547,-0.00087,0.42325,0.07767,-0.36062,-0.01786, 1403636587963555584,0.01604,-0.26618,-0.05105,-0.02592,0.90765,-0.00704,0.41887,0.05261,-0.33472,-0.04167, 1403636588063555584,0.02054,-0.29684,-0.05572,-0.01923,0.91195,-0.01385,0.40962,0.04259,-0.26643,-0.05225, 1403636588163555328,0.02460,-0.32010,-0.06119,-0.01817,0.91412,-0.01539,0.40474,0.03465,-0.20117,-0.04792, 1403636588263555584,0.02810,-0.33634,-0.06610,-0.01968,0.91389,-0.01481,0.40522,0.02442,-0.11077,-0.03777, 1403636588363555328,0.02974,-0.34342,-0.06920,-0.02331,0.91240,-0.01432,0.40839,0.00077,-0.02755,-0.02192, 1403636588463555584,0.02875,-0.34269,-0.07078,-0.02514,0.91154,-0.01558,0.41015,-0.02461,0.04398,-0.00377, 1403636588563555584,0.02509,-0.33341,-0.06879,-0.02194,0.91176,-0.01567,0.40983,-0.04221,0.12909,0.03743, 1403636588663555328,0.01995,-0.31644,-0.06463,-0.02282,0.91146,-0.01479,0.41050,-0.06892,0.20966,0.03432, 1403636588763555584,0.01177,-0.29202,-0.06275,-0.02619,0.91225,-0.01150,0.40864,-0.08583,0.26853,0.00885, 1403636588863555328,0.00363,-0.26182,-0.06224,-0.02952,0.91326,-0.00814,0.40623,-0.08470,0.34110,0.00966, 1403636588963555584,-0.00566,-0.22510,-0.05993,-0.03354,0.91286,-0.00406,0.40688,-0.09694,0.38578,0.03623, 1403636589063555584,-0.01669,-0.18430,-0.05505,-0.03735,0.91090,0.00140,0.41093,-0.11290,0.41538,0.05490, 1403636589163555328,-0.02731,-0.14102,-0.04881,-0.04018,0.90859,0.00696,0.41569,-0.10618,0.44169,0.06434, 1403636589263555584,-0.03758,-0.09523,-0.04298,-0.04280,0.90801,0.01107,0.41660,-0.10848,0.46164,0.04558, 1403636589363555328,-0.04830,-0.04811,-0.04024,-0.04517,0.90932,0.01696,0.41329,-0.10462,0.47017,0.01421, 1403636589463555584,-0.05835,-0.00121,-0.03905,-0.04548,0.91011,0.02256,0.41125,-0.10205,0.46503,0.00706, 1403636589563555584,-0.06890,0.04647,-0.03888,-0.04755,0.90905,0.02509,0.41320,-0.10730,0.46799,-0.00518, 1403636589663555328,-0.07917,0.09256,-0.03928,-0.04874,0.90778,0.02848,0.41563,-0.10641,0.45580,-0.00837, 1403636589763555584,-0.08919,0.13692,-0.04085,-0.05218,0.90743,0.03226,0.41569,-0.09978,0.43413,-0.03160, 1403636589863555328,-0.09836,0.17811,-0.04453,-0.05713,0.90927,0.03858,0.41045,-0.08170,0.36937,-0.04216, 1403636589963555584,-0.10596,0.21260,-0.04879,-0.06392,0.91086,0.04442,0.40530,-0.06691,0.31370,-0.04101, 1403636590063555584,-0.11237,0.24052,-0.05032,-0.07020,0.90912,0.04926,0.40760,-0.04657,0.22390,0.00185, 1403636590163555328,-0.11681,0.25720,-0.04898,-0.07547,0.90579,0.05271,0.41360,-0.02865,0.10738,0.01516, 1403636590263555584,-0.11960,0.26160,-0.04673,-0.07605,0.90444,0.05478,0.41617,-0.01896,-0.02379,0.01853, 1403636590363555328,-0.12084,0.25273,-0.04436,-0.07261,0.90548,0.05460,0.41455,-0.00460,-0.14818,0.02077, 1403636590463555584,-0.12057,0.23120,-0.04168,-0.06706,0.90606,0.05186,0.41457,0.01344,-0.26474,0.03520, 1403636590563555584,-0.11870,0.20039,-0.03788,-0.06078,0.90529,0.04906,0.41755,0.03013,-0.34299,0.04058, 1403636590663555328,-0.11484,0.16275,-0.03487,-0.05607,0.90460,0.04567,0.42007,0.05054,-0.39332,0.02048, 1403636590763555584,-0.10920,0.12076,-0.03437,-0.05265,0.90566,0.04106,0.41871,0.07106,-0.42412,-0.00980, 1403636590863555328,-0.10128,0.07649,-0.03679,-0.05307,0.90829,0.03661,0.41335,0.08818,-0.45671,-0.02931, 1403636590963555584,-0.09170,0.02951,-0.03991,-0.05163,0.91005,0.03377,0.40988,0.09851,-0.47209,-0.03359, 1403636591063555584,-0.08079,-0.01612,-0.04334,-0.05303,0.90964,0.03330,0.41066,0.11031,-0.44694,-0.02970, 1403636591163555328,-0.06959,-0.05904,-0.04587,-0.05516,0.90936,0.03160,0.41113,0.10976,-0.41387,-0.02422, 1403636591263555584,-0.06016,-0.09751,-0.05005,-0.05455,0.91031,0.02786,0.40936,0.09728,-0.36481,-0.04707, 1403636591363555328,-0.05107,-0.13047,-0.05581,-0.05219,0.91167,0.02443,0.40686,0.07705,-0.28123,-0.06557, 1403636591463555584,-0.04337,-0.15327,-0.06396,-0.05118,0.91207,0.02024,0.40632,0.08267,-0.19965,-0.08545, 1403636591563555584,-0.03458,-0.17116,-0.07260,-0.04504,0.91042,0.01468,0.41095,0.09424,-0.16068,-0.08309, 1403636591663555328,-0.02464,-0.18362,-0.08006,-0.03458,0.90426,0.00435,0.42555,0.08755,-0.09190,-0.06213, 1403636591763555584,-0.01655,-0.18966,-0.08360,-0.02540,0.89522,-0.00128,0.44490,0.08067,-0.05856,-0.01115, 1403636591863555328,-0.00882,-0.19432,-0.08322,-0.01754,0.88636,-0.00813,0.46260,0.07733,-0.05141,0.00730, 1403636591963555584,-0.00112,-0.20004,-0.08272,-0.01367,0.88367,-0.01692,0.46761,0.09628,-0.06433,-0.00289, 1403636592063555584,0.01176,-0.20863,-0.08402,-0.00999,0.88689,-0.02811,0.46102,0.17390,-0.10482,-0.01744, 1403636592163555328,0.03066,-0.21988,-0.08641,-0.00601,0.89020,-0.03977,0.45378,0.21921,-0.11231,-0.02903, 1403636592263555584,0.05468,-0.23358,-0.08749,-0.00387,0.89433,-0.04459,0.44517,0.25114,-0.07821,-0.02095, 1403636592363555328,0.08135,-0.23837,-0.08893,-0.00685,0.89773,-0.04606,0.43809,0.29023,-0.00289,-0.01497, 1403636592463555584,0.11191,-0.23401,-0.08872,-0.01430,0.89929,-0.04039,0.43524,0.32042,0.09797,0.01015, 1403636592563555584,0.14448,-0.21963,-0.08712,-0.02315,0.89638,-0.02754,0.44182,0.31710,0.20038,0.00951, 1403636592663555328,0.17553,-0.19576,-0.08724,-0.03127,0.89232,-0.00982,0.45021,0.28819,0.28902,-0.00837, 1403636592763555584,0.20117,-0.16260,-0.08868,-0.04233,0.89044,0.00973,0.45303,0.21309,0.39459,-0.01027, 1403636592863555328,0.21691,-0.11848,-0.09042,-0.06127,0.89028,0.03128,0.45018,0.08598,0.49931,-0.03187, 1403636592963555584,0.21840,-0.06605,-0.09281,-0.08025,0.89101,0.05353,0.44361,-0.06741,0.56246,-0.02102, 1403636593063555584,0.20346,-0.00686,-0.09368,-0.09851,0.89089,0.07624,0.43680,-0.22059,0.58798,-0.00098, 1403636593163555328,0.17410,0.05109,-0.09266,-0.11511,0.88908,0.09737,0.43222,-0.36521,0.56406,0.01991, 1403636593263555584,0.13310,0.10445,-0.08925,-0.13437,0.88674,0.10941,0.42856,-0.46725,0.47655,0.02912, 1403636593363555328,0.08549,0.14451,-0.08664,-0.14344,0.88688,0.11757,0.42314,-0.50600,0.30737,0.00811, 1403636593463555584,0.03384,0.16626,-0.08670,-0.14366,0.88805,0.12070,0.41972,-0.51433,0.10594,-0.02011, 1403636593563555584,-0.01733,0.17084,-0.09002,-0.13887,0.89099,0.11426,0.41689,-0.48751,-0.05172,-0.04922, 1403636593663555328,-0.06447,0.15750,-0.09594,-0.13356,0.89660,0.09928,0.41037,-0.44477,-0.21654,-0.06680, 1403636593663555328,-0.10084,0.13182,-0.10591,-0.11997,0.90340,0.07379,0.40501,-0.38163,-0.36947,-0.05984, 1403636593763555584,-0.09691,0.13180,-0.10656,-0.12129,0.90321,0.07623,0.40459,-0.37775,-0.37030,-0.06075, 1403636593863555328,-0.13101,0.08869,-0.11131,-0.10241,0.90644,0.04423,0.40734,-0.28909,-0.48335,-0.02787, 1403636593963555584,-0.15554,0.03619,-0.11254,-0.07956,0.90490,0.00884,0.41803,-0.18973,-0.55366,0.00534, 1403636594063555584,-0.16791,-0.02167,-0.11166,-0.05410,0.90197,-0.02448,0.42770,-0.05762,-0.58409,0.01743, 1403636594163555328,-0.17237,-0.07899,-0.11226,-0.03070,0.90037,-0.05876,0.43004,0.10699,-0.58349,-0.00752, 1403636594263555584,-0.15388,-0.13613,-0.11437,-0.01244,0.90018,-0.07933,0.42805,0.27780,-0.56836,-0.02762, 1403636594363555328,-0.12050,-0.19160,-0.11628,0.00313,0.89954,-0.08958,0.42753,0.42417,-0.53528,-0.02272, 1403636594463555584,-0.07348,-0.23924,-0.11759,0.01538,0.89627,-0.09151,0.43370,0.52489,-0.45553,-0.01436, 1403636594563555584,-0.01923,-0.27626,-0.11916,0.01912,0.89112,-0.09056,0.44423,0.55799,-0.28508,-0.01597, 1403636594663555328,0.03281,-0.29241,-0.11996,0.01639,0.88728,-0.09066,0.45193,0.50463,-0.04699,-0.01367, 1403636594763555584,0.07783,-0.28270,-0.12110,0.01040,0.88577,-0.08323,0.45649,0.42284,0.21243,-0.03246, 1403636594863555328,0.11384,-0.23325,-0.12318,-0.00362,0.88525,-0.06025,0.46118,0.32476,0.47793,-0.03677, 1403636594963555584,0.13853,-0.17429,-0.12664,-0.01871,0.88421,-0.03049,0.46572,0.16838,0.68734,-0.02763, 1403636595063555584,0.14685,-0.09948,-0.12921,-0.04245,0.88350,0.00488,0.46648,0.00450,0.79116,-0.03132, 1403636595163555328,0.14116,-0.02101,-0.13271,-0.06667,0.88402,0.03805,0.46111,-0.12211,0.76155,-0.03158, 1403636595263555584,0.12255,0.04960,-0.13515,-0.09068,0.88393,0.06540,0.45406,-0.23555,0.61849,-0.02903, 1403636595363555328,0.09439,0.10089,-0.13676,-0.10722,0.88332,0.08690,0.44799,-0.31234,0.38474,-0.01481, 1403636595463555584,0.06176,0.12731,-0.13754,-0.11544,0.88305,0.09746,0.44429,-0.33081,0.10386,-0.00994, 1403636595563555584,0.02777,0.12579,-0.13807,-0.11771,0.88447,0.10154,0.43995,-0.31892,-0.16939,-0.01660, 1403636595663555328,-0.00425,0.09766,-0.13981,-0.11315,0.88701,0.09985,0.43639,-0.29158,-0.40027,-0.03223, 1403636595763555584,-0.03153,0.04891,-0.14405,-0.09935,0.88982,0.08920,0.43634,-0.25290,-0.56772,-0.05189, 1403636595863555328,-0.05514,-0.01154,-0.14960,-0.07675,0.89245,0.06958,0.43910,-0.23192,-0.62305,-0.06147, 1403636595963555584,-0.07817,-0.07212,-0.15516,-0.05575,0.89547,0.04444,0.43938,-0.25214,-0.57696,-0.04308, 1403636596063555584,-0.10623,-0.12215,-0.15892,-0.03570,0.89905,0.01974,0.43593,-0.31119,-0.41437,-0.02581, 1403636596163555328,-0.13836,-0.14907,-0.16015,-0.02328,0.90118,-0.00088,0.43281,-0.33905,-0.14649,0.01762, 1403636596263555584,-0.17220,-0.14545,-0.15614,-0.02041,0.90066,-0.01229,0.43388,-0.36979,0.14056,0.07938, 1403636596363555328,-0.21138,-0.12113,-0.14539,-0.02067,0.89878,-0.01747,0.43756,-0.43060,0.30312,0.13165, 1403636596463555584,-0.25505,-0.08811,-0.13133,-0.01911,0.89628,-0.01497,0.44283,-0.42792,0.34467,0.12715, 1403636596563555584,-0.29315,-0.05483,-0.11992,-0.01818,0.89404,-0.00822,0.44754,-0.33924,0.31222,0.09396, 1403636596663555328,-0.32116,-0.02725,-0.11265,-0.01715,0.89458,-0.00573,0.44655,-0.20314,0.23203,0.04550, 1403636596763555584,-0.33260,-0.01011,-0.11030,-0.01701,0.89511,-0.00154,0.44552,-0.00101,0.10693,0.00212, 1403636596863555328,-0.32174,-0.00514,-0.11148,-0.01427,0.89257,0.00294,0.45068,0.23547,-0.00582,-0.01745, 1403636596963555584,-0.28725,-0.00949,-0.11471,-0.00964,0.89116,0.00639,0.45354,0.44816,-0.07730,-0.03251, 1403636597063555584,-0.23449,-0.01816,-0.11946,-0.00729,0.89467,0.00715,0.44661,0.58653,-0.10878,-0.05415, 1403636597163555328,-0.17164,-0.02825,-0.12327,-0.01221,0.89947,0.01292,0.43662,0.66796,-0.10274,-0.01583, 1403636597263555584,-0.10189,-0.03732,-0.12177,-0.01988,0.90076,0.02480,0.43315,0.70939,-0.07312,0.04366, 1403636597363555328,-0.03042,-0.04170,-0.11744,-0.02865,0.89858,0.03919,0.43612,0.70974,-0.01665,0.04586, 1403636597463555584,0.03663,-0.04001,-0.11460,-0.03659,0.89575,0.05610,0.43948,0.61233,0.04991,0.02286, 1403636597563555584,0.08910,-0.02970,-0.11447,-0.04893,0.89193,0.07461,0.44329,0.41936,0.14268,-0.00854, 1403636597663555328,0.12499,-0.01749,-0.11297,-0.06809,0.88611,0.09050,0.44943,0.15918,0.22296,-0.03031, 1403636597763555584,0.12760,0.00643,-0.11508,-0.08370,0.87993,0.10434,0.45588,-0.08903,0.24017,-0.02098, 1403636597863555328,0.10847,0.02767,-0.11633,-0.09089,0.87499,0.11561,0.46127,-0.27328,0.17322,-0.01735, 1403636597963555584,0.07316,0.04087,-0.11765,-0.09212,0.87295,0.12640,0.46206,-0.39496,0.09130,-0.02519, 1403636598063555584,0.02918,0.04652,-0.12085,-0.09115,0.87516,0.13134,0.45666,-0.45577,0.02105,-0.05687, 1403636598163555328,-0.01662,0.04488,-0.12749,-0.08947,0.88161,0.13092,0.44453,-0.43147,-0.05868,-0.08993, 1403636598263555584,-0.05501,0.03676,-0.13854,-0.08238,0.89050,0.13067,0.42796,-0.31238,-0.10177,-0.13634, 1403636598363555328,-0.07703,0.02535,-0.15214,-0.07379,0.89625,0.12917,0.41786,-0.11699,-0.12213,-0.14492, 1403636598463555584,-0.07724,0.01801,-0.16950,-0.06689,0.89751,0.12802,0.41666,0.09088,-0.11015,-0.15172, 1403636598463555584,-0.07943,0.01229,-0.16647,-0.06703,0.89747,0.12832,0.41664,0.09096,-0.11009,-0.15172, 1403636598563555584,-0.05796,0.00904,-0.18325,-0.06850,0.89387,0.12938,0.42375,0.27184,-0.07597,-0.13226, 1403636598663555328,-0.02589,0.00302,-0.19627,-0.07659,0.88633,0.13274,0.43697,0.36279,-0.05058,-0.13910, 1403636598763555584,0.01057,-0.00050,-0.21270,-0.08912,0.87555,0.13824,0.45426,0.36234,-0.02832,-0.18901, 1403636598863555328,0.04489,-0.00370,-0.23361,-0.10044,0.86344,0.14036,0.47401,0.32182,-0.03103,-0.22984, 1403636598963555584,0.07535,-0.00696,-0.25737,-0.10394,0.85361,0.13910,0.49111,0.26675,-0.02738,-0.23978, 1403636599063555584,0.09903,-0.00870,-0.27910,-0.10852,0.84501,0.13887,0.50487,0.19871,-0.00439,-0.18365, 1403636599163555328,0.11632,-0.00878,-0.29384,-0.12039,0.83266,0.13810,0.52260,0.15549,-0.00118,-0.10707, 1403636599263555584,0.13168,-0.00814,-0.30286,-0.12534,0.81537,0.13425,0.54904,0.14063,0.00806,-0.08170, 1403636599363555328,0.14429,-0.00742,-0.31123,-0.12829,0.80276,0.12895,0.56788,0.11734,0.00343,-0.09217, 1403636599463555584,0.15403,-0.00704,-0.32091,-0.12200,0.79873,0.12706,0.57533,0.08643,0.00203,-0.09467, 1403636599563555584,0.16073,-0.00675,-0.33028,-0.11690,0.80251,0.11999,0.57264,0.04537,-0.00300,-0.08892, 1403636599663555328,0.16393,-0.00738,-0.33941,-0.11326,0.80647,0.11714,0.56839,0.01502,-0.01273,-0.08602, 1403636599763555584,0.16446,-0.00933,-0.34275,-0.11516,0.80023,0.11649,0.57688,0.00150,-0.02237,0.00447, 1403636599863555328,0.16420,-0.01105,-0.34293,-0.11714,0.79197,0.11404,0.58826,-0.02030,-0.01295,-0.00920, 1403636599963555584,0.16379,-0.01239,-0.34381,-0.11882,0.79154,0.11128,0.58903,0.00374,-0.00949,-0.01245, 1403636600063555584,0.16388,-0.01390,-0.34536,-0.12106,0.79207,0.10856,0.58837,0.00759,-0.01925,-0.02878, 1403636600163555328,0.16363,-0.01529,-0.34536,-0.12227,0.78984,0.10785,0.59125,0.00293,0.00475,0.00234, 1403636600263555584,0.16354,-0.01486,-0.34545,-0.12369,0.79005,0.10703,0.59082,-0.00320,-0.00091,0.00274, 1403636600363555328,0.16326,-0.01475,-0.34543,-0.12431,0.79008,0.10630,0.59078,-0.00131,0.00038,0.00419, 1403636600463555584,0.16318,-0.01464,-0.34543,-0.12474,0.79012,0.10600,0.59069,-0.00084,-0.00171,0.00266, 1403636600563555584,0.16327,-0.01483,-0.34536,-0.12411,0.79007,0.10637,0.59082,0.00412,-0.00229,0.00420, 1403636600663555328,0.16324,-0.01479,-0.34537,-0.12431,0.79017,0.10599,0.59071,0.00388,-0.00761,0.00229, 1403636600763555584,0.16324,-0.01474,-0.34536,-0.12449,0.79022,0.10583,0.59065,0.00188,-0.00598,0.00291, 1403636600863555328,0.16320,-0.01477,-0.34535,-0.12434,0.79029,0.10579,0.59058,0.00242,-0.00209,0.00157, 1403636600963555584,0.16320,-0.01478,-0.34534,-0.12427,0.79033,0.10570,0.59056,0.00045,-0.00357,0.00296, 1403636601063555584,0.16320,-0.01480,-0.34533,-0.12417,0.79037,0.10568,0.59053,0.00089,-0.00034,0.00159, 1403636601163555328,0.16319,-0.01485,-0.34532,-0.12404,0.79041,0.10567,0.59051,-0.00044,-0.00086,0.00131, 1403636601263555584,0.16320,-0.01485,-0.34532,-0.12402,0.79045,0.10562,0.59047,-0.00017,-0.00125,0.00126, 1403636601363555328,0.16319,-0.01489,-0.34532,-0.12383,0.79046,0.10564,0.59049,0.00034,0.00135,0.00115, 1403636601463555584,0.16321,-0.01490,-0.34534,-0.12380,0.79049,0.10558,0.59047,0.00006,-0.00043,0.00143, 1403636601563555584,0.16320,-0.01496,-0.34535,-0.12368,0.79048,0.10549,0.59053,0.00097,0.00185,0.00118, 1403636601663555328,0.16322,-0.01498,-0.34536,-0.12355,0.79046,0.10554,0.59056,-0.00040,0.00033,0.00154, 1403636601763555584,0.16322,-0.01500,-0.34537,-0.12351,0.79049,0.10554,0.59054,-0.00010,0.00056,0.00009, 1403636601863555328,0.16323,-0.01501,-0.34538,-0.12350,0.79051,0.10551,0.59051,0.00032,-0.00137,0.00114, 1403636601963555584,0.16320,-0.01503,-0.34536,-0.12337,0.79056,0.10553,0.59048,-0.00079,0.00013,0.00122, 1403636602063555584,0.16318,-0.01508,-0.34536,-0.12325,0.79059,0.10557,0.59045,0.00115,0.00056,0.00041, 1403636602163555328,0.16318,-0.01508,-0.34535,-0.12316,0.79060,0.10557,0.59045,0.00113,0.00021,0.00133, 1403636602263555584,0.16317,-0.01509,-0.34534,-0.12304,0.79062,0.10558,0.59046,0.00102,0.00001,0.00145, 1403636602363555328,0.16318,-0.01510,-0.34534,-0.12309,0.79065,0.10551,0.59042,0.00197,-0.00066,0.00115, 1403636602463555584,0.16318,-0.01510,-0.34536,-0.12311,0.79064,0.10543,0.59044,0.00137,-0.00144,0.00125, 1403636602563555584,0.16316,-0.01510,-0.34533,-0.12306,0.79068,0.10540,0.59040,0.00166,-0.00128,0.00087, 1403636602663555328,0.16316,-0.01512,-0.34533,-0.12297,0.79070,0.10541,0.59039,0.00169,-0.00043,0.00137, 1403636602763555584,0.16315,-0.01512,-0.34533,-0.12293,0.79071,0.10535,0.59040,0.00074,-0.00128,0.00129, 1403636602863555328,0.16313,-0.01514,-0.34533,-0.12291,0.79072,0.10534,0.59039,0.00021,-0.00138,0.00104, 1403636602963555584,0.16313,-0.01515,-0.34533,-0.12286,0.79073,0.10532,0.59039,0.00105,-0.00076,0.00100, 1403636603063555584,0.16313,-0.01515,-0.34533,-0.12284,0.79074,0.10532,0.59038,-0.00041,-0.00082,0.00094, 1403636603063555584,0.16619,-0.01844,-0.34372,-0.12685,0.79001,0.11076,0.58951,-0.00109,-0.00065,0.00236, 1403636603163555328,0.16623,-0.01836,-0.34389,-0.12683,0.79002,0.11078,0.58950,-0.00080,0.00028,0.00068, 1403636603263555584,0.16626,-0.01839,-0.34390,-0.12684,0.79003,0.11079,0.58949,0.00039,-0.00089,0.00034, 1403636603363555328,0.16623,-0.01840,-0.34391,-0.12677,0.79002,0.11077,0.58951,0.00019,-0.00117,0.00087, 1403636603463555584,0.16623,-0.01840,-0.34389,-0.12670,0.79004,0.11078,0.58950,-0.00051,0.00013,0.00094, 1403636603563555584,0.16624,-0.01842,-0.34389,-0.12666,0.79008,0.11077,0.58946,-0.00022,0.00076,0.00095, 1403636603663555328,0.16623,-0.01843,-0.34388,-0.12660,0.79009,0.11075,0.58946,-0.00068,-0.00030,0.00092, 1403636603763555584,0.16619,-0.01845,-0.34388,-0.12659,0.79013,0.11070,0.58942,0.00060,0.00004,-0.00033, 1403636603863555328,0.16625,-0.01846,-0.34389,-0.12655,0.79009,0.11068,0.58949,0.00089,0.00033,0.00185, 1403636603963555584,0.16623,-0.01849,-0.34388,-0.12642,0.79011,0.11072,0.58948,-0.00255,-0.00020,0.00076, 1403636604063555584,0.16623,-0.01850,-0.34389,-0.12646,0.79014,0.11069,0.58944,-0.00125,-0.00118,-0.00046, 1403636604163555328,0.16621,-0.01851,-0.34389,-0.12641,0.79014,0.11070,0.58944,-0.00001,-0.00172,0.00101, 1403636604263555584,0.16621,-0.01851,-0.34388,-0.12642,0.79017,0.11064,0.58942,-0.00004,-0.00150,0.00077, 1403636604363555328,0.16620,-0.01851,-0.34386,-0.12635,0.79020,0.11066,0.58939,0.00040,-0.00102,0.00118, 1403636604463555584,0.16620,-0.01853,-0.34387,-0.12632,0.79021,0.11065,0.58938,0.00044,-0.00124,-0.00006, 1403636604563555584,0.16621,-0.01852,-0.34387,-0.12631,0.79022,0.11060,0.58938,0.00085,-0.00140,0.00097, 1403636604663555328,0.16620,-0.01853,-0.34387,-0.12629,0.79024,0.11061,0.58936,-0.00028,-0.00094,0.00033, 1403636604763555584,0.16620,-0.01854,-0.34386,-0.12624,0.79025,0.11061,0.58935,-0.00022,-0.00092,0.00140, 1403636604863555328,0.16619,-0.01854,-0.34385,-0.12619,0.79027,0.11061,0.58933,-0.00004,0.00012,0.00118, 1403636604963555584,0.16619,-0.01856,-0.34384,-0.12618,0.79029,0.11060,0.58931,-0.00087,-0.00132,0.00092, 1403636605063555584,0.16618,-0.01857,-0.34384,-0.12615,0.79032,0.11056,0.58928,0.00128,-0.00176,0.00056, 1403636605163555328,0.16619,-0.01857,-0.34385,-0.12612,0.79032,0.11053,0.58930,0.00115,-0.00019,0.00161, 1403636605263555584,0.16618,-0.01857,-0.34384,-0.12610,0.79033,0.11052,0.58929,-0.00046,-0.00114,0.00110, 1403636605363555328,0.16618,-0.01858,-0.34384,-0.12609,0.79034,0.11050,0.58928,-0.00071,0.00009,0.00139, 1403636605463555584,0.16618,-0.01859,-0.34383,-0.12605,0.79037,0.11050,0.58925,-0.00033,-0.00027,0.00103, 1403636605563555584,0.16616,-0.01860,-0.34382,-0.12600,0.79039,0.11048,0.58924,-0.00090,0.00011,0.00121, 1403636605663555328,0.16617,-0.01862,-0.34383,-0.12598,0.79039,0.11050,0.58923,0.00003,-0.00051,0.00049, 1403636605763555584,0.16612,-0.01861,-0.34380,-0.12590,0.79042,0.11048,0.58921,-0.00087,-0.00076,0.00157, 1403636605863555328,0.16611,-0.01862,-0.34379,-0.12586,0.79042,0.11048,0.58924,-0.00031,0.00049,0.00098, 1403636605963555584,0.16612,-0.01863,-0.34380,-0.12585,0.79044,0.11048,0.58921,-0.00023,0.00002,0.00030, 1403636606063555584,0.16612,-0.01864,-0.34380,-0.12581,0.79044,0.11048,0.58922,-0.00044,-0.00002,0.00137, 1403636606163555328,0.16611,-0.01865,-0.34379,-0.12576,0.79045,0.11046,0.58921,-0.00088,-0.00017,0.00059, 1403636606263555584,0.16612,-0.01865,-0.34379,-0.12572,0.79045,0.11047,0.58922,0.00076,-0.00047,0.00118, 1403636606363555328,0.16611,-0.01866,-0.34379,-0.12570,0.79047,0.11043,0.58921,-0.00100,0.00049,0.00087, 1403636606463555584,0.16611,-0.01867,-0.34379,-0.12570,0.79048,0.11043,0.58919,0.00035,-0.00035,0.00179, 1403636606563555584,0.16611,-0.01868,-0.34379,-0.12568,0.79050,0.11040,0.58918,0.00031,-0.00104,0.00045, 1403636606663555328,0.16611,-0.01868,-0.34379,-0.12563,0.79049,0.11043,0.58920,-0.00013,0.00046,0.00076, 1403636606763555584,0.16611,-0.01869,-0.34378,-0.12560,0.79051,0.11041,0.58918,-0.00090,0.00005,0.00060, 1403636606863555328,0.16611,-0.01871,-0.34378,-0.12561,0.79052,0.11041,0.58915,-0.00013,-0.00082,0.00076, 1403636606963555584,0.16609,-0.01871,-0.34376,-0.12554,0.79055,0.11040,0.58914,-0.00016,0.00016,0.00138, 1403636607063555584,0.16610,-0.01872,-0.34377,-0.12552,0.79056,0.11039,0.58913,-0.00101,-0.00052,0.00123, 1403636607163555328,0.16609,-0.01873,-0.34376,-0.12548,0.79057,0.11037,0.58912,-0.00044,-0.00085,0.00143, 1403636607263555584,0.16608,-0.01874,-0.34375,-0.12542,0.79060,0.11036,0.58911,-0.00046,0.00001,0.00074, 1403636607363555328,0.16608,-0.01875,-0.34374,-0.12538,0.79062,0.11036,0.58909,-0.00103,-0.00026,0.00104, 1403636607463555584,0.16608,-0.01877,-0.34375,-0.12537,0.79062,0.11033,0.58909,0.00008,-0.00083,0.00115, 1403636607563555584,0.16608,-0.01877,-0.34374,-0.12531,0.79063,0.11033,0.58909,-0.00076,-0.00106,0.00082, 1403636607663555328,0.16606,-0.01877,-0.34373,-0.12524,0.79066,0.11031,0.58908,-0.00114,-0.00005,0.00119, 1403636607763555584,0.16606,-0.01878,-0.34372,-0.12525,0.79068,0.11029,0.58905,0.00010,-0.00087,0.00064, 1403636607863555328,0.16607,-0.01879,-0.34373,-0.12525,0.79067,0.11027,0.58906,-0.00032,-0.00033,0.00091, 1403636607963555584,0.16606,-0.01879,-0.34373,-0.12518,0.79068,0.11027,0.58906,-0.00060,-0.00064,0.00093, 1403636608063555584,0.16606,-0.01880,-0.34372,-0.12518,0.79070,0.11026,0.58904,-0.00012,-0.00139,0.00076, 1403636608163555328,0.16606,-0.01881,-0.34373,-0.12519,0.79070,0.11023,0.58904,-0.00019,-0.00054,0.00062, 1403636608263555584,0.16607,-0.01881,-0.34373,-0.12520,0.79071,0.11023,0.58903,0.00004,-0.00100,0.00066, 1403636608363555328,0.16607,-0.01882,-0.34373,-0.12516,0.79072,0.11024,0.58902,0.00011,-0.00054,0.00064, 1403636608463555584,0.16607,-0.01882,-0.34373,-0.12516,0.79072,0.11022,0.58902,-0.00039,-0.00038,0.00081, 1403636608563555584,0.16607,-0.01882,-0.34373,-0.12513,0.79072,0.11021,0.58904,-0.00003,-0.00140,0.00119, 1403636608663555328,0.16606,-0.01883,-0.34373,-0.12507,0.79073,0.11021,0.58902,-0.00078,-0.00050,0.00102, 1403636608763555584,0.16606,-0.01883,-0.34372,-0.12505,0.79075,0.11021,0.58901,-0.00021,-0.00048,0.00056, 1403636608863555328,0.16606,-0.01884,-0.34371,-0.12504,0.79076,0.11020,0.58899,-0.00097,-0.00081,0.00066, 1403636608963555584,0.16606,-0.01885,-0.34372,-0.12505,0.79076,0.11017,0.58900,-0.00062,-0.00129,0.00082, 1403636609063555584,0.16606,-0.01885,-0.34372,-0.12502,0.79077,0.11020,0.58899,-0.00023,0.00057,0.00122, 1403636609163555328,0.16606,-0.01885,-0.34373,-0.12504,0.79076,0.11018,0.58901,-0.00072,-0.00040,0.00103, 1403636609263555584,0.16607,-0.01886,-0.34373,-0.12503,0.79076,0.11017,0.58900,-0.00033,-0.00032,0.00058, 1403636609363555328,0.16605,-0.01886,-0.34372,-0.12500,0.79080,0.11018,0.58896,-0.00017,0.00041,0.00079, 1403636609463555584,0.16605,-0.01886,-0.34371,-0.12497,0.79079,0.11017,0.58898,-0.00105,-0.00006,0.00100, 1403636609563555584,0.16605,-0.01887,-0.34372,-0.12497,0.79079,0.11015,0.58899,0.00022,-0.00077,0.00089, 1403636609663555328,0.16603,-0.01887,-0.34373,-0.12499,0.79083,0.11012,0.58894,0.00015,0.00041,-0.00026, 1403636609763555584,0.16606,-0.01889,-0.34373,-0.12500,0.79079,0.11009,0.58899,0.00034,0.00036,0.00112, 1403636609863555328,0.16606,-0.01890,-0.34371,-0.12493,0.79081,0.11012,0.58898,-0.00295,-0.00084,-0.00116, 1403636609963555584,0.16604,-0.01890,-0.34372,-0.12495,0.79084,0.11009,0.58893,-0.00140,-0.00113,0.00042, 1403636610063555584,0.16634,-0.01815,-0.34402,-0.12780,0.78946,0.11089,0.59002,-0.00137,-0.00217,0.00029, 1403636610163555328,0.16634,-0.01797,-0.34406,-0.12848,0.78933,0.11119,0.58998,-0.00097,-0.00413,0.00034, 1403636610263555584,0.16633,-0.01792,-0.34406,-0.12867,0.78934,0.11134,0.58991,-0.00143,-0.00351,0.00007, 1403636610363555328,0.16631,-0.01787,-0.34408,-0.12887,0.78931,0.11133,0.58990,0.00044,-0.00308,-0.00074, 1403636610463555584,0.16634,-0.01785,-0.34407,-0.12892,0.78930,0.11135,0.58991,0.00025,-0.00330,0.00185, 1403636610563555584,0.16633,-0.01784,-0.34408,-0.12894,0.78930,0.11137,0.58990,-0.00093,-0.00226,0.00028, 1403636610663555328,0.16632,-0.01784,-0.34407,-0.12894,0.78930,0.11139,0.58989,-0.00165,-0.00309,0.00070, 1403636610763555584,0.16633,-0.01782,-0.34405,-0.12889,0.78938,0.11146,0.58978,-0.00237,-0.00393,-0.00233, 1403636610863555328,0.16629,-0.01783,-0.34407,-0.12900,0.78940,0.11152,0.58971,-0.00232,-0.00365,-0.00039, 1403636610963555584,0.16628,-0.01784,-0.34404,-0.12895,0.78943,0.11157,0.58968,-0.00013,0.00045,0.00205, 1403636611063555584,0.16629,-0.01784,-0.34404,-0.12885,0.78941,0.11161,0.58972,-0.00001,-0.00181,0.00072, 1403636611163555328,0.16635,-0.01785,-0.34405,-0.12888,0.78940,0.11157,0.58974,0.00087,-0.00284,-0.00044, 1403636611263555584,0.16636,-0.01783,-0.34404,-0.12892,0.78941,0.11150,0.58973,0.00133,-0.00013,0.00062, 1403636611363555328,0.16624,-0.01755,-0.34407,-0.12963,0.78950,0.11133,0.58949,-0.01074,0.01855,-0.00108, 1403636611463555584,0.16638,-0.01805,-0.34436,-0.12972,0.78956,0.11094,0.58946,0.00006,-0.00252,-0.01418, 1403636611563555584,0.16647,-0.01831,-0.34437,-0.12902,0.78950,0.11119,0.58964,-0.00235,0.00498,-0.00579, 1403636611663555328,0.16644,-0.01822,-0.34432,-0.12924,0.78946,0.11106,0.58967,-0.00506,0.00134,-0.00397, 1403636611763555584,0.16648,-0.01822,-0.34430,-0.12921,0.78937,0.11113,0.58979,-0.00572,0.01146,-0.00130, 1403636611863555328,0.16655,-0.01847,-0.34428,-0.12864,0.78920,0.11120,0.59013,-0.00141,-0.00732,0.00143, 1403636611963555584,0.16642,-0.01819,-0.34429,-0.12934,0.78938,0.11115,0.58975,-0.01268,0.01012,-0.00739, 1403636612063555584,0.16653,-0.01839,-0.34431,-0.12877,0.78938,0.11146,0.58981,-0.00715,0.00898,-0.00347, 1403636612163555328,0.16656,-0.01797,-0.34425,-0.12992,0.78925,0.11094,0.58983,-0.00364,0.00228,0.00042, 1403636612263555584,0.16673,-0.01878,-0.34426,-0.12739,0.78905,0.11249,0.59035,-0.00673,0.00351,0.00067, 1403636612363555328,0.16664,-0.01871,-0.34425,-0.12783,0.78920,0.11230,0.59009,-0.00426,-0.00819,-0.00185, 1403636612463555584,0.16660,-0.01866,-0.34431,-0.12805,0.78915,0.11204,0.59016,0.00388,-0.00716,-0.00060, 1403636612563555584,0.16659,-0.01857,-0.34429,-0.12840,0.78918,0.11178,0.59010,0.00157,-0.01287,0.00458, 1403636612663555328,0.16657,-0.01855,-0.34428,-0.12850,0.78923,0.11173,0.59001,0.00298,-0.00662,0.00343, 1403636612763555584,0.16656,-0.01854,-0.34430,-0.12853,0.78925,0.11162,0.59001,0.00344,-0.00973,0.00218, 1403636612863555328,0.16652,-0.01844,-0.34426,-0.12889,0.78924,0.11140,0.58998,0.00343,-0.01298,0.00315, 1403636612963555584,0.16649,-0.01843,-0.34427,-0.12904,0.78924,0.11124,0.58998,0.00254,-0.00783,0.00163, 1403636613063555584,0.16648,-0.01838,-0.34432,-0.12928,0.78919,0.11109,0.59002,0.00157,-0.00630,0.00056, 1403636613163555328,0.16649,-0.01834,-0.34430,-0.12933,0.78917,0.11100,0.59005,-0.00137,-0.00549,0.00117, 1403636613263555584,0.16647,-0.01839,-0.34434,-0.12923,0.78923,0.11110,0.58998,-0.00062,-0.00374,0.00083, 1403636613363555328,0.16650,-0.01837,-0.34431,-0.12925,0.78922,0.11109,0.58998,0.00017,-0.00403,0.00031, 1403636613463555584,0.16648,-0.01839,-0.34432,-0.12916,0.78919,0.11111,0.59004,-0.00267,-0.00293,-0.00054, 1403636613563555584,0.16649,-0.01840,-0.34431,-0.12913,0.78920,0.11111,0.59004,-0.00228,-0.00241,0.00049, 1403636613663555328,0.16649,-0.01841,-0.34432,-0.12909,0.78920,0.11113,0.59004,-0.00150,-0.00042,0.00024, 1403636613763555584,0.16650,-0.01844,-0.34432,-0.12894,0.78915,0.11119,0.59013,-0.00192,-0.00077,-0.00099, 1403636613863555328,0.16649,-0.01844,-0.34431,-0.12895,0.78918,0.11121,0.59009,-0.00188,-0.00141,0.00082, 1403636613963555584,0.16650,-0.01845,-0.34431,-0.12891,0.78918,0.11122,0.59009,-0.00142,-0.00115,0.00017, 1403636614063555584,0.16650,-0.01845,-0.34431,-0.12888,0.78915,0.11126,0.59013,-0.00185,-0.00161,0.00034, 1403636614163555328,0.16649,-0.01845,-0.34429,-0.12886,0.78916,0.11129,0.59011,-0.00287,-0.00155,0.00092, 1403636614263555584,0.16650,-0.01846,-0.34430,-0.12879,0.78919,0.11130,0.59009,-0.00200,-0.00187,0.00034, 1403636614363555328,0.16652,-0.01847,-0.34427,-0.12877,0.78915,0.11130,0.59015,-0.00074,-0.00155,0.00072, 1403636614463555584,0.16650,-0.01848,-0.34429,-0.12874,0.78917,0.11135,0.59012,-0.00293,-0.00345,-0.00159, 1403636614563555584,0.16650,-0.01847,-0.34430,-0.12880,0.78919,0.11133,0.59008,-0.00067,-0.00417,0.00173, 1403636614663555328,0.16649,-0.01847,-0.34429,-0.12881,0.78919,0.11132,0.59009,-0.00044,-0.00194,0.00138, 1403636614763555584,0.16649,-0.01846,-0.34429,-0.12883,0.78919,0.11131,0.59008,-0.00136,-0.00465,0.00131, 1403636614863555328,0.16649,-0.01845,-0.34429,-0.12883,0.78921,0.11130,0.59006,0.00083,-0.00289,0.00055, 1403636614963555584,0.16648,-0.01844,-0.34429,-0.12883,0.78918,0.11128,0.59009,-0.00065,-0.00261,0.00051, 1403636615063555584,0.16649,-0.01844,-0.34428,-0.12890,0.78920,0.11127,0.59006,-0.00020,-0.00391,0.00080, 1403636615163555328,0.16649,-0.01844,-0.34430,-0.12888,0.78921,0.11129,0.59005,-0.00027,-0.00294,0.00043, 1403636615263555584,0.16650,-0.01843,-0.34429,-0.12889,0.78917,0.11124,0.59010,-0.00020,-0.00314,0.00034, 1403636615363555328,0.16649,-0.01842,-0.34429,-0.12890,0.78920,0.11124,0.59007,-0.00187,-0.00366,0.00092, 1403636615463555584,0.16650,-0.01844,-0.34429,-0.12887,0.78919,0.11126,0.59008,-0.00046,-0.00291,-0.00040, 1403636615563555584,0.16650,-0.01843,-0.34429,-0.12889,0.78919,0.11125,0.59007,-0.00135,-0.00298,0.00081, 1403636615663555328,0.16649,-0.01843,-0.34428,-0.12890,0.78920,0.11125,0.59006,-0.00210,-0.00321,0.00058, 1403636615763555584,0.16649,-0.01844,-0.34429,-0.12889,0.78920,0.11125,0.59006,0.00001,-0.00456,0.00083, 1403636615863555328,0.16648,-0.01842,-0.34429,-0.12891,0.78920,0.11123,0.59006,-0.00081,-0.00222,0.00071, 1403636615963555584,0.16649,-0.01843,-0.34428,-0.12888,0.78919,0.11127,0.59007,-0.00092,-0.00258,0.00036, 1403636616063555584,0.16649,-0.01845,-0.34430,-0.12889,0.78921,0.11125,0.59005,-0.00065,-0.00322,0.00075, 1403636616163555328,0.16650,-0.01844,-0.34429,-0.12889,0.78919,0.11126,0.59007,-0.00070,-0.00254,-0.00007, 1403636616263555584,0.16649,-0.01843,-0.34429,-0.12884,0.78920,0.11128,0.59007,-0.00135,-0.00302,0.00067, 1403636616363555328,0.16650,-0.01844,-0.34429,-0.12890,0.78919,0.11124,0.59007,-0.00086,-0.00260,0.00039, 1403636616463555584,0.16649,-0.01844,-0.34429,-0.12888,0.78920,0.11126,0.59006,-0.00082,-0.00182,0.00104, 1403636616563555584,0.16648,-0.01843,-0.34428,-0.12886,0.78920,0.11125,0.59008,-0.00123,-0.00228,0.00162, 1403636616663555328,0.16649,-0.01844,-0.34429,-0.12889,0.78919,0.11127,0.59007,-0.00093,-0.00282,0.00005, 1403636616763555584,0.16649,-0.01845,-0.34429,-0.12889,0.78919,0.11125,0.59007,-0.00076,-0.00225,0.00093, 1403636616863555328,0.16649,-0.01844,-0.34429,-0.12887,0.78919,0.11127,0.59008,-0.00125,-0.00341,0.00003, 1403636616963555584,0.16649,-0.01844,-0.34430,-0.12888,0.78920,0.11125,0.59007,-0.00054,-0.00284,-0.00004, 1403636617063555584,0.16649,-0.01844,-0.34429,-0.12886,0.78918,0.11128,0.59009,-0.00082,-0.00302,0.00009, 1403636617163555328,0.16648,-0.01845,-0.34428,-0.12886,0.78920,0.11128,0.59006,-0.00148,-0.00262,0.00044, 1403636617263555584,0.16648,-0.01845,-0.34429,-0.12886,0.78919,0.11127,0.59008,-0.00104,-0.00380,0.00080, 1403636617363555328,0.16649,-0.01845,-0.34429,-0.12887,0.78919,0.11126,0.59007,-0.00055,-0.00284,0.00138, 1403636617463555584,0.16648,-0.01844,-0.34429,-0.12887,0.78919,0.11126,0.59008,-0.00133,-0.00323,0.00112, 1403636617563555584,0.16648,-0.01844,-0.34428,-0.12889,0.78919,0.11124,0.59007,-0.00081,-0.00276,0.00129, 1403636617663555328,0.16649,-0.01844,-0.34429,-0.12890,0.78919,0.11125,0.59007,-0.00014,-0.00250,0.00001, 1403636617763555584,0.16649,-0.01844,-0.34429,-0.12891,0.78918,0.11123,0.59008,-0.00104,-0.00334,-0.00016, 1403636617863555328,0.16648,-0.01843,-0.34428,-0.12887,0.78919,0.11126,0.59007,-0.00056,-0.00255,0.00080, 1403636617963555584,0.16649,-0.01844,-0.34429,-0.12888,0.78920,0.11126,0.59006,-0.00085,-0.00241,0.00055, 1403636618063555584,0.16649,-0.01843,-0.34428,-0.12888,0.78919,0.11125,0.59008,-0.00059,-0.00367,0.00078, 1403636618163555328,0.16650,-0.01844,-0.34429,-0.12889,0.78918,0.11126,0.59009,-0.00039,-0.00333,0.00080, 1403636618263555584,0.16649,-0.01844,-0.34429,-0.12888,0.78920,0.11127,0.59007,-0.00077,-0.00322,0.00109, 1403636618363555328,0.16649,-0.01844,-0.34429,-0.12890,0.78919,0.11124,0.59007,-0.00146,-0.00319,0.00024, 1403636618463555584,0.16650,-0.01844,-0.34429,-0.12887,0.78918,0.11127,0.59008,-0.00124,-0.00278,0.00046, 1403636618563555584,0.16652,-0.01839,-0.34434,-0.12896,0.78913,0.11146,0.59010,0.00043,-0.00736,-0.00208, 1403636618663555328,0.16653,-0.01840,-0.34436,-0.12889,0.78914,0.11137,0.59013,-0.00027,-0.00864,-0.00131, 1403636618763555584,0.16652,-0.01844,-0.34436,-0.12885,0.78915,0.11124,0.59015,-0.00222,0.00131,-0.00077, 1403636618863555328,0.16652,-0.01842,-0.34435,-0.12892,0.78915,0.11126,0.59012,-0.00220,0.00045,0.00094, 1403636618963555584,0.16650,-0.01836,-0.34431,-0.12909,0.78911,0.11146,0.59010,-0.00248,-0.00189,0.00034, 1403636619063555584,0.16651,-0.01838,-0.34433,-0.12904,0.78917,0.11139,0.59005,-0.00119,-0.00376,-0.00044, 1403636619163555328,0.16651,-0.01843,-0.34433,-0.12886,0.78920,0.11116,0.59008,0.00061,-0.01186,-0.00190, 1403636619263555584,0.16652,-0.01845,-0.34431,-0.12883,0.78918,0.11112,0.59012,-0.00061,-0.00620,0.00032, 1403636619363555328,0.16653,-0.01838,-0.34431,-0.12907,0.78915,0.11148,0.59005,-0.00329,0.00892,0.00111, 1403636619463555584,0.16651,-0.01836,-0.34431,-0.12902,0.78914,0.11144,0.59007,-0.00130,0.00056,-0.00047, 1403636619563555584,0.16654,-0.01842,-0.34433,-0.12893,0.78920,0.11128,0.59005,-0.00226,-0.00754,-0.00156, 1403636619663555328,0.16657,-0.01843,-0.34434,-0.12894,0.78912,0.11132,0.59014,-0.00110,-0.00624,-0.00171, 1403636619763555584,0.16656,-0.01845,-0.34434,-0.12887,0.78917,0.11124,0.59011,0.00085,-0.00235,-0.00046, 1403636619863555328,0.16657,-0.01845,-0.34433,-0.12889,0.78918,0.11124,0.59009,-0.00300,-0.00095,-0.00040, 1403636619963555584,0.16656,-0.01845,-0.34433,-0.12888,0.78916,0.11133,0.59011,-0.00318,0.00245,-0.00125, 1403636620063555584,0.16656,-0.01846,-0.34433,-0.12877,0.78918,0.11133,0.59010,-0.00350,0.00048,-0.00011, 1403636620163555328,0.16657,-0.01846,-0.34433,-0.12876,0.78915,0.11143,0.59012,-0.00204,-0.00058,0.00119, 1403636620263555584,0.16656,-0.01846,-0.34433,-0.12879,0.78918,0.11141,0.59009,-0.00138,-0.00240,-0.00024, 1403636620363555328,0.16656,-0.01846,-0.34433,-0.12876,0.78916,0.11139,0.59012,-0.00106,0.00015,-0.00037, 1403636620463555584,0.16656,-0.01848,-0.34432,-0.12875,0.78915,0.11140,0.59013,-0.00147,-0.00329,0.00068, 1403636620563555584,0.16655,-0.01847,-0.34432,-0.12878,0.78916,0.11133,0.59012,-0.00150,-0.00249,-0.00121, 1403636620663555328,0.16656,-0.01848,-0.34433,-0.12874,0.78917,0.11138,0.59011,-0.00066,-0.00294,0.00102, 1403636620763555584,0.16656,-0.01847,-0.34432,-0.12877,0.78917,0.11135,0.59012,-0.00063,-0.00186,0.00089, 1403636620863555328,0.16657,-0.01847,-0.34433,-0.12881,0.78915,0.11134,0.59013,-0.00152,-0.00257,-0.00038, 1403636620963555584,0.16656,-0.01847,-0.34434,-0.12879,0.78917,0.11131,0.59011,-0.00048,-0.00187,-0.00066, 1403636621063555584,0.16664,-0.01858,-0.34428,-0.12831,0.78910,0.11159,0.59025,-0.00284,0.00630,0.00268, 1403636621163555328,0.16662,-0.01856,-0.34431,-0.12849,0.78911,0.11159,0.59020,-0.00013,0.00273,-0.00141, 1403636621263555584,0.16661,-0.01854,-0.34430,-0.12850,0.78914,0.11152,0.59018,0.00154,0.00019,0.00072, 1403636621363555328,0.16662,-0.01854,-0.34430,-0.12857,0.78912,0.11147,0.59019,-0.00201,-0.00303,-0.00156, 1403636621463555584,0.16660,-0.01852,-0.34430,-0.12858,0.78914,0.11145,0.59017,-0.00092,-0.00431,-0.00092, 1403636621563555584,0.16658,-0.01853,-0.34429,-0.12856,0.78916,0.11147,0.59015,-0.00094,-0.00321,0.00093, 1403636621663555328,0.16659,-0.01852,-0.34430,-0.12858,0.78914,0.11144,0.59017,0.00055,-0.00381,-0.00051, 1403636621763555584,0.16659,-0.01852,-0.34431,-0.12867,0.78916,0.11143,0.59013,-0.00103,-0.00373,0.00065, 1403636621863555328,0.16658,-0.01851,-0.34430,-0.12866,0.78917,0.11142,0.59012,-0.00149,-0.00335,-0.00032, 1403636621963555584,0.16659,-0.01849,-0.34431,-0.12874,0.78913,0.11141,0.59016,-0.00120,-0.00142,-0.00131, 1403636622063555584,0.16661,-0.01849,-0.34430,-0.12871,0.78912,0.11143,0.59017,-0.00091,-0.00360,-0.00076, 1403636622163555328,0.16661,-0.01850,-0.34429,-0.12869,0.78915,0.11145,0.59014,-0.00025,-0.00269,-0.00002, 1403636622263555584,0.16660,-0.01850,-0.34429,-0.12868,0.78914,0.11145,0.59015,-0.00078,-0.00335,-0.00038, 1403636622363555328,0.16660,-0.01851,-0.34430,-0.12861,0.78915,0.11144,0.59015,-0.00252,-0.00293,0.00021, 1403636622463555584,0.16660,-0.01851,-0.34430,-0.12866,0.78915,0.11143,0.59014,0.00094,-0.00368,-0.00174, 1403636622563555584,0.16659,-0.01851,-0.34431,-0.12866,0.78916,0.11143,0.59013,-0.00004,-0.00309,0.00111, 1403636622663555328,0.16660,-0.01852,-0.34429,-0.12867,0.78915,0.11144,0.59014,-0.00018,-0.00404,-0.00073, 1403636622763555584,0.16662,-0.01853,-0.34428,-0.12861,0.78914,0.11141,0.59018,-0.00090,-0.00654,0.00098, 1403636622863555328,0.16664,-0.01856,-0.34426,-0.12860,0.78912,0.11128,0.59022,0.00153,-0.00640,-0.00148, 1403636622963555584,0.16666,-0.01860,-0.34420,-0.12870,0.78906,0.11109,0.59032,-0.00066,-0.00318,-0.00436, 1403636623063555584,0.16668,-0.01865,-0.34417,-0.12858,0.78898,0.11113,0.59044,-0.00072,-0.00414,0.00239, 1403636623163555328,0.16670,-0.01868,-0.34413,-0.12857,0.78897,0.11105,0.59047,-0.00290,-0.00203,-0.00336, 1403636623263555584,0.16669,-0.01867,-0.34415,-0.12865,0.78900,0.11100,0.59043,-0.00013,-0.00322,0.00032, 1403636623363555328,0.16673,-0.01871,-0.34416,-0.12864,0.78898,0.11089,0.59048,-0.00404,-0.00385,-0.00246, 1403636623463555584,0.16675,-0.01873,-0.34414,-0.12852,0.78895,0.11095,0.59054,-0.01029,-0.00661,0.00027, 1403636623563555584,0.16678,-0.01874,-0.34412,-0.12851,0.78904,0.11085,0.59043,-0.00170,-0.00272,-0.00392, 1403636623663555328,0.16679,-0.01875,-0.34412,-0.12841,0.78915,0.11080,0.59032,0.00984,0.00043,0.00330, 1403636623763555584,0.16682,-0.01883,-0.34409,-0.12823,0.78915,0.11051,0.59041,0.00159,-0.00138,-0.00348, 1403636623863555328,0.16679,-0.01891,-0.34406,-0.12802,0.78934,0.11007,0.59028,-0.00092,-0.00293,-0.00521, 1403636623963555584,0.16680,-0.01895,-0.34404,-0.12751,0.78967,0.10950,0.59006,-0.00463,-0.00276,0.00145, 1403636624063555584,0.16681,-0.01903,-0.34402,-0.12738,0.78962,0.10949,0.59016,0.00225,0.00077,0.00143, 1403636624163555328,0.16668,-0.01902,-0.34396,-0.12700,0.79007,0.10873,0.58977,-0.00441,-0.00803,0.00104, 1403636624263555584,0.16660,-0.01920,-0.34389,-0.12696,0.79037,0.10885,0.58937,0.01162,-0.00114,0.00591, 1403636624363555328,0.16560,-0.02101,-0.34412,-0.12379,0.79490,0.10907,0.58389,-0.02577,-0.04051,-0.00510, 1403636624463555584,0.16297,-0.02618,-0.34431,-0.12054,0.80022,0.10783,0.57748,-0.03899,-0.06216,-0.01601, 1403636624563555584,0.15915,-0.03391,-0.34421,-0.11683,0.80532,0.10644,0.57139,-0.03506,-0.08328,0.01085, 1403636624663555328,0.15548,-0.04408,-0.34368,-0.11208,0.80472,0.10765,0.57295,-0.03227,-0.11556,-0.01194, 1403636624763555584,0.15018,-0.05747,-0.34455,-0.10607,0.80282,0.11147,0.57602,-0.05936,-0.16018,-0.00382, 1403636624863555328,0.14148,-0.07454,-0.34250,-0.09385,0.80616,0.12093,0.57156,-0.09230,-0.17451,0.04803, 1403636624963555584,0.12964,-0.09309,-0.33278,-0.07969,0.81252,0.13194,0.56218,-0.12714,-0.17678,0.13790, 1403636625063555584,0.11685,-0.10884,-0.31424,-0.07188,0.81868,0.13820,0.55272,-0.12154,-0.12788,0.22121, 1403636625163555328,0.10719,-0.11827,-0.28930,-0.07508,0.82317,0.13705,0.54586,-0.08622,-0.06774,0.27786, 1403636625263555584,0.10271,-0.12230,-0.26030,-0.08258,0.82066,0.13279,0.54960,-0.02879,-0.02640,0.26770, 1403636625363555328,0.10123,-0.12403,-0.24190,-0.08795,0.81732,0.13097,0.55416,-0.00247,-0.00849,0.06222, 1403636625463555584,0.10116,-0.12450,-0.24330,-0.08840,0.81967,0.13227,0.55031,0.01822,0.00519,-0.07692, 1403636625563555584,0.10369,-0.12305,-0.25200,-0.08889,0.82103,0.13335,0.54792,0.05126,0.02997,-0.06478, 1403636625663555328,0.11020,-0.11918,-0.25117,-0.08852,0.81993,0.13515,0.54919,0.08057,0.05951,0.07939, 1403636625763555584,0.12138,-0.11168,-0.23292,-0.09049,0.81521,0.13515,0.55586,0.12264,0.08331,0.26662, 1403636625863555328,0.13505,-0.10224,-0.19805,-0.09403,0.80891,0.13372,0.56475,0.14279,0.10497,0.40907, 1403636625963555584,0.14934,-0.09147,-0.15140,-0.09697,0.80525,0.13240,0.56977,0.13291,0.10910,0.49509, 1403636626063555584,0.16198,-0.08093,-0.10318,-0.10191,0.80343,0.12955,0.57214,0.11112,0.10434,0.46302, 1403636626163555328,0.17355,-0.07162,-0.05833,-0.11081,0.80323,0.12378,0.57203,0.10497,0.08738,0.42731, 1403636626263555584,0.19213,-0.05668,-0.02915,-0.11572,0.80336,0.11800,0.57210,0.09292,0.04708,0.30122, 1403636626363555328,0.20203,-0.05522,-0.00665,-0.11675,0.80319,0.11765,0.57220,0.08904,0.00180,0.15255, 1403636626463555584,0.21232,-0.05605,0.00185,-0.11301,0.80193,0.12060,0.57410,0.09130,-0.03075,0.07086, 1403636626563555584,0.22136,-0.06118,0.00593,-0.10821,0.79727,0.12507,0.58053,0.07418,-0.06254,0.03176, 1403636626663555328,0.22723,-0.06892,0.00709,-0.10470,0.79297,0.12994,0.58597,0.03428,-0.08609,-0.01476, 1403636626763555584,0.22884,-0.07847,0.00292,-0.10456,0.79419,0.13278,0.58370,-0.00848,-0.09840,-0.05978, 1403636626863555328,0.22638,-0.08900,-0.00555,-0.10443,0.79776,0.13492,0.57834,-0.04141,-0.10702,-0.10221, 1403636626963555584,0.22117,-0.10045,-0.01707,-0.10285,0.80275,0.13694,0.57120,-0.05818,-0.11439,-0.12406, 1403636627063555584,0.21477,-0.11202,-0.02975,-0.10123,0.80749,0.13820,0.56446,-0.06522,-0.11700,-0.11405, 1403636627163555328,0.20926,-0.12260,-0.03465,-0.10128,0.80957,0.13877,0.56132,-0.04661,-0.10196,0.03076, 1403636627263555584,0.20526,-0.13251,-0.02739,-0.10174,0.80569,0.13879,0.56678,-0.03907,-0.09393,0.14107, 1403636627363555328,0.20140,-0.14196,-0.01115,-0.10439,0.80308,0.13750,0.57032,-0.05226,-0.10058,0.20638, 1403636627463555584,0.19667,-0.15183,0.01341,-0.10633,0.80247,0.13564,0.57126,-0.02940,-0.10251,0.28090, 1403636627563555584,0.18351,-0.16842,0.04094,-0.10457,0.80461,0.12977,0.56994,-0.05352,-0.13010,0.14272, 1403636627663555328,0.17698,-0.18159,0.04759,-0.09940,0.80820,0.12354,0.56715,-0.06641,-0.13009,-0.03012, 1403636627763555584,0.17081,-0.19680,0.04004,-0.09143,0.81559,0.11663,0.55933,-0.04980,-0.13892,-0.11567, 1403636627863555328,0.16773,-0.21254,0.02735,-0.08153,0.82305,0.10792,0.55163,-0.01239,-0.14825,-0.12669, 1403636627963555584,0.16859,-0.22691,0.01846,-0.07297,0.82818,0.10153,0.54633,0.04246,-0.12588,-0.06638, 1403636628063555584,0.18107,-0.23920,0.01937,-0.06780,0.82814,0.09660,0.54795,0.13058,-0.08707,0.08725, 1403636628163555328,0.19404,-0.24796,0.03365,-0.06747,0.82007,0.09024,0.56106,0.15076,-0.07853,0.17398, 1403636628263555584,0.20928,-0.25555,0.05457,-0.07140,0.81237,0.08319,0.57275,0.15174,-0.07443,0.22238, 1403636628363555328,0.22390,-0.26402,0.07853,-0.07689,0.80939,0.07753,0.57703,0.14406,-0.09853,0.24397, 1403636628463555584,0.23771,-0.27486,0.10104,-0.07977,0.80930,0.07505,0.57709,0.13351,-0.12504,0.18622, 1403636628563555584,0.25078,-0.28920,0.11774,-0.08222,0.81007,0.07418,0.57578,0.12490,-0.15659,0.13322, 1403636628663555328,0.26249,-0.30646,0.12709,-0.08413,0.81034,0.07469,0.57506,0.11820,-0.19835,0.07159, 1403636628763555584,0.27361,-0.32820,0.12989,-0.07773,0.81187,0.07560,0.57368,0.10777,-0.24461,-0.00503, 1403636628863555328,0.28450,-0.35358,0.12690,-0.06733,0.81672,0.07402,0.56829,0.12420,-0.25914,-0.02362, 1403636628963555584,0.28499,-0.38244,0.13105,-0.05885,0.82170,0.07014,0.56252,0.07773,-0.25980,0.06512, 1403636629063555584,0.29170,-0.40924,0.14291,-0.05454,0.82465,0.06637,0.55908,0.05246,-0.27372,0.16563, 1403636629163555328,0.30093,-0.43647,0.15974,-0.04960,0.82565,0.06153,0.55861,0.08458,-0.28194,0.14507, 1403636629263555584,0.31517,-0.46365,0.17199,-0.04400,0.82485,0.05268,0.56117,0.12583,-0.27289,0.10880, 1403636629363555328,0.33523,-0.48910,0.17851,-0.04248,0.82288,0.04351,0.56496,0.16169,-0.25747,0.05791, 1403636629463555584,0.35826,-0.51442,0.17869,-0.04446,0.81978,0.03208,0.57005,0.18393,-0.28442,-0.00949, 1403636629563555584,0.38422,-0.54074,0.17043,-0.04215,0.81965,0.01827,0.57101,0.19890,-0.30838,-0.09430, 1403636629663555328,0.41245,-0.57073,0.15341,-0.03113,0.82479,0.00586,0.56456,0.22498,-0.33872,-0.16801, 1403636629763555584,0.44187,-0.58276,0.13523,-0.01880,0.83041,-0.00495,0.55681,0.24916,-0.35913,-0.22004, 1403636629863555328,0.47391,-0.61505,0.10955,-0.00409,0.83359,-0.02205,0.55193,0.30110,-0.35512,-0.25622, 1403636629963555584,0.50985,-0.64749,0.08560,0.00835,0.83458,-0.03721,0.54956,0.31634,-0.34803,-0.19415, 1403636630063555584,0.54564,-0.67931,0.06899,0.01651,0.83313,-0.04805,0.55073,0.35422,-0.33709,-0.11728, 1403636630163555328,0.58816,-0.71086,0.06234,0.02229,0.82623,-0.05537,0.56016,0.39762,-0.33162,-0.03481, 1403636630263555584,0.63569,-0.74041,0.06352,0.02623,0.81868,-0.05990,0.57051,0.43109,-0.32780,0.03716, 1403636630363555328,0.68392,-0.76984,0.06966,0.02788,0.81695,-0.06138,0.57275,0.43795,-0.31785,0.07079, 1403636630463555584,0.75626,-0.79466,0.06669,0.03147,0.81923,-0.06406,0.56900,0.44860,-0.32380,0.13071, 1403636630563555584,0.79998,-0.82875,0.08224,0.03381,0.82110,-0.06038,0.56657,0.46395,-0.31458,0.17655, 1403636630663555328,0.84893,-0.86252,0.09900,0.03554,0.82237,-0.05497,0.56518,0.48133,-0.30001,0.13357, 1403636630663555328,0.87137,-0.91561,0.12440,0.03370,0.82299,-0.05244,0.56462,0.50257,-0.26839,0.07297, 1403636630763555584,0.89365,-0.89326,0.11028,0.03425,0.82284,-0.05296,0.56476,0.49523,-0.27196,0.07421, 1403636630863555328,0.93835,-0.92267,0.11648,0.03025,0.82323,-0.05315,0.56441,0.50066,-0.25887,0.00949, 1403636630963555584,0.98248,-0.95573,0.11768,0.02645,0.82280,-0.05399,0.56515,0.50072,-0.26510,-0.04086, 1403636631063555584,1.02814,-0.98371,0.11341,0.02523,0.82200,-0.05419,0.56635,0.51233,-0.25562,-0.07435, 1403636631163555328,1.07255,-1.01295,0.10743,0.02529,0.81999,-0.05434,0.56923,0.50600,-0.25088,-0.09452, 1403636631263555584,1.11398,-1.04619,0.10041,0.02568,0.81775,-0.05382,0.57248,0.49506,-0.26504,-0.09717, 1403636631363555328,1.15830,-1.07584,0.09160,0.02568,0.81759,-0.05341,0.57275,0.49243,-0.26460,-0.14015, 1403636631463555584,1.19556,-1.10791,0.07969,0.02682,0.81748,-0.05274,0.57291,0.47096,-0.26389,-0.19208, 1403636631563555584,1.23901,-1.13671,0.06232,0.02868,0.81745,-0.05061,0.57305,0.46152,-0.25355,-0.20801, 1403636631663555328,1.28179,-1.16590,0.04678,0.02969,0.81808,-0.04838,0.57230,0.46718,-0.24843,-0.18026, 1403636631763555584,1.32414,-1.19368,0.03627,0.03036,0.81856,-0.04683,0.57170,0.43814,-0.23494,-0.09760, 1403636631863555328,1.36057,-1.22122,0.03714,0.03181,0.81868,-0.04538,0.57157,0.48029,-0.23648,-0.01616, 1403636631963555584,1.40146,-1.24342,0.03649,0.03320,0.81941,-0.04449,0.57051,0.48188,-0.22278,0.01737, 1403636632063555584,1.44121,-1.26398,0.03793,0.03430,0.81830,-0.04501,0.57200,0.44821,-0.18867,0.01298, 1403636632163555328,1.47975,-1.28234,0.04036,0.03909,0.81643,-0.04296,0.57452,0.43922,-0.17353,0.02175, 1403636632263555584,1.51430,-1.30185,0.04555,0.04032,0.81609,-0.04376,0.57485,0.42895,-0.14470,-0.02336, 1403636632363555328,1.55255,-1.31513,0.03987,0.03957,0.81570,-0.04574,0.57530,0.40229,-0.11127,-0.10306, 1403636632463555584,1.59007,-1.32587,0.02785,0.03755,0.81791,-0.04872,0.57205,0.39109,-0.08048,-0.16560, 1403636632563555584,1.62617,-1.33669,0.01261,0.02729,0.82310,-0.05130,0.56492,0.38649,-0.06503,-0.20889, 1403636632663555328,1.66742,-1.34438,-0.00851,0.01375,0.82830,-0.04872,0.55799,0.41251,-0.06902,-0.21871, 1403636632763555584,1.70812,-1.35555,-0.02500,0.00009,0.83060,-0.03715,0.55563,0.43901,-0.08377,-0.14930, 1403636632863555328,1.75387,-1.36863,-0.03483,-0.00881,0.82876,-0.01456,0.55935,0.46278,-0.10814,-0.07071, 1403636632963555584,1.80303,-1.38111,-0.03734,-0.01458,0.82450,0.01021,0.56558,0.49072,-0.12120,-0.00513, 1403636633063555584,1.85407,-1.39353,-0.03520,-0.01941,0.82207,0.02910,0.56832,0.49187,-0.12261,0.02145, 1403636633163555328,1.90285,-1.40337,-0.03459,-0.02476,0.82091,0.04151,0.56902,0.49294,-0.10714,-0.03577, 1403636633263555584,1.95476,-1.41205,-0.04239,-0.03041,0.82112,0.04782,0.56794,0.49403,-0.09133,-0.10275, 1403636633363555328,2.00651,-1.41983,-0.05449,-0.03679,0.82273,0.04917,0.56510,0.49515,-0.08787,-0.15634, 1403636633463555584,2.05859,-1.42845,-0.06968,-0.04138,0.82506,0.04758,0.56151,0.51403,-0.07782,-0.19441, 1403636633563555584,2.11481,-1.43559,-0.08742,-0.04260,0.82705,0.04537,0.55867,0.54374,-0.08087,-0.21370, 1403636633663555328,2.17304,-1.44495,-0.10437,-0.04119,0.82916,0.04342,0.55580,0.59502,-0.09437,-0.13743, 1403636633763555584,2.23320,-1.45291,-0.11311,-0.04013,0.82880,0.04035,0.55665,0.62571,-0.09816,-0.07226, 1403636633863555328,2.29651,-1.46309,-0.11601,-0.04014,0.82828,0.03652,0.55768,0.65503,-0.10104,0.00208, 1403636633963555584,2.36192,-1.47258,-0.11313,-0.04512,0.82701,0.02927,0.55960,0.67182,-0.10656,0.00508, 1403636634063555584,2.43010,-1.48497,-0.11606,-0.04779,0.82600,0.02353,0.56115,0.69002,-0.13917,-0.02458, 1403636634163555328,2.50033,-1.49933,-0.11516,-0.04723,0.82697,0.02461,0.55972,0.71859,-0.17025,0.04633, 1403636634263555584,2.57375,-1.51794,-0.10899,-0.04207,0.82745,0.03008,0.55915,0.75607,-0.19568,0.10924, 1403636634363555328,2.65147,-1.53820,-0.09487,-0.03807,0.82307,0.03527,0.56557,0.77593,-0.20129,0.12445, 1403636634463555584,2.72944,-1.55836,-0.08794,-0.03521,0.81857,0.03943,0.57197,0.77251,-0.20303,0.05411, 1403636634563555584,2.80558,-1.57950,-0.08627,-0.03499,0.81516,0.04246,0.57661,0.75894,-0.20015,-0.01256, 1403636634663555328,2.87984,-1.59799,-0.09093,-0.03639,0.81382,0.04857,0.57794,0.72798,-0.18161,-0.07166, 1403636634763555584,2.95467,-1.61589,-0.09690,-0.03918,0.81416,0.05698,0.57652,0.70389,-0.16236,-0.04837, 1403636634863555328,3.02328,-1.63213,-0.09872,-0.04358,0.81615,0.06257,0.57278,0.67164,-0.14911,0.00644, 1403636634963555584,3.09383,-1.64620,-0.09472,-0.04711,0.81802,0.06864,0.56914,0.68467,-0.13024,0.07692, 1403636635063555584,3.16056,-1.65876,-0.08604,-0.04866,0.81867,0.07439,0.56735,0.67552,-0.11371,0.08138, 1403636635163555328,3.22783,-1.67026,-0.08346,-0.05053,0.81964,0.07777,0.56532,0.66835,-0.10399,0.00887, 1403636635263555584,3.29476,-1.68063,-0.08530,-0.05410,0.82097,0.07877,0.56292,0.67375,-0.08976,-0.04955, 1403636635363555328,3.36239,-1.68975,-0.09367,-0.06014,0.82288,0.07646,0.55983,0.68467,-0.08171,-0.04882, 1403636635363555328,3.32491,-1.71140,-0.07289,-0.06062,0.82281,0.07718,0.55977,0.68481,-0.08051,-0.04882, 1403636635463555584,3.39625,-1.72005,-0.07640,-0.06375,0.82331,0.07593,0.55887,0.72623,-0.08687,-0.00258, 1403636635563555584,3.47266,-1.72965,-0.07435,-0.06698,0.82375,0.07353,0.55817,0.69226,-0.10120,0.07645, 1403636635663555328,3.54404,-1.74037,-0.06584,-0.06808,0.82293,0.07167,0.55948,0.72009,-0.11740,0.10152, 1403636635763555584,3.62456,-1.75257,-0.05291,-0.06987,0.82048,0.06897,0.56318,0.77085,-0.12703,0.18463, 1403636635863555328,3.70156,-1.76588,-0.02792,-0.07579,0.81712,0.06291,0.56799,0.77930,-0.16292,0.24886, 1403636635963555584,3.78447,-1.78394,-0.00075,-0.07854,0.82005,0.05948,0.56375,0.79980,-0.20566,0.27207, 1403636636063555584,3.86666,-1.80616,0.02573,-0.07185,0.82521,0.06315,0.55667,0.81700,-0.23808,0.23475, 1403636636163555328,3.95033,-1.83011,0.05013,-0.06391,0.82553,0.06849,0.55653,0.84513,-0.25061,0.27293, 1403636636263555584,4.03263,-1.85612,0.08194,-0.05815,0.81889,0.07274,0.56635,0.83854,-0.25506,0.33572, 1403636636363555328,4.11508,-1.87983,0.12003,-0.05604,0.81171,0.07411,0.57662,0.83303,-0.24284,0.37483, 1403636636463555584,4.19185,-1.90360,0.15368,-0.05600,0.80241,0.07470,0.58942,0.77835,-0.24427,0.29423, 1403636636563555584,4.26531,-1.92674,0.17781,-0.05065,0.79963,0.07846,0.59319,0.71717,-0.23247,0.21170, 1403636636663555328,4.33096,-1.94769,0.19442,-0.04298,0.79912,0.08380,0.59375,0.65096,-0.21242,0.12015, 1403636636763555584,4.39706,-1.96644,0.20525,-0.03619,0.79913,0.08849,0.59351,0.58991,-0.16346,0.03575, 1403636636863555328,4.45998,-1.97943,0.20634,-0.02791,0.79964,0.09467,0.59232,0.52794,-0.10583,-0.04524, 1403636636963555584,4.51656,-1.98900,0.20262,-0.03579,0.80308,0.09077,0.58783,0.48073,-0.05033,-0.10914, 1403636637063555584,4.56926,-1.99178,0.19234,-0.04798,0.80629,0.08410,0.58354,0.44323,-0.01850,-0.14885, 1403636637163555328,4.61884,-1.99293,0.17853,-0.05481,0.81025,0.08065,0.57791,0.41916,-0.01093,-0.17843, 1403636637263555584,4.66860,-1.99492,0.16503,-0.05648,0.81460,0.08051,0.57162,0.41909,-0.00088,-0.14719, 1403636637363555328,4.71191,-1.99312,0.15593,-0.05626,0.81706,0.08167,0.56796,0.48717,0.02009,-0.04523, 1403636637463555584,4.76182,-1.98968,0.15526,-0.05735,0.81850,0.08306,0.56556,0.49715,0.03514,0.06835, 1403636637563555584,4.80325,-1.98545,0.15994,-0.05896,0.81957,0.08345,0.56380,0.46325,0.03651,0.09863, 1403636637663555328,4.84610,-1.98166,0.17033,-0.06123,0.82012,0.08321,0.56278,0.47382,0.03499,0.17663, 1403636637763555584,4.89394,-1.97622,0.19004,-0.06170,0.81884,0.08413,0.56446,0.52982,0.04879,0.22717, 1403636637863555328,4.93782,-1.96983,0.21648,-0.06208,0.81646,0.08480,0.56775,0.53290,0.06003,0.31408, 1403636637963555584,4.98440,-1.96152,0.24375,-0.06308,0.81372,0.08500,0.57154,0.52972,0.06300,0.23792, 1403636638063555584,5.03165,-1.95172,0.25532,-0.06404,0.81157,0.08505,0.57447,0.51176,0.07172,0.05644, 1403636638163555328,5.07866,-1.94366,0.25263,-0.06530,0.80940,0.08482,0.57742,0.49188,0.07614,-0.05616, 1403636638263555584,5.12114,-1.93538,0.24139,-0.06542,0.80868,0.08541,0.57833,0.45378,0.06615,-0.11374, 1403636638363555328,5.16141,-1.92885,0.22509,-0.07064,0.81333,0.08251,0.57157,0.41852,0.04937,-0.19991, 1403636638463555584,5.19924,-1.92434,0.20255,-0.07753,0.82513,0.07795,0.55415,0.43076,0.04567,-0.23016, 1403636638563555584,5.23893,-1.92306,0.18246,-0.08186,0.83271,0.07508,0.54245,0.43083,-0.00296,-0.17098, 1403636638663555328,5.28798,-1.92429,0.17337,-0.07583,0.83258,0.07987,0.54285,0.51062,-0.02574,-0.02499, 1403636638763555584,5.34872,-1.92601,0.17411,-0.07074,0.82812,0.08370,0.54974,0.56606,-0.03288,0.06607, 1403636638863555328,5.40954,-1.92908,0.18232,-0.07048,0.82268,0.08405,0.55783,0.58810,-0.03652,0.11656, 1403636638963555584,5.47689,-1.93026,0.19294,-0.07474,0.81951,0.08136,0.56231,0.62410,-0.04347,0.16237, 1403636639063555584,5.54470,-1.93350,0.20558,-0.08429,0.81923,0.07457,0.56231,0.66265,-0.05702,0.12435, 1403636639163555328,5.61514,-1.93931,0.20909,-0.09173,0.81960,0.06887,0.56134,0.68487,-0.09668,-0.01745, 1403636639263555584,5.68553,-1.95008,0.19752,-0.09519,0.82041,0.06580,0.55994,0.71686,-0.14582,-0.17178, 1403636639363555328,5.75684,-1.96757,0.18004,-0.08584,0.82041,0.07224,0.56066,0.73620,-0.19513,-0.21984, 1403636639463555584,5.83496,-1.98579,0.15611,-0.07471,0.82029,0.07982,0.56140,0.75262,-0.21609,-0.25650, 1403636639563555584,5.91271,-2.00667,0.12904,-0.06289,0.81775,0.08811,0.56530,0.75161,-0.21493,-0.28793, 1403636639663555328,5.98903,-2.02650,0.09986,-0.05747,0.81431,0.09284,0.57006,0.75537,-0.17925,-0.26368, 1403636639763555584,6.06854,-2.04335,0.07487,-0.06209,0.80987,0.09013,0.57631,0.70270,-0.16794,-0.21780, 1403636639863555328,6.14256,-2.05992,0.05821,-0.06671,0.80812,0.08735,0.57867,0.68382,-0.16239,-0.13639, 1403636639963555584,6.21100,-2.07617,0.05063,-0.06966,0.80760,0.08598,0.57926,0.65087,-0.17155,-0.06729, 1403636640063555584,6.27597,-2.09219,0.04860,-0.07104,0.80675,0.08891,0.57983,0.65269,-0.14997,0.04437, 1403636640163555328,6.33935,-2.10721,0.05776,-0.07351,0.80556,0.09768,0.57977,0.62871,-0.14847,0.12540, 1403636640263555584,6.40101,-2.12232,0.06893,-0.07747,0.80528,0.10554,0.57826,0.58107,-0.15335,0.16919, 1403636640363555328,6.46084,-2.13982,0.07825,-0.08117,0.80487,0.11092,0.57731,0.55623,-0.15537,0.10042, 1403636640463555584,6.51823,-2.15623,0.08111,-0.08307,0.80421,0.11561,0.57704,0.53055,-0.16135,0.03783, 1403636640563555584,6.57852,-2.17314,0.07521,-0.08479,0.80447,0.11879,0.57579,0.51363,-0.16275,-0.02281, 1403636640663555328,6.62916,-2.18754,0.06901,-0.08616,0.80393,0.12043,0.57600,0.49031,-0.15331,-0.07516, 1403636640763555584,6.67765,-2.20074,0.05867,-0.08722,0.80516,0.12146,0.57390,0.48199,-0.14518,-0.06794, 1403636640863555328,6.72240,-2.21298,0.05116,-0.08677,0.80662,0.12309,0.57156,0.45760,-0.13925,-0.02372, 1403636640963555584,6.77185,-2.22528,0.04571,-0.08639,0.80711,0.12361,0.57082,0.42907,-0.13117,0.04166, 1403636641063555584,6.81308,-2.23581,0.04868,-0.08658,0.80617,0.12272,0.57230,0.41057,-0.11670,0.02192, 1403636641163555328,6.85462,-2.24492,0.04547,-0.08539,0.80604,0.12245,0.57272,0.39580,-0.10893,-0.04930, 1403636641263555584,6.89435,-2.25278,0.03607,-0.08626,0.80512,0.12072,0.57426,0.37281,-0.09528,-0.11705, 1403636641363555328,6.93293,-2.26000,0.02110,-0.09202,0.80618,0.11522,0.57300,0.35335,-0.09503,-0.16692, 1403636641463555584,6.97019,-2.26900,-0.00222,-0.09538,0.80635,0.11195,0.57287,0.34804,-0.11982,-0.17979, 1403636641563555584,7.00607,-2.27910,-0.01772,-0.09207,0.80619,0.11348,0.57332,0.29025,-0.13873,-0.17003, 1403636641663555328,7.04060,-2.29023,-0.02755,-0.08802,0.80628,0.11515,0.57350,0.28357,-0.13787,-0.09088, 1403636641763555584,7.07381,-2.30186,-0.02843,-0.08573,0.80611,0.11576,0.57397,0.26395,-0.14209,0.00727, 1403636641863555328,7.10591,-2.31287,-0.02253,-0.08508,0.80520,0.11572,0.57535,0.30230,-0.12138,0.06301, 1403636641963555584,7.13769,-2.32377,-0.01294,-0.08325,0.80533,0.11165,0.57624,0.29048,-0.12690,0.12459, 1403636642063555584,7.16343,-2.33759,0.00273,-0.07851,0.80512,0.10385,0.57865,0.28613,-0.13459,0.15281, 1403636642163555328,7.18682,-2.35028,0.01722,-0.07275,0.80511,0.09745,0.58053,0.26446,-0.12709,0.10133, 1403636642263555584,7.20672,-2.36305,0.02507,-0.06969,0.80497,0.09152,0.58206,0.23660,-0.12166,0.03861, 1403636642363555328,7.22699,-2.37727,0.02560,-0.06537,0.80524,0.08344,0.58340,0.20416,-0.13421,-0.03600, 1403636642463555584,7.24516,-2.38882,0.02034,-0.06087,0.80553,0.07568,0.58454,0.18309,-0.13378,-0.10353, 1403636642563555584,7.25825,-2.40234,0.00931,-0.05679,0.80860,0.07064,0.58133,0.14643,-0.13389,-0.15625, 1403636642663555328,7.27379,-2.41291,-0.00710,-0.05471,0.81383,0.06674,0.57466,0.13939,-0.12657,-0.19993, 1403636642763555584,7.28911,-2.42445,-0.02428,-0.05292,0.81848,0.06483,0.56840,0.16465,-0.11513,-0.16223, 1403636642863555328,7.30256,-2.43617,-0.03139,-0.05299,0.82133,0.06316,0.56446,0.15838,-0.11042,-0.05880, 1403636642963555584,7.31576,-2.44572,-0.03603,-0.05332,0.82273,0.06243,0.56247,0.14468,-0.11487,0.01664, 1403636643063555584,7.33142,-2.45782,-0.02786,-0.05413,0.82036,0.06189,0.56590,0.20458,-0.12469,0.07838, 1403636643163555328,7.35123,-2.47134,-0.02392,-0.05598,0.81603,0.06109,0.57204,0.20164,-0.13576,0.01002, 1403636643263555584,7.36835,-2.48531,-0.02709,-0.05708,0.81388,0.06089,0.57500,0.19016,-0.14815,-0.05597, 1403636643363555328,7.38959,-2.49837,-0.02968,-0.05655,0.81534,0.06177,0.57289,0.20135,-0.15342,-0.05816, 1403636643463555584,7.41110,-2.51310,-0.03134,-0.05787,0.81881,0.06182,0.56778,0.23370,-0.15622,0.02193, 1403636643563555584,7.43596,-2.52855,-0.02678,-0.05798,0.82284,0.06324,0.56177,0.30356,-0.16458,0.08490, 1403636643663555328,7.46256,-2.54402,-0.01459,-0.05720,0.82657,0.06546,0.55608,0.31031,-0.16707,0.14281, 1403636643763555584,7.48978,-2.55868,0.00636,-0.05643,0.82893,0.06737,0.55240,0.33506,-0.17163,0.19021, 1403636643863555328,7.52482,-2.57352,0.02497,-0.05512,0.83203,0.06915,0.54763,0.39112,-0.16898,0.14658, 1403636643963555584,7.56437,-2.58775,0.03827,-0.05392,0.83408,0.07053,0.54446,0.44975,-0.16099,0.09359, 1403636644063555584,7.60882,-2.60292,0.04599,-0.05337,0.83487,0.07097,0.54324,0.49625,-0.14576,0.04713, 1403636644163555328,7.66225,-2.61389,0.04809,-0.05284,0.83465,0.07118,0.54360,0.55657,-0.12757,0.00012, 1403636644263555584,7.72016,-2.62418,0.04643,-0.06024,0.82804,0.06546,0.55356,0.61124,-0.11015,-0.03452, 1403636644363555328,7.77998,-2.63658,0.03853,-0.07136,0.81941,0.05683,0.56590,0.62561,-0.14032,-0.09756, 1403636644463555584,7.83988,-2.65148,0.02319,-0.07743,0.81314,0.05187,0.57455,0.62509,-0.18855,-0.18335, 1403636644563555584,7.89903,-2.66941,-0.00093,-0.08556,0.80890,0.04610,0.57986,0.61127,-0.24069,-0.25227, 1403636644663555328,7.95810,-2.69405,-0.03112,-0.08277,0.80727,0.04742,0.58243,0.58777,-0.31030,-0.32236, 1403636644763555584,8.01610,-2.72547,-0.06623,-0.06869,0.80689,0.05712,0.58391,0.55878,-0.35485,-0.38179, 1403636644863555328,8.07364,-2.75778,-0.10747,-0.05491,0.80752,0.06509,0.58367,0.52849,-0.36266,-0.42806, 1403636644963555584,8.12873,-2.79017,-0.14715,-0.05170,0.81095,0.06556,0.57912,0.49631,-0.35070,-0.36005, 1403636645063555584,8.17531,-2.82226,-0.17972,-0.05113,0.82198,0.06448,0.56353,0.47829,-0.33953,-0.28802, 1403636645163555328,8.22030,-2.85498,-0.20355,-0.04952,0.83432,0.06436,0.54526,0.47498,-0.34237,-0.21460, 1403636645263555584,8.28021,-2.88769,-0.22001,-0.04906,0.84195,0.06375,0.53353,0.59033,-0.32672,-0.08701, 1403636645363555328,8.34738,-2.91710,-0.22988,-0.04884,0.83925,0.06342,0.53783,0.67492,-0.31071,-0.05097, 1403636645463555584,8.42638,-2.94514,-0.24360,-0.04825,0.83425,0.06257,0.54570,0.75101,-0.29143,-0.09411, 1403636645563555584,8.50597,-2.97205,-0.25602,-0.04752,0.83173,0.06254,0.54959,0.81133,-0.27805,-0.15916, 1403636645663555328,8.59321,-2.99545,-0.27368,-0.04641,0.82978,0.06261,0.55263,0.86145,-0.25753,-0.17659, 1403636645763555584,8.68154,-3.01822,-0.28565,-0.04694,0.82857,0.06232,0.55442,0.91933,-0.22838,-0.10091, 1403636645863555328,8.77709,-3.03650,-0.29088,-0.04788,0.82892,0.06100,0.55396,0.93787,-0.22408,0.03212, 1403636645963555584,8.87385,-3.05843,-0.28379,-0.04964,0.82680,0.05809,0.55729,0.98303,-0.22640,0.14354, 1403636646063555584,8.97226,-3.07798,-0.26984,-0.04998,0.82302,0.04738,0.56382,0.95638,-0.24280,0.19892, 1403636646163555328,9.07238,-3.10198,-0.25433,-0.04482,0.82004,0.02992,0.56976,0.99757,-0.27338,0.16761, 1403636646263555584,9.17407,-3.12886,-0.24181,-0.03661,0.81694,0.00651,0.57553,0.98092,-0.31631,0.10894, 1403636646363555328,9.27240,-3.16007,-0.23376,-0.02534,0.81212,-0.02025,0.58259,0.94500,-0.35846,0.04655, 1403636646463555584,9.36320,-3.19496,-0.23129,-0.00584,0.80670,-0.04101,0.58950,0.90289,-0.38802,0.01887, 1403636646563555584,9.45148,-3.23312,-0.22921,0.01304,0.80154,-0.05394,0.59536,0.82125,-0.41354,0.02082, 1403636646663555328,9.53629,-3.27323,-0.22248,0.02634,0.80219,-0.06294,0.59315,0.77500,-0.41394,0.10823, 1403636646763555584,9.61781,-3.31347,-0.20649,0.03360,0.80331,-0.07041,0.59042,0.74153,-0.40514,0.19447, 1403636646863555328,9.69466,-3.35311,-0.18089,0.03866,0.80263,-0.07507,0.59046,0.68654,-0.40536,0.26489, 1403636646963555584,9.76526,-3.39265,-0.15337,0.04017,0.80380,-0.07773,0.58843,0.64499,-0.40550,0.23146, 1403636647063555584,9.83070,-3.43181,-0.13036,0.04149,0.80610,-0.07815,0.58513,0.61480,-0.39635,0.17153, 1403636647163555328,9.89036,-3.47073,-0.11455,0.04132,0.81028,-0.07816,0.57934,0.59825,-0.39693,0.11953, 1403636647263555584,9.95133,-3.50975,-0.09940,0.04080,0.81153,-0.07753,0.57771,0.58881,-0.39739,0.08686, 1403636647363555328,10.01019,-3.54927,-0.09181,0.03761,0.81002,-0.07749,0.58004,0.57824,-0.40858,0.04454, 1403636647463555584,10.06755,-3.58976,-0.08814,0.03439,0.80789,-0.07717,0.58325,0.54757,-0.41271,-0.00347, 1403636647563555584,10.12119,-3.63076,-0.08686,0.02987,0.80353,-0.07706,0.58949,0.50591,-0.42680,0.00514, 1403636647663555328,10.17327,-3.67445,-0.07912,0.02723,0.79982,-0.07933,0.59435,0.46823,-0.45956,0.04120, 1403636647763555584,10.21818,-3.71978,-0.06735,0.03085,0.80194,-0.07943,0.59129,0.44804,-0.48777,0.10073, 1403636647863555328,10.25739,-3.76723,-0.05957,0.03630,0.80350,-0.08170,0.58856,0.39372,-0.50458,0.05261, 1403636647963555584,10.28703,-3.81343,-0.05386,0.04421,0.80293,-0.08978,0.58761,0.33547,-0.51299,-0.02918, 1403636648063555584,10.30929,-3.86315,-0.05968,0.05582,0.80207,-0.10418,0.58542,0.27132,-0.52592,-0.13074, 1403636648163555328,10.33127,-3.91614,-0.07251,0.07005,0.79974,-0.12429,0.58315,0.24644,-0.54026,-0.17342, 1403636648263555584,10.35143,-3.97000,-0.08940,0.08530,0.79600,-0.14594,0.58121,0.18681,-0.54622,-0.21592, 1403636648363555328,10.36575,-4.02416,-0.11124,0.10145,0.79146,-0.16849,0.57871,0.13906,-0.54159,-0.24887, 1403636648463555584,10.37552,-4.07885,-0.13725,0.11808,0.78680,-0.19175,0.57466,0.08620,-0.53844,-0.28218, 1403636648563555584,10.38265,-4.12974,-0.16346,0.13286,0.78139,-0.21358,0.57110,0.04771,-0.52044,-0.24058, 1403636648663555328,10.38681,-4.18092,-0.18340,0.14483,0.77821,-0.22889,0.56659,-0.00716,-0.49282,-0.18062, 1403636648763555584,10.38894,-4.23176,-0.19807,0.15338,0.77896,-0.23901,0.55908,-0.00273,-0.48999,-0.12713, 1403636648863555328,10.39465,-4.28336,-0.20514,0.15477,0.78537,-0.24695,0.54612,0.00797,-0.49807,-0.03699, 1403636648963555584,10.40270,-4.33599,-0.20129,0.15579,0.79122,-0.25021,0.53582,0.04982,-0.52312,0.06709, 1403636649063555584,10.41342,-4.38801,-0.18610,0.15863,0.79000,-0.24980,0.53696,0.11303,-0.54662,0.17118, 1403636649163555328,10.42569,-4.43959,-0.16256,0.16225,0.78193,-0.24665,0.54903,0.12600,-0.54124,0.23093, 1403636649263555584,10.43625,-4.49064,-0.13429,0.16543,0.77560,-0.24251,0.55881,0.12152,-0.52982,0.29789, 1403636649363555328,10.44343,-4.53804,-0.10123,0.16505,0.77587,-0.24001,0.55963,0.08434,-0.49560,0.32780, 1403636649463555584,10.44845,-4.58347,-0.06320,0.16247,0.78008,-0.23817,0.55529,0.05897,-0.47003,0.41172, 1403636649563555584,10.45387,-4.62763,-0.01688,0.15984,0.77922,-0.23533,0.55847,0.05821,-0.46253,0.48815, 1403636649663555328,10.45697,-4.67118,0.03269,0.15823,0.77452,-0.23727,0.56462,0.01219,-0.44680,0.46548, 1403636649763555584,10.45336,-4.71011,0.07795,0.16241,0.77020,-0.24392,0.56649,-0.03406,-0.43084,0.39453, 1403636649863555328,10.45007,-4.74969,0.11664,0.16896,0.76728,-0.24885,0.56640,-0.06023,-0.41170,0.32473, 1403636649963555584,10.44414,-4.78541,0.14767,0.17471,0.76545,-0.25275,0.56541,-0.07429,-0.39306,0.23094, 1403636650063555584,10.43484,-4.82046,0.16772,0.17871,0.76394,-0.25649,0.56451,-0.11429,-0.36444,0.11807, 1403636650163555328,10.41975,-4.85544,0.17776,0.18450,0.76079,-0.26476,0.56308,-0.17745,-0.34384,0.03832, 1403636650263555584,10.40117,-4.88767,0.17796,0.19591,0.75630,-0.27946,0.55815,-0.22066,-0.31892,-0.03868, 1403636650363555328,10.37772,-4.92139,0.17622,0.20796,0.75043,-0.29991,0.55105,-0.23999,-0.31913,-0.07447, 1403636650463555584,10.35117,-4.95297,0.17069,0.22223,0.74488,-0.32165,0.54066,-0.28850,-0.28057,-0.11631, 1403636650563555584,10.32299,-4.98009,0.16014,0.23344,0.74026,-0.34255,0.52933,-0.28837,-0.25333,-0.14769, 1403636650663555328,10.29437,-5.00427,0.14425,0.24165,0.73621,-0.35726,0.52151,-0.28421,-0.23720,-0.18006, 1403636650763555584,10.26651,-5.02850,0.12874,0.24839,0.73262,-0.36736,0.51636,-0.28048,-0.22213,-0.13093, 1403636650863555328,10.24048,-5.05076,0.12119,0.25446,0.72801,-0.37263,0.51614,-0.26379,-0.22331,-0.02776, 1403636650963555584,10.21672,-5.07114,0.12144,0.26023,0.72216,-0.37366,0.52072,-0.26064,-0.20622,0.03702, 1403636651063555584,10.19266,-5.09035,0.12742,0.26374,0.71914,-0.37297,0.52362,-0.25517,-0.18173,0.10322, 1403636651163555328,10.16801,-5.10882,0.13943,0.26396,0.71867,-0.37218,0.52471,-0.26891,-0.15767,0.15162, 1403636651263555584,10.13850,-5.12180,0.15514,0.26152,0.72167,-0.37189,0.52202,-0.28709,-0.12634,0.20029, 1403636651363555328,10.10957,-5.13347,0.17449,0.25766,0.72584,-0.37154,0.51839,-0.28438,-0.11384,0.19245, 1403636651463555584,10.07899,-5.14285,0.18907,0.25498,0.72840,-0.36978,0.51737,-0.29212,-0.10319,0.11515, 1403636651563555584,10.04796,-5.15090,0.19681,0.25404,0.72773,-0.36676,0.52093,-0.28942,-0.09476,0.05872, 1403636651663555328,10.01667,-5.15674,0.19885,0.25476,0.72463,-0.36321,0.52735,-0.30403,-0.06884,0.00257, 1403636651763555584,9.98373,-5.16091,0.19540,0.25255,0.72106,-0.36502,0.53202,-0.34122,-0.05647,-0.04248, 1403636651863555328,9.94579,-5.16188,0.18882,0.25160,0.71376,-0.37443,0.53577,-0.39940,-0.02600,-0.09129, 1403636651963555584,9.90412,-5.16361,0.17842,0.25684,0.70503,-0.38842,0.53488,-0.45186,-0.02217,-0.13907, 1403636652063555584,9.85377,-5.16286,0.16371,0.27025,0.69583,-0.40522,0.52781,-0.52418,-0.02042,-0.19961, 1403636652163555328,9.79680,-5.16161,0.14507,0.29000,0.68768,-0.42511,0.51213,-0.58080,0.00169,-0.21163, 1403636652263555584,9.73524,-5.15725,0.12282,0.30847,0.67706,-0.45099,0.49299,-0.62172,0.07828,-0.28232, 1403636652363555328,9.66639,-5.14957,0.09470,0.32212,0.66640,-0.47937,0.47154,-0.63730,0.08624,-0.32037, 1403636652463555584,9.59950,-5.14410,0.06782,0.33269,0.65527,-0.50148,0.45658,-0.61331,0.06855,-0.28282, 1403636652563555584,9.53438,-5.14262,0.04676,0.33791,0.64740,-0.51855,0.44475,-0.59884,0.03578,-0.20938, 1403636652663555328,9.47202,-5.14888,0.03536,0.33990,0.64504,-0.52999,0.43302,-0.59070,-0.03999,-0.10891, 1403636652763555584,9.41434,-5.16031,0.03436,0.34262,0.64367,-0.53512,0.42655,-0.55455,-0.09934,0.00003, 1403636652863555328,9.36023,-5.17838,0.04271,0.34476,0.64196,-0.53667,0.42547,-0.52823,-0.17557,0.09374, 1403636652963555584,9.30855,-5.20250,0.05884,0.34820,0.64078,-0.53420,0.42753,-0.51379,-0.23374,0.14859, 1403636653063555584,9.25986,-5.22717,0.06939,0.35170,0.64017,-0.53038,0.43034,-0.48651,-0.26838,0.02613, 1403636653163555328,9.21161,-5.26125,0.06895,0.35472,0.63879,-0.52518,0.43625,-0.46256,-0.32516,-0.08434, 1403636653263555584,9.16455,-5.29269,0.06045,0.35981,0.63585,-0.51784,0.44506,-0.45214,-0.32264,-0.07093, 1403636653363555328,9.11803,-5.32702,0.05929,0.36273,0.63552,-0.51117,0.45083,-0.43031,-0.31509,0.00072, 1403636653463555584,9.07265,-5.35722,0.06557,0.36161,0.63753,-0.50764,0.45287,-0.41880,-0.29159,0.08512, 1403636653563555584,9.02937,-5.38373,0.07735,0.35910,0.64015,-0.50516,0.45394,-0.40885,-0.27197,0.16205, 1403636653663555328,8.98730,-5.40509,0.09700,0.35937,0.63957,-0.50209,0.45794,-0.40435,-0.24453,0.18370, 1403636653763555584,8.94738,-5.42601,0.11125,0.36100,0.63823,-0.49895,0.46194,-0.39338,-0.21003,0.11377, 1403636653863555328,8.90745,-5.44220,0.11831,0.36279,0.63789,-0.49645,0.46370,-0.39497,-0.16894,0.04419, 1403636653963555584,8.86491,-5.45124,0.11839,0.36107,0.63804,-0.49732,0.46390,-0.38985,-0.11970,-0.02778, 1403636654063555584,8.82626,-5.45830,0.11042,0.35647,0.63792,-0.50057,0.46413,-0.38850,-0.08347,-0.09025, 1403636654163555328,8.78719,-5.46107,0.09996,0.35725,0.63432,-0.50396,0.46479,-0.40132,-0.05848,-0.06125, 1403636654263555584,8.74647,-5.46076,0.09419,0.36289,0.62962,-0.50941,0.46086,-0.42666,-0.02738,-0.00120, 1403636654363555328,8.70281,-5.45522,0.09403,0.36937,0.62396,-0.51883,0.45283,-0.43760,0.00306,0.07803, 1403636654463555584,8.65970,-5.44782,0.10245,0.37675,0.61631,-0.53124,0.44272,-0.43707,0.03546,0.15232, 1403636654563555584,8.61703,-5.43898,0.12024,0.38684,0.60624,-0.54430,0.43194,-0.42702,0.06210,0.20197, 1403636654663555328,8.57730,-5.42930,0.13888,0.38959,0.59706,-0.55913,0.42322,-0.40515,0.07139,0.16706, 1403636654763555584,8.53858,-5.41923,0.15181,0.39260,0.59069,-0.56935,0.41568,-0.39014,0.07167,0.10099, 1403636654863555328,8.50035,-5.40958,0.15709,0.39669,0.58592,-0.57583,0.40957,-0.37887,0.06619,0.02826, 1403636654963555584,8.46401,-5.39987,0.15497,0.40350,0.58047,-0.57757,0.40823,-0.36648,0.06194,-0.02741, 1403636655063555584,8.42955,-5.38889,0.14810,-0.41098,-0.57331,0.57625,-0.41273,-0.34557,0.07650,-0.07313, 1403636655163555328,8.39504,-5.37504,0.13848,-0.41587,-0.56856,0.57506,-0.41605,-0.33682,0.11025,-0.10477, 1403636655263555584,8.35963,-5.35817,0.12757,-0.41794,-0.56381,0.57573,-0.41950,-0.34930,0.15925,-0.12251, 1403636655363555328,8.32458,-5.33949,0.11472,-0.42095,-0.55739,0.57974,-0.41954,-0.35170,0.21044,-0.12251, 1403636655463555584,8.28863,-5.31091,0.10624,-0.42482,-0.55047,0.58484,-0.41770,-0.36325,0.27733,-0.05182, 1403636655563555584,8.25066,-5.27614,0.10482,-0.42666,-0.54448,0.59439,-0.41012,-0.36988,0.36791,-0.02138, 1403636655663555328,8.21098,-5.23597,0.10209,-0.42334,-0.53924,0.60962,-0.39795,-0.38000,0.40940,-0.06664, 1403636655763555584,8.17082,-5.19550,0.08977,-0.42121,-0.53406,0.62210,-0.38775,-0.39847,0.40637,-0.18606, 1403636655863555328,8.12722,-5.15511,0.06395,-0.42307,-0.52449,0.63406,-0.37933,-0.42532,0.40706,-0.30973, 1403636655963555584,8.08020,-5.11573,0.03079,-0.42841,-0.51094,0.64627,-0.37112,-0.44706,0.39215,-0.30724, 1403636656063555584,8.02990,-5.07744,0.00280,-0.43489,-0.49900,0.65474,-0.36494,-0.48515,0.40806,-0.22569, 1403636656163555328,7.97565,-5.04029,-0.01593,-0.44042,-0.49283,0.66001,-0.35712,-0.51258,0.38573,-0.12597, 1403636656263555584,7.92199,-5.00366,-0.02231,-0.44380,-0.49066,0.66344,-0.34949,-0.52100,0.38028,-0.01491, 1403636656363555328,7.86708,-4.97101,-0.01917,-0.44406,-0.48927,0.66617,-0.34588,-0.53672,0.33447,0.08620, 1403636656463555584,7.81232,-4.94533,-0.00491,-0.44400,-0.48938,0.66686,-0.34448,-0.55730,0.28757,0.16447, 1403636656563555584,7.75790,-4.92651,0.01515,-0.44519,-0.49123,0.66514,-0.34364,-0.56536,0.23644,0.18088, 1403636656663555328,7.70427,-4.90760,0.03429,-0.44942,-0.49078,0.66085,-0.34703,-0.56658,0.20310,0.15748, 1403636656763555584,7.64788,-4.89291,0.04769,-0.45802,-0.48688,0.65264,-0.35668,-0.57470,0.20299,0.09121, 1403636656863555328,7.59049,-4.87612,0.05260,-0.46460,-0.48711,0.64601,-0.35993,-0.57151,0.21494,0.01320, 1403636656963555584,7.53532,-4.85527,0.05016,-0.46710,-0.48715,0.64214,-0.36354,-0.54227,0.24596,-0.06162, 1403636657063555584,7.48198,-4.82922,0.04028,-0.46638,-0.48596,0.64092,-0.36819,-0.53074,0.28820,-0.13514, 1403636657163555328,7.42977,-4.79762,0.02559,-0.46391,-0.48450,0.64119,-0.37272,-0.51624,0.34406,-0.13940, 1403636657263555584,7.37998,-4.76196,0.01576,-0.45897,-0.48471,0.64383,-0.37401,-0.51596,0.37995,-0.08492, 1403636657363555328,7.32768,-4.72050,0.00979,-0.45391,-0.48616,0.64739,-0.37215,-0.52377,0.41842,-0.04269, 1403636657463555584,7.27829,-4.67603,0.00519,-0.44510,-0.48794,0.65398,-0.36892,-0.53762,0.43806,-0.07349, 1403636657563555584,7.22685,-4.62954,-0.00331,-0.43911,-0.48970,0.65857,-0.36558,-0.56737,0.43973,-0.10902, 1403636657663555328,7.17273,-4.58234,-0.01564,-0.43814,-0.49031,0.65953,-0.36422,-0.59000,0.44539,-0.14753, 1403636657763555584,7.11333,-4.53554,-0.03180,-0.43899,-0.48790,0.66050,-0.36466,-0.62421,0.43957,-0.18769, 1403636657863555328,7.05054,-4.49053,-0.05329,-0.43614,-0.48362,0.66512,-0.36539,-0.65172,0.44304,-0.23139, 1403636657963555584,6.98430,-4.44221,-0.07285,-0.43060,-0.48048,0.67105,-0.36527,-0.70566,0.43561,-0.17684, 1403636658063555584,6.91165,-4.39488,-0.08309,-0.43159,-0.48155,0.67217,-0.36059,-0.76048,0.45892,-0.06714, 1403636658163555328,6.83406,-4.34673,-0.08434,-0.43845,-0.48542,0.66919,-0.35258,-0.80070,0.44321,-0.00689, 1403636658263555584,6.75414,-4.30264,-0.08317,-0.44461,-0.48881,0.66641,-0.34539,-0.81173,0.42728,-0.03249, 1403636658363555328,6.67493,-4.26291,-0.08689,-0.44743,-0.49073,0.66559,-0.34056,-0.80524,0.39696,-0.08656, 1403636658463555584,6.59774,-4.22689,-0.09423,-0.44947,-0.49283,0.66507,-0.33584,-0.79662,0.36328,-0.10994, 1403636658563555584,6.52029,-4.19531,-0.10410,-0.45369,-0.49612,0.66268,-0.32998,-0.77847,0.32099,-0.12267, 1403636658663555328,6.44345,-4.16771,-0.11707,-0.45696,-0.49825,0.66058,-0.32644,-0.74318,0.28266,-0.14874, 1403636658763555584,6.37007,-4.14467,-0.13260,-0.46143,-0.49899,0.65761,-0.32503,-0.70429,0.24100,-0.17253, 1403636658863555328,6.29934,-4.12357,-0.15011,-0.46874,-0.49908,0.65253,-0.32467,-0.65290,0.20710,-0.19375, 1403636658963555584,6.23360,-4.10614,-0.17128,-0.47032,-0.49385,0.65153,-0.33230,-0.59964,0.18674,-0.23275, 1403636659063555584,6.17208,-4.09068,-0.19626,-0.47144,-0.48850,0.65056,-0.34041,-0.56957,0.18629,-0.27219, 1403636659163555328,6.11620,-4.07422,-0.22503,-0.47273,-0.48560,0.64938,-0.34500,-0.53527,0.19532,-0.30793, 1403636659263555584,6.06379,-4.05237,-0.25507,-0.47284,-0.48453,0.64901,-0.34704,-0.49361,0.21587,-0.28812, 1403636659363555328,6.01504,-4.03155,-0.28031,-0.46881,-0.48688,0.65160,-0.34436,-0.46594,0.22057,-0.23536, 1403636659463555584,5.96999,-4.00755,-0.30133,-0.46320,-0.49136,0.65527,-0.33858,-0.44558,0.21135,-0.15594, 1403636659563555584,5.92701,-3.98440,-0.31289,-0.46172,-0.49189,0.65651,-0.33741,-0.42100,0.21181,-0.11435, 1403636659663555328,5.88721,-3.96138,-0.32529,-0.46515,-0.49075,0.65480,-0.33769,-0.39288,0.20488,-0.15683, 1403636659763555584,5.85270,-3.94140,-0.34452,-0.46844,-0.48867,0.65361,-0.33847,-0.35173,0.19793,-0.22315, 1403636659863555328,5.82102,-3.91847,-0.36832,-0.46922,-0.48727,0.65413,-0.33840,-0.30894,0.21174,-0.27881, 1403636659963555584,5.79283,-3.89617,-0.39882,-0.46645,-0.48853,0.65733,-0.33418,-0.27747,0.21693,-0.33163, 1403636660063555584,5.76655,-3.87061,-0.43198,-0.46087,-0.49055,0.66221,-0.32930,-0.24854,0.21183,-0.36133, 1403636660163555328,5.74241,-3.84800,-0.46756,-0.45684,-0.49273,0.66569,-0.32462,-0.22549,0.18413,-0.38040, 1403636660263555584,5.72167,-3.82960,-0.50534,-0.45414,-0.49377,0.66803,-0.32201,-0.20905,0.14560,-0.38784, 1403636660363555328,5.70300,-3.81593,-0.54025,-0.44749,-0.49746,0.67302,-0.31516,-0.19416,0.09584,-0.32863, 1403636660463555584,5.68716,-3.81065,-0.56729,-0.44090,-0.49992,0.67798,-0.30988,-0.17399,0.01662,-0.22641, 1403636660563555584,5.67260,-3.81448,-0.58897,-0.44427,-0.49915,0.67588,-0.31090,-0.16080,-0.07805,-0.21521, 1403636660663555328,5.65822,-3.82649,-0.61337,-0.45618,-0.49539,0.66757,-0.31754,-0.13956,-0.13873,-0.26462, 1403636660763555584,5.64571,-3.84332,-0.64306,-0.46968,-0.49335,0.65760,-0.32182,-0.10226,-0.16762,-0.31580, 1403636660863555328,5.63813,-3.86284,-0.67793,-0.48029,-0.49475,0.64939,-0.32068,-0.04378,-0.17128,-0.37639, 1403636660963555584,5.63624,-3.88241,-0.71804,-0.48351,-0.49399,0.64670,-0.32245,0.02349,-0.17929,-0.42568, 1403636661063555584,5.64152,-3.90001,-0.76313,-0.48135,-0.48998,0.64818,-0.32876,0.08280,-0.17327,-0.45669, 1403636661163555328,5.65249,-3.91720,-0.81068,-0.47897,-0.48625,0.64997,-0.33419,0.13239,-0.16442,-0.46775, 1403636661263555584,5.66787,-3.93393,-0.85423,-0.47735,-0.48375,0.65117,-0.33778,0.17926,-0.12610,-0.41717, 1403636661363555328,5.68747,-3.94603,-0.89064,-0.47711,-0.48246,0.65129,-0.33972,0.20763,-0.09707,-0.33199, 1403636661463555584,5.70919,-3.95695,-0.91935,-0.47663,-0.48229,0.65147,-0.34031,0.22813,-0.08685,-0.23954, 1403636661563555584,5.73416,-3.96348,-0.94197,-0.47362,-0.48234,0.65320,-0.34113,0.26313,-0.04963,-0.17435, 1403636661663555328,5.76191,-3.96921,-0.95777,-0.47085,-0.48151,0.65481,-0.34304,0.28439,-0.02914,-0.11411, 1403636661763555584,5.79100,-3.97230,-0.96541,-0.46998,-0.47835,0.65514,-0.34797,0.29282,-0.01325,-0.03026, 1403636661863555328,5.82096,-3.97440,-0.96724,-0.46685,-0.47429,0.65652,-0.35508,0.27755,0.01276,0.03749, 1403636661963555584,5.84929,-3.97208,-0.96116,-0.46619,-0.47322,0.65641,-0.35757,0.26771,0.04910,0.10770, 1403636662063555584,5.87794,-3.96765,-0.94959,-0.46772,-0.47332,0.65540,-0.35730,0.25809,0.08395,0.17629, 1403636662163555328,5.90459,-3.95686,-0.93588,-0.46922,-0.47576,0.65435,-0.35400,0.24663,0.12662,0.14303, 1403636662263555584,5.93264,-3.94429,-0.92818,-0.47287,-0.48127,0.65245,-0.34510,0.25785,0.16118,0.07242, 1403636662363555328,5.95958,-3.92767,-0.92440,-0.47404,-0.48527,0.65234,-0.33800,0.27401,0.17243,0.02149, 1403636662463555584,5.98962,-3.90990,-0.92694,-0.47015,-0.48852,0.65605,-0.33151,0.30737,0.17652,-0.06073, 1403636662563555584,6.02515,-3.89175,-0.93549,-0.46389,-0.48972,0.66141,-0.32790,0.33864,0.15986,-0.09667, 1403636662663555328,6.05926,-3.87560,-0.94596,-0.45489,-0.48280,0.66867,-0.33595,0.33710,0.14020,-0.12011, 1403636662763555584,6.09460,-3.86132,-0.96074,-0.45007,-0.47421,0.67258,-0.34669,0.30708,0.12554,-0.16096, 1403636662863555328,6.12642,-3.84801,-0.97985,-0.44715,-0.47037,0.67529,-0.35042,0.26736,0.12325,-0.20432, 1403636662963555584,6.15287,-3.83581,-1.00362,-0.44369,-0.47283,0.67850,-0.34526,0.22615,0.12087,-0.24842, 1403636663063555584,6.17231,-3.82422,-1.02592,-0.43556,-0.47105,0.68439,-0.34641,0.16693,0.10517,-0.18920, 1403636663163555328,6.18703,-3.81411,-1.04199,-0.42980,-0.46945,0.68869,-0.34727,0.08194,0.08110,-0.11505, 1403636663263555584,6.19153,-3.80710,-1.05347,-0.43757,-0.47685,0.68438,-0.33581,0.00769,0.05160,-0.12066, 1403636663363555328,6.19090,-3.80461,-1.06967,-0.44477,-0.48704,0.67965,-0.32101,-0.02262,0.01446,-0.19580, 1403636663463555584,6.18953,-3.80689,-1.09388,-0.44595,-0.49531,0.67834,-0.30928,-0.01874,-0.04876,-0.25924, 1403636663563555584,6.18756,-3.81666,-1.12240,-0.44532,-0.50025,0.67864,-0.30149,0.00317,-0.12504,-0.30225, 1403636663663555328,6.18750,-3.83499,-1.15326,-0.44407,-0.50217,0.67917,-0.29894,0.00266,-0.22259,-0.31314, 1403636663763555584,6.18919,-3.86297,-1.18149,-0.44956,-0.50667,0.67447,-0.29371,0.02638,-0.31345,-0.25824, 1403636663863555328,6.19359,-3.90292,-1.20238,-0.46073,-0.51112,0.66507,-0.29010,0.08248,-0.43502,-0.17709, 1403636663963555584,6.20494,-3.95204,-1.21668,-0.47147,-0.51250,0.65580,-0.29153,0.16320,-0.52654,-0.09931, 1403636664063555584,6.22528,-4.00980,-1.22342,-0.48028,-0.51177,0.64797,-0.29589,0.27154,-0.57510,-0.04059, 1403636664163555328,6.25574,-4.07255,-1.22525,-0.48081,-0.50700,0.64618,-0.30696,0.36287,-0.62212,0.01066, 1403636664263555584,6.29583,-4.13861,-1.22409,-0.48037,-0.50115,0.64460,-0.32030,0.43573,-0.63980,-0.01663, 1403636664363555328,6.34166,-4.20355,-1.23566,-0.47878,-0.49803,0.64353,-0.32954,0.48933,-0.61942,-0.18923, 1403636664463555584,6.39128,-4.26768,-1.26233,-0.47676,-0.49742,0.64237,-0.33560,0.53524,-0.59464,-0.30856, 1403636664563555584,6.44406,-4.33027,-1.29470,-0.47463,-0.49703,0.64080,-0.34215,0.57350,-0.58553,-0.31384, 1403636664663555328,6.50186,-4.38968,-1.32333,-0.46919,-0.49462,0.64140,-0.35187,0.59892,-0.54247,-0.25445, 1403636664763555584,6.56156,-4.44356,-1.34387,-0.46483,-0.49745,0.64180,-0.35294,0.60422,-0.50067,-0.16324, 1403636664863555328,6.62295,-4.49380,-1.35695,-0.46385,-0.50266,0.63967,-0.35071,0.62573,-0.48760,-0.10133, 1403636664963555584,6.68590,-4.54332,-1.36469,-0.45969,-0.50527,0.64033,-0.35123,0.64102,-0.47806,-0.04024, 1403636665063555584,6.75185,-4.59134,-1.36600,-0.45170,-0.50908,0.64425,-0.34893,0.65450,-0.43455,0.03130, 1403636665163555328,6.81783,-4.63624,-1.35887,-0.44551,-0.51221,0.64724,-0.34675,0.66100,-0.45343,0.10024, 1403636665263555584,6.88368,-4.68298,-1.34702,-0.44080,-0.50721,0.64956,-0.35567,0.64476,-0.45818,0.13247, 1403636665363555328,6.94707,-4.73435,-1.33482,-0.44207,-0.50319,0.64812,-0.36236,0.61666,-0.47123,0.12566, 1403636665463555584,7.00834,-4.78551,-1.32431,-0.44627,-0.50618,0.64571,-0.35733,0.58844,-0.45632,0.11145, 1403636665563555584,7.06832,-4.83255,-1.31367,-0.45243,-0.51154,0.64265,-0.34732,0.60053,-0.43756,0.12424, 1403636665663555328,7.12932,-4.87696,-1.29938,-0.45507,-0.51470,0.64186,-0.34062,0.62283,-0.43792,0.13438, 1403636665763555584,7.19406,-4.92601,-1.28534,-0.46036,-0.51299,0.63974,-0.34005,0.64512,-0.45432,0.14549, 1403636665863555328,7.26024,-4.97220,-1.26873,-0.46078,-0.50532,0.64097,-0.34856,0.67910,-0.46372,0.15623, 1403636665963555584,7.33051,-5.01827,-1.25094,-0.45479,-0.49511,0.64666,-0.36038,0.68623,-0.44003,0.12929, 1403636666063555584,7.39867,-5.06268,-1.23886,-0.45015,-0.49029,0.65090,-0.36511,0.65955,-0.41268,0.07760, 1403636666163555328,7.46344,-5.10438,-1.23135,-0.45470,-0.49196,0.64857,-0.36136,0.62605,-0.39384,0.03406, 1403636666263555584,7.52676,-5.14383,-1.22885,-0.45730,-0.49253,0.64673,-0.36061,0.61415,-0.36917,-0.02497, 1403636666363555328,7.59051,-5.17902,-1.23357,-0.45679,-0.48976,0.64667,-0.36509,0.60794,-0.33634,-0.06278, 1403636666463555584,7.65238,-5.21021,-1.23780,-0.45156,-0.48861,0.64955,-0.36800,0.59191,-0.28855,-0.03002, 1403636666563555584,7.71131,-5.23656,-1.23576,-0.44862,-0.49203,0.65063,-0.36512,0.54743,-0.22820,0.06942, 1403636666663555328,7.76466,-5.25946,-1.22528,-0.44749,-0.49940,0.65021,-0.35719,0.53289,-0.19385,0.12274, 1403636666763555584,7.81774,-5.28084,-1.21123,-0.44365,-0.50685,0.65155,-0.34894,0.52645,-0.20494,0.15482, 1403636666863555328,7.87367,-5.30300,-1.19714,-0.43493,-0.50956,0.65657,-0.34656,0.52401,-0.22324,0.11290, 1403636666963555584,7.92562,-5.32729,-1.19007,-0.41708,-0.50549,0.66787,-0.35281,0.48332,-0.25311,0.05587, 1403636667063555584,7.97224,-5.35595,-1.18736,-0.41451,-0.50285,0.66912,-0.35722,0.40909,-0.30184,0.04123, 1403636667163555328,8.01137,-5.38828,-1.18651,-0.43280,-0.50363,0.65647,-0.35790,0.36211,-0.33132,0.00043, 1403636667263555584,8.04849,-5.42075,-1.18934,-0.44943,-0.50569,0.64447,-0.35630,0.34963,-0.33405,-0.04344, 1403636667363555328,8.08740,-5.45113,-1.20057,-0.45681,-0.50720,0.63871,-0.35514,0.36725,-0.31108,-0.10769, 1403636667463555584,8.12821,-5.47939,-1.21569,-0.45527,-0.50982,0.63946,-0.35199,0.39615,-0.28538,-0.15035, 1403636667563555584,8.17055,-5.50711,-1.23041,-0.44644,-0.51221,0.64525,-0.34927,0.41110,-0.29288,-0.14559, 1403636667663555328,8.21405,-5.53504,-1.24048,-0.43550,-0.51138,0.65275,-0.35035,0.41270,-0.29405,-0.05306, 1403636667763555584,8.25902,-5.56474,-1.24239,-0.43187,-0.50895,0.65548,-0.35328,0.39376,-0.31484,0.02579, 1403636667863555328,8.30075,-5.59587,-1.23904,-0.43231,-0.50670,0.65594,-0.35511,0.37337,-0.31408,0.05417, 1403636667963555584,8.33513,-5.62706,-1.23690,-0.43166,-0.50588,0.65700,-0.35511,0.34894,-0.31748,0.01604, 1403636668063555584,8.37016,-5.66008,-1.24051,-0.43478,-0.50559,0.65627,-0.35306,0.31928,-0.34166,-0.06525, 1403636668163555328,8.40069,-5.69471,-1.25212,-0.43950,-0.50683,0.65466,-0.34839,0.30143,-0.35896,-0.13408, 1403636668263555584,8.43098,-5.73032,-1.26889,-0.44150,-0.50869,0.65512,-0.34223,0.29321,-0.37600,-0.18429, 1403636668363555328,8.46292,-5.76829,-1.28927,-0.44363,-0.50816,0.65591,-0.33874,0.30913,-0.40145,-0.20652, 1403636668463555584,8.49616,-5.80944,-1.31188,-0.44746,-0.50595,0.65527,-0.33824,0.31393,-0.42588,-0.23757, 1403636668563555584,8.52973,-5.85284,-1.33650,-0.44933,-0.50425,0.65574,-0.33741,0.32981,-0.43900,-0.25280, 1403636668663555328,8.56746,-5.89676,-1.36309,-0.45087,-0.50321,0.65644,-0.33552,0.33879,-0.45492,-0.24548, 1403636668763555584,8.60309,-5.94277,-1.38766,-0.45807,-0.50552,0.65245,-0.33005,0.36016,-0.47502,-0.24131, 1403636668863555328,8.64358,-5.99082,-1.40989,-0.46799,-0.50764,0.64605,-0.32543,0.41901,-0.49145,-0.18923, 1403636668963555584,8.69065,-6.04055,-1.42526,-0.47432,-0.50600,0.64196,-0.32692,0.49574,-0.50128,-0.11004, 1403636669063555584,8.74546,-6.08676,-1.43212,-0.47219,-0.49921,0.64398,-0.33633,0.56344,-0.46615,-0.04056, 1403636669163555328,8.80139,-6.13321,-1.43220,-0.46988,-0.49413,0.64624,-0.34267,0.59482,-0.43600,-0.03983, 1403636669263555584,8.86085,-6.17371,-1.43418,-0.47026,-0.49186,0.64659,-0.34474,0.61894,-0.40722,-0.08759, 1403636669363555328,8.92256,-6.21058,-1.44208,-0.46995,-0.49162,0.64748,-0.34386,0.63863,-0.38463,-0.14404, 1403636669463555584,8.98809,-6.24955,-1.45594,-0.47187,-0.48954,0.64614,-0.34668,0.66211,-0.36434,-0.17163, 1403636669563555584,9.05473,-6.28371,-1.47229,-0.47604,-0.48774,0.64259,-0.35010,0.67809,-0.33134,-0.19920, 1403636669663555328,9.12464,-6.31290,-1.49077,-0.48303,-0.48466,0.63666,-0.35562,0.71269,-0.27878,-0.16020, 1403636669763555584,9.19575,-6.33536,-1.50210,-0.48712,-0.48154,0.63293,-0.36088,0.74268,-0.23485,-0.10168, 1403636669863555328,9.26964,-6.34957,-1.50911,-0.47339,-0.47152,0.64108,-0.37761,0.76012,-0.13647,-0.02043, 1403636669963555584,9.34002,-6.35148,-1.50951,-0.45297,-0.46386,0.65401,-0.38976,0.70103,-0.04457,0.05201, 1403636670063555584,9.40680,-6.34596,-1.50300,-0.44125,-0.46211,0.66141,-0.39278,0.62227,0.04645,0.09739, 1403636670163555328,9.45851,-6.33279,-1.48843,-0.43391,-0.46082,0.66308,-0.39961,0.51297,0.11477,0.16204, 1403636670263555584,9.50210,-6.31253,-1.47310,-0.42833,-0.45387,0.66077,-0.41704,0.39618,0.20306,0.12825, 1403636670363555328,9.53509,-6.28224,-1.46409,-0.42113,-0.45149,0.65914,-0.42935,0.26291,0.30348,0.04511, 1403636670463555584,9.55161,-6.24111,-1.45996,-0.40982,-0.45645,0.65727,-0.43783,0.09933,0.40162,0.04615, 1403636670563555584,9.55165,-6.19096,-1.45145,-0.39816,-0.46606,0.65152,-0.44696,-0.08093,0.50627,0.10909, 1403636670663555328,9.53532,-6.13294,-1.43636,-0.38822,-0.47971,0.64183,-0.45521,-0.28293,0.58768,0.20787, 1403636670763555584,9.50003,-6.06759,-1.41450,-0.37899,-0.49454,0.62921,-0.46463,-0.46968,0.65584,0.23208, 1403636670863555328,9.44789,-5.99657,-1.39342,-0.36433,-0.50680,0.61680,-0.47956,-0.63581,0.72683,0.19401, 1403636670963555584,9.37739,-5.91955,-1.37368,-0.34672,-0.51693,0.60222,-0.49990,-0.83807,0.79917,0.21715, 1403636671063555584,9.28369,-5.83421,-1.34583,-0.33329,-0.53328,0.58118,-0.51650,-1.09179,0.87406,0.32766, 1403636671163555328,9.16361,-5.74125,-1.30952,0.32508,0.55783,-0.55727,0.52211,-1.33165,0.93602,0.39898, 1403636671263555584,9.01936,-5.64292,-1.26743,0.31755,0.58341,-0.53759,0.51942,-1.55432,0.99405,0.43198, 1403636671363555328,8.85543,-5.54101,-1.22478,0.32004,0.61566,-0.51816,0.50004,-1.70623,1.00579,0.43926, 1403636671463555584,8.68190,-5.43950,-1.18449,0.32309,0.64828,-0.49696,0.47788,-1.74204,0.99960,0.38370, 1403636671563555584,8.50734,-5.33913,-1.15026,0.31480,0.67200,-0.48406,0.46368,-1.72771,0.96939,0.31204, 1403636671663555328,8.33468,-5.24274,-1.12223,0.30392,0.68847,-0.47828,0.45264,-1.70335,0.90575,0.25955, 1403636671763555584,8.16530,-5.15428,-1.09979,0.29568,0.69828,-0.47589,0.44554,-1.66133,0.82410,0.20038, 1403636671863555328,8.00264,-5.07538,-1.08062,0.29110,0.70599,-0.47516,0.43711,-1.59138,0.71479,0.19034, 1403636671963555584,7.84838,-5.00996,-1.05820,0.28730,0.71326,-0.47768,0.42491,-1.50379,0.57166,0.28345, 1403636672063555584,7.70681,-4.95976,-1.02699,0.29086,0.71998,-0.47658,0.41219,-1.38921,0.41418,0.36342, 1403636672163555328,7.58162,-4.92732,-0.98679,0.30384,0.72280,-0.46976,0.40568,-1.23979,0.28328,0.44667, 1403636672263555584,7.47557,-4.90780,-0.94218,0.31220,0.72278,-0.46707,0.40245,-1.07374,0.15002,0.43400, 1403636672363555328,7.38969,-4.90061,-0.89973,0.32487,0.72169,-0.46054,0.40191,-0.90570,0.04832,0.37675, 1403636672463555584,7.31891,-4.90028,-0.86738,0.34193,0.71690,-0.44942,0.40885,-0.73109,-0.01315,0.25969, 1403636672563555584,7.26209,-4.90379,-0.84803,0.34858,0.70000,-0.44646,0.43493,-0.58244,-0.02843,0.08705, 1403636672663555328,7.21453,-4.90871,-0.84798,0.34653,0.68465,-0.44878,0.45801,-0.49164,-0.02440,-0.10969, 1403636672763555584,7.17091,-4.91026,-0.86945,0.33605,0.68130,-0.45658,0.46307,-0.43797,0.00518,-0.30185, 1403636672863555328,7.13356,-4.91063,-0.90029,0.32554,0.68648,-0.45835,0.46118,-0.37635,0.00894,-0.26972, 1403636672963555584,7.09927,-4.90963,-0.92353,0.31886,0.69371,-0.44960,0.46362,-0.31844,0.01421,-0.19778, 1403636673063555584,7.07584,-4.91191,-0.93803,0.31247,0.70365,-0.43652,0.46550,-0.24966,-0.00584,-0.12276, 1403636673163555328,7.05668,-4.91500,-0.94388,0.30215,0.71504,-0.42745,0.46337,-0.19025,-0.02931,-0.03338, 1403636673263555584,7.04320,-4.91951,-0.94192,0.29029,0.72869,-0.42284,0.45382,-0.11783,-0.06197,0.04333, 1403636673363555328,7.03759,-4.92620,-0.93335,0.28154,0.73743,-0.42041,0.44742,-0.02159,-0.12104,0.10302, 1403636673463555584,7.04042,-4.94178,-0.92736,0.27852,0.73972,-0.41652,0.44918,0.06414,-0.18471,0.06269, 1403636673563555584,7.05500,-4.96207,-0.92661,0.27926,0.73900,-0.41241,0.45367,0.15475,-0.23879,0.02099, 1403636673663555328,7.07786,-4.98800,-0.92769,0.28512,0.73349,-0.40587,0.46472,0.24028,-0.27513,-0.01864, 1403636673763555584,7.10951,-5.01536,-0.93277,0.29209,0.72660,-0.39923,0.47682,0.29722,-0.25365,-0.02315, 1403636673863555328,7.14285,-5.04128,-0.93610,0.29536,0.72221,-0.39520,0.48476,0.34245,-0.23816,-0.01355, 1403636673963555584,7.18134,-5.06693,-0.93511,0.29643,0.72348,-0.39383,0.48332,0.40443,-0.22983,0.03198, 1403636674063555584,7.22584,-5.09160,-0.93519,0.29459,0.72380,-0.39530,0.48277,0.46439,-0.22741,-0.02630, 1403636674163555328,7.27307,-5.11374,-0.94021,0.29281,0.72261,-0.39657,0.48458,0.49565,-0.19928,-0.08120, 1403636674263555584,7.32508,-5.13429,-0.95120,0.29033,0.71845,-0.39892,0.49031,0.53050,-0.17328,-0.15734, 1403636674363555328,7.38000,-5.15241,-0.96921,0.28016,0.71383,-0.40552,0.49750,0.55031,-0.15293,-0.22515, 1403636674463555584,7.43652,-5.16751,-0.99485,0.26722,0.71183,-0.40684,0.50633,0.54890,-0.14962,-0.28748, 1403636674563555584,7.49184,-5.18476,-1.02525,0.25751,0.71351,-0.40103,0.51358,0.51589,-0.15626,-0.33299, 1403636674663555328,7.54346,-5.20291,-1.05989,0.25398,0.71677,-0.39221,0.51760,0.48570,-0.16929,-0.37083, 1403636674763555584,7.59330,-5.22271,-1.09828,0.24631,0.72280,-0.38860,0.51564,0.44592,-0.17744,-0.42823, 1403636674863555328,7.63820,-5.24278,-1.13574,0.23584,0.73292,-0.38927,0.50564,0.41486,-0.20182,-0.36196, 1403636674963555584,7.68017,-5.26817,-1.16663,0.22848,0.74137,-0.39001,0.49605,0.40474,-0.27631,-0.27287, 1403636675063555584,7.72335,-5.30217,-1.18747,0.22433,0.74771,-0.39053,0.48794,0.41295,-0.36046,-0.14404, 1403636675163555328,7.77027,-5.34593,-1.19949,0.23110,0.75388,-0.38523,0.47944,0.45227,-0.45971,-0.04562, 1403636675263555584,7.82226,-5.39812,-1.20394,0.24404,0.75774,-0.37608,0.47417,0.54125,-0.56308,0.01381, 1403636675363555328,7.88219,-5.45458,-1.20075,0.25437,0.75700,-0.36871,0.47571,0.62186,-0.58822,0.09040, 1403636675463555584,7.95299,-5.51506,-1.19780,0.26036,0.75188,-0.35838,0.48832,0.71344,-0.59731,0.04760, 1403636675563555584,8.02940,-5.57359,-1.19925,0.26456,0.75022,-0.34418,0.49872,0.77053,-0.57438,-0.02257, 1403636675663555328,8.11001,-5.62847,-1.20732,0.26531,0.75012,-0.33148,0.50700,0.81596,-0.52695,-0.08775, 1403636675763555584,8.19457,-5.67693,-1.22092,0.25816,0.74928,-0.32034,0.51896,0.85530,-0.45393,-0.14721, 1403636675863555328,8.28193,-5.71906,-1.24084,0.23468,0.74652,-0.31679,0.53599,0.86187,-0.39411,-0.22378, 1403636675963555584,8.36861,-5.75683,-1.26732,0.20491,0.74930,-0.31440,0.54563,0.83186,-0.36044,-0.27872, 1403636676063555584,8.45131,-5.79364,-1.29712,0.18538,0.75762,-0.30310,0.54752,0.78021,-0.36991,-0.31297, 1403636676163555328,8.52974,-5.83132,-1.32760,0.18129,0.76978,-0.28048,0.54398,0.75091,-0.38283,-0.32476, 1403636676263555584,8.60529,-5.86823,-1.35851,0.18370,0.78250,-0.25116,0.53932,0.74353,-0.37332,-0.34933, 1403636676363555328,8.68101,-5.90183,-1.39018,0.18223,0.79343,-0.22188,0.53668,0.77737,-0.34156,-0.30086, 1403636676463555584,8.76132,-5.93052,-1.41541,0.17488,0.80167,-0.19701,0.53659,0.80679,-0.25917,-0.21974, 1403636676563555584,8.84353,-5.95200,-1.43296,0.15493,0.80590,-0.18270,0.54143,0.86430,-0.21015,-0.14922, 1403636676663555328,8.93197,-5.97112,-1.44182,0.13573,0.80686,-0.17382,0.54803,0.89831,-0.19275,-0.05659, 1403636676763555584,9.02262,-5.98768,-1.44476,0.12076,0.80571,-0.17007,0.55437,0.90537,-0.17484,0.00452, 1403636676863555328,9.11339,-6.00330,-1.44145,0.10644,0.80602,-0.16837,0.55736,0.88517,-0.16557,0.04994, 1403636676963555584,9.19948,-6.01821,-1.43796,0.09907,0.80538,-0.16601,0.56035,0.85479,-0.17722,-0.01257, 1403636677063555584,9.28327,-6.03473,-1.44714,0.10326,0.80573,-0.15995,0.56085,0.84087,-0.19153,-0.15962, 1403636677163555328,9.36443,-6.05022,-1.46968,0.11192,0.80649,-0.15347,0.55991,0.81735,-0.17333,-0.29888, 1403636677263555584,9.44920,-6.06752,-1.49967,0.11778,0.80426,-0.14859,0.56322,0.80551,-0.14067,-0.30281, 1403636677363555328,9.53336,-6.07948,-1.52447,0.11794,0.79979,-0.14080,0.57149,0.81554,-0.09283,-0.23591, 1403636677463555584,9.61717,-6.08860,-1.54017,0.11338,0.79381,-0.13032,0.58312,0.78707,-0.04598,-0.14108, 1403636677563555584,9.69756,-6.09193,-1.54764,0.10186,0.79068,-0.11987,0.59167,0.72411,0.01681,-0.06910, 1403636677663555328,9.77054,-6.09056,-1.54908,0.08489,0.79235,-0.10953,0.59413,0.66899,0.05413,-0.01946, 1403636677763555584,9.83849,-6.08636,-1.54612,0.06465,0.79691,-0.09774,0.59263,0.59886,0.07193,0.04351, 1403636677863555328,9.89506,-6.07914,-1.54061,0.03762,0.80537,-0.08678,0.58518,0.52357,0.06839,0.05661, 1403636677963555584,9.94663,-6.07439,-1.53212,0.01340,0.81287,-0.07143,0.57790,0.49292,0.02790,0.09497, 1403636678063555584,9.99480,-6.07403,-1.52262,-0.00131,0.81715,-0.05062,0.57419,0.47117,-0.03791,0.06832, 1403636678163555328,10.04530,-6.07867,-1.51841,-0.00382,0.81754,-0.02840,0.57515,0.46178,-0.08277,0.00754, 1403636678263555584,10.09340,-6.08536,-1.52000,-0.00297,0.81805,-0.00883,0.57507,0.44312,-0.10139,-0.02060, 1403636678363555328,10.13687,-6.09197,-1.51898,-0.01124,0.81743,0.00587,0.57589,0.43010,-0.08712,0.02736, 1403636678463555584,10.17777,-6.09824,-1.51280,-0.02165,0.81818,0.02038,0.57420,0.42857,-0.08393,0.09612, 1403636678563555584,10.21794,-6.10443,-1.50018,-0.03235,0.81812,0.03624,0.57299,0.41636,-0.07260,0.16224, 1403636678663555328,10.25645,-6.11058,-1.48406,-0.04266,0.81625,0.05769,0.57323,0.39079,-0.07474,0.14898, 1403636678763555584,10.29382,-6.11637,-1.47174,-0.04572,0.81339,0.08730,0.57331,0.37571,-0.05100,0.10079, 1403636678863555328,10.33111,-6.11937,-1.46661,-0.04612,0.81051,0.11787,0.57188,0.35966,-0.01425,0.01585, 1403636678963555584,10.36737,-6.11631,-1.46617,-0.04882,0.80746,0.14258,0.57035,0.32930,0.05697,-0.04269, 1403636679063555584,10.40025,-6.10566,-1.47126,-0.06282,0.80335,0.15911,0.57040,0.29586,0.13901,-0.08195, 1403636679163555328,10.43045,-6.08839,-1.48177,-0.08492,0.79820,0.17093,0.57135,0.25861,0.20421,-0.13359, 1403636679263555584,10.45561,-6.06558,-1.49385,-0.11459,0.79149,0.18020,0.57266,0.21474,0.23437,-0.11881, 1403636679363555328,10.47779,-6.04103,-1.50101,-0.13950,0.78553,0.19403,0.57082,0.18402,0.23373,-0.04540, 1403636679463555584,10.49576,-6.01756,-1.50057,-0.15817,0.78054,0.21180,0.56646,0.15241,0.21517,0.04632, 1403636679563555584,10.51052,-5.99717,-1.49401,-0.17128,0.77673,0.22949,0.56097,0.13448,0.18807,0.09612, 1403636679663555328,10.52245,-5.97881,-1.48238,-0.18077,0.77508,0.24198,0.55499,0.09458,0.14302,0.15443, 1403636679763555584,10.53067,-5.96522,-1.46415,-0.18405,0.77301,0.25241,0.55214,0.06848,0.10260,0.23239, 1403636679863555328,10.53811,-5.95310,-1.43635,-0.18607,0.76676,0.25797,0.55760,0.07653,0.10689,0.32218, 1403636679963555584,10.54589,-5.94024,-1.40154,-0.18166,0.76108,0.26414,0.56390,0.05285,0.10073,0.38105, 1403636680063555584,10.54734,-5.93003,-1.36115,-0.16918,0.75750,0.27237,0.56867,-0.03456,0.08449,0.42081, 1403636680163555328,10.54053,-5.91991,-1.32123,-0.15766,0.75450,0.27861,0.57293,-0.11583,0.10629,0.36356, 1403636680263555584,10.52505,-5.90717,-1.28757,-0.15862,0.75325,0.27646,0.57534,-0.19969,0.13712,0.31459, 1403636680363555328,10.50141,-5.89174,-1.25921,-0.14869,0.75310,0.28004,0.57646,-0.28482,0.16408,0.23198, 1403636680463555584,10.46908,-5.87146,-1.23801,-0.13904,0.75616,0.28296,0.57343,-0.37280,0.22078,0.18147, 1403636680563555584,10.42727,-5.84560,-1.22200,-0.13567,0.75961,0.28166,0.57031,-0.46295,0.29592,0.12460, 1403636680663555328,10.37824,-5.81192,-1.20947,-0.13896,0.76190,0.27645,0.56901,-0.53364,0.36383,0.08249, 1403636680763555584,10.32204,-5.77267,-1.20235,-0.14830,0.76736,0.26782,0.56342,-0.59896,0.42216,0.03826, 1403636680863555328,10.26235,-5.72818,-1.19757,-0.16449,0.77391,0.25504,0.55585,-0.62932,0.44565,0.00234, 1403636680963555584,10.20031,-5.68324,-1.19958,-0.17698,0.78404,0.24516,0.54209,-0.62725,0.43930,-0.06399, 1403636681063555584,10.14020,-5.63828,-1.20868,-0.17526,0.79342,0.24536,0.52875,-0.58762,0.43922,-0.11934, 1403636681163555328,10.08563,-5.59221,-1.21908,-0.17032,0.79636,0.24841,0.52450,-0.51804,0.45121,-0.11037, 1403636681263555584,10.03791,-5.54328,-1.22806,-0.16526,0.78715,0.25276,0.53778,-0.47112,0.48460,-0.07896, 1403636681363555328,9.99298,-5.49189,-1.23351,-0.16609,0.77910,0.25274,0.54913,-0.46428,0.50307,-0.01686, 1403636681463555584,9.94611,-5.44095,-1.23447,-0.17124,0.77462,0.25038,0.55494,-0.47950,0.49617,0.03692, 1403636681563555584,9.89838,-5.38944,-1.22939,-0.17364,0.77315,0.25044,0.55620,-0.49288,0.49568,0.05483, 1403636681663555328,9.84886,-5.33852,-1.22553,-0.17114,0.77221,0.25365,0.55684,-0.50894,0.49195,0.00910, 1403636681763555584,9.79801,-5.28709,-1.22553,-0.17019,0.77101,0.25597,0.55772,-0.52623,0.50216,-0.02758, 1403636681863555328,9.74577,-5.23598,-1.22729,-0.17128,0.76996,0.25718,0.55829,-0.54764,0.50545,-0.01125, 1403636681963555584,9.69136,-5.18208,-1.22363,-0.17372,0.76928,0.25728,0.55842,-0.56561,0.51804,0.05774, 1403636682063555584,9.63516,-5.13133,-1.21423,-0.17726,0.76966,0.25666,0.55707,-0.59373,0.51025,0.14385, 1403636682163555328,9.57572,-5.08041,-1.19712,-0.17972,0.76988,0.25618,0.55619,-0.61748,0.49097,0.18070, 1403636682263555584,9.51388,-5.03146,-1.18242,-0.18082,0.77409,0.25579,0.55014,-0.62406,0.47631,0.11818, 1403636682363555328,9.45425,-4.98257,-1.17453,-0.17911,0.78110,0.25650,0.54038,-0.60975,0.47711,0.05759, 1403636682463555584,9.39755,-4.93336,-1.16979,-0.17859,0.78225,0.25564,0.53929,-0.57118,0.47698,0.03638, 1403636682563555584,9.34208,-4.88363,-1.16854,-0.17592,0.77741,0.25513,0.54734,-0.55772,0.47956,-0.00322, 1403636682663555328,9.28527,-4.83422,-1.17030,-0.17402,0.77320,0.25443,0.55421,-0.57049,0.48145,-0.03832, 1403636682763555584,9.22786,-4.78305,-1.17109,-0.17602,0.77142,0.25078,0.55771,-0.57093,0.48198,0.02131, 1403636682863555328,9.16996,-4.73561,-1.16652,-0.17467,0.76936,0.24968,0.56145,-0.58809,0.47108,0.06695, 1403636682963555584,9.10874,-4.68720,-1.15666,-0.16896,0.76893,0.25128,0.56307,-0.62765,0.47041,0.13015, 1403636683063555584,9.04483,-4.64048,-1.14367,-0.16601,0.76802,0.25282,0.56451,-0.64791,0.47356,0.10690, 1403636683163555328,8.97879,-4.59251,-1.13662,-0.16797,0.76928,0.25209,0.56253,-0.67926,0.48370,0.03058, 1403636683263555584,8.91004,-4.54491,-1.13582,-0.17105,0.77098,0.25117,0.55969,-0.69961,0.48488,-0.01600, 1403636683363555328,8.83903,-4.49575,-1.14058,-0.17438,0.77168,0.25073,0.55789,-0.71306,0.47628,-0.06754, 1403636683463555584,8.76870,-4.44825,-1.14962,-0.17698,0.77211,0.25109,0.55630,-0.71852,0.47223,-0.10587, 1403636683563555584,8.69799,-4.40057,-1.16177,-0.17786,0.77190,0.25190,0.55596,-0.71920,0.45926,-0.13236, 1403636683663555328,8.62653,-4.35564,-1.17688,-0.17839,0.77393,0.25299,0.55245,-0.72209,0.44786,-0.15857, 1403636683763555584,8.55549,-4.31296,-1.19241,-0.17956,0.77695,0.25352,0.54757,-0.71419,0.44272,-0.11849, 1403636683863555328,8.48398,-4.27010,-1.19931,-0.18109,0.78010,0.25436,0.54218,-0.69924,0.43612,-0.02752, 1403636683963555584,8.41480,-4.22786,-1.19928,-0.18088,0.77972,0.25625,0.54190,-0.66472,0.44249,0.04571, 1403636684063555584,8.34656,-4.18293,-1.18998,-0.18299,0.77716,0.25681,0.54460,-0.63348,0.44717,0.14252, 1403636684163555328,8.27823,-4.14019,-1.17686,-0.18445,0.77505,0.25828,0.54642,-0.63645,0.43561,0.20023, 1403636684263555584,8.20653,-4.09697,-1.15581,-0.18724,0.77299,0.25864,0.54821,-0.64892,0.41897,0.23949, 1403636684363555328,8.13579,-4.05462,-1.13518,-0.18851,0.77018,0.25959,0.55127,-0.66330,0.40088,0.18873, 1403636684463555584,8.06599,-4.01532,-1.12110,-0.19059,0.76897,0.25959,0.55224,-0.67244,0.38349,0.11431, 1403636684563555584,7.99659,-3.97973,-1.11428,-0.19544,0.76825,0.25757,0.55250,-0.67343,0.35123,0.05798, 1403636684663555328,7.92995,-3.94659,-1.11194,-0.19840,0.76782,0.25652,0.55253,-0.67185,0.31821,0.00238, 1403636684763555584,7.86190,-3.91729,-1.11757,-0.19895,0.76686,0.25661,0.55362,-0.67572,0.28071,-0.06516, 1403636684863555328,7.79725,-3.89209,-1.12661,-0.19812,0.76688,0.25740,0.55353,-0.68209,0.23536,-0.12732, 1403636684963555584,7.72989,-3.86920,-1.14181,-0.19018,0.76583,0.26260,0.55532,-0.69274,0.19752,-0.10883, 1403636685063555584,7.66206,-3.84758,-1.15142,-0.18393,0.76634,0.26559,0.55529,-0.68693,0.20436,-0.04209, 1403636685163555328,7.59313,-3.82818,-1.15420,-0.18209,0.76924,0.26508,0.55212,-0.70979,0.18807,0.01932, 1403636685263555584,7.52269,-3.80925,-1.15118,-0.17532,0.77168,0.26682,0.55008,-0.72292,0.19120,0.07072, 1403636685363555328,7.44946,-3.78668,-1.14103,-0.17039,0.77296,0.26719,0.54965,-0.72530,0.21385,0.11328, 1403636685463555584,7.37518,-3.76365,-1.13418,-0.17093,0.77412,0.26401,0.54938,-0.73464,0.24066,0.06640, 1403636685563555584,7.30167,-3.73973,-1.13128,-0.17249,0.77545,0.26092,0.54849,-0.73772,0.25492,-0.00648, 1403636685663555328,7.22807,-3.71456,-1.13413,-0.17315,0.77543,0.25892,0.54926,-0.73556,0.25750,-0.05715, 1403636685763555584,7.15424,-3.68551,-1.14229,-0.17151,0.77445,0.25938,0.55094,-0.73793,0.27768,-0.10149, 1403636685863555328,7.07980,-3.65971,-1.15423,-0.16810,0.77223,0.26150,0.55409,-0.74386,0.29382,-0.14483, 1403636685963555584,7.00104,-3.63293,-1.17201,-0.17160,0.77353,0.25950,0.55214,-0.76586,0.29410,-0.17833, 1403636686063555584,6.92182,-3.60791,-1.18831,-0.17628,0.77679,0.25699,0.54725,-0.77786,0.29610,-0.11209, 1403636686163555328,6.84466,-3.58083,-1.19694,-0.17853,0.77796,0.25537,0.54562,-0.75189,0.29617,-0.02621, 1403636686263555584,6.76840,-3.55193,-1.19886,-0.17978,0.77958,0.25395,0.54354,-0.72957,0.29676,0.02883, 1403636686363555328,6.69495,-3.52458,-1.19462,-0.18078,0.78102,0.25300,0.54159,-0.70341,0.29487,0.09303, 1403636686463555584,6.62113,-3.49444,-1.18831,-0.18015,0.78252,0.25307,0.53960,-0.68486,0.29031,0.05643, 1403636686563555584,6.55363,-3.46719,-1.18717,-0.18020,0.78332,0.25322,0.53834,-0.65455,0.28831,0.00193, 1403636686663555328,6.48471,-3.43888,-1.19407,-0.17887,0.78295,0.25505,0.53846,-0.63326,0.28783,-0.07010, 1403636686763555584,6.42382,-3.41155,-1.20149,-0.18016,0.78172,0.25498,0.53986,-0.60435,0.30001,-0.06682, 1403636686863555328,6.36152,-3.38108,-1.20562,-0.18184,0.77992,0.25449,0.54212,-0.58686,0.29909,-0.00567, 1403636686963555584,6.30568,-3.35162,-1.20205,-0.18403,0.77873,0.25369,0.54347,-0.56792,0.29268,0.07312, 1403636687063555584,6.24865,-3.32147,-1.19290,-0.18568,0.77806,0.25277,0.54429,-0.53839,0.28875,0.14051, 1403636687163555328,6.19456,-3.29371,-1.18012,-0.18620,0.77769,0.25322,0.54443,-0.53474,0.27252,0.15079, 1403636687263555584,6.13943,-3.26594,-1.17118,-0.18608,0.77642,0.25354,0.54614,-0.52671,0.26688,0.08907, 1403636687363555328,6.08946,-3.24352,-1.16668,-0.18596,0.77467,0.25425,0.54833,-0.51251,0.24469,0.03104, 1403636687463555584,6.03858,-3.21845,-1.17018,-0.18560,0.77312,0.25510,0.55023,-0.50525,0.24212,-0.03381, 1403636687563555584,5.99124,-3.19878,-1.17753,-0.18736,0.77297,0.25457,0.55010,-0.50299,0.22390,-0.10278, 1403636687663555328,5.94347,-3.17865,-1.19585,-0.18967,0.77225,0.25385,0.55065,-0.50481,0.19816,-0.18708, 1403636687763555584,5.89504,-3.16469,-1.21777,-0.19209,0.77165,0.25328,0.55092,-0.50011,0.17880,-0.24389, 1403636687863555328,5.84812,-3.14992,-1.24722,-0.19151,0.77169,0.25415,0.55066,-0.48928,0.15179,-0.29411, 1403636687963555584,5.80007,-3.13726,-1.27903,-0.18832,0.77137,0.25686,0.55096,-0.48904,0.13404,-0.34401, 1403636688063555584,5.75392,-3.12523,-1.31458,-0.18362,0.77028,0.26050,0.55236,-0.49132,0.13003,-0.34366, 1403636688163555328,5.70622,-3.11459,-1.34660,-0.18064,0.76998,0.26271,0.55271,-0.50231,0.13018,-0.27930, 1403636688263555584,5.65920,-3.10507,-1.37024,-0.18208,0.76981,0.26213,0.55275,-0.51197,0.13815,-0.22001, 1403636688363555328,5.60938,-3.09211,-1.39111,-0.18620,0.77124,0.25958,0.55058,-0.52210,0.13810,-0.18974, 1403636688463555584,5.55839,-3.07994,-1.41239,-0.18491,0.77175,0.26050,0.54986,-0.51928,0.13334,-0.23233, 1403636688563555584,5.50790,-3.06768,-1.43799,-0.17888,0.77118,0.26492,0.55054,-0.51334,0.13075,-0.26348, 1403636688663555328,5.45812,-3.05411,-1.46703,-0.17270,0.77025,0.26989,0.55141,-0.52140,0.15396,-0.30740, 1403636688763555584,5.40599,-3.03762,-1.50046,-0.17130,0.76972,0.27260,0.55125,-0.53946,0.17730,-0.31983, 1403636688863555328,5.35138,-3.01886,-1.52924,-0.17421,0.77000,0.27440,0.54905,-0.55305,0.22142,-0.26134, 1403636688963555584,5.29570,-2.99547,-1.55308,-0.18117,0.77074,0.27731,0.54428,-0.56336,0.24255,-0.17585, 1403636689063555584,5.23987,-2.97038,-1.56841,-0.18870,0.77130,0.27972,0.53967,-0.55340,0.26833,-0.09297, 1403636689163555328,5.18529,-2.94465,-1.57468,-0.19629,0.77223,0.28201,0.53442,-0.53801,0.28502,-0.01912, 1403636689263555584,5.13105,-2.91875,-1.57327,-0.20089,0.77237,0.28524,0.53078,-0.52132,0.27528,0.06289, 1403636689363555328,5.08151,-2.88992,-1.56580,-0.20401,0.77197,0.28812,0.52860,-0.47017,0.28886,0.14749, 1403636689463555584,5.03634,-2.85900,-1.55036,-0.20634,0.77081,0.29025,0.52823,-0.44670,0.28960,0.22248, 1403636689563555584,4.99474,-2.82869,-1.52724,-0.20808,0.77181,0.29146,0.52541,-0.43616,0.27733,0.27187, 1403636689663555328,4.95498,-2.79884,-1.49976,-0.21033,0.77527,0.29071,0.51981,-0.40298,0.28340,0.27958, 1403636689763555584,4.92077,-2.76701,-1.47668,-0.21517,0.78147,0.28651,0.51079,-0.33837,0.29780,0.22066, 1403636689863555328,4.89426,-2.73421,-1.45718,-0.22035,0.78414,0.28150,0.50728,-0.24942,0.31239,0.18847, 1403636689963555584,4.87626,-2.70041,-1.44173,-0.22849,0.78403,0.27355,0.50819,-0.15548,0.30153,0.14950, 1403636690063555584,4.86807,-2.66852,-1.43083,-0.22686,0.78317,0.27266,0.51072,-0.06336,0.27542,0.09697, 1403636690163555328,4.86763,-2.63908,-1.42386,-0.21610,0.78317,0.27850,0.51223,0.02182,0.26941,0.08193, 1403636690263555584,4.87242,-2.61111,-1.41170,-0.20932,0.78256,0.28197,0.51408,0.08480,0.27874,0.15409, 1403636690363555328,4.88372,-2.58138,-1.39290,-0.20521,0.78147,0.28384,0.51636,0.14779,0.29670,0.20290, 1403636690463555584,4.90070,-2.54904,-1.37258,-0.20547,0.78088,0.28274,0.51774,0.19137,0.31396,0.20340, 1403636690563555584,4.92116,-2.51715,-1.35464,-0.21221,0.78031,0.27774,0.51859,0.23462,0.31553,0.14440, 1403636690663555328,4.94764,-2.48347,-1.34394,-0.22189,0.78163,0.26982,0.51674,0.29219,0.30282,0.09267, 1403636690763555584,4.97986,-2.45186,-1.34307,-0.23842,0.78087,0.25709,0.51700,0.34142,0.27028,-0.06486, 1403636690863555328,5.01793,-2.42667,-1.35690,-0.25081,0.77770,0.24763,0.52053,0.40691,0.18954,-0.18568, 1403636690963555584,5.06189,-2.41057,-1.37479,-0.26102,0.76983,0.24003,0.53068,0.48071,0.08153,-0.15528, 1403636691063555584,5.11238,-2.40888,-1.38600,-0.26864,0.75899,0.23499,0.54457,0.53103,-0.06763,-0.07776, 1403636691163555328,5.16531,-2.42528,-1.38853,-0.26762,0.75030,0.23732,0.55597,0.54676,-0.24968,0.03228, 1403636691263555584,5.21802,-2.45916,-1.38162,-0.25309,0.74185,0.24943,0.56868,0.49910,-0.42894,0.09886, 1403636691363555328,5.26408,-2.50860,-1.36883,-0.23718,0.73902,0.26286,0.57315,0.43898,-0.54216,0.16133, 1403636691463555584,5.30782,-2.56397,-1.35171,-0.22896,0.74727,0.27239,0.56122,0.40023,-0.60282,0.21791, 1403636691563555584,5.34945,-2.62457,-1.32581,-0.22722,0.75459,0.27707,0.54972,0.38629,-0.63099,0.32548, 1403636691663555328,5.39097,-2.68872,-1.29247,-0.22027,0.75753,0.28459,0.54464,0.39330,-0.66818,0.37384, 1403636691763555584,5.43258,-2.75506,-1.25960,-0.21005,0.75770,0.29344,0.54375,0.37460,-0.66759,0.32340, 1403636691863555328,5.46962,-2.82209,-1.23326,-0.21001,0.75882,0.29455,0.54160,0.34426,-0.66808,0.23520, 1403636691963555584,5.50459,-2.88754,-1.21377,-0.21590,0.75712,0.29080,0.54368,0.33236,-0.65994,0.20235, 1403636692063555584,5.53638,-2.95324,-1.19715,-0.22054,0.75513,0.28734,0.54643,0.30813,-0.67657,0.17709, 1403636692163555328,5.56594,-3.01916,-1.17854,-0.21375,0.75488,0.29111,0.54747,0.30426,-0.66611,0.22127, 1403636692263555584,5.59271,-3.08455,-1.15335,-0.20891,0.75614,0.29359,0.54627,0.26028,-0.65901,0.29566, 1403636692363555328,5.61346,-3.15066,-1.11939,-0.21087,0.75793,0.29147,0.54417,0.20704,-0.67408,0.36840, 1403636692463555584,5.63365,-3.21632,-1.07923,-0.20727,0.75914,0.29291,0.54309,0.19438,-0.66347,0.44175, 1403636692563555584,5.60555,-3.29325,-1.01180,-0.20488,0.75744,0.29802,0.54360,0.17521,-0.63459,0.49436, 1403636692663555328,5.62090,-3.35830,-0.95913,-0.20601,0.75835,0.29607,0.54296,0.15525,-0.62695,0.54158, 1403636692763555584,5.63449,-3.42120,-0.90734,-0.19534,0.76021,0.30187,0.54111,0.13934,-0.62098,0.48754, 1403636692863555328,5.64584,-3.47770,-0.86032,-0.17978,0.76123,0.31097,0.53991,0.10947,-0.55165,0.43399, 1403636692963555584,5.65269,-3.52895,-0.81598,-0.17463,0.76233,0.31412,0.53823,0.06977,-0.45868,0.39750, 1403636693063555584,5.65726,-3.57235,-0.77478,-0.18104,0.76395,0.30993,0.53623,0.04704,-0.37979,0.36159, 1403636693163555328,5.66109,-3.60936,-0.74172,-0.18883,0.76317,0.30564,0.53712,0.02590,-0.32498,0.28699, 1403636693263555584,5.66334,-3.64201,-0.71561,-0.19242,0.76666,0.30418,0.53168,0.01606,-0.28153,0.21857, 1403636693363555328,5.66397,-3.67159,-0.69273,-0.19674,0.77237,0.30225,0.52286,0.02080,-0.24055,0.17513, 1403636693463555584,5.66821,-3.69679,-0.67602,-0.19985,0.77726,0.30154,0.51477,0.04793,-0.20539,0.13815, 1403636693563555584,5.67506,-3.71463,-0.66312,-0.19959,0.78459,0.30168,0.50355,0.10016,-0.16127,0.10381, 1403636693663555328,5.68899,-3.72995,-0.65302,-0.19838,0.78820,0.30330,0.49739,0.16946,-0.10295,0.09426, 1403636693763555584,5.70742,-3.73792,-0.64178,-0.20065,0.78430,0.30296,0.50282,0.23739,-0.04536,0.09306, 1403636693863555328,5.73297,-3.74007,-0.63462,-0.20353,0.77723,0.30164,0.51333,0.28455,0.00589,0.06267, 1403636693963555584,5.76244,-3.73895,-0.62912,-0.20848,0.76818,0.29896,0.52637,0.31352,0.04085,0.03690, 1403636694063555584,5.79178,-3.73481,-0.62486,-0.21325,0.76014,0.29647,0.53741,0.31216,0.03967,-0.00645, 1403636694163555328,5.82220,-3.73244,-0.62698,-0.21438,0.75703,0.29502,0.54214,0.29593,0.02959,0.02501, 1403636694263555584,5.84873,-3.73063,-0.61999,-0.21410,0.75605,0.29587,0.54315,0.27157,0.02722,0.06859, 1403636694363555328,5.87543,-3.73027,-0.61001,-0.21429,0.75870,0.29616,0.53921,0.25152,0.00595,0.11189, 1403636694463555584,5.90028,-3.73122,-0.59672,-0.21255,0.76163,0.29771,0.53490,0.25408,0.00806,0.09832, 1403636694563555584,5.92539,-3.73024,-0.58871,-0.21078,0.76296,0.29866,0.53317,0.25240,0.01801,0.04938, 1403636694663555328,5.94980,-3.72889,-0.58660,-0.20934,0.76356,0.29873,0.53283,0.25278,0.02492,-0.01244, 1403636694763555584,5.97235,-3.72725,-0.58937,-0.21101,0.76209,0.29657,0.53547,0.24162,0.02497,-0.05844, 1403636694863555328,5.99513,-3.72582,-0.59838,-0.21612,0.76261,0.29175,0.53535,0.22530,0.02398,-0.13152, 1403636694963555584,6.01833,-3.72595,-0.61034,-0.22308,0.76501,0.28538,0.53250,0.23374,-0.00232,-0.15115, 1403636695063555584,6.04332,-3.72866,-0.62298,-0.22206,0.76662,0.28481,0.53090,0.25617,-0.03103,-0.11311, 1403636695163555328,6.07010,-3.73435,-0.62999,-0.21712,0.76629,0.28642,0.53256,0.26799,-0.05430,-0.05329, 1403636695263555584,6.09792,-3.74008,-0.63126,-0.21122,0.76502,0.28846,0.53565,0.27099,-0.06322,0.03785, 1403636695363555328,6.12320,-3.74644,-0.62600,-0.20741,0.76415,0.28896,0.53811,0.25372,-0.07774,0.10015, 1403636695463555584,6.14924,-3.75308,-0.61535,-0.20551,0.76360,0.28890,0.53965,0.24831,-0.07847,0.12198, 1403636695563555584,6.17354,-3.75838,-0.60561,-0.20196,0.76229,0.29005,0.54221,0.22858,-0.06523,0.08689, 1403636695663555328,6.19677,-3.76310,-0.59991,-0.19727,0.76110,0.29182,0.54466,0.20494,-0.05269,0.02878, 1403636695763555584,6.21807,-3.76659,-0.60043,-0.19880,0.76203,0.28957,0.54400,0.17540,-0.04506,-0.02957, 1403636695863555328,6.23675,-3.77114,-0.60420,-0.19946,0.76273,0.28845,0.54337,0.15563,-0.04693,-0.06998, 1403636695963555584,6.25481,-3.77438,-0.61135,-0.19945,0.76501,0.28757,0.54063,0.14095,-0.03744,-0.11606, 1403636696063555584,6.27246,-3.77699,-0.62356,-0.19879,0.76890,0.28688,0.53570,0.13916,-0.02920,-0.14483, 1403636696163555328,6.28824,-3.77838,-0.63619,-0.19224,0.77133,0.29007,0.53287,0.14227,-0.00968,-0.15806, 1403636696263555584,6.30692,-3.77697,-0.65015,-0.18877,0.77025,0.29179,0.53474,0.13516,0.01922,-0.16542, 1403636696363555328,6.32230,-3.77268,-0.66495,-0.19278,0.76419,0.28851,0.54370,0.12734,0.04893,-0.14247, 1403636696463555584,6.33712,-3.76795,-0.67666,-0.20166,0.75798,0.28175,0.55265,0.10424,0.04637,-0.10004, 1403636696463555584,6.35059,-3.76842,-0.67611,-0.20096,0.75921,0.27724,0.55349,0.03946,0.01197,-0.05797, 1403636696563555584,6.35059,-3.76842,-0.67611,-0.20096,0.75921,0.27724,0.55349,0.03946,0.01197,-0.05797, 1403636696663555328,6.35535,-3.76762,-0.67946,-0.20167,0.76112,0.27632,0.55107,0.00736,-0.01134,0.00143, 1403636696763555584,6.35800,-3.76858,-0.67915,-0.20160,0.76264,0.27625,0.54902,-0.00971,-0.02768,-0.02140, 1403636696863555328,6.35915,-3.77057,-0.68278,-0.19942,0.76325,0.27752,0.54832,-0.03011,-0.04263,-0.07565, 1403636696963555584,6.35847,-3.77268,-0.69060,-0.19232,0.76188,0.28233,0.55030,-0.04400,-0.03488,-0.10270, 1403636697063555584,6.35414,-3.77441,-0.70006,-0.18105,0.75932,0.28982,0.55377,-0.07459,-0.00963,-0.08558, 1403636697163555328,6.34457,-3.77402,-0.70436,-0.16990,0.75710,0.29675,0.55666,-0.13863,0.02795,-0.01580, 1403636697263555584,6.32999,-3.76715,-0.70201,-0.16633,0.75557,0.29859,0.55883,-0.21412,0.08861,0.06643, 1403636697363555328,6.30615,-3.75537,-0.69108,-0.17279,0.76024,0.29377,0.55306,-0.26943,0.15065,0.12282, 1403636697463555584,6.27804,-3.73741,-0.67433,-0.18188,0.76589,0.28771,0.54549,-0.30997,0.19131,0.21395, 1403636697563555584,6.24592,-3.71698,-0.64724,-0.18457,0.76995,0.28263,0.54151,-0.31475,0.21258,0.29104, 1403636697663555328,6.21524,-3.69620,-0.61925,-0.17795,0.77235,0.27527,0.54409,-0.31035,0.21973,0.25101, 1403636697763555584,6.18306,-3.67456,-0.59866,-0.16665,0.77836,0.26055,0.54636,-0.31988,0.22312,0.18638, 1403636697863555328,6.14960,-3.65080,-0.58350,-0.15488,0.78348,0.24306,0.55054,-0.33453,0.25392,0.12188, 1403636697963555584,6.11429,-3.62398,-0.57359,-0.14634,0.78817,0.22690,0.55307,-0.34651,0.28240,0.06467, 1403636698063555584,6.07820,-3.59549,-0.56933,-0.13989,0.79105,0.21534,0.55525,-0.35717,0.30422,0.02456, 1403636698163555328,6.04226,-3.56409,-0.57157,-0.13644,0.79353,0.20632,0.55599,-0.35771,0.33018,-0.03815, 1403636698263555584,6.00627,-3.53072,-0.57763,-0.13218,0.79536,0.19766,0.55754,-0.35423,0.32376,-0.08383, 1403636698363555328,5.97152,-3.49786,-0.58699,-0.12286,0.79777,0.18655,0.56006,-0.34599,0.33381,-0.11491, 1403636698463555584,5.93651,-3.46477,-0.59908,-0.11168,0.80218,0.17157,0.56089,-0.34434,0.33561,-0.15054, 1403636698563555584,5.90266,-3.43039,-0.61555,-0.09810,0.80610,0.15419,0.56285,-0.33488,0.34628,-0.17540, 1403636698663555328,5.87052,-3.39456,-0.63567,-0.08515,0.81041,0.13266,0.56426,-0.32583,0.35152,-0.21330, 1403636698763555584,5.83763,-3.35677,-0.65960,-0.07089,0.81435,0.10896,0.56563,-0.31245,0.36659,-0.25226, 1403636698863555328,5.80529,-3.31790,-0.68595,-0.05655,0.81791,0.08293,0.56652,-0.30559,0.38391,-0.26963, 1403636698963555584,5.77469,-3.27823,-0.71033,-0.04147,0.82022,0.05726,0.56766,-0.29600,0.39074,-0.21199, 1403636699063555584,5.74330,-3.23813,-0.72876,-0.02805,0.82140,0.03171,0.56878,-0.27679,0.39972,-0.14855, 1403636699163555328,5.71371,-3.19862,-0.73882,-0.01728,0.82123,0.00321,0.57033,-0.28175,0.38132,-0.07668, 1403636699263555584,5.68457,-3.16198,-0.74169,-0.00483,0.82145,-0.02593,0.56967,-0.26718,0.34618,-0.01381, 1403636699363555328,5.65912,-3.12883,-0.73852,0.00742,0.82172,-0.05515,0.56717,-0.23528,0.30966,0.04272, 1403636699463555584,5.63719,-3.09997,-0.73123,0.01931,0.82098,-0.08253,0.56464,-0.20749,0.26460,0.06992, 1403636699563555584,5.61737,-3.07551,-0.72555,0.03269,0.81892,-0.10690,0.56291,-0.19137,0.21518,0.02303, 1403636699663555328,5.59933,-3.05612,-0.72574,0.04479,0.81574,-0.13118,0.56157,-0.17115,0.14853,-0.02624, 1403636699763555584,5.58044,-3.04530,-0.73163,0.06174,0.81177,-0.14965,0.56110,-0.17392,0.08796,-0.06961, 1403636699863555328,5.56183,-3.03985,-0.74061,0.07933,0.80749,-0.15954,0.56234,-0.17523,0.05001,-0.08631, 1403636699963555584,5.54498,-3.03653,-0.75043,0.08774,0.80326,-0.16732,0.56487,-0.17156,0.02765,-0.12152, 1403636700063555584,5.52860,-3.03643,-0.76463,0.09080,0.80167,-0.17248,0.56509,-0.18803,0.01259,-0.16442, 1403636700163555328,5.51144,-3.03932,-0.78206,0.09161,0.80048,-0.17501,0.56588,-0.18894,-0.03471,-0.19973, 1403636700263555584,5.49459,-3.04450,-0.80487,0.09160,0.80030,-0.17637,0.56571,-0.19852,-0.06939,-0.25187, 1403636700363555328,5.47734,-3.05518,-0.83420,0.09141,0.80107,-0.18294,0.56256,-0.20504,-0.10974,-0.32264, 1403636700463555584,5.45784,-3.06896,-0.86899,0.09647,0.79892,-0.19133,0.56197,-0.20540,-0.16324,-0.36647, 1403636700563555584,5.44282,-3.08888,-0.90851,0.10558,0.79467,-0.19585,0.56480,-0.20699,-0.19883,-0.39147, 1403636700663555328,5.42619,-3.11086,-0.94853,0.11871,0.79008,-0.20181,0.56653,-0.22603,-0.23239,-0.41435, 1403636700763555584,5.40917,-3.13282,-0.98884,0.12941,0.78635,-0.20949,0.56659,-0.22686,-0.23145,-0.37001, 1403636700863555328,5.39052,-3.15438,-1.02250,0.13565,0.78658,-0.21552,0.56253,-0.23433,-0.22572,-0.29113, 1403636700963555584,5.37120,-3.17608,-1.04811,0.13861,0.78919,-0.21967,0.55652,-0.23025,-0.21811,-0.22480, 1403636701063555584,5.35223,-3.19770,-1.06622,0.13844,0.79338,-0.22212,0.54959,-0.21626,-0.22453,-0.13287, 1403636701163555328,5.33386,-3.22081,-1.07546,0.13779,0.79766,-0.22186,0.54363,-0.18593,-0.24240,-0.04462, 1403636701263555584,5.31670,-3.24671,-1.07484,0.13645,0.80009,-0.22002,0.54114,-0.14587,-0.26546,0.05014, 1403636701363555328,5.30269,-3.27353,-1.06747,0.13499,0.80105,-0.21778,0.54099,-0.11878,-0.28175,0.09467, 1403636701463555584,5.29003,-3.30363,-1.05849,0.13504,0.79936,-0.21339,0.54520,-0.08557,-0.29919,0.10914, 1403636701563555584,5.28217,-3.33424,-1.04889,0.13570,0.79526,-0.20871,0.55279,-0.07170,-0.30326,0.09355, 1403636701663555328,5.27501,-3.36500,-1.04093,0.13598,0.79342,-0.20388,0.55716,-0.06270,-0.30563,0.08543, 1403636701763555584,5.26713,-3.39504,-1.03382,0.13620,0.79179,-0.20508,0.55898,-0.07023,-0.30536,0.04439, 1403636701863555328,5.26012,-3.42513,-1.03329,0.14069,0.79045,-0.21284,0.55686,-0.08212,-0.31483,-0.02979, 1403636701963555584,5.25316,-3.45571,-1.03903,0.14696,0.78743,-0.22626,0.55423,-0.09139,-0.31578,-0.07624, 1403636702063555584,5.24340,-3.48626,-1.04910,0.15631,0.78272,-0.24303,0.55123,-0.10372,-0.31364,-0.12868, 1403636702163555328,5.23275,-3.51738,-1.06521,0.16849,0.77563,-0.26226,0.54885,-0.11242,-0.32755,-0.17793, 1403636702263555584,5.22245,-3.54825,-1.08604,0.17934,0.77093,-0.27852,0.54400,-0.11373,-0.31550,-0.24243, 1403636702363555328,5.21327,-3.57723,-1.11270,0.18828,0.76977,-0.28970,0.53674,-0.10229,-0.31848,-0.26875, 1403636702463555584,5.20583,-3.60787,-1.14273,0.19679,0.76984,-0.29629,0.52993,-0.07742,-0.32080,-0.30094, 1403636702563555584,5.20212,-3.63766,-1.17791,0.20298,0.76965,-0.30006,0.52574,-0.04443,-0.31899,-0.35769, 1403636702663555328,5.20221,-3.66743,-1.21817,0.20623,0.77123,-0.30286,0.52053,-0.00271,-0.32103,-0.40244, 1403636702763555584,5.20560,-3.69708,-1.26166,0.20765,0.77200,-0.30421,0.51803,0.04121,-0.31657,-0.45078, 1403636702863555328,5.21311,-3.72774,-1.30920,0.20848,0.77413,-0.30391,0.51468,0.08963,-0.33097,-0.46625, 1403636702963555584,5.22344,-3.75988,-1.35334,0.20750,0.77469,-0.30387,0.51425,0.13078,-0.32451,-0.40685, 1403636703063555584,5.23975,-3.79323,-1.39210,0.20533,0.77189,-0.30452,0.51894,0.17826,-0.34833,-0.32667, 1403636703163555328,5.25810,-3.82937,-1.42091,0.19989,0.76503,-0.30720,0.52954,0.20514,-0.35237,-0.22342, 1403636703263555584,5.27722,-3.86703,-1.44260,0.19192,0.75865,-0.31153,0.53904,0.20952,-0.38397,-0.16933, 1403636703363555328,5.29442,-3.90573,-1.45943,0.18823,0.75433,-0.31313,0.54544,0.15983,-0.39694,-0.10898, 1403636703463555584,5.30694,-3.94629,-1.46838,0.19599,0.75647,-0.30752,0.54292,0.10731,-0.40804,-0.04019, 1403636703563555584,5.31526,-3.98810,-1.47000,0.20445,0.76108,-0.30094,0.53701,0.09590,-0.41848,0.02378, 1403636703663555328,5.32290,-4.02899,-1.46577,0.20542,0.76472,-0.29913,0.53247,0.09594,-0.40599,0.05070, 1403636703763555584,5.33085,-4.06811,-1.46235,0.20445,0.76499,-0.29835,0.53289,0.09498,-0.39386,0.01097, 1403636703863555328,5.34125,-4.10736,-1.46411,0.20134,0.76245,-0.29897,0.53735,0.10227,-0.38267,-0.03237, 1403636703963555584,5.35187,-4.14643,-1.47067,0.19847,0.76071,-0.29905,0.54083,0.09240,-0.37704,-0.07701, 1403636704063555584,5.36126,-4.18511,-1.48080,0.19447,0.75972,-0.29956,0.54339,0.07369,-0.37500,-0.12780, 1403636704163555328,5.36896,-4.22385,-1.49537,0.19298,0.76289,-0.29869,0.53994,0.05840,-0.37497,-0.17357, 1403636704263555584,5.37497,-4.26254,-1.51507,0.19322,0.76784,-0.29702,0.53373,0.05734,-0.38900,-0.19207, 1403636704363555328,5.38324,-4.30313,-1.53102,0.19515,0.77137,-0.29460,0.52926,0.06986,-0.39958,-0.12222, 1403636704463555584,5.39348,-4.34512,-1.53989,0.19535,0.77410,-0.29347,0.52581,0.09911,-0.41783,-0.03162, 1403636704563555584,5.40611,-4.38793,-1.54044,0.19560,0.77717,-0.29283,0.52153,0.13475,-0.42595,0.03138, 1403636704663555328,5.42299,-4.43255,-1.53934,0.19562,0.77955,-0.29248,0.51816,0.18165,-0.44174,-0.00431, 1403636704763555584,5.44226,-4.47801,-1.54439,0.19550,0.78405,-0.29148,0.51194,0.22013,-0.45691,-0.06962, 1403636704863555328,5.46728,-4.52558,-1.55600,0.19230,0.78804,-0.29235,0.50650,0.28503,-0.47840,-0.14031, 1403636704963555584,5.49859,-4.57490,-1.57270,0.19001,0.79150,-0.29215,0.50207,0.34360,-0.51236,-0.18392, 1403636705063555584,5.53627,-4.62874,-1.59234,0.19087,0.79350,-0.29008,0.49978,0.42921,-0.55477,-0.19395, 1403636705163555328,5.58227,-4.68532,-1.61167,0.19498,0.78693,-0.28623,0.51069,0.49961,-0.57886,-0.17846, 1403636705263555584,5.63426,-4.74254,-1.62643,0.20103,0.77903,-0.28088,0.52327,0.57017,-0.59288,-0.10696, 1403636705363555328,5.69176,-4.80125,-1.63592,0.20281,0.77512,-0.27895,0.52938,0.59132,-0.56868,-0.06941, 1403636705463555584,5.75398,-4.85913,-1.64299,0.20098,0.77719,-0.28020,0.52638,0.61734,-0.55407,-0.06792, 1403636705563555584,5.81908,-4.91439,-1.65299,0.19920,0.78114,-0.28178,0.52033,0.65518,-0.54678,-0.11706, 1403636705663555328,5.88571,-4.96885,-1.66717,0.19828,0.78243,-0.28320,0.51797,0.69391,-0.55347,-0.17335, 1403636705763555584,5.95680,-5.02309,-1.68700,0.19855,0.77941,-0.28356,0.52219,0.73617,-0.56304,-0.19319, 1403636705863555328,6.03172,-5.07879,-1.70372,0.19682,0.77454,-0.28444,0.52957,0.77044,-0.56678,-0.15207, 1403636705963555584,6.10817,-5.13512,-1.71425,0.19104,0.76859,-0.28795,0.53838,0.76906,-0.56835,-0.06779, 1403636706063555584,6.18234,-5.19053,-1.71785,0.18012,0.76337,-0.29391,0.54630,0.73273,-0.57051,0.00010, 1403636706163555328,6.25302,-5.24847,-1.72031,0.17824,0.76044,-0.29370,0.55110,0.69050,-0.59860,-0.01498, 1403636706263555584,6.31713,-5.30857,-1.72842,0.18699,0.76345,-0.28587,0.54814,0.64142,-0.60367,-0.10462, 1403636706363555328,6.37819,-5.36759,-1.74419,0.19556,0.76857,-0.27814,0.54194,0.62076,-0.58712,-0.17378, 1403636706463555584,6.43878,-5.42499,-1.76386,0.20008,0.77303,-0.27372,0.53617,0.62569,-0.57181,-0.18504, 1403636706563555584,6.50097,-5.48000,-1.77997,0.20234,0.77568,-0.27213,0.53227,0.63155,-0.54582,-0.11922, 1403636706663555328,6.56166,-5.53146,-1.79109,0.20248,0.77694,-0.27231,0.53030,0.65640,-0.53658,-0.05932, 1403636706763555584,6.62864,-5.58465,-1.79401,0.20051,0.77931,-0.27458,0.52638,0.69031,-0.52118,0.01786, 1403636706863555328,6.69646,-5.63512,-1.79193,0.20041,0.78147,-0.27670,0.52209,0.71541,-0.51089,0.05906, 1403636706963555584,6.77042,-5.68669,-1.78520,0.20103,0.78093,-0.27900,0.52144,0.75936,-0.51608,0.09373, 1403636707063555584,6.84579,-5.73708,-1.77602,0.20389,0.77727,-0.27972,0.52538,0.77922,-0.50983,0.09320, 1403636707163555328,6.92650,-5.78838,-1.76764,0.20606,0.77492,-0.28159,0.52702,0.80191,-0.49817,0.08304, 1403636707263555584,7.00843,-5.83952,-1.75855,0.20818,0.77445,-0.28421,0.52547,0.82862,-0.49334,0.13781, 1403636707363555328,7.09370,-5.88784,-1.74199,0.21041,0.77486,-0.28593,0.52303,0.86846,-0.47909,0.20987, 1403636707463555584,7.18064,-5.93559,-1.71888,0.21110,0.77532,-0.28721,0.52137,0.89934,-0.46011,0.30457, 1403636707563555584,7.27355,-5.98230,-1.68667,0.20932,0.77481,-0.28967,0.52149,0.92668,-0.44686,0.34346, 1403636707663555328,7.36482,-6.02717,-1.65860,0.20128,0.77436,-0.29578,0.52189,0.94186,-0.45253,0.25762, 1403636707763555584,7.45984,-6.07324,-1.63879,0.19421,0.77385,-0.30017,0.52282,0.96389,-0.47760,0.14106, 1403636707863555328,7.55681,-6.12214,-1.63404,0.19225,0.77215,-0.30065,0.52577,0.96135,-0.49334,0.02543, 1403636707963555584,7.65142,-6.16998,-1.63742,0.19240,0.77076,-0.29826,0.52910,0.96441,-0.51287,-0.07138, 1403636708063555584,7.74180,-6.21762,-1.65212,0.19487,0.77056,-0.29346,0.53118,0.96409,-0.52613,-0.14670, 1403636708163555328,7.83124,-6.26635,-1.67206,0.19782,0.77013,-0.28783,0.53377,0.95714,-0.52029,-0.19531, 1403636708263555584,7.92316,-6.31692,-1.69661,0.19411,0.76809,-0.28793,0.53801,0.94990,-0.51726,-0.24464, 1403636708363555328,8.01113,-6.36599,-1.72574,0.18300,0.76277,-0.29429,0.54597,0.91464,-0.52179,-0.24800, 1403636708463555584,8.09541,-6.41780,-1.75053,0.17461,0.75860,-0.29921,0.55182,0.88145,-0.57126,-0.18362, 1403636708563555584,8.17996,-6.47728,-1.76757,0.17454,0.75676,-0.29907,0.55444,0.83876,-0.62086,-0.10076, 1403636708663555328,8.25709,-6.53918,-1.78037,0.18516,0.75853,-0.29284,0.55190,0.76776,-0.64563,-0.10565, 1403636708763555584,8.33057,-6.60059,-1.79612,0.20007,0.76025,-0.28423,0.54883,0.73229,-0.62697,-0.15172, 1403636708863555328,8.39566,-6.65803,-1.81401,0.21054,0.76057,-0.27955,0.54685,0.70393,-0.58273,-0.19500, 1403636708963555584,8.46061,-6.71199,-1.83113,0.21736,0.75851,-0.27919,0.54724,0.68944,-0.53112,-0.14491, 1403636709063555584,8.52208,-6.75878,-1.84105,0.22267,0.75525,-0.27940,0.54949,0.66262,-0.45616,-0.07241, 1403636709163555328,8.58656,-6.79988,-1.84590,0.22533,0.75430,-0.28169,0.54855,0.64253,-0.39083,-0.01362, 1403636709263555584,8.64377,-6.83339,-1.84245,0.22510,0.75740,-0.28504,0.54261,0.62148,-0.31855,0.04322, 1403636709363555328,8.70536,-6.86147,-1.83679,0.22308,0.76072,-0.28785,0.53730,0.62255,-0.26573,0.04380, 1403636709463555584,8.76138,-6.88375,-1.83386,0.22355,0.76226,-0.28798,0.53484,0.62530,-0.22369,-0.02456, 1403636709563555584,8.82445,-6.90181,-1.84030,0.22384,0.76376,-0.28779,0.53266,0.63551,-0.16541,-0.10214, 1403636709663555328,8.88825,-6.91606,-1.85188,0.22315,0.76545,-0.28779,0.53053,0.65203,-0.13195,-0.13831, 1403636709763555584,8.94957,-6.92601,-1.86399,0.22142,0.76639,-0.28712,0.53027,0.66847,-0.08515,-0.14794, 1403636709863555328,9.01958,-6.93420,-1.87633,0.22053,0.76650,-0.28570,0.53124,0.68783,-0.04944,-0.09922, 1403636709963555584,9.09137,-6.93924,-1.88164,0.22087,0.76647,-0.28348,0.53234,0.70916,-0.01104,-0.02216, 1403636710063555584,9.16126,-6.93942,-1.88063,0.21936,0.76641,-0.28240,0.53361,0.71605,0.03191,0.01387, 1403636710163555328,9.23509,-6.93547,-1.88163,0.21883,0.76645,-0.28003,0.53503,0.72812,0.06339,-0.04363, 1403636710263555584,9.31411,-6.93057,-1.89155,0.21616,0.76634,-0.27862,0.53700,0.74942,0.09939,-0.14363, 1403636710363555328,9.38999,-6.91858,-1.90961,0.20283,0.76379,-0.28340,0.54329,0.75258,0.13716,-0.20171, 1403636710463555584,9.47023,-6.90594,-1.93138,0.18732,0.76380,-0.28430,0.54835,0.74860,0.14699,-0.25161, 1403636710563555584,9.54737,-6.89194,-1.95594,0.17305,0.76556,-0.27981,0.55289,0.72393,0.13010,-0.27942, 1403636710663555328,9.62022,-6.88019,-1.98434,0.16307,0.76826,-0.27064,0.55672,0.69752,0.11069,-0.30507, 1403636710763555584,9.68673,-6.87104,-2.01416,0.15604,0.77224,-0.25572,0.56026,0.66243,0.08435,-0.27523, 1403636710863555328,9.74714,-6.86416,-2.03941,0.15432,0.77876,-0.23352,0.56141,0.62020,0.06997,-0.20995, 1403636710963555584,9.80381,-6.85812,-2.05802,0.15150,0.78497,-0.21222,0.56199,0.60025,0.06672,-0.13570, 1403636711063555584,9.86267,-6.85030,-2.07154,0.15035,0.79031,-0.19484,0.56111,0.57545,0.08693,-0.09297, 1403636711163555328,9.91929,-6.83963,-2.07992,0.15020,0.79243,-0.18305,0.56212,0.56362,0.12216,-0.05673, 1403636711263555584,9.97542,-6.82383,-2.08629,0.14914,0.79312,-0.17696,0.56339,0.54562,0.17409,-0.04415, 1403636711363555328,10.02911,-6.80278,-2.08958,0.14392,0.79152,-0.17715,0.56692,0.53953,0.21524,0.00044, 1403636711463555584,10.08138,-6.77778,-2.08685,0.14137,0.78374,-0.17821,0.57794,0.49287,0.26142,0.04947, 1403636711563555584,10.12895,-6.74852,-2.08081,0.14488,0.77656,-0.17733,0.58696,0.47016,0.29249,0.06927, 1403636711663555328,10.16937,-6.71478,-2.07327,0.14996,0.77395,-0.17629,0.58944,0.40855,0.35167,0.07210, 1403636711763555584,10.20176,-6.67572,-2.06527,0.15314,0.77487,-0.17711,0.58716,0.34140,0.41611,0.06908, 1403636711863555328,10.22962,-6.63123,-2.05717,0.14803,0.77753,-0.18310,0.58311,0.28576,0.47108,0.09086, 1403636711963555584,10.25016,-6.58270,-2.04521,0.13787,0.78074,-0.19223,0.57835,0.22850,0.50086,0.13134, 1403636712063555584,10.26482,-6.53286,-2.02901,0.12723,0.78193,-0.20109,0.57616,0.17294,0.49670,0.18851, 1403636712163555328,10.26827,-6.48582,-2.00556,0.12278,0.78096,-0.20380,0.57749,0.12182,0.46827,0.23188, 1403636712263555584,10.27188,-6.44146,-1.97923,0.12707,0.78364,-0.19183,0.57704,0.08474,0.46455,0.29247, 1403636712363555328,10.27718,-6.39462,-1.94849,0.12946,0.78634,-0.17376,0.57855,0.04583,0.47907,0.32269, 1403636712463555584,10.27478,-6.34816,-1.91397,0.12812,0.78785,-0.15854,0.58115,0.00514,0.50394,0.37395, 1403636712563555584,10.27088,-6.29829,-1.87597,0.12311,0.78877,-0.14743,0.58391,-0.03647,0.54026,0.38919, 1403636712663555328,10.26531,-6.24299,-1.83996,0.11512,0.78624,-0.14065,0.59058,-0.08048,0.56681,0.32644, 1403636712763555584,10.25540,-6.18390,-1.81052,0.10785,0.78447,-0.13623,0.59532,-0.13499,0.59185,0.26343, 1403636712863555328,10.23954,-6.12308,-1.78723,0.10318,0.78676,-0.13373,0.59370,-0.19760,0.60947,0.21275, 1403636712963555584,10.21620,-6.06112,-1.77024,0.10136,0.79045,-0.13218,0.58945,-0.26037,0.62711,0.15026, 1403636713063555584,10.15529,-5.96010,-1.72816,0.09530,0.79282,-0.12289,0.58928,-0.32417,0.64043,0.17998, 1403636713163555328,10.12099,-5.89519,-1.70640,0.09140,0.79350,-0.12333,0.58890,-0.37705,0.65315,0.24750, 1403636713263555584,10.08195,-5.82759,-1.67909,0.08896,0.79583,-0.12462,0.58585,-0.41376,0.65842,0.33178, 1403636713363555328,10.04182,-5.76203,-1.64320,0.08901,0.79790,-0.12509,0.58291,-0.40961,0.65270,0.41927, 1403636713463555584,10.00058,-5.69605,-1.60474,0.08611,0.79817,-0.12792,0.58237,-0.43553,0.64518,0.44357, 1403636713563555584,9.95533,-5.63220,-1.55956,0.08249,0.79842,-0.13152,0.58174,-0.46370,0.63554,0.42974, 1403636713663555328,9.90881,-5.56901,-1.52250,0.07769,0.79993,-0.13703,0.57905,-0.48496,0.61774,0.36563, 1403636713763555584,9.85981,-5.51004,-1.48787,0.07350,0.80052,-0.14228,0.57751,-0.50131,0.57485,0.30117, 1403636713863555328,9.80815,-5.45395,-1.46153,0.07265,0.80067,-0.14513,0.57670,-0.52193,0.52446,0.25689, 1403636713963555584,9.75456,-5.40361,-1.43871,0.07736,0.80137,-0.14421,0.57535,-0.54173,0.48390,0.20020, 1403636714063555584,9.69847,-5.35611,-1.42171,0.08435,0.80183,-0.14223,0.57422,-0.55203,0.44881,0.17120, 1403636714163555328,9.64362,-5.31195,-1.40928,0.08808,0.80192,-0.14266,0.57342,-0.56622,0.43188,0.11480, 1403636714263555584,9.58521,-5.26743,-1.39882,0.08749,0.79990,-0.14562,0.57559,-0.57808,0.41781,0.07483, 1403636714363555328,9.52816,-5.22637,-1.39459,0.08612,0.79976,-0.14914,0.57509,-0.59027,0.38845,0.03222, 1403636714463555584,9.46941,-5.18763,-1.39430,0.08541,0.80454,-0.14972,0.56833,-0.59417,0.36951,-0.00847, 1403636714563555584,9.40960,-5.14982,-1.39619,0.07971,0.81024,-0.14599,0.56199,-0.57152,0.34970,-0.04332, 1403636714663555328,9.34610,-5.11759,-1.43193,0.07307,0.81520,-0.13614,0.55819,-0.53675,0.31548,-0.02685, 1403636714763555584,9.29243,-5.08610,-1.43169,0.06363,0.81955,-0.11926,0.55683,-0.50150,0.28221,0.04322, 1403636714863555328,9.24067,-5.05655,-1.42233,0.05549,0.82227,-0.09509,0.55835,-0.47345,0.25767,0.11306, 1403636714963555584,9.19758,-5.03103,-1.40602,0.04390,0.82522,-0.07137,0.55856,-0.42858,0.24764,0.19348, 1403636715063555584,9.15896,-5.00548,-1.38382,0.03359,0.82727,-0.05121,0.55846,-0.38656,0.23279,0.26781, 1403636715163555328,9.12470,-4.98248,-1.35765,0.02611,0.82900,-0.03611,0.55746,-0.34105,0.22544,0.29786, 1403636715263555584,9.09472,-4.95880,-1.33157,0.02042,0.82885,-0.02704,0.55845,-0.29331,0.21951,0.25952, 1403636715363555328,9.07004,-4.93549,-1.31170,0.01487,0.82678,-0.02326,0.56185,-0.24618,0.22102,0.18331, 1403636715363555328,9.05425,-4.88981,-1.28291,0.00690,0.82260,-0.02490,0.56803,-0.21698,0.20773,0.08859, 1403636715463555584,9.05425,-4.88981,-1.28291,0.00690,0.82260,-0.02490,0.56803,-0.21698,0.20773,0.08859, 1403636715563555584,9.03314,-4.86842,-1.27814,-0.00572,0.81890,-0.03267,0.57298,-0.20874,0.17576,0.02743, 1403636715663555328,9.01231,-4.85176,-1.27824,-0.00728,0.81607,-0.03471,0.57686,-0.20236,0.11673,-0.01375, 1403636715763555584,8.99178,-4.83934,-1.27927,0.00235,0.80945,-0.03048,0.58639,-0.22241,0.07924,-0.01948, 1403636715863555328,8.97047,-4.83135,-1.27903,0.00987,0.80411,-0.02781,0.59374,-0.23491,0.07002,0.04943, 1403636715963555584,8.94449,-4.82255,-1.27138,0.01568,0.80009,-0.02693,0.59907,-0.30359,0.06681,0.11319, 1403636716063555584,8.91271,-4.81571,-1.26078,0.01766,0.79963,-0.02850,0.59956,-0.34274,0.06365,0.13672, 1403636716163555328,8.87505,-4.80922,-1.24651,0.01616,0.80331,-0.03296,0.59442,-0.40702,0.06260,0.19444, 1403636716263555584,8.83295,-4.80317,-1.22830,0.01594,0.80954,-0.03750,0.58566,-0.44959,0.05974,0.18252, 1403636716363555328,8.78861,-4.79717,-1.21562,0.01717,0.81327,-0.04102,0.58019,-0.45833,0.04783,0.12097, 1403636716463555584,8.74373,-4.79279,-1.20837,0.01902,0.81418,-0.04325,0.57868,-0.45311,0.03699,0.05261, 1403636716563555584,8.69991,-4.78919,-1.20789,0.02171,0.81362,-0.04291,0.57940,-0.44745,0.03968,-0.00505, 1403636716663555328,8.65478,-4.78474,-1.21222,0.02046,0.81363,-0.03783,0.57979,-0.44768,0.05286,-0.07098, 1403636716763555584,8.61005,-4.77971,-1.22270,0.01480,0.81365,-0.02935,0.58042,-0.46122,0.05925,-0.11325, 1403636716863555328,8.56448,-4.77427,-1.23555,0.00516,0.81424,-0.01717,0.58026,-0.47183,0.05765,-0.10389, 1403636716963555584,8.51650,-4.76748,-1.24510,-0.00690,0.81525,0.00023,0.57907,-0.47748,0.07070,-0.03362, 1403636717063555584,8.46745,-4.76116,-1.24749,-0.01991,0.81524,0.02021,0.57842,-0.48014,0.06953,0.03485, 1403636717163555328,8.41745,-4.75384,-1.24284,-0.03024,0.81473,0.03856,0.57777,-0.46578,0.05876,0.08751, 1403636717263555584,8.36587,-4.74811,-1.23586,-0.03529,0.81412,0.05379,0.57713,-0.48109,0.04481,0.10247, 1403636717363555328,8.31221,-4.74314,-1.22855,-0.03134,0.81303,0.06885,0.57729,-0.49538,0.05239,0.10796, 1403636717463555584,8.25811,-4.73514,-1.22072,-0.02749,0.81167,0.07911,0.57808,-0.51381,0.08877,0.07014, 1403636717563555584,8.20451,-4.72299,-1.21707,-0.03047,0.81141,0.08072,0.57807,-0.52323,0.13860,-0.00276, 1403636717663555328,8.15192,-4.70779,-1.22024,-0.03558,0.81152,0.07832,0.57796,-0.52887,0.17365,-0.07563, 1403636717763555584,8.09503,-4.68747,-1.22781,-0.03486,0.81144,0.07969,0.57792,-0.54447,0.21336,-0.11293, 1403636717863555328,8.04046,-4.66408,-1.23800,-0.03572,0.81148,0.08637,0.57686,-0.55627,0.27196,-0.06959, 1403636717963555584,7.98337,-4.63290,-1.24042,-0.03960,0.81049,0.09747,0.57623,-0.57255,0.33188,-0.00504, 1403636718063555584,7.92597,-4.59687,-1.23738,-0.04944,0.80911,0.11064,0.57502,-0.57852,0.39833,0.07865, 1403636718163555328,7.86570,-4.55285,-1.22709,-0.06725,0.80784,0.12208,0.57268,-0.57706,0.44393,0.08359, 1403636718263555584,7.80638,-4.50687,-1.22204,-0.08242,0.80696,0.13189,0.56976,-0.59958,0.45084,0.00725, 1403636718363555328,7.74692,-4.46023,-1.22614,-0.09524,0.80608,0.14071,0.56689,-0.60282,0.47369,-0.06999, 1403636718463555584,7.69019,-4.41189,-1.23781,-0.10907,0.80506,0.15026,0.56338,-0.58936,0.49180,-0.12938, 1403636718563555584,7.63526,-4.36421,-1.25442,-0.12261,0.80312,0.15992,0.56070,-0.56998,0.48182,-0.17471, 1403636718663555328,7.58195,-4.31720,-1.27448,-0.13707,0.80271,0.16469,0.55654,-0.53400,0.46237,-0.20432, 1403636718763555584,7.53102,-4.27340,-1.29917,-0.14697,0.80207,0.16956,0.55347,-0.49332,0.42223,-0.24435, 1403636718863555328,7.48376,-4.23448,-1.32630,-0.14357,0.80016,0.18027,0.55374,-0.45462,0.37792,-0.22860, 1403636718963555584,7.43800,-4.19891,-1.34785,-0.13807,0.79725,0.18936,0.55631,-0.43313,0.35809,-0.14814, 1403636719063555584,7.39401,-4.16532,-1.36187,-0.13585,0.79453,0.19423,0.55905,-0.43326,0.35094,-0.05882, 1403636719163555328,7.39898,-4.15329,-1.41123,-0.13698,0.79049,0.19640,0.56373,-0.40842,0.36795,0.02750, 1403636719263555584,7.35244,-4.11746,-1.41388,-0.13850,0.78678,0.19473,0.56910,-0.43896,0.35119,0.00019, 1403636719363555328,7.30486,-4.08311,-1.42074,-0.14146,0.78452,0.19119,0.57268,-0.46477,0.33484,-0.06967, 1403636719463555584,7.25091,-4.05088,-1.43564,-0.14304,0.78366,0.18824,0.57444,-0.49245,0.31335,-0.13953, 1403636719563555584,7.20010,-4.02205,-1.45375,-0.14271,0.78588,0.18613,0.57217,-0.51157,0.28160,-0.18526, 1403636719663555328,7.14269,-3.99607,-1.47226,-0.14172,0.78999,0.18409,0.56740,-0.52737,0.26136,-0.10798, 1403636719763555584,7.08835,-3.97228,-1.47941,-0.13948,0.79414,0.18182,0.56287,-0.53667,0.23312,-0.02966, 1403636719863555328,7.03024,-3.95258,-1.48115,-0.13808,0.79625,0.17899,0.56114,-0.52997,0.20882,0.05322, 1403636719963555584,6.97618,-3.93371,-1.47737,-0.13702,0.79819,0.17651,0.55943,-0.52200,0.18842,0.04795, 1403636720063555584,6.91957,-3.91888,-1.48007,-0.13580,0.79887,0.17526,0.55915,-0.50207,0.17651,-0.01230, 1403636720163555328,6.86982,-3.90045,-1.48579,-0.13444,0.79990,0.17492,0.55811,-0.48587,0.16651,-0.08528, 1403636720263555584,6.82205,-3.88370,-1.49651,-0.13526,0.79999,0.17425,0.55799,-0.46156,0.16058,-0.12230, 1403636720363555328,6.77245,-3.87182,-1.51292,-0.13616,0.79974,0.17432,0.55811,-0.44369,0.14362,-0.15968, 1403636720463555584,6.72801,-3.86003,-1.53097,-0.13789,0.79990,0.17469,0.55735,-0.43393,0.11851,-0.15358, 1403636720563555584,6.68504,-3.84958,-1.54460,-0.13972,0.80006,0.17500,0.55656,-0.41636,0.09992,-0.09318, 1403636720663555328,6.64061,-3.84343,-1.54985,-0.14035,0.80010,0.17656,0.55586,-0.40371,0.07687,-0.01180, 1403636720763555584,6.60136,-3.83658,-1.54808,-0.14002,0.79985,0.17489,0.55683,-0.38495,0.04601,0.05024, 1403636720863555328,6.56437,-3.83333,-1.54238,-0.13403,0.80141,0.16968,0.55766,-0.34467,0.03225,0.05078, 1403636720963555584,6.53094,-3.83137,-1.54088,-0.12677,0.80305,0.15927,0.56007,-0.32419,0.01854,-0.01222, 1403636721063555584,6.49869,-3.83008,-1.54436,-0.12160,0.80356,0.14684,0.56386,-0.30970,0.00772,-0.06971, 1403636721163555328,6.49512,-3.82450,-1.59015,-0.10913,0.80512,0.12769,0.56883,-0.29418,-0.01460,-0.09764, 1403636721263555584,6.46591,-3.82929,-1.59544,-0.10121,0.80512,0.11331,0.57333,-0.29639,-0.03695,-0.03787, 1403636721363555328,6.43548,-3.83531,-1.59474,-0.09294,0.80651,0.10301,0.57471,-0.30147,-0.06701,0.04557, 1403636721463555584,6.40229,-3.84317,-1.58857,-0.08636,0.80721,0.09588,0.57599,-0.31555,-0.07824,0.09485, 1403636721563555584,6.36966,-3.85241,-1.58096,-0.07802,0.80917,0.08832,0.57564,-0.32577,-0.09557,0.05862, 1403636721663555328,6.33483,-3.86242,-1.57911,-0.06931,0.81129,0.08141,0.57478,-0.33408,-0.09926,-0.00361, 1403636721763555584,6.30058,-3.87294,-1.58441,-0.06261,0.81154,0.07807,0.57566,-0.34212,-0.09680,-0.06821, 1403636721863555328,6.26433,-3.88260,-1.59324,-0.06094,0.81134,0.07509,0.57652,-0.34826,-0.09821,-0.11554, 1403636721963555584,6.23152,-3.89239,-1.60642,-0.05780,0.81106,0.07638,0.57707,-0.34408,-0.09443,-0.13188, 1403636722063555584,6.19574,-3.90117,-1.61562,-0.05264,0.81064,0.08071,0.57756,-0.34531,-0.07135,-0.07265, 1403636722163555328,6.15982,-3.90710,-1.61733,-0.05430,0.81101,0.08152,0.57677,-0.35685,-0.05306,0.01827, 1403636722263555584,6.12310,-3.91154,-1.61174,-0.05773,0.81158,0.08124,0.57567,-0.36669,-0.03929,0.06441, 1403636722363555328,6.08645,-3.91566,-1.60646,-0.06063,0.81169,0.08070,0.57530,-0.36874,-0.03999,0.02171, 1403636722463555584,6.04989,-3.92041,-1.60633,-0.05823,0.81179,0.07787,0.57580,-0.37115,-0.04455,-0.03305, 1403636722563555584,6.01228,-3.92407,-1.61122,-0.05585,0.81222,0.07356,0.57599,-0.37443,-0.03451,-0.06674, 1403636722663555328,5.97466,-3.92693,-1.61911,-0.05195,0.81271,0.07194,0.57587,-0.37515,-0.03071,-0.08771, 1403636722763555584,5.93916,-3.92901,-1.62745,-0.04866,0.81280,0.07105,0.57615,-0.37194,-0.01888,-0.08686, 1403636722863555328,5.89980,-3.92925,-1.63308,-0.04612,0.81318,0.07103,0.57582,-0.39646,-0.00337,-0.01262, 1403636722963555584,5.86001,-3.92770,-1.63167,-0.04756,0.81378,0.06935,0.57506,-0.41492,0.01286,0.03767, 1403636723063555584,5.82150,-3.92432,-1.62282,-0.05139,0.81320,0.06682,0.57585,-0.39665,0.02616,0.08886, 1403636723163555328,5.78540,-3.92086,-1.61097,-0.05235,0.81305,0.06595,0.57607,-0.38976,0.02461,0.05635, 1403636723263555584,5.74968,-3.91778,-1.60328,-0.05138,0.81357,0.06614,0.57540,-0.38763,0.02325,0.01114, 1403636723363555328,5.71348,-3.91471,-1.60020,-0.04719,0.81338,0.06632,0.57601,-0.38353,0.03028,-0.03895, 1403636723463555584,5.67824,-3.91011,-1.59895,-0.03818,0.81212,0.06250,0.57888,-0.37283,0.03311,-0.02855, 1403636723563555584,5.64026,-3.90574,-1.59914,-0.03398,0.80681,0.05122,0.58761,-0.35332,0.05899,0.01985, 1403636723663555328,5.60115,-3.89836,-1.59048,-0.03144,0.80301,0.04069,0.59374,-0.38740,0.07000,0.07266, 1403636723763555584,5.55928,-3.89094,-1.57930,-0.02670,0.80817,0.03394,0.58736,-0.43223,0.07908,0.09698, 1403636723863555328,5.51525,-3.88321,-1.57017,-0.02493,0.81329,0.02755,0.58067,-0.44381,0.08444,0.05982, 1403636723963555584,5.46880,-3.87552,-1.56589,-0.02369,0.81798,0.02253,0.57432,-0.44750,0.08274,-0.00558, 1403636724063555584,5.42514,-3.86768,-1.56681,-0.01880,0.82253,0.01615,0.56819,-0.43779,0.06334,-0.05466, 1403636724163555328,5.38314,-3.86142,-1.57190,-0.01000,0.82488,0.00639,0.56518,-0.41754,0.05652,-0.08572, 1403636724263555584,5.34514,-3.85713,-1.57981,-0.00055,0.82560,-0.00775,0.56420,-0.38312,0.04417,-0.11674, 1403636724363555328,5.30573,-3.85257,-1.58931,0.01196,0.82344,-0.02283,0.56682,-0.36061,0.04478,-0.10045, 1403636724463555584,5.26676,-3.84721,-1.59524,0.02297,0.81998,-0.03733,0.57071,-0.35350,0.05600,-0.03808, 1403636724563555584,5.23094,-3.84109,-1.59471,0.03171,0.81636,-0.04690,0.57476,-0.35438,0.07303,0.01752, 1403636724663555328,5.19971,-3.83399,-1.58697,0.03730,0.81519,-0.05357,0.57551,-0.33532,0.09021,0.10448, 1403636724763555584,5.17060,-3.82402,-1.56999,0.03994,0.81527,-0.05808,0.57477,-0.31625,0.10821,0.19318, 1403636724863555328,5.14194,-3.81236,-1.54734,0.04018,0.81624,-0.06027,0.57315,-0.29729,0.12408,0.26507, 1403636724963555584,5.11523,-3.79986,-1.51619,0.03613,0.81717,-0.06238,0.57188,-0.28995,0.13251,0.32861, 1403636724963555584,5.08474,-3.81679,-1.45191,0.03395,0.81740,-0.05927,0.57201,-0.29095,0.13030,0.32861, 1403636725063555584,5.05934,-3.80425,-1.41533,0.02858,0.81722,-0.06105,0.57238,-0.27635,0.12757,0.40623, 1403636725163555328,5.03529,-3.79335,-1.37362,0.02462,0.81804,-0.06193,0.57129,-0.25270,0.10363,0.42519, 1403636725263555584,5.01204,-3.78497,-1.33310,0.02409,0.81830,-0.06058,0.57108,-0.23730,0.08163,0.38716, 1403636725363555328,4.98940,-3.77798,-1.29858,0.02679,0.81880,-0.05827,0.57049,-0.22646,0.06215,0.32048, 1403636725463555584,4.96760,-3.77248,-1.26945,0.02897,0.81862,-0.05649,0.57081,-0.21469,0.05340,0.25586, 1403636725563555584,4.94668,-3.76749,-1.24647,0.03126,0.81912,-0.05518,0.57011,-0.21329,0.04396,0.18496, 1403636725663555328,4.92673,-3.76248,-1.22896,0.03286,0.81803,-0.05439,0.57166,-0.19443,0.04502,0.12221, 1403636725763555584,4.90804,-3.75724,-1.21674,0.03264,0.81805,-0.05479,0.57161,-0.18174,0.05408,0.06225, 1403636725863555328,4.88879,-3.75226,-1.21154,0.03192,0.81864,-0.05479,0.57080,-0.16662,0.05000,0.02018, 1403636725963555584,4.85171,-3.68663,-1.20983,0.03152,0.81863,-0.05906,0.57041,-0.15000,0.05096,-0.02290, 1403636726063555584,4.83469,-3.68138,-1.21190,0.03100,0.81790,-0.05825,0.57156,-0.13178,0.04124,-0.06074, 1403636726163555328,4.81840,-3.67631,-1.21787,0.03221,0.81666,-0.05622,0.57347,-0.12056,0.04194,-0.10034, 1403636726263555584,4.80149,-3.67107,-1.22678,0.03263,0.81614,-0.05445,0.57436,-0.11969,0.04616,-0.11631, 1403636726363555328,4.78559,-3.66577,-1.23295,0.03248,0.81522,-0.05285,0.57581,-0.09560,0.04550,-0.04828, 1403636726463555584,4.77113,-3.66075,-1.23143,0.03099,0.81449,-0.05176,0.57703,-0.07727,0.05472,0.04548, 1403636726563555584,4.75655,-3.65538,-1.22386,0.02225,0.81321,-0.05611,0.57884,-0.07101,0.05364,0.11352, 1403636726663555328,4.74008,-3.65058,-1.20686,0.00789,0.81068,-0.06468,0.58185,-0.07925,0.01559,0.16858, 1403636726763555584,4.72262,-3.65222,-1.18707,0.00987,0.81016,-0.06268,0.58276,-0.09547,-0.04204,0.23875, 1403636726863555328,4.70313,-3.65649,-1.15685,0.01821,0.81027,-0.05380,0.58330,-0.13328,-0.06898,0.30715, 1403636726963555584,4.68349,-3.66331,-1.12483,0.02204,0.81071,-0.04593,0.58322,-0.15579,-0.07248,0.34209, 1403636727063555584,4.66309,-3.67052,-1.08760,0.02347,0.81053,-0.04144,0.58375,-0.16640,-0.07506,0.36896, 1403636727163555328,4.63804,-3.67779,-1.05125,0.02644,0.81145,-0.03675,0.58267,-0.20000,-0.07058,0.30857, 1403636727263555584,4.61691,-3.68337,-1.02441,0.03035,0.81221,-0.03250,0.58167,-0.21152,-0.03936,0.24182, 1403636727363555328,4.59352,-3.68657,-1.00240,0.03336,0.81358,-0.02981,0.57973,-0.22174,-0.00455,0.20281, 1403636727463555584,4.56852,-3.68615,-0.98337,0.03506,0.81358,-0.02836,0.57971,-0.23213,0.02550,0.18061, 1403636727563555584,4.53784,-3.68205,-0.96060,0.03029,0.81174,-0.03167,0.58237,-0.25564,0.05954,0.22029, 1403636727663555328,4.51185,-3.67604,-0.93612,0.02741,0.81062,-0.03369,0.58396,-0.25959,0.08220,0.27303, 1403636727763555584,4.48540,-3.66801,-0.90425,0.02872,0.81189,-0.03297,0.58217,-0.25675,0.10088,0.35233, 1403636727863555328,4.45794,-3.65836,-0.86563,0.03441,0.81277,-0.02915,0.58084,-0.28391,0.11682,0.41052, 1403636727963555584,4.42931,-3.64501,-0.82686,0.03959,0.81275,-0.02580,0.58070,-0.28922,0.15475,0.35534, 1403636728063555584,4.39470,-3.62821,-0.79110,0.03701,0.81311,-0.02790,0.58026,-0.29153,0.19352,0.29665, 1403636728163555328,4.36518,-3.60752,-0.76544,0.03288,0.81151,-0.03143,0.58257,-0.30540,0.21525,0.23489, 1403636728263555584,4.33424,-3.58486,-0.74543,0.03035,0.81047,-0.03343,0.58404,-0.31305,0.23012,0.17771, 1403636728363555328,4.30204,-3.56063,-0.73147,0.02956,0.81077,-0.03448,0.58359,-0.32236,0.25167,0.11790, 1403636728463555584,4.26360,-3.53614,-0.71632,0.02059,0.81077,-0.04085,0.58357,-0.33140,0.25634,0.05522, 1403636728563555584,4.22867,-3.51143,-0.71260,0.01089,0.81108,-0.04704,0.58294,-0.35210,0.23427,0.01318, 1403636728663555328,4.19202,-3.49032,-0.70900,0.00918,0.81154,-0.04205,0.58271,-0.36416,0.20063,0.01938, 1403636728763555584,4.15568,-3.46927,-0.70371,0.01067,0.81203,-0.02707,0.58288,-0.37247,0.19616,0.08672, 1403636728863555328,4.11463,-3.44644,-0.68887,0.00818,0.81252,-0.01024,0.58279,-0.38873,0.19843,0.16050, 1403636728963555584,4.08679,-3.40785,-0.70396,0.00200,0.81120,0.00439,0.58475,-0.39759,0.20773,0.14362, 1403636729063555584,4.04622,-3.38434,-0.69215,-0.00252,0.80933,0.01381,0.58719,-0.41394,0.21671,0.09006, 1403636729163555328,4.00410,-3.36013,-0.68746,-0.00621,0.80874,0.02015,0.58779,-0.43969,0.22555,0.02624, 1403636729263555584,3.95903,-3.33527,-0.68858,-0.01101,0.81052,0.02253,0.58517,-0.45431,0.24867,-0.02510, 1403636729363555328,3.91316,-3.30822,-0.69499,-0.01315,0.81059,0.02408,0.58498,-0.47469,0.26051,-0.04363, 1403636729463555584,3.86486,-3.28101,-0.69772,-0.01354,0.80999,0.02532,0.58574,-0.51285,0.26267,0.00727, 1403636729563555584,3.81346,-3.25513,-0.69409,-0.01479,0.81032,0.02551,0.58525,-0.51753,0.26842,0.07694, 1403636729663555328,3.76185,-3.22795,-0.68377,-0.01516,0.81116,0.02549,0.58407,-0.54666,0.26429,0.12652, 1403636729663555328,3.69820,-3.24875,-0.63656,-0.01649,0.81213,0.02594,0.58267,-0.54063,0.26632,0.14135, 1403636729763555584,3.69923,-3.24614,-0.63631,-0.01630,0.81211,0.02572,0.58270,-0.53959,0.26961,0.14198, 1403636729863555328,3.64505,-3.21806,-0.62295,-0.01558,0.81492,0.02516,0.57881,-0.55475,0.27331,0.08780, 1403636729963555584,3.59041,-3.18944,-0.61471,-0.01595,0.81700,0.02427,0.57591,-0.54535,0.28028,0.04556, 1403636730063555584,3.53817,-3.15828,-0.61206,-0.01667,0.81841,0.02350,0.57391,-0.53283,0.29580,0.00460, 1403636730163555328,3.48583,-3.12916,-0.61399,-0.01795,0.81867,0.02324,0.57351,-0.52407,0.29516,-0.03929, 1403636730263555584,3.43482,-3.09907,-0.61868,-0.01733,0.81934,0.02411,0.57254,-0.50670,0.28910,-0.07476, 1403636730363555328,3.38376,-3.07060,-0.62626,-0.01953,0.81933,0.02336,0.57251,-0.50256,0.28470,-0.10916, 1403636730463555584,3.33473,-3.04272,-0.63362,-0.01965,0.82215,0.02415,0.56842,-0.47312,0.28369,-0.06797, 1403636730563555584,3.28950,-3.01480,-0.63529,-0.01929,0.82672,0.02474,0.56174,-0.43118,0.28458,0.01535, 1403636730663555328,3.25007,-2.98727,-0.62957,-0.02024,0.82924,0.02436,0.55800,-0.35261,0.28958,0.11190, 1403636730763555584,3.21480,-2.95671,-0.61175,-0.02000,0.83050,0.02478,0.55611,-0.27986,0.29336,0.22322, 1403636730863555328,3.18311,-2.92730,-0.58975,-0.02163,0.83114,0.02384,0.55514,-0.25479,0.28869,0.24589, 1403636730963555584,3.14836,-2.89419,-0.56830,-0.02877,0.83090,0.01810,0.55538,-0.22061,0.28105,0.20545, 1403636731063555584,3.12580,-2.86615,-0.55074,-0.03872,0.82712,0.01004,0.56060,-0.17393,0.25718,0.16741, 1403636731163555328,3.10658,-2.84261,-0.53693,-0.04350,0.82039,0.00535,0.57012,-0.14695,0.20084,0.11400, 1403636731263555584,3.09268,-2.82555,-0.52998,-0.03817,0.81622,0.00796,0.57643,-0.14152,0.13996,0.06376, 1403636731363555328,3.08098,-2.81415,-0.52786,-0.02963,0.81477,0.01283,0.57888,-0.13910,0.10906,0.00260, 1403636731463555584,3.06927,-2.80621,-0.52968,-0.02372,0.81493,0.01665,0.57884,-0.13610,0.09177,-0.02474, 1403636731563555584,3.05623,-2.80034,-0.53659,-0.02856,0.81581,0.01364,0.57745,-0.14054,0.08028,-0.07568, 1403636731663555328,3.04312,-2.79501,-0.54657,-0.03328,0.81584,0.01025,0.57723,-0.14169,0.04290,-0.11015, 1403636731763555584,3.03015,-2.79482,-0.56107,-0.03007,0.81594,0.01233,0.57722,-0.14155,0.00230,-0.14165, 1403636731863555328,3.01661,-2.79687,-0.57860,-0.02649,0.81600,0.01525,0.57724,-0.14156,-0.02058,-0.18118, 1403636731963555584,3.00355,-2.80028,-0.59974,-0.02532,0.81531,0.01706,0.57823,-0.14168,-0.02973,-0.21704, 1403636732063555584,2.99015,-2.80499,-0.62530,-0.02453,0.81420,0.01850,0.57977,-0.14385,-0.04055,-0.25565, 1403636732163555328,2.97581,-2.81064,-0.65498,-0.02462,0.81401,0.01926,0.58000,-0.15217,-0.04323,-0.28754, 1403636732263555584,2.95960,-2.81772,-0.68127,-0.02319,0.81256,0.02108,0.58203,-0.17204,-0.05751,-0.23224, 1403636732363555328,2.94106,-2.82449,-0.70171,-0.02225,0.81228,0.02248,0.58241,-0.20316,-0.06429,-0.16027, 1403636732463555584,2.92116,-2.83140,-0.71133,-0.01256,0.80964,0.02947,0.58605,-0.23192,-0.05933,-0.04961, 1403636732563555584,2.89766,-2.83589,-0.71316,-0.00048,0.80564,0.03859,0.59115,-0.28789,-0.02119,0.00450, 1403636732663555328,2.86986,-2.83608,-0.71147,0.00249,0.80206,0.04172,0.59578,-0.32610,0.04065,0.04748, 1403636732763555584,2.83499,-2.82963,-0.70526,-0.00570,0.80108,0.03693,0.59738,-0.36871,0.09517,0.10859, 1403636732863555328,2.79650,-2.81844,-0.69293,-0.01215,0.80073,0.03313,0.59799,-0.42082,0.11283,0.10296, 1403636732963555584,2.75304,-2.80497,-0.68483,-0.01440,0.80270,0.03226,0.59534,-0.46504,0.13570,0.04234, 1403636733063555584,2.70835,-2.78903,-0.68330,-0.01500,0.80608,0.03251,0.59073,-0.47704,0.15565,0.04431, 1403636733163555328,2.65752,-2.77198,-0.67551,-0.01303,0.80846,0.03439,0.58740,-0.51044,0.17197,0.11554, 1403636733263555584,2.60813,-2.75137,-0.66088,-0.01264,0.80973,0.03569,0.58559,-0.53020,0.20267,0.18503, 1403636733363555328,2.55337,-2.72888,-0.64070,-0.01617,0.81142,0.03419,0.58324,-0.56185,0.22535,0.21017, 1403636733463555584,2.49912,-2.70432,-0.61471,-0.02003,0.81404,0.03214,0.57957,-0.53703,0.24023,0.26444, 1403636733563555584,2.44673,-2.67782,-0.58764,-0.02108,0.81620,0.03161,0.57652,-0.54270,0.25092,0.24064, 1403636733663555328,2.39267,-2.65206,-0.56495,-0.02193,0.81767,0.03155,0.57441,-0.54276,0.25373,0.18040, 1403636733763555584,2.33964,-2.62441,-0.54712,-0.02117,0.81847,0.03252,0.57324,-0.53086,0.26103,0.12621, 1403636733863555328,2.28666,-2.59725,-0.53730,-0.02172,0.82011,0.03248,0.57087,-0.52611,0.27278,0.07523, 1403636733963555584,2.23477,-2.56896,-0.53087,-0.01773,0.81806,0.03528,0.57378,-0.51081,0.28168,0.04364, 1403636734063555584,2.18099,-2.53836,-0.52610,-0.01641,0.81472,0.03560,0.57853,-0.51192,0.29589,0.01377, 1403636734163555328,2.13008,-2.50823,-0.52493,-0.01744,0.81316,0.03375,0.58080,-0.52477,0.30662,0.02953, 1403636734263555584,2.07837,-2.47668,-0.51747,-0.02050,0.81265,0.03261,0.58147,-0.53931,0.31776,0.09874, 1403636734363555328,2.02453,-2.44321,-0.50209,-0.02322,0.81424,0.03157,0.57921,-0.52626,0.32213,0.17583, 1403636734463555584,1.97204,-2.41112,-0.48087,-0.02528,0.81621,0.03137,0.57634,-0.52982,0.32024,0.21170, 1403636734563555584,1.92057,-2.37884,-0.46012,-0.02641,0.81841,0.03186,0.57315,-0.52010,0.32084,0.17411, 1403636734663555328,1.86892,-2.34783,-0.44528,-0.02654,0.82055,0.03300,0.57001,-0.50302,0.31280,0.11660, 1403636734763555584,1.81845,-2.31739,-0.43635,-0.02575,0.82068,0.03531,0.56972,-0.48722,0.31188,0.06277, 1403636734863555328,1.77059,-2.28744,-0.43411,-0.02691,0.82132,0.03617,0.56868,-0.46118,0.31195,0.00436, 1403636734963555584,1.72588,-2.25771,-0.43677,-0.02852,0.82157,0.03620,0.56823,-0.43060,0.30527,-0.03695, 1403636735063555584,1.68287,-2.22888,-0.44251,-0.02861,0.82111,0.03687,0.56887,-0.40999,0.30266,-0.06748, 1403636735163555328,1.64279,-2.19967,-0.44865,-0.03028,0.82123,0.03550,0.56869,-0.39564,0.29404,-0.03554, 1403636735263555584,1.60567,-2.17090,-0.44914,-0.03165,0.82258,0.03348,0.56679,-0.34685,0.29198,0.04748, 1403636735363555328,1.57316,-2.14352,-0.43847,-0.03249,0.82414,0.03097,0.56461,-0.28179,0.28477,0.17937, 1403636735463555584,1.54080,-2.11509,-0.41671,-0.03169,0.82274,0.02810,0.56684,-0.26979,0.27104,0.28450, 1403636735563555584,1.51088,-2.08910,-0.38903,-0.02856,0.82136,0.02602,0.56910,-0.23071,0.25564,0.35325, 1403636735663555328,1.48310,-2.06372,-0.35777,-0.02539,0.82047,0.02418,0.57061,-0.22152,0.24299,0.32805, 1403636735763555584,1.45754,-2.03993,-0.33208,-0.02340,0.82121,0.02245,0.56970,-0.20229,0.23663,0.26124, 1403636735863555328,1.43390,-2.01653,-0.31060,-0.02307,0.82073,0.02090,0.57046,-0.18807,0.22728,0.20675, 1403636735963555584,1.41307,-1.99499,-0.29496,-0.02243,0.82034,0.02090,0.57106,-0.16897,0.21910,0.16781, 1403636736063555584,1.39422,-1.97398,-0.28016,-0.02395,0.81920,0.02588,0.57242,-0.16769,0.22331,0.19985, 1403636736163555328,1.37680,-1.95252,-0.25816,-0.03091,0.81645,0.03427,0.57557,-0.17392,0.23264,0.27491, 1403636736263555584,1.35946,-1.93153,-0.22974,-0.03871,0.81435,0.04460,0.57737,-0.17950,0.22549,0.30264, 1403636736363555328,1.34356,-1.91024,-0.20481,-0.04336,0.81325,0.05440,0.57775,-0.18259,0.21373,0.24785, 1403636736463555584,1.32617,-1.88975,-0.18413,-0.04426,0.81244,0.06348,0.57789,-0.18232,0.20715,0.18473, 1403636736563555584,1.30810,-1.87075,-0.17013,-0.04057,0.81078,0.07304,0.57936,-0.18514,0.21351,0.10639, 1403636736663555328,1.28928,-1.84885,-0.16169,-0.03898,0.80954,0.07846,0.58050,-0.20728,0.24052,0.06556, 1403636736763555584,1.26785,-1.82529,-0.15921,-0.03879,0.80927,0.08134,0.58048,-0.22704,0.26234,0.00997, 1403636736863555328,1.24491,-1.79666,-0.15889,-0.04055,0.80840,0.08253,0.58140,-0.25417,0.29027,-0.01818, 1403636736963555584,1.22058,-1.76473,-0.16076,-0.04293,0.80896,0.08338,0.58033,-0.26824,0.32490,-0.04912, 1403636737063555584,1.19549,-1.72936,-0.16797,-0.05040,0.80916,0.08148,0.57973,-0.27986,0.34657,-0.07477, 1403636737163555328,1.16821,-1.69401,-0.17687,-0.05684,0.80900,0.08043,0.57951,-0.29263,0.35042,-0.11191, 1403636737263555584,1.13992,-1.65747,-0.19081,-0.06177,0.80950,0.08041,0.57830,-0.30731,0.34611,-0.15460, 1403636737363555328,1.11001,-1.62219,-0.20379,-0.06344,0.81170,0.08225,0.57476,-0.31847,0.33873,-0.11566, 1403636737463555584,1.07691,-1.58607,-0.21133,-0.06453,0.81413,0.08473,0.57083,-0.32171,0.33109,-0.04018, 1403636737563555584,1.04760,-1.55158,-0.20893,-0.06581,0.81563,0.08578,0.56838,-0.29984,0.32827,0.02886, 1403636737663555328,1.02266,-1.51826,-0.20115,-0.07123,0.81726,0.08336,0.56574,-0.30826,0.30721,0.07220, 1403636737763555584,0.99741,-1.48751,-0.18573,-0.07591,0.81871,0.08028,0.56348,-0.27499,0.28320,0.13862, 1403636737863555328,0.98220,-1.45776,-0.16282,-0.07887,0.81943,0.07732,0.56243,-0.25229,0.25296,0.23397, 1403636737963555584,0.96630,-1.43561,-0.13173,-0.07938,0.81938,0.07617,0.56260,-0.20172,0.21308,0.32628, 1403636738063555584,0.95695,-1.41722,-0.09331,-0.07745,0.81939,0.07555,0.56293,-0.17349,0.17525,0.38930, 1403636738163555328,0.94616,-1.40285,-0.05489,-0.07310,0.81782,0.07688,0.56561,-0.14696,0.14782,0.35339, 1403636738263555584,0.93859,-1.39048,-0.02197,-0.07051,0.81716,0.07657,0.56693,-0.12665,0.12751,0.28931, 1403636738363555328,0.93814,-1.38005,0.00301,-0.06883,0.81759,0.07550,0.56667,-0.09139,0.11272,0.22775, 1403636738463555584,0.93294,-1.37070,0.02247,-0.06675,0.81823,0.07370,0.56623,-0.06436,0.08926,0.16658, 1403636738563555584,0.93222,-1.36416,0.03465,-0.06216,0.82055,0.06621,0.56431,-0.03279,0.06462,0.09375, 1403636738663555328,0.93512,-1.36093,0.03886,-0.05514,0.82278,0.05324,0.56317,0.00174,0.03203,0.02669, 1403636738763555584,0.94612,-1.36280,0.03520,-0.04622,0.82195,0.03534,0.56659,0.02098,0.00011,-0.04461, 1403636738863555328,0.95551,-1.36663,0.02399,-0.03702,0.81937,0.01290,0.57193,0.04069,-0.03655,-0.10380, 1403636738963555584,1.08532,-1.95887,0.00188,-0.04221,0.81795,0.01072,0.57363,0.04807,-0.06743,-0.16164, 1403636739063555584,1.09500,-1.96760,-0.01564,-0.03107,0.81730,-0.01637,0.57514,0.06790,-0.12154,-0.20139, 1403636739163555328,1.10459,-1.98425,-0.03876,-0.01027,0.81705,-0.03801,0.57522,0.06110,-0.17350,-0.25251, 1403636739263555584,1.11204,-2.00611,-0.06675,0.01392,0.81578,-0.05796,0.57528,0.04834,-0.21356,-0.28011, 1403636739363555328,1.11830,-2.02960,-0.09726,0.04062,0.81399,-0.07560,0.57451,0.04257,-0.23202,-0.27258, 1403636739463555584,1.12286,-2.05256,-0.12111,0.06259,0.81208,-0.09405,0.57251,0.03417,-0.22779,-0.18956, 1403636739563555584,1.12590,-2.07517,-0.13547,0.07571,0.80911,-0.11496,0.57131,0.02780,-0.20101,-0.10602, 1403636739663555328,1.13200,-2.09571,-0.14256,0.08416,0.80744,-0.13031,0.56919,0.04805,-0.19065,-0.04237, 1403636739763555584,1.13927,-2.11506,-0.14711,0.08751,0.80543,-0.14117,0.56895,0.07358,-0.18170,-0.06466, 1403636739863555328,1.14647,-2.13425,-0.15682,0.08902,0.80457,-0.14745,0.56833,0.08271,-0.18681,-0.12018, 1403636739963555584,1.15340,-2.15504,-0.17150,0.09080,0.80420,-0.14906,0.56815,0.09207,-0.20158,-0.16103, 1403636740063555584,1.15778,-2.17608,-0.19027,0.09291,0.80511,-0.14712,0.56703,0.08011,-0.19945,-0.20377, 1403636740163555328,1.15859,-2.19881,-0.21240,0.09780,0.80550,-0.14161,0.56705,0.06254,-0.20467,-0.24005, 1403636740263555584,1.16184,-2.21902,-0.23823,0.10274,0.80582,-0.13503,0.56733,0.05420,-0.18927,-0.27181, 1403636740363555328,1.16328,-2.23767,-0.26782,0.09869,0.80460,-0.13430,0.56995,0.05016,-0.15965,-0.31384, 1403636740463555584,1.16273,-2.25467,-0.30136,0.09218,0.80434,-0.13592,0.57102,0.03786,-0.14406,-0.35453, 1403636740563555584,1.16406,-2.27026,-0.33502,0.08856,0.80291,-0.13538,0.57373,0.01823,-0.14346,-0.29517, 1403636740663555328,1.16300,-2.28638,-0.36336,0.08538,0.80350,-0.13477,0.57353,0.01053,-0.15433,-0.23590, 1403636740763555584,1.15994,-2.30278,-0.38588,0.08352,0.80648,-0.13369,0.56987,-0.01440,-0.15592,-0.18703, 1403636740863555328,1.15471,-2.32091,-0.40624,0.08187,0.80646,-0.13286,0.57032,-0.03547,-0.17094,-0.22753, 1403636740963555584,1.15068,-2.33775,-0.43294,0.08794,0.80514,-0.12710,0.57259,-0.04721,-0.17176,-0.28373, 1403636741063555584,1.14589,-2.35299,-0.46601,0.10042,0.80426,-0.11887,0.57354,-0.05955,-0.15189,-0.35714, 1403636741163555328,1.14080,-2.36511,-0.50692,0.10568,0.80156,-0.12246,0.57562,-0.09090,-0.11787,-0.44004, 1403636741263555584,1.13342,-2.37402,-0.55296,0.10268,0.79885,-0.13484,0.57716,-0.09465,-0.07059,-0.49948, 1403636741363555328,1.12507,-2.38043,-0.60396,0.09763,0.80035,-0.14586,0.57327,-0.12388,-0.04963,-0.50938, 1403636741463555584,1.11446,-2.38637,-0.65131,0.09628,0.80427,-0.15313,0.56607,-0.12357,-0.04770,-0.43203, 1403636741563555584,1.10221,-2.39414,-0.69285,0.09260,0.80991,-0.15975,0.55672,-0.11361,-0.06386,-0.36008, 1403636741663555328,1.09124,-2.40380,-0.72649,0.09550,0.81307,-0.15956,0.55166,-0.08222,-0.08159,-0.28384, 1403636741763555584,1.08483,-2.41522,-0.75056,0.09773,0.81165,-0.15778,0.55387,-0.03556,-0.10820,-0.20163, 1403636741863555328,1.08187,-2.42652,-0.77092,0.10174,0.80919,-0.15304,0.55806,-0.02652,-0.11566,-0.22736, 1403636741963555584,1.07980,-2.43867,-0.79755,0.10761,0.80848,-0.14558,0.55999,-0.00804,-0.11119,-0.30312, 1403636742063555584,1.07971,-2.44941,-0.83053,0.11017,0.81153,-0.13984,0.55653,0.02145,-0.08533,-0.31642, 1403636742163555328,1.08231,-2.46001,-0.86294,0.10369,0.81641,-0.14046,0.55045,0.04268,-0.06684,-0.28875, 1403636742263555584,1.08780,-2.46746,-0.88763,0.09600,0.81758,-0.14217,0.54967,0.08836,-0.06090,-0.19804, 1403636742363555328,1.09823,-2.47563,-0.90431,0.09197,0.81425,-0.14170,0.55540,0.10431,-0.06179,-0.14122, 1403636742463555584,1.11122,-2.48309,-0.91072,0.09019,0.81049,-0.14002,0.56157,0.13004,-0.07394,-0.07447, 1403636742563555584,1.12540,-2.49097,-0.91371,0.08919,0.80903,-0.13828,0.56427,0.15608,-0.08239,-0.05444, 1403636742663555328,1.14087,-2.49902,-0.91688,0.08943,0.80643,-0.13658,0.56834,0.15733,-0.08226,-0.09164, 1403636742763555584,1.15423,-2.50776,-0.92943,0.08857,0.80391,-0.13493,0.57243,0.14948,-0.08071,-0.19295, 1403636742863555328,1.16618,-2.51604,-0.94865,0.08709,0.80245,-0.13424,0.57486,0.12713,-0.07266,-0.26263, 1403636742963555584,1.17634,-2.52605,-0.97477,0.08597,0.80208,-0.13505,0.57536,0.09790,-0.07770,-0.33055, 1403636743063555584,1.18500,-2.53539,-1.01260,0.08769,0.80575,-0.14047,0.56864,0.09001,-0.08750,-0.39484, 1403636743163555328,1.19182,-2.54506,-1.05170,0.08736,0.81594,-0.14921,0.55167,0.09858,-0.09826,-0.36265, 1403636743263555584,1.20006,-2.55769,-1.08279,0.08927,0.82458,-0.15416,0.53697,0.13871,-0.12340,-0.29718, 1403636743363555328,1.21355,-2.57249,-1.10471,0.08800,0.82685,-0.15919,0.53220,0.20247,-0.15711,-0.19219, 1403636743463555584,1.23614,-2.59142,-1.11856,0.08864,0.82441,-0.16122,0.53526,0.28550,-0.20121,-0.11485, 1403636743563555584,1.26692,-2.61245,-1.13440,0.09141,0.81949,-0.16101,0.54237,0.32438,-0.22395,-0.15615, 1403636743663555328,1.30294,-2.63637,-1.15551,0.09669,0.81506,-0.15738,0.54915,0.36235,-0.23961,-0.24349, 1403636743763555584,1.34114,-2.66295,-1.18244,0.10271,0.80916,-0.15547,0.55727,0.39085,-0.25726,-0.29835, 1403636743863555328,1.38032,-2.68762,-1.21530,0.10470,0.80429,-0.16208,0.56204,0.40163,-0.24364,-0.36188, 1403636743863555328,1.45637,-2.70972,-1.26419,0.11035,0.80021,-0.16380,0.56626,0.40900,-0.23913,-0.41660, 1403636743963555584,1.45632,-2.71200,-1.26283,0.11012,0.80031,-0.16364,0.56622,0.41084,-0.24341,-0.41748, 1403636744063555584,1.49684,-2.73835,-1.30592,0.11508,0.79790,-0.16686,0.56769,0.41716,-0.23438,-0.47760, 1403636744163555328,1.53665,-2.76154,-1.35647,0.12049,0.79533,-0.17366,0.56813,0.38900,-0.21804,-0.53422, 1403636744263555584,1.57246,-2.78403,-1.41118,0.12086,0.79174,-0.18155,0.57059,0.36632,-0.19600,-0.58440, 1403636744363555328,1.60870,-2.80522,-1.47294,0.11980,0.79009,-0.18787,0.57106,0.34672,-0.18890,-0.64633, 1403636744463555584,1.64249,-2.82362,-1.53960,0.11496,0.78849,-0.19382,0.57227,0.32539,-0.17798,-0.69366, 1403636744563555584,1.67140,-2.84378,-1.61015,0.10884,0.78626,-0.19757,0.57525,0.29829,-0.20932,-0.73270, 1403636744663555328,1.69827,-2.86471,-1.68554,0.10564,0.78405,-0.19884,0.57843,0.25804,-0.22906,-0.76029, 1403636744763555584,1.72062,-2.88881,-1.76348,0.11266,0.78339,-0.19362,0.57976,0.20098,-0.24282,-0.77261, 1403636744863555328,1.73601,-2.91389,-1.83742,0.11888,0.78559,-0.18906,0.57704,0.13884,-0.23596,-0.68799, 1403636744963555584,1.74961,-2.93709,-1.90307,0.12141,0.79034,-0.18614,0.57093,0.09386,-0.21579,-0.59968, 1403636745063555584,1.75980,-2.96028,-1.96035,0.11955,0.79552,-0.18441,0.56467,0.10695,-0.20769,-0.51725, 1403636745163555328,1.77014,-2.98274,-2.00929,0.11422,0.79963,-0.18369,0.56018,0.11108,-0.21771,-0.45172, 1403636745263555584,1.78232,-3.00616,-2.05183,0.11279,0.80163,-0.17885,0.55918,0.13350,-0.23168,-0.41943, 1403636745363555328,1.79713,-3.03020,-2.08724,0.11273,0.79981,-0.17270,0.56371,0.14275,-0.23687,-0.31204, 1403636745463555584,1.81266,-3.05314,-2.10821,0.11486,0.79536,-0.16788,0.57098,0.12995,-0.22581,-0.21361, 1403636745563555584,1.82867,-3.07489,-2.12290,0.11522,0.79264,-0.16843,0.57453,0.11707,-0.20585,-0.15312, 1403636745663555328,1.84294,-3.09475,-2.13063,0.11804,0.79096,-0.17147,0.57537,0.10085,-0.18159,-0.07574, 1403636745763555584,1.85577,-3.11224,-2.13260,0.12391,0.79003,-0.17438,0.57454,0.09471,-0.15647,-0.00709, 1403636745863555328,1.86704,-3.12698,-2.12636,0.13063,0.79028,-0.17760,0.57170,0.09632,-0.13116,0.06502, 1403636745963555584,1.87937,-3.13881,-2.11501,0.13943,0.78972,-0.18021,0.56959,0.09797,-0.08992,0.12570, 1403636746063555584,1.88837,-3.14749,-2.10064,0.14718,0.78971,-0.18233,0.56697,0.08605,-0.03858,0.19890, 1403636746163555328,1.89876,-3.15064,-2.08068,0.15321,0.78958,-0.18401,0.56501,0.07848,0.03099,0.28636, 1403636746263555584,1.90777,-3.14677,-2.05376,0.15633,0.78533,-0.18557,0.56955,0.08645,0.08682,0.33230, 1403636746363555328,1.91856,-3.13958,-2.02753,0.15843,0.77645,-0.18590,0.58092,0.06798,0.15208,0.25726, 1403636746463555584,1.92513,-3.12260,-2.00943,0.15571,0.77579,-0.18806,0.58184,0.04047,0.21824,0.13205, 1403636746563555584,1.92953,-3.10256,-2.00212,0.14695,0.78413,-0.19220,0.57150,0.02435,0.26355,0.03824, 1403636746663555328,1.93101,-3.07545,-2.00151,0.13464,0.79717,-0.19715,0.55455,0.03728,0.28061,-0.01333, 1403636746763555584,1.93541,-3.04992,-2.00594,0.11949,0.80616,-0.20337,0.54265,0.06899,0.25668,-0.05198, 1403636746863555328,1.94413,-3.02765,-2.01006,0.10921,0.80705,-0.20678,0.54220,0.10901,0.20497,-0.04611, 1403636746963555584,1.95655,-3.01001,-2.01210,0.10648,0.80588,-0.20532,0.54504,0.13902,0.13840,-0.00916, 1403636747063555584,1.97123,-2.99900,-2.00623,0.11119,0.80330,-0.19976,0.54994,0.16743,0.07555,0.11984, 1403636747163555328,1.98977,-2.99195,-1.99078,0.12051,0.79952,-0.19292,0.55590,0.22504,0.03604,0.20296, 1403636747263555584,2.01147,-2.98602,-1.96617,0.12990,0.79675,-0.18715,0.55972,0.25462,0.03697,0.29111, 1403636747363555328,2.03474,-2.97665,-1.93402,0.13419,0.79577,-0.18594,0.56051,0.26031,0.06493,0.33470, 1403636747463555584,2.05909,-2.96559,-1.90518,0.13713,0.79484,-0.18669,0.56087,0.26194,0.09279,0.27069, 1403636747563555584,2.08067,-2.95226,-1.88240,0.13692,0.79559,-0.19004,0.55872,0.26690,0.11814,0.21484, 1403636747663555328,2.10544,-2.93800,-1.86177,0.13700,0.79598,-0.19293,0.55715,0.26647,0.14682,0.25850, 1403636747763555584,2.12964,-2.92187,-1.83559,0.13827,0.79596,-0.19541,0.55601,0.26957,0.16365,0.32716, 1403636747863555328,2.15698,-2.90491,-1.80056,0.13812,0.79485,-0.19855,0.55651,0.30635,0.17125,0.42011, 1403636747963555584,2.18612,-2.88668,-1.75844,0.12730,0.79034,-0.20934,0.56155,0.31163,0.17679,0.43931, 1403636748063555584,2.21053,-2.86878,-1.71990,0.11781,0.78701,-0.21911,0.56456,0.28467,0.15106,0.37754, 1403636748163555328,2.23465,-2.85417,-1.68633,0.11423,0.78646,-0.22403,0.56412,0.25543,0.11144,0.30993, 1403636748263555584,2.25900,-2.84613,-1.66014,0.12096,0.78846,-0.22068,0.56125,0.23643,0.06162,0.24555, 1403636748363555328,2.28118,-2.84220,-1.64052,0.13425,0.79124,-0.21166,0.55778,0.24043,0.03481,0.20281, 1403636748463555584,2.30515,-2.83947,-1.62134,0.14231,0.79207,-0.20530,0.55698,0.24545,0.03418,0.21853, 1403636748563555584,2.32894,-2.83630,-1.60055,0.14514,0.79169,-0.20292,0.55766,0.24591,0.05712,0.27107, 1403636748663555328,2.35236,-2.83128,-1.57013,0.13915,0.78948,-0.20767,0.56056,0.23757,0.06020,0.31526, 1403636748763555584,2.37541,-2.82713,-1.53950,0.13141,0.78721,-0.21420,0.56316,0.22369,0.05361,0.28577, 1403636748863555328,2.39631,-2.82574,-1.51587,0.12736,0.78550,-0.21908,0.56460,0.20756,0.02772,0.23351, 1403636748963555584,2.41714,-2.82552,-1.49370,0.12648,0.78375,-0.22362,0.56546,0.18462,-0.00771,0.19369, 1403636749063555584,2.43693,-2.82724,-1.47721,0.12490,0.78193,-0.22847,0.56638,0.16296,-0.03255,0.16716, 1403636749163555328,2.45256,-2.83327,-1.46204,0.12180,0.77970,-0.23351,0.56807,0.12756,-0.07447,0.14851, 1403636749263555584,2.46460,-2.84375,-1.44488,0.13009,0.78009,-0.23075,0.56683,0.08878,-0.11626,0.21000, 1403636749363555328,2.47372,-2.85711,-1.42145,0.14039,0.78119,-0.22556,0.56494,0.05440,-0.12586,0.28017, 1403636749463555584,2.47873,-2.87080,-1.39080,0.14887,0.78225,-0.22049,0.56330,0.03496,-0.12005,0.31819, 1403636749563555584,2.48378,-2.88524,-1.35967,0.15627,0.78434,-0.21475,0.56060,0.03795,-0.11033,0.33509, 1403636749663555328,2.48775,-2.89734,-1.32843,0.16064,0.78484,-0.21055,0.56026,0.04198,-0.08197,0.26539, 1403636749763555584,2.49449,-2.90789,-1.30504,0.15875,0.78278,-0.20966,0.56400,0.05450,-0.05422,0.21218, 1403636749863555328,2.49857,-2.91346,-1.28602,0.15623,0.77768,-0.20881,0.57202,0.03961,-0.02659,0.15314, 1403636749963555584,2.50188,-2.91685,-1.27323,0.15414,0.77458,-0.20658,0.57757,0.01553,0.00385,0.09500, 1403636750063555584,2.50348,-2.91770,-1.26591,0.15130,0.77405,-0.20439,0.57981,-0.02233,0.04072,0.05596, 1403636750163555328,2.49791,-2.91244,-1.26010,0.14823,0.77681,-0.20301,0.57739,-0.06941,0.07423,0.08168, 1403636750263555584,2.48974,-2.90597,-1.24866,0.14528,0.78042,-0.20165,0.57374,-0.08568,0.09092,0.15385, 1403636750363555328,2.47835,-2.89544,-1.22858,0.14265,0.78420,-0.19973,0.56990,-0.08750,0.09781,0.26407, 1403636750463555584,2.46626,-2.88456,-1.19786,0.13925,0.78647,-0.19811,0.56818,-0.11050,0.12668,0.36235, 1403636750563555584,2.45139,-2.87091,-1.15774,0.13728,0.78758,-0.19467,0.56831,-0.11209,0.13173,0.48207, 1403636750663555328,2.43554,-2.85662,-1.11004,0.13658,0.78888,-0.19117,0.56787,-0.12095,0.12207,0.53280, 1403636750763555584,2.41971,-2.84132,-1.05637,0.13490,0.79067,-0.18914,0.56646,-0.13135,0.13897,0.57503, 1403636750863555328,2.40088,-2.82342,-0.99673,0.13461,0.79190,-0.18721,0.56544,-0.16742,0.16843,0.64997, 1403636750963555584,2.38358,-2.80561,-0.93039,0.13672,0.79421,-0.18470,0.56252,-0.16576,0.17986,0.65225, 1403636751063555584,2.36725,-2.78572,-0.86803,0.13797,0.79527,-0.18417,0.56089,-0.15356,0.20087,0.60420, 1403636751163555328,2.34937,-2.76286,-0.81354,0.13635,0.79650,-0.18643,0.55879,-0.13889,0.21682,0.52417, 1403636751263555584,2.33617,-2.74046,-0.76418,0.13503,0.80157,-0.18882,0.55100,-0.12184,0.23286,0.49885, 1403636751363555328,2.32480,-2.71696,-0.71607,0.13022,0.80213,-0.19387,0.54959,-0.09178,0.22874,0.49085, 1403636751463555584,2.31749,-2.69493,-0.66749,0.12372,0.79776,-0.20024,0.55514,-0.06939,0.21276,0.48338, 1403636751563555584,2.31024,-2.67597,-0.62091,0.11737,0.79329,-0.20199,0.56223,-0.06029,0.19268,0.45713, 1403636751663555328,2.30520,-2.65951,-0.57706,0.10979,0.79312,-0.20073,0.56446,-0.07204,0.15719,0.42433, 1403636751763555584,2.32976,-2.66067,-0.59647,0.10310,0.79829,-0.19838,0.55924,-0.08465,0.11159,0.39058, 1403636751863555328,2.32223,-2.65309,-0.55909,0.10109,0.80491,-0.19475,0.55134,-0.08555,0.05326,0.38266, 1403636751963555584,2.31817,-2.65038,-0.52253,0.10664,0.80927,-0.18687,0.54662,-0.04758,0.00351,0.38091, 1403636752063555584,2.31702,-2.65235,-0.48628,0.11161,0.81055,-0.18124,0.54562,-0.01118,-0.02592,0.36673, 1403636752163555328,2.32254,-2.65659,-0.45421,0.11350,0.80753,-0.17724,0.55100,0.04194,-0.03977,0.35667, 1403636752263555584,2.32870,-2.66253,-0.42282,0.11135,0.80334,-0.16988,0.55981,0.05038,-0.03757,0.36153, 1403636752363555328,2.33725,-2.66557,-0.38802,0.10530,0.79953,-0.15910,0.56951,0.07117,-0.03298,0.40887, 1403636752463555584,2.34552,-2.66850,-0.34686,0.09529,0.80050,-0.14516,0.57363,0.06559,-0.02649,0.44932, 1403636752563555584,2.35225,-2.67102,-0.30468,0.08450,0.80419,-0.12768,0.57432,0.05224,-0.02345,0.41761, 1403636752663555328,2.35645,-2.67381,-0.26822,0.07266,0.80903,-0.10903,0.57298,0.03407,-0.02224,0.34335, 1403636752763555584,2.35899,-2.67608,-0.23822,0.05930,0.81197,-0.09052,0.57358,0.03107,-0.02630,0.28024, 1403636752863555328,2.34439,-2.67857,-0.20756,0.04740,0.81537,-0.06839,0.57293,0.01534,-0.01533,0.21121, 1403636752963555584,2.34554,-2.67788,-0.18962,0.03893,0.81655,-0.04426,0.57426,0.01667,0.00598,0.13617, 1403636753063555584,2.34718,-2.67501,-0.17981,0.02882,0.81820,-0.02011,0.57386,0.01689,0.02942,0.07045, 1403636753163555328,2.34938,-2.66856,-0.17626,0.01623,0.81895,0.00339,0.57363,0.02320,0.06893,0.00399, 1403636753263555584,2.35046,-2.65905,-0.17956,0.00413,0.81882,0.02768,0.57337,0.02301,0.10966,-0.05534, 1403636753363555328,2.35220,-2.64659,-0.18680,-0.00688,0.81689,0.05111,0.57449,0.02585,0.14340,-0.09385, 1403636753363555328,2.34077,-2.65615,-0.23071,-0.00671,0.81690,0.05086,0.57449,0.02594,0.14338,-0.09385, 1403636753463555584,2.34172,-2.64030,-0.23905,-0.02217,0.81509,0.06280,0.57549,0.03196,0.18559,-0.05839, 1403636753563555584,2.34024,-2.62194,-0.24000,-0.03543,0.81418,0.06785,0.57554,-0.00321,0.20041,0.02436, 1403636753663555328,2.33517,-2.60203,-0.23432,-0.04296,0.81268,0.07015,0.57687,-0.03921,0.20140,0.08057, 1403636753763555584,2.33044,-2.58099,-0.22800,-0.04539,0.81189,0.07092,0.57770,-0.04204,0.20196,0.05816, 1403636753863555328,2.32394,-2.55978,-0.22600,-0.04437,0.81108,0.07122,0.57888,-0.07148,0.21212,-0.01476, 1403636753963555584,2.31677,-2.53771,-0.22938,-0.04383,0.81115,0.06927,0.57906,-0.08327,0.22320,-0.04277, 1403636754063555584,2.30854,-2.51481,-0.23122,-0.04212,0.80910,0.06700,0.58231,-0.08218,0.23389,0.02322, 1403636754163555328,2.29914,-2.49047,-0.22656,-0.04074,0.80570,0.06458,0.58738,-0.11871,0.24330,0.07779, 1403636754263555584,2.27520,-2.50001,-0.22271,-0.03448,0.80388,0.06068,0.59069,-0.15555,0.25455,0.13337, 1403636754363555328,2.25854,-2.47389,-0.20701,-0.03120,0.80377,0.06069,0.59102,-0.16868,0.27768,0.18336, 1403636754463555584,2.23716,-2.44553,-0.18708,-0.03324,0.80398,0.06246,0.59043,-0.22209,0.30713,0.18549, 1403636754563555584,2.21086,-2.41351,-0.17051,-0.03564,0.80500,0.06952,0.58811,-0.25895,0.33859,0.12676, 1403636754663555328,2.18174,-2.38005,-0.16109,-0.04213,0.80528,0.08033,0.58591,-0.29199,0.36401,0.07108, 1403636754763555584,2.14871,-2.34432,-0.15573,-0.05302,0.80434,0.09288,0.58447,-0.32395,0.38709,0.02586, 1403636754863555328,2.11346,-2.30681,-0.15538,-0.06319,0.80303,0.10510,0.58318,-0.34869,0.38648,-0.02140, 1403636754963555584,2.07627,-2.26720,-0.16000,-0.07059,0.80165,0.11460,0.58245,-0.37431,0.39475,-0.06770, 1403636755063555584,2.03683,-2.22718,-0.16738,-0.07519,0.80067,0.12178,0.58176,-0.40606,0.40296,-0.07363, 1403636755163555328,1.99229,-2.18720,-0.17158,-0.07948,0.80062,0.12586,0.58039,-0.46860,0.40394,-0.02048, 1403636755263555584,1.94425,-2.14570,-0.16914,-0.08189,0.80191,0.12799,0.57780,-0.47975,0.41561,0.04821, 1403636755363555328,1.89524,-2.10420,-0.16002,-0.08248,0.80831,0.12931,0.56843,-0.49660,0.41543,0.11441, 1403636755463555584,1.80690,-2.05223,-0.13841,-0.08092,0.81335,0.12931,0.56142,-0.45553,0.43515,0.15999, 1403636755563555584,1.76539,-2.00840,-0.12152,-0.07796,0.81396,0.13009,0.56077,-0.43087,0.45646,0.12813, 1403636755663555328,1.72461,-1.95965,-0.11020,-0.07762,0.81245,0.12844,0.56338,-0.41186,0.48172,0.05891, 1403636755763555584,1.68329,-1.90906,-0.10625,-0.07857,0.81137,0.12593,0.56537,-0.39904,0.49640,0.01760, 1403636755863555328,1.64325,-1.85704,-0.10641,-0.07994,0.80895,0.12369,0.56912,-0.39508,0.51102,-0.03540, 1403636755963555584,1.60277,-1.80260,-0.10729,-0.08104,0.80709,0.12225,0.57192,-0.38903,0.52475,-0.00684, 1403636756063555584,1.56239,-1.74816,-0.10528,-0.08221,0.80589,0.12121,0.57366,-0.37533,0.53109,0.05161, 1403636756163555328,1.51957,-1.68989,-0.09499,-0.08064,0.80462,0.12205,0.57549,-0.37032,0.55135,0.13311, 1403636756263555584,1.47781,-1.63456,-0.08267,-0.08169,0.80372,0.12167,0.57668,-0.39613,0.55193,0.14299, 1403636756363555328,1.43353,-1.57567,-0.07057,-0.08184,0.80320,0.12229,0.57724,-0.40186,0.55522,0.10341, 1403636756463555584,1.38910,-1.52000,-0.06567,-0.08209,0.80251,0.12309,0.57800,-0.42219,0.55148,0.03003, 1403636756563555584,1.34127,-1.44432,-0.06727,-0.08257,0.80224,0.12347,0.57822,-0.43755,0.55828,-0.03123, 1403636756663555328,1.29569,-1.38950,-0.07192,-0.08399,0.80165,0.12362,0.57881,-0.46193,0.55693,-0.02701, 1403636756763555584,1.24059,-1.33220,-0.07310,-0.08478,0.80028,0.12384,0.58054,-0.51344,0.54182,0.01340, 1403636756863555328,1.18835,-1.27836,-0.06760,-0.08643,0.79997,0.12411,0.58067,-0.54158,0.52926,0.05818, 1403636756963555584,1.12610,-1.22416,-0.05503,-0.08730,0.80014,0.12423,0.58028,-0.58255,0.52042,0.13243, 1403636757063555584,1.07044,-1.17416,-0.04074,-0.08924,0.80036,0.12396,0.57974,-0.59233,0.51081,0.11568, 1403636757163555328,1.00772,-1.12393,-0.02961,-0.08935,0.80048,0.12414,0.57951,-0.61054,0.50159,0.06632, 1403636757263555584,0.94693,-1.07266,-0.02494,-0.08913,0.80037,0.12466,0.57960,-0.62717,0.49002,0.01237, 1403636757363555328,0.88035,-1.02086,-0.02448,-0.08714,0.80128,0.12640,0.57825,-0.64694,0.49192,-0.04504, 1403636757463555584,0.81644,-0.96905,-0.02992,-0.08386,0.80324,0.12925,0.57539,-0.65292,0.49906,-0.03789, 1403636757563555584,0.74784,-0.91442,-0.03010,-0.08361,0.80629,0.13487,0.56983,-0.67778,0.52194,0.00104, 1403636757663555328,0.68086,-0.86165,-0.02597,-0.08587,0.81213,0.14238,0.55929,-0.65851,0.54646,0.07600, 1403636757763555584,0.61423,-0.80088,-0.01443,-0.08980,0.81745,0.14835,0.54927,-0.60459,0.58244,0.16344, 1403636757863555328,0.55413,-0.73741,0.00578,-0.09441,0.82008,0.15256,0.54340,-0.53646,0.61666,0.29458, 1403636757963555584,0.47467,-0.63291,0.05952,-0.10237,0.81756,0.15767,0.54430,-0.48814,0.64172,0.35149, 1403636758063555584,0.42412,-0.56149,0.09269,-0.10701,0.80880,0.15831,0.55618,-0.44754,0.65495,0.29972, 1403636758063555584,0.36256,-0.46810,0.11725,-0.11125,0.80205,0.15691,0.56543,-0.43894,0.65671,0.23901, 1403636758163555328,0.37768,-0.49003,0.11921,-0.11151,0.80198,0.15727,0.56538,-0.43953,0.65632,0.23901, 1403636758263555584,0.32985,-0.41845,0.13792,-0.11441,0.79808,0.15642,0.57053,-0.45470,0.64179,0.16515, 1403636758363555328,0.28334,-0.35250,0.14817,-0.11687,0.79808,0.15511,0.57039,-0.46644,0.62317,0.06070, 1403636758463555584,0.23574,-0.29061,0.14726,-0.12024,0.80749,0.15261,0.55696,-0.46580,0.61791,-0.03269, 1403636758563555584,0.34115,-0.39904,0.13699,-0.12911,0.82040,0.14839,0.53689,-0.42067,0.58745,-0.05785, 1403636758663555328,0.30426,-0.34494,0.12979,-0.13350,0.82655,0.14460,0.52734,-0.30189,0.54869,-0.03333, 1403636758763555584,0.27805,-0.29422,0.12363,-0.13087,0.81806,0.14501,0.54094,-0.20701,0.50795,-0.03501, 1403636758863555328,0.25961,-0.24654,0.11451,-0.12927,0.80830,0.14595,0.55555,-0.16132,0.46259,-0.05693, 1403636758963555584,0.24914,-0.20383,0.10130,-0.12316,0.80200,0.14969,0.56500,-0.15381,0.42060,-0.12412, 1403636759063555584,0.23667,-0.16358,0.08694,-0.12392,0.79932,0.14925,0.56873,-0.15751,0.40747,-0.14890, 1403636759163555328,0.22274,-0.12560,0.07077,-0.13958,0.79961,0.13854,0.56740,-0.15304,0.36849,-0.17659, 1403636759263555584,0.20837,-0.09367,0.05244,-0.15609,0.80056,0.12731,0.56438,-0.13160,0.27965,-0.21593, 1403636759363555328,0.19861,-0.07361,0.02984,-0.15276,0.79966,0.13045,0.56586,-0.10533,0.17082,-0.24324, 1403636759463555584,0.19996,-0.04665,-0.00775,-0.13553,0.79898,0.13482,0.57017,-0.09104,0.08734,-0.28660, 1403636759563555584,0.19179,-0.04000,-0.03826,-0.12483,0.79746,0.14342,0.57263,-0.10225,0.05359,-0.30168, 1403636759663555328,0.18216,-0.03765,-0.06345,-0.11733,0.79641,0.14998,0.57399,-0.12745,0.01405,-0.20587, 1403636759763555584,0.17120,-0.03689,-0.07745,-0.11668,0.79613,0.15176,0.57405,-0.12857,0.01628,-0.09797, 1403636759863555328,0.15852,-0.03600,-0.08304,-0.11580,0.79598,0.15243,0.57425,-0.14052,0.00459,-0.05964, 1403636759963555584,0.14289,-0.03732,-0.08954,-0.11402,0.80214,0.15375,0.56562,-0.16390,-0.00550,-0.08929, 1403636760063555584,0.12676,-0.03824,-0.09891,-0.11466,0.80740,0.15273,0.55824,-0.15119,-0.00494,-0.11020, 1403636760163555328,0.11288,-0.03884,-0.11004,-0.11254,0.81198,0.15366,0.55173,-0.12696,0.00884,-0.10966, 1403636760263555584,0.10258,-0.03859,-0.12017,-0.11500,0.80556,0.15166,0.56111,-0.09170,0.00292,-0.10068, 1403636760363555328,0.09308,-0.03964,-0.13251,-0.11672,0.79917,0.15049,0.57013,-0.09101,-0.01065,-0.14065, 1403636760463555584,0.08300,-0.04153,-0.14791,-0.11465,0.79587,0.15218,0.57470,-0.11319,-0.02426,-0.16017, 1403636760563555584,0.07565,0.01494,-0.16219,-0.11278,0.79933,0.15603,0.56922,-0.13722,-0.01894,-0.17074, 1403636760663555328,0.06210,0.01256,-0.17969,-0.10896,0.80442,0.15923,0.56185,-0.14225,-0.01672,-0.16328, 1403636760763555584,0.04901,0.01158,-0.19472,-0.10572,0.80582,0.16234,0.55957,-0.12606,0.00505,-0.13457, 1403636760863555328,0.03705,0.01321,-0.20753,-0.10458,0.80193,0.16394,0.56488,-0.12495,0.02557,-0.12090, 1403636760963555584,0.02442,0.01720,-0.21968,-0.10427,0.80011,0.16511,0.56718,-0.13915,0.05437,-0.13119, 1403636761063555584,0.01015,0.02400,-0.23429,-0.10687,0.80526,0.16369,0.55977,-0.14637,0.08111,-0.14404, 1403636761163555328,-0.00355,0.03300,-0.24967,-0.10828,0.81346,0.16325,0.54764,-0.12399,0.10489,-0.14559, 1403636761263555584,-0.01447,0.04433,-0.26368,-0.10753,0.81969,0.16436,0.53809,-0.08352,0.13307,-0.11406, 1403636761363555328,-0.01931,0.05892,-0.27232,-0.10781,0.81875,0.16451,0.53941,-0.02175,0.16574,-0.04979, 1403636761463555584,-0.01823,0.07617,-0.27473,-0.10918,0.81046,0.16337,0.55185,0.01618,0.17955,-0.01042, 1403636761563555584,0.00234,0.04343,-0.26070,-0.11058,0.80327,0.16009,0.56294,0.02845,0.19027,0.00927, 1403636761663555328,0.00553,0.06253,-0.25883,-0.11532,0.79988,0.15591,0.56797,0.02023,0.18731,0.01989, 1403636761763555584,0.00774,0.08097,-0.25580,-0.12567,0.79995,0.14769,0.56787,0.02132,0.16948,0.03698, 1403636761863555328,0.00941,0.09582,-0.25105,-0.13208,0.80063,0.14242,0.56680,0.01405,0.12415,0.05484, 1403636761963555584,0.01082,0.10492,-0.24455,-0.12730,0.80116,0.14493,0.56650,0.02166,0.07355,0.07644, 1403636762063555584,0.00801,0.10146,-0.24529,-0.11131,0.79886,0.15501,0.57045,0.01849,0.05176,0.10130, 1403636762163555328,0.00773,0.10651,-0.23527,-0.09314,0.79731,0.16696,0.57249,-0.01460,0.06967,0.09186, 1403636762263555584,0.00436,0.11555,-0.22775,-0.08925,0.80445,0.16882,0.56249,-0.02764,0.12430,0.04473, 1403636762363555328,0.00107,0.12957,-0.22743,-0.09225,0.81360,0.16597,0.54955,-0.01609,0.16938,-0.03487, 1403636762463555584,0.00009,0.14826,-0.23550,-0.10211,0.81762,0.15879,0.54392,0.01800,0.21919,-0.10716, 1403636762563555584,0.00272,0.17037,-0.25023,-0.12698,0.81437,0.14125,0.54839,0.04257,0.22295,-0.17769, 1403636762663555328,0.00903,0.18925,-0.27302,-0.14594,0.80861,0.12806,0.55539,0.08679,0.16083,-0.26247, 1403636762763555584,0.01883,0.20093,-0.30319,-0.13856,0.80368,0.13395,0.56299,0.11014,0.09171,-0.33740, 1403636762863555328,0.02594,0.20307,-0.32280,-0.14167,0.78386,0.13301,0.58975,-0.07444,-0.11984,-0.03054, 1403636762863555328,0.03490,0.22115,-0.36264,-0.11547,0.79527,0.13540,0.57955,-0.15850,-0.18554,-0.17257, 1403636762963555584,0.04888,0.21413,-0.36749,-0.11457,0.79554,0.13393,0.57969,-0.15658,-0.18664,-0.16625, 1403636763063555584,0.03800,0.19410,-0.37656,-0.11103,0.80519,0.10816,0.57240,-0.10347,-0.19496,-0.20919, 1403636763163555328,0.03104,0.18313,-0.37451,-0.10919,0.79244,0.09008,0.59330,-0.07071,-0.05428,-0.11721, 1403636763263555584,0.03206,0.18037,-0.38225,-0.11005,0.80281,0.08615,0.57963,0.02791,0.00740,-0.07198, 1403636763363555328,0.03369,0.18008,-0.37824,-0.11070,0.80115,0.08584,0.58183,-0.00923,0.00039,-0.01067, 1403636763463555584,0.03784,0.18338,-0.37588,-0.11047,0.80205,0.08557,0.58069,-0.00934,0.00124,-0.00238, 1403636763563555584,0.03809,0.18350,-0.37579,-0.11075,0.80212,0.08540,0.58055,-0.00975,-0.00227,-0.00203, 1403636763663555328,0.03804,0.18341,-0.37565,-0.11080,0.80206,0.08538,0.58063,-0.01222,-0.00109,-0.00117, 1403636763763555584,0.03804,0.18338,-0.37567,-0.11059,0.80218,0.08537,0.58051,-0.00457,-0.00235,0.00362, ================================================ FILE: config/vins_rviz_config.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 Splitter Ratio: 0.465116 Tree Height: 156 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 Tree Height: 359 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 130; 130; 130 Enabled: true Line Style: Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.3 Head Length: 0.2 Length: 0.3 Line Style: Lines Line Width: 0.5 Name: ground_truth_path Offset: X: 0 Y: 0 Z: 0 Pose Style: None Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 Topic: /benchmark_publisher/path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 3000 Enabled: true Invert Rainbow: true Max Color: 0; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: history_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.5 Style: Points Topic: /vins_estimator/history_cloud Unreliable: false Use Fixed Frame: true Use rainbow: false Value: true - Class: rviz/Image Enabled: true Image Topic: /vins_estimator/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: tracked image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 255; 0 Enabled: true Head Diameter: 0.3 Head Length: 0.2 Length: 0.3 Line Style: Lines Line Width: 0.03 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Style: None Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 Topic: /vins_estimator/path Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vins_estimator/pose_graph Name: loop_link Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: current_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.01 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: true - Class: rviz/Axes Enabled: true Length: 1 Name: Axes Radius: 0.1 Reference Frame: Value: true - Class: rviz/Image Enabled: true Image Topic: /cam0/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: raw_image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 20.5065 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 1.21347 Y: -1.35133 Z: 1.5734 Name: Current View Near Clip Distance: 0.01 Pitch: 1.1848 Target Frame: Value: Orbit (rviz) Yaw: 3.1181 Saved: ~ Window Geometry: Displays: collapsed: false Height: 746 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000019400000263fc020000000ffb0000000a0049006d00610067006501000000280000013d0000000000000000fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200720061007700200049006d0061006700650000000028000000300000000000000000fb00000012007200610077005f0069006d0061006700650100000028000000b40000001600fffffffb0000001a0074007200610063006b0065006400200069006d00610067006501000000e2000000c60000001600fffffffb000000100044006900730070006c00610079007301000001ae000000dd000000dd00fffffffc000000280000011e0000000000fffffffa000000000100000002fb0000001200720061007700200049006d0061006700650000000000ffffffff0000000000000000fb0000001a0074007200610063006b0065006400200069006d0061006700650100000000000002370000000000000000fb0000001000410052005f0069006d0061006700650100000373000000160000000000000000fb0000001200720061007700200069006d006100670065010000038f000000160000000000000000000000010000020800000361fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc00000028000003610000000000fffffffaffffffff0100000002fb000000100044006900730070006c0061007900730000000000ffffffff0000016a00fffffffb0000000a00560069006500770073000000023f0000016a0000010f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004230000003bfc0100000002fb0000000800540069006d0065010000000000000423000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002890000026300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1059 X: 101 Y: 55 raw_image: collapsed: false tracked image: collapsed: false ================================================ FILE: generate.sh ================================================ #/bin/bash cd camera_model mkdir -p build cd build cmake .. make cd ../../vins_estimator mkdir -p build cd build cmake .. make ================================================ FILE: include/ChannelFloat32.h ================================================ #ifndef SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H #define SENSOR_MSGS_MESSAGE_CHANNELFLOAT32_H #include #include #include #include namespace sensor_msgs { template struct ChannelFloat32_ { typedef ChannelFloat32_ Type; ChannelFloat32_() : name() , values() { } ChannelFloat32_(const ContainerAllocator& _alloc) : name(_alloc) , values(_alloc) { (void)_alloc; } typedef std::basic_string, typename ContainerAllocator::template rebind::other > _name_type; _name_type name; typedef std::vector::other > _values_type; _values_type values; typedef boost::shared_ptr< sensor_msgs::ChannelFloat32_ > Ptr; typedef boost::shared_ptr< sensor_msgs::ChannelFloat32_ const> ConstPtr; }; // struct ChannelFloat32_ typedef sensor_msgs::ChannelFloat32_ > ChannelFloat32; typedef boost::shared_ptr< sensor_msgs::ChannelFloat32 > ChannelFloat32Ptr; typedef boost::shared_ptr< sensor_msgs::ChannelFloat32 const> ChannelFloat32ConstPtr; } #endif ================================================ FILE: include/Float32.h ================================================ #ifndef STD_MSGS_MESSAGE_FLOAT32_H #define STD_MSGS_MESSAGE_FLOAT32_H #include #include #include namespace std_msgs { template struct Float32_ { typedef Float32_ Type; Float32_() : data(0.0) { } Float32_(const ContainerAllocator& _alloc) : data(0.0) { (void)_alloc; } typedef float _data_type; _data_type data; typedef boost::shared_ptr< ::std_msgs::Float32_ > Ptr; typedef boost::shared_ptr< ::std_msgs::Float32_ const> ConstPtr; }; // struct Float32_ } #endif ================================================ FILE: include/Header.h ================================================ #ifndef STD_MSGS_MESSAGE_HEADER_H #define STD_MSGS_MESSAGE_HEADER_H #include #include #include #include #include #include "Time.h" namespace std_msgs { template struct Header_ { typedef Header_ Type; Header_() : seq(0) , stamp() , frame_id() { } Header_(const ContainerAllocator& _alloc) : seq(0) , stamp() , frame_id(_alloc) { (void)_alloc; } typedef uint32_t _seq_type; _seq_type seq; typedef ros::Time _stamp_type; _stamp_type stamp; typedef std::basic_string, typename ContainerAllocator::template rebind::other > _frame_id_type; _frame_id_type frame_id; typedef boost::shared_ptr< std_msgs::Header_ > Ptr; typedef boost::shared_ptr< std_msgs::Header_ const> ConstPtr; }; // struct Header_ typedef std_msgs::Header_ > Header; typedef boost::shared_ptr< std_msgs::Header > HeaderPtr; typedef boost::shared_ptr< std_msgs::Header const> HeaderConstPtr; } #endif ================================================ FILE: include/Imu.h ================================================ #ifndef SENSOR_MSGS_MESSAGE_IMU_H #define SENSOR_MSGS_MESSAGE_IMU_H #include #include #include #include #include "Time.h" #include "Quaternion.h" #include "Vector3.h" #include "Header.h" namespace sensor_msgs { template struct Imu_ { typedef Imu_ Type; Imu_() : header() , orientation() , orientation_covariance() , angular_velocity() , angular_velocity_covariance() , linear_acceleration() , linear_acceleration_covariance() { orientation_covariance.assign(0.0); angular_velocity_covariance.assign(0.0); linear_acceleration_covariance.assign(0.0); } Imu_(const ContainerAllocator& _alloc) : header(_alloc) , orientation(_alloc) , orientation_covariance() , angular_velocity(_alloc) , angular_velocity_covariance() , linear_acceleration(_alloc) , linear_acceleration_covariance() { (void)_alloc; orientation_covariance.assign(0.0); angular_velocity_covariance.assign(0.0); linear_acceleration_covariance.assign(0.0); } typedef std_msgs::Header_ _header_type; _header_type header; typedef geometry_msgs::Quaternion_ _orientation_type; _orientation_type orientation; typedef boost::array _orientation_covariance_type; _orientation_covariance_type orientation_covariance; typedef geometry_msgs::Vector3_ _angular_velocity_type; _angular_velocity_type angular_velocity; typedef boost::array _angular_velocity_covariance_type; _angular_velocity_covariance_type angular_velocity_covariance; typedef geometry_msgs::Vector3_ _linear_acceleration_type; _linear_acceleration_type linear_acceleration; typedef boost::array _linear_acceleration_covariance_type; _linear_acceleration_covariance_type linear_acceleration_covariance; typedef boost::shared_ptr< sensor_msgs::Imu_ > Ptr; typedef boost::shared_ptr< sensor_msgs::Imu_ const> ConstPtr; }; // struct Imu_ typedef sensor_msgs::Imu_ > Imu; typedef boost::shared_ptr< sensor_msgs::Imu > ImuPtr; typedef boost::shared_ptr< sensor_msgs::Imu const> ImuConstPtr; } #endif ================================================ FILE: include/Odometry.h ================================================ #ifndef NAV_MSGS_MESSAGE_ODOMETRY_H #define NAV_MSGS_MESSAGE_ODOMETRY_H #include #include #include #include "Header.h" #include "PoseWithCovariance.h" #include "TwistWithCovariance.h" namespace nav_msgs { template struct Odometry_ { typedef Odometry_ Type; Odometry_() : header() , child_frame_id() , pose() , twist() { } Odometry_(const ContainerAllocator& _alloc) : header(_alloc) , child_frame_id(_alloc) , pose(_alloc) , twist(_alloc) { (void)_alloc; } typedef ::std_msgs::Header_ _header_type; _header_type header; typedef std::basic_string, typename ContainerAllocator::template rebind::other > _child_frame_id_type; _child_frame_id_type child_frame_id; typedef ::geometry_msgs::PoseWithCovariance_ _pose_type; _pose_type pose; typedef ::geometry_msgs::TwistWithCovariance_ _twist_type; _twist_type twist; typedef boost::shared_ptr< ::nav_msgs::Odometry_ > Ptr; typedef boost::shared_ptr< ::nav_msgs::Odometry_ const> ConstPtr; }; // struct Odometry_ typedef ::nav_msgs::Odometry_ > Odometry; typedef boost::shared_ptr< ::nav_msgs::Odometry > OdometryPtr; typedef boost::shared_ptr< ::nav_msgs::Odometry const> OdometryConstPtr; } #endif ================================================ FILE: include/Path.h ================================================ #ifndef NAV_MSGS_MESSAGE_PATH_H #define NAV_MSGS_MESSAGE_PATH_H #include #include #include #include "Header.h" #include "PoseStamped.h" namespace nav_msgs { template struct Path_ { typedef Path_ Type; Path_() : header() , poses() { } Path_(const ContainerAllocator& _alloc) : header(_alloc) , poses(_alloc) { (void)_alloc; } typedef ::std_msgs::Header_ _header_type; _header_type header; typedef std::vector< ::geometry_msgs::PoseStamped_ , typename ContainerAllocator::template rebind< ::geometry_msgs::PoseStamped_ >::other > _poses_type; _poses_type poses; typedef boost::shared_ptr< ::nav_msgs::Path_ > Ptr; typedef boost::shared_ptr< ::nav_msgs::Path_ const> ConstPtr; }; // struct Path_ typedef ::nav_msgs::Path_ > Path; typedef boost::shared_ptr< ::nav_msgs::Path > PathPtr; typedef boost::shared_ptr< ::nav_msgs::Path const> PathConstPtr; } #endif ================================================ FILE: include/Point.h ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_POINT_H #define GEOMETRY_MSGS_MESSAGE_POINT_H #include #include #include namespace geometry_msgs { template struct Point_ { typedef Point_ Type; Point_() : x(0.0) , y(0.0) , z(0.0) { } Point_(const ContainerAllocator& _alloc) : x(0.0) , y(0.0) , z(0.0) { (void)_alloc; } typedef double _x_type; _x_type x; typedef double _y_type; _y_type y; typedef double _z_type; _z_type z; typedef boost::shared_ptr< ::geometry_msgs::Point_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::Point_ const> ConstPtr; }; // struct Point_ typedef ::geometry_msgs::Point_ > Point; typedef boost::shared_ptr< ::geometry_msgs::Point > PointPtr; typedef boost::shared_ptr< ::geometry_msgs::Point const> PointConstPtr; } #endif ================================================ FILE: include/Point32.h ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_POINT32_H #define GEOMETRY_MSGS_MESSAGE_POINT32_H #include #include #include #include namespace geometry_msgs { template struct Point32_ { typedef Point32_ Type; Point32_() : x(0.0) , y(0.0) , z(0.0) { } Point32_(const ContainerAllocator& _alloc) : x(0.0) , y(0.0) , z(0.0) { (void)_alloc; } typedef float _x_type; _x_type x; typedef float _y_type; _y_type y; typedef float _z_type; _z_type z; typedef boost::shared_ptr< ::geometry_msgs::Point32_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::Point32_ const> ConstPtr; }; // struct Point32_ typedef ::geometry_msgs::Point32_ > Point32; typedef boost::shared_ptr< ::geometry_msgs::Point32 > Point32Ptr; typedef boost::shared_ptr< ::geometry_msgs::Point32 const> Point32ConstPtr; } #endif ================================================ FILE: include/PointCloud.h ================================================ #ifndef SENSOR_MSGS_MESSAGE_POINTCLOUD_H #define SENSOR_MSGS_MESSAGE_POINTCLOUD_H #include #include #include #include #include "Header.h" #include "Point32.h" #include "ChannelFloat32.h" namespace sensor_msgs { template struct PointCloud_ { typedef PointCloud_ Type; PointCloud_() : header() , points() , channels() { } PointCloud_(const ContainerAllocator& _alloc) : header(_alloc) , points(_alloc) , channels(_alloc) { (void)_alloc; } typedef std_msgs::Header_ _header_type; _header_type header; typedef std::vector< geometry_msgs::Point32_ , typename ContainerAllocator::template rebind< geometry_msgs::Point32_ >::other > _points_type; _points_type points; typedef std::vector< sensor_msgs::ChannelFloat32_ , typename ContainerAllocator::template rebind< sensor_msgs::ChannelFloat32_ >::other > _channels_type; _channels_type channels; typedef boost::shared_ptr< sensor_msgs::PointCloud_ > Ptr; typedef boost::shared_ptr< sensor_msgs::PointCloud_ const> ConstPtr; }; // struct PointCloud_ typedef sensor_msgs::PointCloud_ > PointCloud; typedef boost::shared_ptr< sensor_msgs::PointCloud > PointCloudPtr; typedef boost::shared_ptr< sensor_msgs::PointCloud const> PointCloudConstPtr; } #endif ================================================ FILE: include/PointStamped ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H #define GEOMETRY_MSGS_MESSAGE_POINTSTAMPED_H #include #include #include #include "Header.h" #include "Point.h" namespace geometry_msgs { template struct PointStamped_ { typedef PointStamped_ Type; PointStamped_() : header() , point() { } PointStamped_(const ContainerAllocator& _alloc) : header(_alloc) , point(_alloc) { (void)_alloc; } typedef ::std_msgs::Header_ _header_type; _header_type header; typedef ::geometry_msgs::Point_ _point_type; _point_type point; typedef boost::shared_ptr< ::geometry_msgs::PointStamped_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::PointStamped_ const> ConstPtr; }; // struct PointStamped_ typedef ::geometry_msgs::PointStamped_ > PointStamped; typedef boost::shared_ptr< ::geometry_msgs::PointStamped > PointStampedPtr; typedef boost::shared_ptr< ::geometry_msgs::PointStamped const> PointStampedConstPtr; } #endif ================================================ FILE: include/Pose.h ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_POSE_H #define GEOMETRY_MSGS_MESSAGE_POSE_H #include #include #include #include "Point.h" #include "Quaternion.h" namespace geometry_msgs { template struct Pose_ { typedef Pose_ Type; Pose_() : position() , orientation() { } Pose_(const ContainerAllocator& _alloc) : position(_alloc) , orientation(_alloc) { (void)_alloc; } typedef ::geometry_msgs::Point_ _position_type; _position_type position; typedef ::geometry_msgs::Quaternion_ _orientation_type; _orientation_type orientation; typedef boost::shared_ptr< ::geometry_msgs::Pose_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::Pose_ const> ConstPtr; }; // struct Pose_ typedef ::geometry_msgs::Pose_ > Pose; typedef boost::shared_ptr< ::geometry_msgs::Pose > PosePtr; typedef boost::shared_ptr< ::geometry_msgs::Pose const> PoseConstPtr; } #endif ================================================ FILE: include/PoseStamped.h ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H #define GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H #include #include #include #include "Header.h" #include "Pose.h" namespace geometry_msgs { template struct PoseStamped_ { typedef PoseStamped_ Type; PoseStamped_() : header() , pose() { } PoseStamped_(const ContainerAllocator& _alloc) : header(_alloc) , pose(_alloc) { (void)_alloc; } typedef ::std_msgs::Header_ _header_type; _header_type header; typedef ::geometry_msgs::Pose_ _pose_type; _pose_type pose; typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_ const> ConstPtr; }; // struct PoseStamped_ typedef ::geometry_msgs::PoseStamped_ > PoseStamped; typedef boost::shared_ptr< ::geometry_msgs::PoseStamped > PoseStampedPtr; typedef boost::shared_ptr< ::geometry_msgs::PoseStamped const> PoseStampedConstPtr; } #endif ================================================ FILE: include/PoseWithCovariance.h ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H #define GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H #include #include #include #include #include "Pose.h" namespace geometry_msgs { template struct PoseWithCovariance_ { typedef PoseWithCovariance_ Type; PoseWithCovariance_() : pose() , covariance() { covariance.assign(0.0); } PoseWithCovariance_(const ContainerAllocator& _alloc) : pose(_alloc) , covariance() { (void)_alloc; covariance.assign(0.0); } typedef ::geometry_msgs::Pose_ _pose_type; _pose_type pose; typedef boost::array _covariance_type; _covariance_type covariance; typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_ const> ConstPtr; }; // struct PoseWithCovariance_ typedef ::geometry_msgs::PoseWithCovariance_ > PoseWithCovariance; typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance > PoseWithCovariancePtr; typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance const> PoseWithCovarianceConstPtr; } #endif ================================================ FILE: include/Quaternion.h ================================================ #ifndef GEOMETRY_MSGS_MESSAGE_QUATERNION_H #define GEOMETRY_MSGS_MESSAGE_QUATERNION_H #include #include #include namespace geometry_msgs { template struct Quaternion_ { typedef Quaternion_ Type; Quaternion_() : x(0.0) , y(0.0) , z(0.0) , w(0.0) { } Quaternion_(const ContainerAllocator& _alloc) : x(0.0) , y(0.0) , z(0.0) , w(0.0) { (void)_alloc; } typedef double _x_type; _x_type x; typedef double _y_type; _y_type y; typedef double _z_type; _z_type z; typedef double _w_type; _w_type w; typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ > Ptr; typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ const> ConstPtr; }; // struct Quaternion_ typedef ::geometry_msgs::Quaternion_ > Quaternion; typedef boost::shared_ptr< ::geometry_msgs::Quaternion > QuaternionPtr; typedef boost::shared_ptr< ::geometry_msgs::Quaternion const> QuaternionConstPtr; } #endif ================================================ FILE: include/Time.h ================================================ /********************************************************************* *all edit by solomon *solomon.he@zhaoxin.com *********************************************************************/ #ifndef ROS_TIME_H_INCLUDED #define ROS_TIME_H_INCLUDED /********************************************************************* ** Pragmas *********************************************************************/ /********************************************************************* ** Headers *********************************************************************/ //#include #include #include #include #include /********************************************************************* ** Cross Platform Headers *********************************************************************/ namespace ros //DataStucture { /********************************************************************* ** Functions *********************************************************************/ inline void normalizeSecNSec(uint64_t& sec, uint64_t& nsec) { uint64_t nsec_part = nsec % 1000000000UL; uint64_t sec_part = nsec / 1000000000UL; if (sec_part > UINT_MAX) throw std::runtime_error("Time is out of dual 32-bit range"); sec += sec_part; nsec = nsec_part; } inline void normalizeSecNSec(uint32_t& sec, uint32_t& nsec) { uint64_t sec64 = sec; uint64_t nsec64 = nsec; normalizeSecNSec(sec64, nsec64); sec = (uint32_t)sec64; nsec = (uint32_t)nsec64; } inline void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec) { int64_t nsec_part = nsec; int64_t sec_part = sec; while (nsec_part >= 1000000000L) { nsec_part -= 1000000000L; ++sec_part; } while (nsec_part < 0) { nsec_part += 1000000000L; --sec_part; } if (sec_part < 0 || sec_part > INT_MAX) throw std::runtime_error("Time is out of dual 32-bit range"); sec = sec_part; nsec = nsec_part; } /********************************************************************* ** Time Classes *********************************************************************/ /** * \brief Base class for Time implementations. Provides storage, common functions and operator overloads. * This should not need to be used directly. */ template class TimeBase { public: uint32_t sec, nsec; TimeBase() : sec(0), nsec(0) { } TimeBase(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) { normalizeSecNSec(sec, nsec); } explicit TimeBase(double t) { fromSec(t); } ~TimeBase() {} bool operator==(const T &rhs) const {return sec == rhs.sec && nsec == rhs.nsec;} inline bool operator!=(const T &rhs) const { return !(*static_cast(this) == rhs); } bool operator>(const T &rhs) const { if(sec > rhs.sec) return true; else if(sec==rhs.sec && nsec > rhs.nsec) return true; return false; } bool operator<(const T &rhs) const { if(sec < rhs.sec) return true; else if(sec==rhs.sec && nsec < rhs.nsec) return true; return false; } bool operator>=(const T &rhs) const { if(sec > rhs.sec) return true; else if(sec==rhs.sec && nsec >= rhs.nsec) return true; return false; } bool operator<=(const T &rhs) const { if(sec < rhs.sec) return true; else if(sec==rhs.sec && nsec <= rhs.nsec) return true; return false; } double toSec() const { return (double)sec + 1e-9*(double)nsec; }; T& fromSec(double t) { sec = (uint32_t)floor(t); nsec = (uint32_t)boost::math::round((t-sec) * 1e9); // avoid rounding errors sec += (nsec / 1000000000ul); nsec %= 1000000000ul; return *static_cast(this); } uint64_t toNSec() const {return (uint64_t)sec*1000000000ull + (uint64_t)nsec; } T& fromNSec(uint64_t t); inline bool isZero() const { return sec == 0 && nsec == 0; } inline bool is_zero() const { return isZero(); } }; /** * \brief Time representation. May either represent wall clock time or ROS clock time. * * ros::TimeBase provides most of its functionality. */ class Time : public TimeBase