Repository: lovelyyoshino/FAST_LIO2_Noted Branch: main Commit: 0a6e863a9824 Files: 99 Total size: 1.2 MB Directory structure: gitextract_30l0tz1c/ ├── .gitignore ├── FAST-LIO/ │ ├── CMakeLists.txt │ ├── LICENSE │ ├── Log/ │ │ ├── fast_lio_time_log_analysis.m │ │ ├── guide.md │ │ └── plot.py │ ├── PCD/ │ │ └── 1 │ ├── README.md │ ├── config/ │ │ ├── avia.yaml │ │ ├── horizon.yaml │ │ ├── ouster64.yaml │ │ ├── ouster64_mulran.yaml │ │ └── velodyne.yaml │ ├── include/ │ │ ├── Exp_mat.h │ │ ├── IKFoM_toolkit/ │ │ │ ├── esekfom/ │ │ │ │ ├── esekfom.hpp │ │ │ │ └── util.hpp │ │ │ └── mtk/ │ │ │ ├── build_manifold.hpp │ │ │ ├── src/ │ │ │ │ ├── SubManifold.hpp │ │ │ │ ├── mtkmath.hpp │ │ │ │ └── vectview.hpp │ │ │ ├── startIdx.hpp │ │ │ └── types/ │ │ │ ├── S2.hpp │ │ │ ├── SOn.hpp │ │ │ ├── vect.hpp │ │ │ └── wrapped_cv_mat.hpp │ │ ├── common_lib.h │ │ ├── ikd-Tree/ │ │ │ ├── README.md │ │ │ ├── ikd_Tree.cpp │ │ │ └── ikd_Tree.h │ │ ├── matplotlibcpp.h │ │ ├── so3_math.h │ │ └── use-ikfom.hpp │ ├── launch/ │ │ ├── gdb_debug_example.launch │ │ ├── mapping_avia.launch │ │ ├── mapping_horizon.launch │ │ ├── mapping_ouster64.launch │ │ ├── mapping_ouster64_mulran.launch │ │ └── mapping_velodyne.launch │ ├── msg/ │ │ └── Pose6D.msg │ ├── package.xml │ ├── rviz_cfg/ │ │ ├── .gitignore │ │ └── loam_livox.rviz │ └── src/ │ ├── IMU_Processing.hpp │ ├── laserMapping.cpp │ ├── preprocess.cpp │ └── preprocess.h ├── README.md └── SC-PGO/ ├── CMakeLists.txt ├── README.md ├── docker/ │ ├── Dockerfile │ ├── Makefile │ └── run.sh ├── include/ │ ├── aloam_velodyne/ │ │ ├── common.h │ │ └── tic_toc.h │ └── scancontext/ │ ├── KDTreeVectorOfVectorsAdaptor.h │ ├── Scancontext.cpp │ ├── Scancontext.h │ └── nanoflann.hpp ├── launch/ │ ├── aloam_mulran.launch │ ├── aloam_velodyne_HDL_32.launch │ ├── aloam_velodyne_HDL_64.launch │ ├── aloam_velodyne_VLP_16.launch │ ├── fastlio_ouster64.launch │ └── kitti_helper.launch ├── package.xml ├── rviz_cfg/ │ └── aloam_velodyne.rviz ├── src/ │ ├── kittiHelper.cpp │ ├── laserMapping.cpp │ ├── laserOdometry.cpp │ ├── laserPosegraphOptimization.cpp │ ├── lidarFactor.hpp │ └── scanRegistration.cpp └── utils/ ├── python/ │ ├── bone_table.npy │ ├── jet_table.npy │ ├── makeMergedMap.py │ └── pypcdMyUtils.py └── sample_data/ └── KAIST03/ ├── Scans/ │ ├── 000000.pcd │ ├── 000001.pcd │ ├── 000002.pcd │ ├── 000003.pcd │ ├── 000004.pcd │ ├── 000005.pcd │ ├── 000006.pcd │ ├── 000007.pcd │ ├── 000008.pcd │ ├── 000009.pcd │ ├── 000010.pcd │ ├── 000011.pcd │ ├── 000012.pcd │ ├── 000013.pcd │ ├── 000014.pcd │ ├── 000015.pcd │ ├── 000016.pcd │ ├── 000017.pcd │ ├── 000018.pcd │ ├── 000019.pcd │ └── 000020.pcd ├── optimized_poses.txt └── times.txt ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ build FAST-LIO/Log/*.png FAST-LIO/Log/*.txt FAST-LIO/Log/*.csv FAST-LIO/Log/*.pdf .vscode/c_cpp_properties.json .vscode/settings.json PCD/*.pcd *.pyc ================================================ FILE: FAST-LIO/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(fast_lio) SET(CMAKE_BUILD_TYPE "Debug") ADD_COMPILE_OPTIONS(-std=c++14 ) ADD_COMPILE_OPTIONS(-std=c++14 ) set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) include(ProcessorCount) ProcessorCount(N) message("Processer number: ${N}") if(N GREATER 4) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=3) message("core for MP: 3") elseif(N GREATER 3) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=2) message("core for MP: 2") else() add_definitions(-DMP_PROC_NUM=1) endif() else() add_definitions(-DMP_PROC_NUM=1) endif() find_package(OpenMP QUIET) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") find_package(PythonLibs REQUIRED) find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs sensor_msgs roscpp rospy std_msgs pcl_ros tf livox_ros_driver message_generation eigen_conversions ) find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) message(Eigen: ${EIGEN3_INCLUDE_DIR}) include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} include) add_message_files( FILES Pose6D.msg ) generate_messages( DEPENDENCIES geometry_msgs ) catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime DEPENDS EIGEN3 PCL INCLUDE_DIRS ) add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) ================================================ FILE: FAST-LIO/LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 2, June 1991 Copyright (C) 1989, 1991 Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. This General Public License applies to most of the Free Software Foundation's software and to any other program whose authors commit to using it. (Some other Free Software Foundation software is covered by the GNU Lesser General Public License instead.) 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 this service 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 make restrictions that forbid anyone to deny you these rights or to ask you to surrender the rights. These restrictions translate to certain responsibilities for you if you distribute copies of the software, or if you modify it. For example, if you distribute copies of such a program, whether gratis or for a fee, you must give the recipients all the rights that you have. 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. We protect your rights with two steps: (1) copyright the software, and (2) offer you this license which gives you legal permission to copy, distribute and/or modify the software. Also, for each author's protection and ours, we want to make certain that everyone understands that there is no warranty for this free software. If the software is modified by someone else and passed on, we want its recipients to know that what they have is not the original, so that any problems introduced by others will not reflect on the original authors' reputations. Finally, any free program is threatened constantly by software patents. We wish to avoid the danger that redistributors of a free program will individually obtain patent licenses, in effect making the program proprietary. To prevent this, we have made it clear that any patent must be licensed for everyone's free use or not licensed at all. The precise terms and conditions for copying, distribution and modification follow. GNU GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License applies to any program or other work which contains a notice placed by the copyright holder saying it may be distributed under the terms of this General Public License. The "Program", below, refers to any such program or work, and a "work based on the Program" means either the Program or any derivative work under copyright law: that is to say, a work containing the Program or a portion of it, either verbatim or with modifications and/or translated into another language. (Hereinafter, translation is included without limitation in the term "modification".) Each licensee is addressed as "you". Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running the Program is not restricted, and the output from the Program is covered only if its contents constitute a work based on the Program (independent of having been made by running the Program). Whether that is true depends on what the Program does. 1. You may copy and distribute 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 and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and give any other recipients of the Program a copy of this License along with the Program. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. 2. You may modify your copy or copies of the Program or any portion of it, thus forming a work based on the Program, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: a) You must cause the modified files to carry prominent notices stating that you changed the files and the date of any change. b) You must cause any work that you distribute or publish, that in whole or in part contains or is derived from the Program or any part thereof, to be licensed as a whole at no charge to all third parties under the terms of this License. c) If the modified program normally reads commands interactively when run, you must cause it, when started running for such interactive use in the most ordinary way, to print or display an announcement including an appropriate copyright notice and a notice that there is no warranty (or else, saying that you provide a warranty) and that users may redistribute the program under these conditions, and telling the user how to view a copy of this License. (Exception: if the Program itself is interactive but does not normally print such an announcement, your work based on the Program is not required to print an announcement.) These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Program, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Program, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Program. In addition, mere aggregation of another work not based on the Program with the Program (or with a work based on the Program) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. 3. You may copy and distribute the Program (or a work based on it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you also do one of the following: a) Accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, b) Accompany it with a written offer, valid for at least three years, to give any third party, for a charge no more than your cost of physically performing source distribution, a complete machine-readable copy of the corresponding source code, to be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, c) Accompany it with the information you received as to the offer to distribute corresponding source code. (This alternative is allowed only for noncommercial distribution and only if you received the program in object code or executable form with such an offer, in accord with Subsection b above.) The source code for a work means the preferred form of the work for making modifications to it. For an executable work, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the executable. However, as a special exception, the source code distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. If distribution of executable or object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place counts as distribution of the source code, even though third parties are not compelled to copy the source along with the object code. 4. You may not copy, modify, sublicense, or distribute the Program except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense or distribute the Program is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. 5. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Program or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Program (or any work based on the Program), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Program or works based on it. 6. Each time you redistribute the Program (or any work based on the Program), the recipient automatically receives a license from the original licensor to copy, distribute or modify the Program subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties to this License. 7. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), 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 distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Program at all. For example, if a patent license would not permit royalty-free redistribution of the Program by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Program. If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply and the section as a whole is intended to apply in other circumstances. It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system, which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. 8. If the distribution and/or use of the Program is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Program under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. 9. The Free Software Foundation may publish revised and/or new versions of the 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 a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of this License, you may choose any version ever published by the Free Software Foundation. 10. If you wish to incorporate parts of the Program into other free programs whose distribution conditions are different, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. NO WARRANTY 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, 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. 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE 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. 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 convey the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. Also add information on how to contact you by electronic and paper mail. If the program is interactive, make it output a short notice like this when it starts in an interactive mode: Gnomovision version 69, Copyright (C) year name of author Gnomovision 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, the commands you use may be called something other than `show w' and `show c'; they could even be mouse-clicks or menu items--whatever suits your program. You should also get your employer (if you work as a programmer) or your school, if any, to sign a "copyright disclaimer" for the program, if necessary. Here is a sample; alter the names: Yoyodyne, Inc., hereby disclaims all copyright interest in the program `Gnomovision' (which makes passes at compilers) written by James Hacker. , 1 April 1989 Ty Coon, President of Vice This 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. ================================================ FILE: FAST-LIO/Log/fast_lio_time_log_analysis.m ================================================ clear close all Color_red = [0.6350 0.0780 0.1840]; Color_blue = [0 0.4470 0.7410]; Color_orange = [0.8500 0.3250 0.0980]; Color_green = [0.4660 0.6740 0.1880]; Color_lightblue = [0.3010 0.7450 0.9330]; Color_purple = [0.4940 0.1840 0.5560]; Color_yellow = [0.9290 0.6940 0.1250]; fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0); timestamp_ikd = fast_lio_ikdtree(:,1); timestamp_ikd = timestamp_ikd - min(timestamp_ikd); total_time_ikd = fast_lio_ikdtree(:,2)*1e3; scan_num = fast_lio_ikdtree(:,3); incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3; search_time_ikd = fast_lio_ikdtree(:,5)*1e3; delete_size_ikd = fast_lio_ikdtree(:,6); delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3; tree_size_ikd_st = fast_lio_ikdtree(:,8); tree_size_ikd = fast_lio_ikdtree(:,9); add_points = fast_lio_ikdtree(:,10); fast_lio_forest = csvread("fast_lio_time_log.csv",1,0); fov_check_time_forest = fast_lio_forest(:,5)*1e3; average_time_forest = fast_lio_forest(:,2)*1e3; total_time_forest = fast_lio_forest(:,6)*1e3; incremental_time_forest = fast_lio_forest(:,3)*1e3; search_time_forest = fast_lio_forest(:,4)*1e3; timestamp_forest = fast_lio_forest(:,1); % Use slide window to calculate average L = 1; % Length of slide window for i = 1:length(timestamp_ikd) if (i 0); search_time_ikd = search_time_ikd(index_ikd); index_forest = find(search_time_forest > 0); search_time_forest = search_time_forest(index_forest); t = nexttile; hold on; boxplot_data_ikd = [incremental_time_ikd,total_time_ikd]; boxplot_data_forest = [incremental_time_forest,total_time_forest]; Colors_ikd = [Color_blue;Color_blue;Color_blue]; Colors_forest = [Color_orange;Color_orange;Color_orange]; % xticks([3,8,13]) h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3); h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3); h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3); h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3); ax2 = gca; ax2.YAxis.Scale = 'log'; xlim([0.5,6.0]) ylim([0.0008,100]) xticks([1.25 3.25 5.25]) xticklabels({'Nearest Search',' Incremental Updates','Total Time'}); yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2]) ax2.YAxis.FontSize = 12; ax2.XAxis.FontSize = 14.5; % ax.XAxis.FontWeight = 'bold'; ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman') box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')]; for j=1:length(box_vars) if (j<=3) Color = Color_blue; else Color = Color_orange; end patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color); end Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman'); grid on set(gca,'YMinorGrid','off') nexttile; hold on; grid on; box on; set(gca,'FontSize',12,'FontName','Times New Roman') plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2); plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2); plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2); lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman') title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman') xlabel("time/s",'FontSize',16,'FontName','Times New Roman') yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1]) xlim([32,390]); ylim([0,0.85]); ax3 = gca; ax3.YAxis.FontSize = 12; ax3.XAxis.FontSize = 12; % print('./Figures/fastlio_exp_combine','-depsc','-r1200') % exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector') ================================================ FILE: FAST-LIO/Log/guide.md ================================================ Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. ================================================ FILE: FAST-LIO/Log/plot.py ================================================ # import matplotlib # matplotlib.use('Agg') import numpy as np import matplotlib.pyplot as plt #######for ikfom fig, axs = plt.subplots(4,2) lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] lab_out = ['', 'out-x', 'out-y', 'out-z'] plot_ind = range(7,10) a_pre=np.loadtxt('mat_pre.txt') a_out=np.loadtxt('mat_out.txt') time=a_pre[:,0] axs[0,0].set_title('Attitude') axs[1,0].set_title('Translation') axs[2,0].set_title('Extrins-R') axs[3,0].set_title('Extrins-T') axs[0,1].set_title('Velocity') axs[1,1].set_title('bg') axs[2,1].set_title('ba') axs[3,1].set_title('Gravity') for i in range(1,4): for j in range(8): axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) for j in range(8): # axs[j].set_xlim(386,389) axs[j%4, j/4].grid() axs[j%4, j/4].legend() plt.grid() #######for ikfom####### #### Draw IMU data fig, axs = plt.subplots(2) imu=np.loadtxt('imu.txt') time=imu[:,0] axs[0].set_title('Gyroscope') axs[1].set_title('Accelerameter') lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] lab_2 = ['acc-x', 'acc-y', 'acc-z'] for i in range(3): # if i==1: axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) for i in range(2): # axs[i].set_xlim(386,389) axs[i].grid() axs[i].legend() plt.grid() # #### Draw time calculation # plt.figure(3) # fig = plt.figure() # font1 = {'family' : 'Times New Roman', # 'weight' : 'normal', # 'size' : 12, # } # c="red" # a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') # a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') # a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') # # n = a_out[:,1].size # # time_mean = a_out[:,1].mean() # # time_se = a_out[:,1].std() / np.sqrt(n) # # time_err = a_out[:,1] - time_mean # # feat_mean = a_out[:,2].mean() # # feat_err = a_out[:,2] - feat_mean # # feat_se = a_out[:,2].std() / np.sqrt(n) # ax1 = fig.add_subplot(111) # ax1.set_ylabel('Effective Feature Numbers',font1) # ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) # ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) # ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) # ax1.set_ylim([0, 3000]) # ax2 = ax1.twinx() # ax2.spines['right'].set_color('red') # ax2.set_ylabel('Compute Time (ms)',font1) # ax2.yaxis.label.set_color('red') # ax2.tick_params(axis='y', colors='red') # ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) # ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) # ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) # ax2.set_xlim([0.5, 3.5]) # ax2.set_ylim([0, 100]) # plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) # # # print(time_se) # # # print(a_out3[:,2]) # plt.grid() # plt.savefig("time.pdf", dpi=1200) plt.show() ================================================ FILE: FAST-LIO/PCD/1 ================================================ 1 ================================================ FILE: FAST-LIO/README.md ================================================ ## Related Works 1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search. 2. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter. 3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning. 4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end. 5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds. 6. [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM#for-livox-lidar): A [Scan-Context](https://github.com/irapkaist/scancontext) loop closure module that can directly work with FAST-LIO. ## FAST-LIO **FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues: 1. Fast iterated Kalman filter for odometry optimization; 2. Automaticaly initialized at most steady environments; 3. Parallel KD-Tree Search to decrease the computation; ## FAST-LIO 2.0 (2021-07-05 Update)
**Related video:** [FAST-LIO2](https://youtu.be/2OvjGnxszf8), [FAST-LIO1](https://youtu.be/iYCY6T79oNU), [FAST-LIO1 + Scan-context loop closure](https://youtu.be/Fw9S6D6HozA) **Pipeline:**
**New Features:** 1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate. 2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy. 3. Since no requirements for feature extraction, FAST-LIO2 can support may types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs. 4. Support external IMU. 5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM). **Related papers**: [FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf) [FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196) **Contributors** [Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan) ## 1. Prerequisites ### 1.1 **Ubuntu** and **ROS** **Ubuntu >= 16.04** For **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally. ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) ### 1.2. **PCL && Eigen** PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). ### 1.3. **livox_ros_driver** Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). *Remarks:* - Since the FAST-LIO must support Livox serials LiDAR firstly, so the **livox_ros_driver** must be installed and **sourced** before run any FAST-LIO luanch file. - How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document). ## 2. Build Clone the repository and catkin_make: ``` cd ~/$A_ROS_DIR$/src git clone https://github.com/hku-mars/FAST_LIO.git cd FAST_LIO git submodule update --init cd ../.. catkin_make source devel/setup.bash ``` - Remember to source the livox_ros_driver before build (follow 1.3 **livox_ros_driver**) - If you want to use a custom build of PCL, add the following line to ~/.bashrc ```export PCL_ROOT={CUSTOM_PCL_PATH}``` ## 3. Directly run Please make sure the IMU and LiDAR are **Synchronized**, that's important. ### 3.1 For Avia Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then ``` cd ~/$FAST_LIO_ROS_DIR$ source devel/setup.bash roslaunch fast_lio mapping_avia.launch roslaunch livox_ros_driver livox_lidar_msg.launch ``` - For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. ``` livox_lidar.launch ``` can not produce it right now. - If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage. ### 3.2 For Livox serials with external IMU mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run: Edit ``` config/avia.yaml ``` to set the below parameters: 1. LiDAR point cloud topic name: ``` lid_topic ``` 2. IMU topic name: ``` imu_topic ``` 3. Translational extrinsic: ``` extrinsic_T ``` 4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) - The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual. - FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter ```time_sync_en``` to ture to turn on. But turn on **ONLY IF external time synchronization is really not possible**, since the software time sync cannot make sure accuracy. ### 3.3 For Velodyne or Ouster (Velodyne as an example) Step A: Setup before run Edit ``` config/velodyne.yaml ``` to set the below parameters: 1. LiDAR point cloud topic name: ``` lid_topic ``` 2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine) 3. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ``` 4. Translational extrinsic: ``` extrinsic_T ``` 5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) - The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). Step B: Run below ``` cd ~/$FAST_LIO_ROS_DIR$ source devel/setup.bash roslaunch fast_lio mapping_velodyne.launch ``` Step C: Run LiDAR's ros driver or play rosbag. *Remarks:* - We will produce some velodyne datasets which is already transfered to Rosbags, please wait for a while. ### 3.4 PCD file save Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds. *Tips for pcl_viewer:* - change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. ``` 1 is all random 2 is X values 3 is Y values 4 is Z values 5 is intensity ``` ## 4. Rosbag Example ### 4.1 Indoor rosbag (Livox Avia LiDAR)
Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWmrwlUD5FlyA-bTr1rakIYx1GxS4xNl/view?usp=sharing) or [avia_indoor_quick_shake_example2](https://drive.google.com/file/d/1wD485CIbzZlNs4z8e20Dv2Q1q-7Gv_AT/view?usp=sharing) and then ``` roslaunch fast_lio mapping_avia.launch rosbag play YOUR_DOWNLOADED.bag ``` ### 4.2 Outdoor rosbag (Livox Avia LiDAR)
Download [avia_hku_main building_mapping](https://drive.google.com/file/d/1GSb9eLQuwqmgI3VWSB5ApEUhOCFG_Sv5/view?usp=sharing) and then ``` roslaunch fast_lio mapping_avia.launch rosbag play YOUR_DOWNLOADED.bag ``` ## 5.Implementation on UAV In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below. The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future.
## 6.Acknowledgments Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox). ================================================ FILE: FAST-LIO/config/avia.yaml ================================================ common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 6 blind: 4 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 70 det_range: 260.0 extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # extrinsic_T: [ 0.0, 0.00, -0.0284 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1] publish: scan_publish_en: true # 'false' will close all the point cloud output dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame ================================================ FILE: FAST-LIO/config/horizon.yaml ================================================ common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 6 blind: 4 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 100 det_range: 260.0 extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1] publish: scan_publish_en: true # 'false' will close all the point cloud output dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame ================================================ FILE: FAST-LIO/config/ouster64.yaml ================================================ common: lid_topic: "/os_cloud_node/points" imu_topic: "/os_cloud_node/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 64 blind: 4 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 150.0 extrinsic_T: [ 0.0, 0.0, 0.0 ] extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] publish: scan_publish_en: true # 'false' will close all the point cloud output dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame ================================================ FILE: FAST-LIO/config/ouster64_mulran.yaml ================================================ common: lid_topic: "/os1_points" imu_topic: "/imu/data_raw" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 64 blind: 4 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 150.0 extrinsic_T: [ 1.77, 0.0, -0.05 ] extrinsic_R: [-1, 0, 0, 0, -1, 0, 0, 0, 1] publish: scan_publish_en: true # 'false' will close all the point cloud output dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame ================================================ FILE: FAST-LIO/config/velodyne.yaml ================================================ common: lid_topic: "/velodyne_points" imu_topic: "/imu/data" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 blind: 4 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_T: [ 0, 0, 0.28] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1] publish: scan_publish_en: true # 'false' will close all the point cloud output dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame ================================================ FILE: FAST-LIO/include/Exp_mat.h ================================================ #ifndef EXP_MAT_H #define EXP_MAT_H #include #include #include // #include #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 template Eigen::Matrix Exp(const Eigen::Matrix &&ang) { T ang_norm = ang.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_norm > 0.0000001) { Eigen::Matrix r_axis = ang / ang_norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_axis); /// Roderigous Tranformation return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) { T ang_vel_norm = ang_vel.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_vel_norm > 0.0000001) { Eigen::Matrix r_axis = ang_vel / ang_vel_norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_axis); T r_ang = ang_vel_norm * dt; /// Roderigous Tranformation return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) { T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (norm > 0.00001) { T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_ang); /// Roderigous Tranformation return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; } else { return Eye3; } } /* Logrithm of a Rotation Matrix */ template Eigen::Matrix Log(const Eigen::Matrix &R) { T &&theta = std::acos(0.5 * (R.trace() - 1)); Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); } // template // cv::Mat Exp(const T &v1, const T &v2, const T &v3) // { // T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); // cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); // if (norm > 0.0000001) // { // T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; // cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); // /// Roderigous Tranformation // return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; // } // else // { // return Eye3; // } // } #endif ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp ================================================ /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Author: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef ESEKFOM_EKF_HPP #define ESEKFOM_EKF_HPP #include #include #include #include #include #include #include #include "../mtk/types/vect.hpp" #include "../mtk/types/SOn.hpp" #include "../mtk/types/S2.hpp" #include "../mtk/startIdx.hpp" #include "../mtk/build_manifold.hpp" #include "util.hpp" //#define USE_sparse namespace esekfom { using namespace Eigen; // used for iterated error state EKF update // for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. // applied for measurement as a manifold. template struct share_datastruct { bool valid; bool converge; M z; Eigen::Matrix h_v; Eigen::Matrix h_x; Eigen::Matrix R; }; // used for iterated error state EKF update // for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. // applied for measurement as an Eigen matrix whose dimension is changing template struct dyn_share_datastruct { bool valid; bool converge; Eigen::Matrix z; Eigen::Matrix h; Eigen::Matrix h_v; Eigen::Matrix h_x; Eigen::Matrix R; }; // used for iterated error state EKF update // for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. // applied for measurement as a dynamic manifold whose dimension or type is changing template struct dyn_runtime_share_datastruct { bool valid; bool converge; // Z z; Eigen::Matrix h_v; Eigen::Matrix h_x; Eigen::Matrix R; }; //状态量，噪声维度，输入状态量这三个参数输入 template class esekf { typedef esekf self; enum { n = state::DOF, //状态量自由度，一般代表x的维度 m = state::DIM, //状态量自由度，一般代表res的维度 l = measurement::DOF //测量噪声维度 }; public: typedef typename state::scalar scalar_type; typedef Matrix cov; typedef Matrix cov_; typedef SparseMatrix spMt; typedef Matrix vectorized_state; typedef Matrix flatted_state; typedef flatted_state processModel(state &, const input &); //声明一个函数指针 typedef Eigen::Matrix processMatrix1(state &, const input &); typedef Eigen::Matrix processMatrix2(state &, const input &); typedef Eigen::Matrix processnoisecovariance; typedef measurement measurementModel(state &, bool &); typedef measurement measurementModel_share(state &, share_datastruct &); typedef Eigen::Matrix measurementModel_dyn(state &, bool &); // typedef Eigen::Matrix measurementModel_dyn_share(state &, dyn_share_datastruct &); typedef void measurementModel_dyn_share(state &, dyn_share_datastruct &); typedef Eigen::Matrix measurementMatrix1(state &, bool &); typedef Eigen::Matrix measurementMatrix1_dyn(state &, bool &); typedef Eigen::Matrix measurementMatrix2(state &, bool &); typedef Eigen::Matrix measurementMatrix2_dyn(state &, bool &); typedef Eigen::Matrix measurementnoisecovariance; typedef Eigen::Matrix measurementnoisecovariance_dyn; esekf(const state &x = state(), const cov &P = cov::Identity()) : x_(x), P_(P) // esekf初始化，主要是初始化了x_和P_ { #ifdef USE_sparse SparseMatrix ref(n, n); ref.setIdentity(); l_ = ref; f_x_2 = ref; f_x_1 = ref; #endif }; // receive system-specific models and their differentions. // for measurement as a manifold. void init(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel h_in, measurementMatrix1 h_x_in, measurementMatrix2 h_v_in, int maximum_iteration, scalar_type limit_vector[n]) { f = f_in; f_x = f_x_in; f_w = f_w_in; h = h_in; h_x = h_x_in; h_v = h_v_in; maximum_iter = maximum_iteration; for (int i = 0; i < n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } // receive system-specific models and their differentions. // for measurement as an Eigen matrix whose dimention is chaing. void init_dyn(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn h_in, measurementMatrix1_dyn h_x_in, measurementMatrix2_dyn h_v_in, int maximum_iteration, scalar_type limit_vector[n]) { f = f_in; f_x = f_x_in; f_w = f_w_in; h_dyn = h_in; h_x_dyn = h_x_in; h_v_dyn = h_v_in; maximum_iter = maximum_iteration; for (int i = 0; i < n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } // receive system-specific models and their differentions. // for measurement as a dynamic manifold whose dimension or type is changing. void init_dyn_runtime(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementMatrix1_dyn h_x_in, measurementMatrix2_dyn h_v_in, int maximum_iteration, scalar_type limit_vector[n]) { f = f_in; f_x = f_x_in; f_w = f_w_in; h_x_dyn = h_x_in; h_v_dyn = h_v_in; maximum_iter = maximum_iteration; for (int i = 0; i < n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } // receive system-specific models and their differentions // for measurement as a manifold. // calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_share_in). void init_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_share h_share_in, int maximum_iteration, scalar_type limit_vector[n]) { f = f_in; f_x = f_x_in; f_w = f_w_in; h_share = h_share_in; maximum_iter = maximum_iteration; for (int i = 0; i < n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } //接收系统特定的模型及其差异 //作为特征矩阵的测量，其维数是变化的。 //通过一个函数(h_dyn_share_in)完成了测量(z)，估计测量(h)，偏微分矩阵(h_x, h_v)和噪声协方差(R)的同时计算。 void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n]) { f = f_in; f_x = f_x_in; f_w = f_w_in; h_dyn_share = h_dyn_share_in; maximum_iter = maximum_iteration; for (int i = 0; i < n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } // receive system-specific models and their differentions // for measurement as a dynamic manifold whose dimension or type is changing. // calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_dyn_share_in). // for any scenarios where it is needed void init_dyn_runtime_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, int maximum_iteration, scalar_type limit_vector[n]) { f = f_in; f_x = f_x_in; f_w = f_w_in; maximum_iter = maximum_iteration; for (int i = 0; i < n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } // 迭代误差状态EKF传播 void predict(double &dt, processnoisecovariance &Q, const input &i_in) //这里的参数均从use-ikfom中传入 { // f函数对应use-ikfom.hpp中的 get_f函数，对应fast_lio2论文公式(2) flatted_state f_ = f(x_, i_in); // m*1 // 对应use-ikfom.hpp中的 df_dx函数 // 对应fast_lio2论文公式(7) cov_ f_x_ = f_x(x_, i_in); // m*n cov f_x_final; // n*n // 对应use-ikfom.hpp中的 df_dw函数 // 对应fast_lio2论文公式(7) Matrix f_w_ = f_w(x_, i_in); Matrix f_w_final; state x_before = x_; //保存x_的值，用于后面的更新 // 对应fast_lio2论文公式(2) x_.oplus(f_, dt); //这个是公式2的整个函数，用于更新x的 F_x1 = cov::Identity(); //状态转移矩阵 // 更新f_x和f_w for (std::vector, int>>::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) { int idx = (*it).first.first; //状态变量的索引 int dim = (*it).first.second; //状态变量的维数 int dof = (*it).second; //状态变量的自由度 for (int i = 0; i < n; i++) { for (int j = 0; j < dof; j++) { f_x_final(idx + j, i) = f_x_(dim + j, i); //更新f_x_final，形成n*n阵，用于更新 } } for (int i = 0; i < process_noise_dof; i++) { for (int j = 0; j < dof; j++) { f_w_final(idx + j, i) = f_w_(dim + j, i); //更新f_w_final，形成n*dof，用于更新 } } } Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; //状态变量的索引 int dim = (*it).second; //状态变量的维数 for (int i = 0; i < 3; i++) { seg_SO3(i) = -1 * f_(dim + i) * dt; //拿到S03更新值 } MTK::SO3 res; //残差计算 res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1 / 2)); //残差计算 #ifdef USE_sparse res_temp_SO3 = res.toRotationMatrix(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { f_x_1.coeffRef(idx + i, idx + j) = res_temp_SO3(i, j); } } #else F_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix(); //更新f_x_1 #endif res_temp_SO3 = MTK::A_matrix(seg_SO3); //转为矩阵形式 for (int i = 0; i < n; i++) { f_x_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_.template block<3, 1>(dim, i)); // F矩阵 } for (int i = 0; i < process_noise_dof; i++) { f_w_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_.template block<3, 1>(dim, i)); } } Matrix res_temp_S2; Matrix res_temp_S2_; MTK::vect<3, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_S2(i) = f_(dim + i) * dt; } MTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero(); MTK::SO3 res; res.w() = MTK::exp(res.vec(), seg_S2, scalar_type(1 / 2)); Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_before.S2_Mx(Mx, vec, idx); #ifdef USE_sparse res_temp_S2_ = Nx * res.toRotationMatrix() * Mx; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { f_x_1.coeffRef(idx + i, idx + j) = res_temp_S2_(i, j); } } #else F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx; #endif Eigen::Matrix x_before_hat; x_before.S2_hat(x_before_hat, idx); res_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat * MTK::A_matrix(seg_S2).transpose(); for (int i = 0; i < n; i++) { f_x_final.template block<2, 1>(idx, i) = res_temp_S2 * (f_x_.template block<3, 1>(dim, i)); } for (int i = 0; i < process_noise_dof; i++) { f_w_final.template block<2, 1>(idx, i) = res_temp_S2 * (f_w_.template block<3, 1>(dim, i)); } } #ifdef USE_sparse f_x_1.makeCompressed(); spMt f_x2 = f_x_final.sparseView(); spMt f_w1 = f_w_final.sparseView(); spMt xp = f_x_1 + f_x2 * dt; P_ = xp * P_ * xp.transpose() + (f_w1 * dt) * Q * (f_w1 * dt).transpose(); #else F_x1 += f_x_final * dt; P_ = (F_x1)*P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose(); #endif } // iterated error state EKF update for measurement as a manifold. void update_iterated(measurement &z, measurementnoisecovariance &R) { if (!(is_same())) { std::cerr << "the scalar type of measurment must be the same as the state" << std::endl; std::exit(100); } int t = 0; bool converg = true; bool valid = true; state x_propagated = x_; cov P_propagated = P_; for (int i = -1; i < maximum_iter; i++) { vectorized_state dx, dx_new; x_.boxminus(dx, x_propagated); dx_new = dx; #ifdef USE_sparse spMt h_x_ = h_x(x_, valid).sparseView(); spMt h_v_ = h_v(x_, valid).sparseView(); spMt R_ = R.sparseView(); #else Matrix h_x_ = h_x(x_, valid); Matrix h_v_ = h_v(x_, valid); #endif if (!valid) { continue; } P_ = P_propagated; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } Matrix K_; if (n > l) { #ifdef USE_sparse Matrix K_temp = h_x_ * P_ * h_x_.transpose(); spMt R_temp = h_v_ * R_ * h_v_.transpose(); K_temp += R_temp; K_ = P_ * h_x_.transpose() * K_temp.inverse(); #else K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); #endif } else { #ifdef USE_sparse measurementnoisecovariance b = measurementnoisecovariance::Identity(); Eigen::SparseQR, Eigen::COLAMDOrdering> solver; solver.compute(R_); measurementnoisecovariance R_in_temp = solver.solve(b); spMt R_in = R_in_temp.sparseView(); spMt K_temp = h_x_.transpose() * R_in * h_x_; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x_.transpose() * R_in; #else measurementnoisecovariance R_in = (h_v_ * R * h_v_.transpose()).inverse(); K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; #endif } Matrix innovation; z.boxminus(innovation, h(x_, valid)); cov K_x = K_ * h_x_; Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; state x_before = x_; x_.boxplus(dx_); converg = true; for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { converg = false; break; } } if (converg) t++; if (t > 1 || i == maximum_iter - 1) { L_ = P_; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = A_matrix(seg_SO3).transpose(); for (int i = 0; i < n; i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } if (n > l) { for (int i = 0; i < l; i++) { K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } if (n > l) { for (int i = 0; i < l; i++) { K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } if (n > l) { P_ = L_ - K_ * h_x_ * P_; } else { P_ = L_ - K_x * P_; } return; } } } // iterated error state EKF update for measurement as a manifold. // calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. void update_iterated_share() { if (!(is_same())) { std::cerr << "the scalar type of measurment must be the same as the state" << std::endl; std::exit(100); } int t = 0; share_datastruct _share; _share.valid = true; _share.converge = true; state x_propagated = x_; cov P_propagated = P_; for (int i = -1; i < maximum_iter; i++) { vectorized_state dx, dx_new; x_.boxminus(dx, x_propagated); dx_new = dx; measurement h = h_share(x_, _share); measurement z = _share.z; measurementnoisecovariance R = _share.R; #ifdef USE_sparse spMt h_x_ = _share.h_x.sparseView(); spMt h_v_ = _share.h_v.sparseView(); spMt R_ = _share.R.sparseView(); #else Matrix h_x_ = _share.h_x; Matrix h_v_ = _share.h_v; #endif if (!_share.valid) { continue; } P_ = P_propagated; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } Matrix K_; if (n > l) { #ifdef USE_sparse Matrix K_temp = h_x_ * P_ * h_x_.transpose(); spMt R_temp = h_v_ * R_ * h_v_.transpose(); K_temp += R_temp; K_ = P_ * h_x_.transpose() * K_temp.inverse(); #else K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); #endif } else { #ifdef USE_sparse measurementnoisecovariance b = measurementnoisecovariance::Identity(); Eigen::SparseQR, Eigen::COLAMDOrdering> solver; solver.compute(R_); measurementnoisecovariance R_in_temp = solver.solve(b); spMt R_in = R_in_temp.sparseView(); spMt K_temp = h_x_.transpose() * R_in * h_x_; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x_.transpose() * R_in; #else measurementnoisecovariance R_in = (h_v_ * R * h_v_.transpose()).inverse(); K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; #endif } Matrix innovation; z.boxminus(innovation, h); cov K_x = K_ * h_x_; Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; state x_before = x_; x_.boxplus(dx_); _share.converge = true; for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { _share.converge = false; break; } } if (_share.converge) t++; if (t > 1 || i == maximum_iter - 1) { L_ = P_; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = A_matrix(seg_SO3).transpose(); for (int i = 0; i < n; i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } if (n > l) { for (int i = 0; i < l; i++) { K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } if (n > l) { for (int i = 0; i < l; i++) { K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } if (n > l) { P_ = L_ - K_ * h_x_ * P_; } else { P_ = L_ - K_x * P_; } return; } } } // iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing. void update_iterated_dyn(Eigen::Matrix z, measurementnoisecovariance_dyn R) { int t = 0; bool valid = true; bool converg = true; state x_propagated = x_; cov P_propagated = P_; int dof_Measurement; int dof_Measurement_noise = R.rows(); for (int i = -1; i < maximum_iter; i++) { valid = true; #ifdef USE_sparse spMt h_x_ = h_x_dyn(x_, valid).sparseView(); spMt h_v_ = h_v_dyn(x_, valid).sparseView(); spMt R_ = R.sparseView(); #else Matrix h_x_ = h_x_dyn(x_, valid); Matrix h_v_ = h_v_dyn(x_, valid); #endif Matrix h_ = h_dyn(x_, valid); dof_Measurement = h_.rows(); vectorized_state dx, dx_new; x_.boxminus(dx, x_propagated); dx_new = dx; if (!valid) { continue; } P_ = P_propagated; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } Matrix K_; if (n > dof_Measurement) { #ifdef USE_sparse Matrix K_temp = h_x_ * P_ * h_x_.transpose(); spMt R_temp = h_v_ * R_ * h_v_.transpose(); K_temp += R_temp; K_ = P_ * h_x_.transpose() * K_temp.inverse(); #else K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); #endif } else { #ifdef USE_sparse Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); Eigen::SparseQR, Eigen::COLAMDOrdering> solver; solver.compute(R_); Eigen::Matrix R_in_temp = solver.solve(b); spMt R_in = R_in_temp.sparseView(); spMt K_temp = h_x_.transpose() * R_in * h_x_; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x_.transpose() * R_in; #else Eigen::Matrix R_in = (h_v_ * R * h_v_.transpose()).inverse(); K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; #endif } cov K_x = K_ * h_x_; Matrix dx_ = K_ * (z - h_) + (K_x - Matrix::Identity()) * dx_new; state x_before = x_; x_.boxplus(dx_); converg = true; for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { converg = false; break; } } if (converg) t++; if (t > 1 || i == maximum_iter - 1) { L_ = P_; std::cout << "iteration time:" << t << "," << i << std::endl; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); for (int i = 0; i < n; i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } if (n > dof_Measurement) { P_ = L_ - K_ * h_x_ * P_; } else { P_ = L_ - K_x * P_; } return; } } } // iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing. // calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. void update_iterated_dyn_share() { int t = 0; dyn_share_datastruct dyn_share; dyn_share.valid = true; dyn_share.converge = true; state x_propagated = x_; cov P_propagated = P_; int dof_Measurement; int dof_Measurement_noise; for (int i = -1; i < maximum_iter; i++) { dyn_share.valid = true; h_dyn_share(x_, dyn_share); // Matrix h = h_dyn_share (x_, dyn_share); Matrix z = dyn_share.z; Matrix h = dyn_share.h; #ifdef USE_sparse spMt h_x = dyn_share.h_x.sparseView(); spMt h_v = dyn_share.h_v.sparseView(); spMt R_ = dyn_share.R.sparseView(); #else Matrix R = dyn_share.R; Matrix h_x = dyn_share.h_x; Matrix h_v = dyn_share.h_v; #endif dof_Measurement = h_x.rows(); dof_Measurement_noise = dyn_share.R.rows(); vectorized_state dx, dx_new; x_.boxminus(dx, x_propagated); dx_new = dx; if (!(dyn_share.valid)) { continue; } P_ = P_propagated; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } Matrix K_; if (n > dof_Measurement) { #ifdef USE_sparse Matrix K_temp = h_x * P_ * h_x.transpose(); spMt R_temp = h_v * R_ * h_v.transpose(); K_temp += R_temp; K_ = P_ * h_x.transpose() * K_temp.inverse(); #else K_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); #endif } else { #ifdef USE_sparse Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); Eigen::SparseQR, Eigen::COLAMDOrdering> solver; solver.compute(R_); Eigen::Matrix R_in_temp = solver.solve(b); spMt R_in = R_in_temp.sparseView(); spMt K_temp = h_x.transpose() * R_in * h_x; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x.transpose() * R_in; #else Eigen::Matrix R_in = (h_v * R * h_v.transpose()).inverse(); K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in; #endif } cov K_x = K_ * h_x; Matrix dx_ = K_ * (z - h) + (K_x - Matrix::Identity()) * dx_new; state x_before = x_; x_.boxplus(dx_); dyn_share.converge = true; for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { dyn_share.converge = false; break; } } if (dyn_share.converge) t++; if (t > 1 || i == maximum_iter - 1) { L_ = P_; std::cout << "iteration time:" << t << "," << i << std::endl; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); for (int i = 0; i < int(n); i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } if (n > dof_Measurement) { P_ = L_ - K_ * h_x * P_; } else { P_ = L_ - K_x * P_; } return; } } } // iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing. // the measurement and the measurement model are received in a dynamic manner. template void update_iterated_dyn_runtime(measurement_runtime z, measurementnoisecovariance_dyn R, measurementModel_runtime h_runtime) { int t = 0; bool valid = true; bool converg = true; state x_propagated = x_; cov P_propagated = P_; int dof_Measurement; int dof_Measurement_noise; for (int i = -1; i < maximum_iter; i++) { valid = true; #ifdef USE_sparse spMt h_x_ = h_x_dyn(x_, valid).sparseView(); spMt h_v_ = h_v_dyn(x_, valid).sparseView(); spMt R_ = R.sparseView(); #else Matrix h_x_ = h_x_dyn(x_, valid); Matrix h_v_ = h_v_dyn(x_, valid); #endif measurement_runtime h_ = h_runtime(x_, valid); dof_Measurement = measurement_runtime::DOF; dof_Measurement_noise = R.rows(); vectorized_state dx, dx_new; x_.boxminus(dx, x_propagated); dx_new = dx; if (!valid) { continue; } P_ = P_propagated; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } Matrix K_; if (n > dof_Measurement) { #ifdef USE_sparse Matrix K_temp = h_x_ * P_ * h_x_.transpose(); spMt R_temp = h_v_ * R_ * h_v_.transpose(); K_temp += R_temp; K_ = P_ * h_x_.transpose() * K_temp.inverse(); #else K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); #endif } else { #ifdef USE_sparse Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); Eigen::SparseQR, Eigen::COLAMDOrdering> solver; solver.compute(R_); Eigen::Matrix R_in_temp = solver.solve(b); spMt R_in = R_in_temp.sparseView(); spMt K_temp = h_x_.transpose() * R_in * h_x_; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x_.transpose() * R_in; #else Eigen::Matrix R_in = (h_v_ * R * h_v_.transpose()).inverse(); K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; #endif } cov K_x = K_ * h_x_; Eigen::Matrix innovation; z.boxminus(innovation, h_); Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; state x_before = x_; x_.boxplus(dx_); converg = true; for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { converg = false; break; } } if (converg) t++; if (t > 1 || i == maximum_iter - 1) { L_ = P_; std::cout << "iteration time:" << t << "," << i << std::endl; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); for (int i = 0; i < n; i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } if (n > dof_Measurement) { P_ = L_ - K_ * h_x_ * P_; } else { P_ = L_ - K_x * P_; } return; } } } // iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing. // the measurement and the measurement model are received in a dynamic manner. // calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. template void update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h) { int t = 0; dyn_runtime_share_datastruct dyn_share; dyn_share.valid = true; dyn_share.converge = true; state x_propagated = x_; cov P_propagated = P_; int dof_Measurement; int dof_Measurement_noise; for (int i = -1; i < maximum_iter; i++) { dyn_share.valid = true; measurement_runtime h_ = h(x_, dyn_share); // measurement_runtime z = dyn_share.z; #ifdef USE_sparse spMt h_x = dyn_share.h_x.sparseView(); spMt h_v = dyn_share.h_v.sparseView(); spMt R_ = dyn_share.R.sparseView(); #else Matrix R = dyn_share.R; Matrix h_x = dyn_share.h_x; Matrix h_v = dyn_share.h_v; #endif dof_Measurement = measurement_runtime::DOF; dof_Measurement_noise = dyn_share.R.rows(); vectorized_state dx, dx_new; x_.boxminus(dx, x_propagated); dx_new = dx; if (!(dyn_share.valid)) { continue; } P_ = P_propagated; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } Matrix K_; if (n > dof_Measurement) { #ifdef USE_sparse Matrix K_temp = h_x * P_ * h_x.transpose(); spMt R_temp = h_v * R_ * h_v.transpose(); K_temp += R_temp; K_ = P_ * h_x.transpose() * K_temp.inverse(); #else K_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); #endif } else { #ifdef USE_sparse Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); Eigen::SparseQR, Eigen::COLAMDOrdering> solver; solver.compute(R_); Eigen::Matrix R_in_temp = solver.solve(b); spMt R_in = R_in_temp.sparseView(); spMt K_temp = h_x.transpose() * R_in * h_x; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x.transpose() * R_in; #else Eigen::Matrix R_in = (h_v * R * h_v.transpose()).inverse(); K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in; #endif } cov K_x = K_ * h_x; Eigen::Matrix innovation; z.boxminus(innovation, h_); Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; state x_before = x_; x_.boxplus(dx_); dyn_share.converge = true; for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { dyn_share.converge = false; break; } } if (dyn_share.converge) t++; if (t > 1 || i == maximum_iter - 1) { L_ = P_; std::cout << "iteration time:" << t << "," << i << std::endl; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); for (int i = 0; i < int(n); i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } if (n > dof_Measurement) { for (int i = 0; i < dof_Measurement; i++) { K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i)); } } else { for (int i = 0; i < n; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } } for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } if (n > dof_Measurement) { P_ = L_ - K_ * h_x * P_; } else { P_ = L_ - K_x * P_; } return; } } } // 迭代错误状态EKF更新修改为一个特定的系统 void update_iterated_dyn_share_modified(double R, double &solve_time) { dyn_share_datastruct dyn_share; dyn_share.valid = true; dyn_share.converge = true; int t = 0; //获取上一次的状态和协方差矩阵 state x_propagated = x_; cov P_propagated = P_; int dof_Measurement; Matrix K_h; Matrix K_x; vectorized_state dx_new = vectorized_state::Zero(); // 最多进行maximum_iter次迭代优化 for (int i = -1; i < maximum_iter; i++) { dyn_share.valid = true; // 计算测量模型方程的雅克比，也就是点面残差的导数 H(代码里是h_x) h_dyn_share(x_, dyn_share); // Matrix h = h_dyn_share(x_, dyn_share); #ifdef USE_sparse spMt h_x_ = dyn_share.h_x.sparseView(); #else // 获取测量模型的雅克比d(pos, rot, 0, 0)/dx Eigen::Matrix h_x_ = dyn_share.h_x; #endif double solve_start = omp_get_wtime(); dof_Measurement = h_x_.rows(); // 观测方程个数m vectorized_state dx; // 定义误差状态 x_.boxminus(dx, x_propagated); //获取误差dx dx_new = dx; // 用于迭代的误差状态 if (!dyn_share.valid) { continue; } // 预测得到的误差状态协方差矩阵 //协方差矩阵在迭代过程中不会代入下一次迭代，直到最后一次退出时更新，在迭代过程中更新的只是先验 P_ = P_propagated; // 这一大段都在求协方差的先验更新，大致上是P=(J^-1)*P*(J^-T)如论文式16~18 Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } // Matrix K_; // Matrix K_h; // Matrix K_x; /* if(n > dof_Measurement) { K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse()/R; } else { K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); } */ // 状态维度 n > 测量维度 dof_Measurement // 如果状态量维度大于观测方程 n > m，不满秩 if (n > dof_Measurement) { //#ifdef USE_sparse // Matrix K_temp = h_x * P_ * h_x.transpose(); // spMt R_temp = h_v * R_ * h_v.transpose(); // K_temp += R_temp; Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); //每一次迭代将重新计算增益K，即论文式18 h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; /* h_x_cur.col(0) = h_x_.col(0); h_x_cur.col(1) = h_x_.col(1); h_x_cur.col(2) = h_x_.col(2); h_x_cur.col(3) = h_x_.col(3); h_x_cur.col(4) = h_x_.col(4); h_x_cur.col(5) = h_x_.col(5); h_x_cur.col(6) = h_x_.col(6); h_x_cur.col(7) = h_x_.col(7); h_x_cur.col(8) = h_x_.col(8); h_x_cur.col(9) = h_x_.col(9); h_x_cur.col(10) = h_x_.col(10); h_x_cur.col(11) = h_x_.col(11); */ // 重新计算增益矩阵K Matrix K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose() / R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse() / R; K_h = K_ * dyn_share.h; K_x = K_ * h_x_cur; //#else // K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); //#endif } else { //避免求逆矩阵，K按稀疏矩阵分解的方法如论文式20 #ifdef USE_sparse // Eigen::Matrix b = Eigen::Matrix::Identity(); // Eigen::SparseQR, Eigen::COLAMDOrdering> solver; spMt A = h_x_.transpose() * h_x_; cov P_temp = (P_ / R).inverse(); P_temp.template block<12, 12>(0, 0) += A; P_temp = P_temp.inverse(); /* Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); h_x_cur.col(0) = h_x_.col(0); h_x_cur.col(1) = h_x_.col(1); h_x_cur.col(2) = h_x_.col(2); h_x_cur.col(3) = h_x_.col(3); h_x_cur.col(4) = h_x_.col(4); h_x_cur.col(5) = h_x_.col(5); h_x_cur.col(6) = h_x_.col(6); h_x_cur.col(7) = h_x_.col(7); h_x_cur.col(8) = h_x_.col(8); h_x_cur.col(9) = h_x_.col(9); h_x_cur.col(10) = h_x_.col(10); h_x_cur.col(11) = h_x_.col(11); */ K_ = P_temp.template block(0, 0) * h_x_.transpose(); K_x = cov::Zero(); K_x.template block(0, 0) = P_inv.template block(0, 0) * HTH; /* solver.compute(R_); Eigen::Matrix R_in_temp = solver.solve(b); spMt R_in =R_in_temp.sparseView(); spMt K_temp = h_x.transpose() * R_in * h_x; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x.transpose() * R_in; */ #else cov P_temp = (P_ / R).inverse(); // Eigen::Matrix h_T = h_x_.transpose(); Eigen::Matrix HTH = h_x_.transpose() * h_x_; P_temp.template block<12, 12>(0, 0) += HTH; /* Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); //std::cout << "line 1767" << std::endl; h_x_cur.col(0) = h_x_.col(0); h_x_cur.col(1) = h_x_.col(1); h_x_cur.col(2) = h_x_.col(2); h_x_cur.col(3) = h_x_.col(3); h_x_cur.col(4) = h_x_.col(4); h_x_cur.col(5) = h_x_.col(5); h_x_cur.col(6) = h_x_.col(6); h_x_cur.col(7) = h_x_.col(7); h_x_cur.col(8) = h_x_.col(8); h_x_cur.col(9) = h_x_.col(9); h_x_cur.col(10) = h_x_.col(10); h_x_cur.col(11) = h_x_.col(11); */ cov P_inv = P_temp.inverse(); // std::cout << "line 1781" << std::endl; K_h = P_inv.template block(0, 0) * h_x_.transpose() * dyn_share.h; // (H_T_H + P^-1)^-1 * H^T * h(残差) = K * h // std::cout << "line 1780" << std::endl; // cov HTH_cur = cov::Zero(); // HTH_cur. template block<12, 12>(0, 0) = HTH; K_x.setZero(); // = cov::Zero(); K_x.template block(0, 0) = P_inv.template block(0, 0) * HTH; //(H_T_H + P^-1)^-1 * H_T_H = KH // K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); #endif } // K_x = K_ * h_x_; //由于是误差迭代KF，得到的是误差的最优估计！ Matrix dx_ = K_h + (K_x - Matrix::Identity()) * dx_new; //误差增量后验 K*h + (K*H - I) dx state x_before = x_; // 加上校正后的误差状态dx_ x_.boxplus(dx_); //根据计算得到的误差增量后验，更新状态量 // 判断迭代是否发散 dyn_share.converge = true; //判断已收敛的条件是误差的估计值小于阈值 for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { dyn_share.converge = false; break; } } if (dyn_share.converge) t++; if (!t && i == maximum_iter - 2) { dyn_share.converge = true; } // 迭代完成后更新误差状态协方差矩阵 //结束迭代后，更新协方差矩阵的后验值，大致上是P=(I-K*H)*P，如论文式19 if (t > 1 || i == maximum_iter - 1) { L_ = P_; // std::cout << "iteration time" << t << "," << i << std::endl; Matrix res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); for (int i = 0; i < n; i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } // if(n > dof_Measurement) // { // for(int i = 0; i < dof_Measurement; i++){ // K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); // } // } // else // { for (int i = 0; i < 12; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } //} for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } // if(n > dof_Measurement) // { // for(int i = 0; i < dof_Measurement; i++){ // K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); // } // } // else // { for (int i = 0; i < 12; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } //} for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } // if(n > dof_Measurement) // { // Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); // h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; // /* // h_x_cur.col(0) = h_x_.col(0); // h_x_cur.col(1) = h_x_.col(1); // h_x_cur.col(2) = h_x_.col(2); // h_x_cur.col(3) = h_x_.col(3); // h_x_cur.col(4) = h_x_.col(4); // h_x_cur.col(5) = h_x_.col(5); // h_x_cur.col(6) = h_x_.col(6); // h_x_cur.col(7) = h_x_.col(7); // h_x_cur.col(8) = h_x_.col(8); // h_x_cur.col(9) = h_x_.col(9); // h_x_cur.col(10) = h_x_.col(10); // h_x_cur.col(11) = h_x_.col(11); // */ // P_ = L_ - K_*h_x_cur * P_; // } // else //{ P_ = L_ - K_x.template block(0, 0) * P_.template block<12, n>(0, 0); //} solve_time += omp_get_wtime() - solve_start; return; } solve_time += omp_get_wtime() - solve_start; } } void change_x(state &input_state) { x_ = input_state; if ((!x_.vect_state.size()) && (!x_.SO3_state.size()) && (!x_.S2_state.size())) { x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); } } // 修改P_的状态 void change_P(cov &input_cov) { P_ = input_cov; } // 获得x_的状态 const state &get_x() const { return x_; } const cov &get_P() const { return P_; } private: state x_; measurement m_; cov P_; spMt l_; spMt f_x_1; spMt f_x_2; cov F_x1 = cov::Identity(); cov F_x2 = cov::Identity(); cov L_ = cov::Identity(); processModel *f; processMatrix1 *f_x; processMatrix2 *f_w; measurementModel *h; measurementMatrix1 *h_x; measurementMatrix2 *h_v; measurementModel_dyn *h_dyn; measurementMatrix1_dyn *h_x_dyn; measurementMatrix2_dyn *h_v_dyn; measurementModel_share *h_share; measurementModel_dyn_share *h_dyn_share; int maximum_iter = 0; scalar_type limit[n]; template T check_safe_update(T _temp_vec) { T temp_vec = _temp_vec; if (std::isnan(temp_vec(0, 0))) { temp_vec.setZero(); return temp_vec; } double angular_dis = temp_vec.block(0, 0, 3, 1).norm() * 57.3; double pos_dis = temp_vec.block(3, 0, 3, 1).norm(); if (angular_dis >= 20 || pos_dis > 1) { printf("Angular dis = %.2f, pos dis = %.2f\r\n", angular_dis, pos_dis); temp_vec.setZero(); } return temp_vec; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace esekfom #endif // ESEKFOM_EKF_HPP ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp ================================================ /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Author: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef __MEKFOM_UTIL_HPP__ #define __MEKFOM_UTIL_HPP__ #include #include "../mtk/src/mtkmath.hpp" namespace esekfom { template class is_same { public: operator bool() { return false; } }; template class is_same { public: operator bool() { return true; } }; template class is_double { public: operator bool() { return false; } }; template<> class is_double { public: operator bool() { return true; } }; template static T id(const T &x) { return x; } } // namespace esekfom #endif // __MEKFOM_UTIL_HPP__ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/build_manifold.hpp * @brief Macro to automatically construct compound manifolds. * */ #ifndef MTK_AUTOCONSTRUCT_HPP_ #define MTK_AUTOCONSTRUCT_HPP_ #include #include #include #include #include "src/SubManifold.hpp" #include "startIdx.hpp" #ifndef PARSED_BY_DOXYGEN //////// internals ////// #define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple #define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) #define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) #define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() #define MTK_CONSTRUCTOR_COPY( type, id) id(id) #define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); #define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); #define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); #define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} #define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} #define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} #define MTK_OSTREAM( type, id) << __var.id << " " #define MTK_ISTREAM( type, id) >> __var.id #define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} #define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} #define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} #define MTK_SUBVARLIST(seq, S2state, SO3state) \ BOOST_PP_FOR_1( \ ( \ BOOST_PP_SEQ_SIZE(seq), \ BOOST_PP_SEQ_HEAD(seq), \ BOOST_PP_SEQ_TAIL(seq) (~), \ 0,\ 0,\ S2state,\ SO3state ),\ MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) #define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ MTK::SubManifold id; #define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \ MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ enum {DOF = type::DOF + dof}; \ enum {DIM = type::DIM+dim}; \ typedef type::scalar scalar; #define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state #define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \ MTK_APPLY_MACRO_ON_TUPLE(~, \ BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) #define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state //! this used to be BOOST_PP_TUPLE_ELEM_4_0: #define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a #define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state #define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \ BOOST_PP_DEC(len), \ BOOST_PP_SEQ_HEAD(seq), \ BOOST_PP_SEQ_TAIL(seq), \ dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ S2state,\ SO3state) #endif /* not PARSED_BY_DOXYGEN */ /** * Construct a manifold. * @param name is the class-name of the manifold, * @param entries is the list of sub manifolds * * Entries must be given in a list like this: * @code * typedef MTK::trafo > Pose; * typedef MTK::vect Vec3; * MTK_BUILD_MANIFOLD(imu_state, * ((Pose, pose)) * ((Vec3, vel)) * ((Vec3, acc_bias)) * ) * @endcode * Whitespace is optional, but the double parentheses are necessary. * Construction is done entirely in preprocessor. * After construction @a name is also a manifold. Its members can be * accessed by names given in @a entries. * * @note Variable types are not allowed to have commas, thus types like * @c vect need to be typedef'ed ahead. */ #define MTK_BUILD_MANIFOLD(name, entries) \ struct name { \ typedef name self; \ std::vector > S2_state;\ std::vector > SO3_state;\ std::vector, int> > vect_state;\ MTK_SUBVARLIST(entries, S2_state, SO3_state) \ name ( \ MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ ) : \ MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ int getDOF() const { return DOF; } \ void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ MTK_TRANSFORM(MTK_BOXPLUS, entries)\ } \ void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ MTK_TRANSFORM(MTK_OPLUS, entries)\ } \ void boxminus(MTK::vectview __res, const name& __oth) const { \ MTK_TRANSFORM(MTK_BOXMINUS, entries)\ } \ friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ } \ void build_S2_state(){\ MTK_TRANSFORM(MTK_S2_state, entries)\ }\ void build_vect_state(){\ MTK_TRANSFORM(MTK_vect_state, entries)\ }\ void build_SO3_state(){\ MTK_TRANSFORM(MTK_SO3_state, entries)\ }\ void S2_hat(Eigen::Matrix &res, int idx) {\ MTK_TRANSFORM(MTK_S2_hat, entries)\ }\ void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ }\ void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ MTK_TRANSFORM(MTK_S2_Mx, entries)\ }\ friend std::istream& operator>>(std::istream& __is, name& __var){ \ return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ } \ }; #endif /*MTK_AUTOCONSTRUCT_HPP_*/ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/src/SubManifold.hpp * @brief Defines the SubManifold class */ #ifndef SUBMANIFOLD_HPP_ #define SUBMANIFOLD_HPP_ #include "vectview.hpp" namespace MTK { /** * @ingroup SubManifolds * Helper class for compound manifolds. * This class wraps a manifold T and provides an enum IDX refering to the * index of the SubManifold within the compound manifold. * * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". * * @tparam T The manifold type of the sub-type * @tparam idx The index of the sub-type within the compound manifold */ template struct SubManifold : public T { enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; //! manifold type typedef T type; //! Construct from derived type template explicit SubManifold(const X& t) : T(t) {}; //! Construct from internal type //explicit SubManifold(const T& t) : T(t) {}; //! inherit assignment operator using T::operator=; }; } // namespace MTK #endif /* SUBMANIFOLD_HPP_ */ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/src/mtkmath.hpp * @brief several math utility functions. */ #ifndef MTKMATH_H_ #define MTKMATH_H_ #include #include #include "../types/vect.hpp" #ifndef M_PI #define M_PI 3.1415926535897932384626433832795 #endif namespace MTK { namespace internal { template struct traits { typedef typename Manifold::scalar scalar; enum {DOF = Manifold::DOF}; typedef vect vectorized_type; typedef Eigen::Matrix matrix_type; }; template<> struct traits : traits > {}; template<> struct traits : traits > {}; } // namespace internal /** * \defgroup MTKMath Mathematical helper functions */ //@{ //! constant @f$ \pi @f$ const double pi = M_PI; template inline scalar tolerance(); template<> inline float tolerance() { return 1e-5f; } template<> inline double tolerance() { return 1e-11; } /** * normalize @a x to @f$[-bound, bound] @f$. * * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. */ template inline scalar normalize(scalar x, scalar bound){ //not used if(std::fabs(x) <= bound) return x; int r = (int)(x *(scalar(1.0)/ bound)); return x - ((r + (r>>31) + 1) & ~1)*bound; } /** * Calculate cosine and sinc of sqrt(x2). * @param x2 the squared angle must be non-negative * @return a pair containing cos and sinc of sqrt(x2) */ template std::pair cos_sinc_sqrt(const scalar &x2){ using std::sqrt; using std::cos; using std::sin; static scalar const taylor_0_bound = boost::math::tools::epsilon(); static scalar const taylor_2_bound = sqrt(taylor_0_bound); static scalar const taylor_n_bound = sqrt(taylor_2_bound); assert(x2>=0 && "argument must be non-negative"); // FIXME check if bigger bounds are possible if(x2>=taylor_n_bound) { // slow fall-back solution scalar x = sqrt(x2); return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. } // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) // TODO Find optimal coefficients using Remez algorithm static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; scalar cosi = 1., sinc=1; scalar term = -1/2. * x2; for(int i=0; i<3; ++i) { cosi += term; term *= inv[2*i]; sinc += term; term *= -inv[2*i+1] * x2; } return std::make_pair(cosi, sinc); } template Eigen::Matrix hat(const Base& v) { Eigen::Matrix res; res << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0; return res; } template Eigen::Matrix A_inv_trans(const Base& v){ Eigen::Matrix res; if(v.norm() > MTK::tolerance()) { res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); } else { res = Eigen::Matrix::Identity(); } return res; } template Eigen::Matrix A_inv(const Base& v){ Eigen::Matrix res; if(v.norm() > MTK::tolerance()) { res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); } else { res = Eigen::Matrix::Identity(); } return res; } template Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) { Eigen::Matrix res; scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); if(norm < MTK::tolerance()){ res = Eigen::Matrix::Zero(); res(0, 1) = 1; res(1, 2) = 1; res /= length; } else{ res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); res /= length; } } template Eigen::Matrix A_matrix(const Base & v){ Eigen::Matrix res; double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; double norm = std::sqrt(squaredNorm); if(norm < MTK::tolerance()){ res = Eigen::Matrix::Identity(); } else{ res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); } return res; } template scalar exp(vectview result, vectview vec, const scalar& scale = 1) { scalar norm2 = vec.squaredNorm(); std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); scalar mult = cos_sinc.second * scale; result = mult * vec; return cos_sinc.first; } /** * Inverse function to @c exp. * * @param result @c vectview to the result * @param w scalar part of input * @param vec vector part of input * @param scale scale result by this value * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result */ template void log(vectview result, const scalar &w, const vectview vec, const scalar &scale, bool plus_minus_periodicity) { // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division scalar nv = vec.norm(); if(nv < tolerance()) { if(!plus_minus_periodicity && w < 0) { // find the maximal entry: int i; nv = vec.cwiseAbs().maxCoeff(&i); result = scale * std::atan2(nv, w) * vect::Unit(i); return; } nv = tolerance(); } scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); result = s * vec; } } // namespace MTK #endif /* MTKMATH_H_ */ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp ================================================ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/src/vectview.hpp * @brief Wrapper class around a pointer used as interface for plain vectors. */ #ifndef VECTVIEW_HPP_ #define VECTVIEW_HPP_ #include namespace MTK { /** * A view to a vector. * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. * The dimension of the vector is given as template parameter and type-checked when used in expressions. * Data has to be modifiable. * * @tparam scalar Scalar type of the vector. * @tparam dim Dimension of the vector. * * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct */ namespace internal { template struct CovBlock { typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; }; template struct CovBlock_ { typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; }; template struct CrossCovBlock { typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; }; template struct CrossCovBlock_ { typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; }; template struct VectviewBase { typedef Eigen::Matrix matrix_type; typedef typename matrix_type::MapType Type; typedef typename matrix_type::ConstMapType ConstType; }; template struct UnalignedType { typedef T type; }; } template class vectview : public internal::VectviewBase::Type { typedef internal::VectviewBase VectviewBase; public: //! plain matrix type typedef typename VectviewBase::matrix_type matrix_type; //! base type typedef typename VectviewBase::Type base; //! construct from pointer explicit vectview(scalar* data, int dim_=dim) : base(data, dim_) {} //! construct from plain matrix vectview(matrix_type& m) : base(m.data(), m.size()) {} //! construct from another @c vectview vectview(const vectview &v) : base(v) {} //! construct from Eigen::Block: template vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} template vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} //! inherit assignment operator using base::operator=; //! data pointer scalar* data() {return const_cast(base::data());} }; /** * @c const version of @c vectview. * Compared to @c Eigen::Map this implementation is const correct, i.e., * data will not be modifiable using this view. * * @tparam scalar Scalar type of the vector. * @tparam dim Dimension of the vector. * * @sa vectview */ template class vectview : public internal::VectviewBase::ConstType { typedef internal::VectviewBase VectviewBase; public: //! plain matrix type typedef typename VectviewBase::matrix_type matrix_type; //! base type typedef typename VectviewBase::ConstType base; //! construct from const pointer explicit vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} //! construct from column vector template vectview(const Eigen::Matrix& m) : base(m.data()) {} //! construct from row vector template vectview(const Eigen::Matrix& m) : base(m.data()) {} //! construct from another @c vectview vectview(vectview x) : base(x.data()) {} //! construct from base vectview(const base &x) : base(x) {} /** * Construct from Block * @todo adapt this, when Block gets const-correct */ template vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} template vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} }; } // namespace MTK #endif /* VECTVIEW_HPP_ */ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/startIdx.hpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/startIdx.hpp * @brief Tools to access sub-elements of compound manifolds. */ #ifndef GET_START_INDEX_H_ #define GET_START_INDEX_H_ #include #include "src/SubManifold.hpp" #include "src/vectview.hpp" namespace MTK { /** * \defgroup SubManifolds Accessing Submanifolds * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers * @c &pose::orient and @c &pose::trans give all required information and are still * valid if the base type gets extended or the actual types of @a orient and @a trans * change (e.g. from 2D to 3D). * * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. */ //@{ /** * Determine the index of a sub-variable within a compound variable. */ template int getStartIdx( MTK::SubManifold Base::*) { return idx; } template int getStartIdx_( MTK::SubManifold Base::*) { return dim; } /** * Determine the degrees of freedom of a sub-variable within a compound variable. */ template int getDof( MTK::SubManifold Base::*) { return T::DOF; } template int getDim( MTK::SubManifold Base::*) { return T::DIM; } /** * set the diagonal elements of a covariance matrix corresponding to a sub-variable */ template void setDiagonal(Eigen::Matrix &cov, MTK::SubManifold Base::*, const typename Base::scalar &val) { cov.diagonal().template segment(idx).setConstant(val); } template void setDiagonal_(Eigen::Matrix &cov, MTK::SubManifold Base::*, const typename Base::scalar &val) { cov.diagonal().template segment(dim).setConstant(val); } /** * Get the subblock of corresponding to two members, i.e. * \code * Eigen::Matrix m; * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); * \endcode * lets you modify mixed covariance entries in a bigger covariance matrix. */ template typename MTK::internal::CovBlock::Type subblock(Eigen::Matrix &cov, MTK::SubManifold Base::*, MTK::SubManifold Base::*) { return cov.template block(idx1, idx2); } template typename MTK::internal::CovBlock_::Type subblock_(Eigen::Matrix &cov, MTK::SubManifold Base::*, MTK::SubManifold Base::*) { return cov.template block(dim1, dim2); } template typename MTK::internal::CrossCovBlock::Type subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) { return cov.template block(idx1, idx2); } template typename MTK::internal::CrossCovBlock_::Type subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) { return cov.template block(dim1, dim2); } /** * Get the subblock of corresponding to a member, i.e. * \code * Eigen::Matrix m; * MTK::subblock(m, &Pose::orient) = some_expression; * \endcode * lets you modify covariance entries in a bigger covariance matrix. */ template typename MTK::internal::CovBlock_::Type subblock_(Eigen::Matrix &cov, MTK::SubManifold Base::*) { return cov.template block(dim, dim); } template typename MTK::internal::CovBlock::Type subblock(Eigen::Matrix &cov, MTK::SubManifold Base::*) { return cov.template block(idx, idx); } template class get_cov { public: typedef Eigen::Matrix type; typedef const Eigen::Matrix const_type; }; template class get_cov_ { public: typedef Eigen::Matrix type; typedef const Eigen::Matrix const_type; }; template class get_cross_cov { public: typedef Eigen::Matrix type; typedef const type const_type; }; template class get_cross_cov_ { public: typedef Eigen::Matrix type; typedef const type const_type; }; template vectview subvector_impl_(vectview vec, SubManifold Base::*) { return vec.template segment(dim); } template vectview subvector_impl(vectview vec, SubManifold Base::*) { return vec.template segment(idx); } /** * Get the subvector corresponding to a sub-manifold from a bigger vector. */ template vectview subvector_(vectview vec, SubManifold Base::* ptr) { return subvector_impl_(vec, ptr); } template vectview subvector(vectview vec, SubManifold Base::* ptr) { return subvector_impl(vec, ptr); } /** * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) */ template vectview subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) { return subvector_impl(vectview(vec), ptr); } template vectview subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) { return subvector_impl_(vectview(vec), ptr); } template vectview subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) { return subvector_impl_(vectview(vec), ptr); } template vectview subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) { return subvector_impl(vectview(vec), ptr); } /** * const version of subvector(vectview vec,SubManifold Base::*) */ template vectview subvector_impl(const vectview cvec, SubManifold Base::*) { return cvec.template segment(idx); } template vectview subvector_impl_(const vectview cvec, SubManifold Base::*) { return cvec.template segment(dim); } template vectview subvector(const vectview cvec, SubManifold Base::* ptr) { return subvector_impl(cvec, ptr); } } // namespace MTK #endif // GET_START_INDEX_H_ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/S2.hpp ================================================ // This is a NEW implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/types/S2.hpp * @brief Unit vectors on the sphere, or directions in 3D. */ #ifndef S2_H_ #define S2_H_ #include "vect.hpp" #include "SOn.hpp" #include "../src/mtkmath.hpp" namespace MTK { /** * Manifold representation of @f$ S^2 @f$. * Used for unit vectors on the sphere or directions in 3D. * * @todo add conversions from/to polar angles? */ template struct S2 { typedef _scalar scalar; typedef vect<3, scalar> vect_type; typedef SO3 SO3_type; typedef typename vect_type::base vec3; scalar length = scalar(den)/scalar(num); enum {DOF=2, TYP = 1, DIM = 3}; //private: /** * Unit vector on the sphere, or vector pointing in a direction */ vect_type vec; public: S2() { if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); } S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { vec.normalize(); vec = vec * length; } S2(const vect_type &_vec) : vec(_vec) { vec.normalize(); vec = vec * length; } void oplus(MTK::vectview delta, scalar scale = 1) { SO3_type res; res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); vec = res.toRotationMatrix() * vec; } void boxplus(MTK::vectview delta, scalar scale=1) { Eigen::Matrix Bx; S2_Bx(Bx); vect_type Bu = Bx*delta;SO3_type res; res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); vec = res.toRotationMatrix() * vec; } void boxminus(MTK::vectview res, const S2& other) const { scalar v_sin = (MTK::hat(vec)*other.vec).norm(); scalar v_cos = vec.transpose() * other.vec; scalar theta = std::atan2(v_sin, v_cos); if(v_sin < MTK::tolerance()) { if(std::fabs(theta) > MTK::tolerance() ) { res[0] = 3.1415926; res[1] = 0; } else{ res[0] = 0; res[1] = 0; } } else { S2 other_copy = other; Eigen::MatrixBx; other_copy.S2_Bx(Bx); res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; } } void S2_hat(Eigen::Matrix &res) { Eigen::Matrix skew_vec; skew_vec << scalar(0), -vec[2], vec[1], vec[2], scalar(0), -vec[0], -vec[1], vec[0], scalar(0); res = skew_vec; } void S2_Bx(Eigen::Matrix &res) { if(S2_typ == 3) { if(vec[2] + length > tolerance()) { res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), -vec[0], -vec[1]; res /= length; } else { res = Eigen::Matrix::Zero(); res(1, 1) = -1; res(2, 0) = 1; } } else if(S2_typ == 2) { if(vec[1] + length > tolerance()) { res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), -vec[0], -vec[2], -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); res /= length; } else { res = Eigen::Matrix::Zero(); res(1, 1) = -1; res(2, 0) = 1; } } else { if(vec[0] + length > tolerance()) { res << -vec[1], -vec[2], length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); res /= length; } else { res = Eigen::Matrix::Zero(); res(1, 1) = -1; res(2, 0) = 1; } } } void S2_Nx(Eigen::Matrix &res, S2& subtrahend) { if((vec+subtrahend.vec).norm() > tolerance()) { Eigen::Matrix Bx; S2_Bx(Bx); if((vec-subtrahend.vec).norm() > tolerance()) { scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); scalar v_cos = vec.transpose() * subtrahend.vec; res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); } else { res = 1/length/length*Bx.transpose()*MTK::hat(vec); } } else { std::cerr << "No N(x, y) for x=-y" << std::endl; std::exit(100); } } void S2_Nx_yy(Eigen::Matrix &res) { Eigen::Matrix Bx; S2_Bx(Bx); res = 1/length/length*Bx.transpose()*MTK::hat(vec); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { Eigen::Matrix Bx; S2_Bx(Bx); if(delta.norm() < tolerance()) { res = -MTK::hat(vec)*Bx; } else{ vect_type Bu = Bx*delta; SO3_type exp_delta; exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; } } operator const vect_type&() const{ return vec; } const vect_type& get_vect() const { return vec; } friend S2 operator*(const SO3& rot, const S2& dir) { S2 ret; ret.vec = rot * dir.vec; return ret; } scalar operator[](int idx) const {return vec[idx]; } friend std::ostream& operator<<(std::ostream &os, const S2& vec){ return os << vec.vec.transpose() << " "; } friend std::istream& operator>>(std::istream &is, S2& vec){ for(int i=0; i<3; ++i) is >> vec.vec[i]; vec.vec.normalize(); vec.vec = vec.vec * vec.length; return is; } }; } // namespace MTK #endif /*S2_H_*/ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/types/SOn.hpp * @brief Standard Orthogonal Groups i.e.\ rotatation groups. */ #ifndef SON_H_ #define SON_H_ #include #include "vect.hpp" #include "../src/mtkmath.hpp" namespace MTK { /** * Two-dimensional orientations represented as scalar. * There is no guarantee that the representing scalar is within any interval, * but the result of boxminus will always have magnitude @f$\le\pi @f$. */ template struct SO2 : public Eigen::Rotation2D<_scalar> { enum {DOF = 1, DIM = 2, TYP = 3}; typedef _scalar scalar; typedef Eigen::Rotation2D base; typedef vect vect_type; //! Construct from angle SO2(const scalar& angle = 0) : base(angle) { } //! Construct from Eigen::Rotation2D SO2(const base& src) : base(src) {} /** * Construct from 2D vector. * Resulting orientation will rotate the first unit vector to point to vec. */ SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; //! Calculate @c this->inverse() * @c r SO2 operator%(const base &r) const { return base::inverse() * r; } //! Calculate @c this->inverse() * @c r template vect_type operator%(const Eigen::MatrixBase &vec) const { return base::inverse() * vec; } //! Calculate @c *this * @c r.inverse() SO2 operator/(const SO2 &r) const { return *this * r.inverse(); } //! Gets the angle as scalar. operator scalar() const { return base::angle(); } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } //! @name Manifold requirements void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void oplus(MTK::vectview vec, scalar scale = 1) { base::angle() += scale * vec[0]; } void boxplus(MTK::vectview vec, scalar scale = 1) { base::angle() += scale * vec[0]; } void boxminus(MTK::vectview res, const SO2& other) const { res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); } friend std::istream& operator>>(std::istream &is, SO2& ang){ return is >> ang.angle(); } }; /** * Three-dimensional orientations represented as Quaternion. * It is assumed that the internal Quaternion always stays normalized, * should this not be the case, call inherited member function @c normalize(). */ template struct SO3 : public Eigen::Quaternion<_scalar, Options> { enum {DOF = 3, DIM = 3, TYP = 2}; typedef _scalar scalar; typedef Eigen::Quaternion base; typedef Eigen::Quaternion Quaternion; typedef vect vect_type; //! Calculate @c this->inverse() * @c r template EIGEN_STRONG_INLINE Quaternion operator%(const Eigen::QuaternionBase &r) const { return base::conjugate() * r; } //! Calculate @c this->inverse() * @c r template vect_type operator%(const Eigen::MatrixBase &vec) const { return base::conjugate() * vec; } //! Calculate @c this * @c r.conjugate() template EIGEN_STRONG_INLINE Quaternion operator/(const Eigen::QuaternionBase &r) const { return *this * r.conjugate(); } /** * Construct from real part and three imaginary parts. * Quaternion is normalized after construction. */ SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { base::normalize(); } /** * Construct from Eigen::Quaternion. * @note Non-normalized input may result result in spurious behavior. */ SO3(const base& src = base::Identity()) : base(src) {} /** * Construct from rotation matrix. * @note Invalid rotation matrices may lead to spurious behavior. */ template SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} /** * Construct from arbitrary rotation type. * @note Invalid rotation matrices may lead to spurious behavior. */ template SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} //! @name Manifold requirements void boxplus(MTK::vectview vec, scalar scale=1) { SO3 delta = exp(vec, scale); *this = *this * delta; } void boxminus(MTK::vectview res, const SO3& other) const { res = SO3::log(other.conjugate() * *this); } //} void oplus(MTK::vectview vec, scalar scale=1) { SO3 delta = exp(vec, scale); *this = *this * delta; } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } friend std::ostream& operator<<(std::ostream &os, const SO3& q){ return os << q.coeffs().transpose() << " "; } friend std::istream& operator>>(std::istream &is, SO3& q){ vect<4,scalar> coeffs; is >> coeffs; q.coeffs() = coeffs.normalized(); return is; } //! @name Helper functions //{ /** * Calculate the exponential map. In matrix terms this would correspond * to the Rodrigues formula. */ // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround // static SO3 exp(MTK::vectview dvec, scalar scale = 1){ static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ SO3 res; res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); return res; } /** * Calculate the inverse of @c exp. * Only guarantees that exp(log(x)) == x */ static typename base::Vector3 log(const SO3 &orient){ typename base::Vector3 res; MTK::log(res, orient.w(), orient.vec(), scalar(2), true); return res; } }; namespace internal { template struct UnalignedType >{ typedef SO2 type; }; template struct UnalignedType >{ typedef SO3 type; }; } // namespace internal } // namespace MTK #endif /*SON_H_*/ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/vect.hpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 /* * Copyright (c) 2019--2023, The University of Hong Kong * All rights reserved. * * Modifier: Dongjiao HE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Copyright (c) 2008--2011, Universitaet Bremen * All rights reserved. * * Author: Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** * @file mtk/types/vect.hpp * @brief Basic vectors interpreted as manifolds. * * This file also implements a simple wrapper for matrices, for arbitrary scalars * and for positive scalars. */ #ifndef VECT_H_ #define VECT_H_ #include #include #include #include "../src/vectview.hpp" namespace MTK { static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); /** * A simple vector class. * Implementation is basically a wrapper around Eigen::Matrix with manifold * requirements added. */ template struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { typedef Eigen::Matrix<_scalar, D, 1, _Options> base; enum {DOF = D, DIM = D, TYP = 0}; typedef _scalar scalar; //using base::operator=; /** Standard constructor. Sets all values to zero. */ vect(const base &src = base::Zero()) : base(src) {} /** Constructor copying the value of the expression \a other */ template EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} /** Construct from memory. */ vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } void boxplus(MTK::vectview vec, scalar scale=1) { *this += scale * vec; } void boxminus(MTK::vectview res, const vect& other) const { res = *this - other; } void oplus(MTK::vectview vec, scalar scale=1) { *this += scale * vec; } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } friend std::ostream& operator<<(std::ostream &os, const vect& v){ // Eigen sometimes messes with the streams flags, so output manually: for(int i=0; i>(std::istream &is, vect& v){ char term=0; is >> std::ws; // skip whitespace switch(is.peek()) { case '(': term=')'; is.ignore(1); break; case '[': term=']'; is.ignore(1); break; case '{': term='}'; is.ignore(1); break; default: break; } if(D==Eigen::Dynamic) { assert(term !=0 && "Dynamic vectors must be embraced"); std::vector temp; while(is.good() && is.peek() != term) { scalar x; is >> x; temp.push_back(x); if(is.peek()==',') is.ignore(1); } v = vect::Map(temp.data(), temp.size()); } else for(int i=0; i> v[i]; if(is.peek()==',') { // ignore commas between values is.ignore(1); } } if(term!=0) { char x; is >> x; if(x!=term) { is.setstate(is.badbit); // assert(x==term && "start and end bracket do not match!"); } } return is; } template vectview tail(){ BOOST_STATIC_ASSERT(0< dim && dim <= DOF); return base::template tail(); } template vectview tail() const{ BOOST_STATIC_ASSERT(0< dim && dim <= DOF); return base::template tail(); } template vectview head(){ BOOST_STATIC_ASSERT(0< dim && dim <= DOF); return base::template head(); } template vectview head() const{ BOOST_STATIC_ASSERT(0< dim && dim <= DOF); return base::template head(); } }; /** * A simple matrix class. * Implementation is basically a wrapper around Eigen::Matrix with manifold * requirements added, i.e., matrix is viewed as a plain vector for that. */ template::Options> struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { typedef Eigen::Matrix<_scalar, M, N, _Options> base; enum {DOF = M * N, TYP = 4, DIM=0}; typedef _scalar scalar; using base::operator=; /** Standard constructor. Sets all values to zero. */ matrix() { base::setZero(); } /** Constructor copying the value of the expression \a other */ template EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} /** Construct from memory. */ matrix(const scalar* src) : base(src) { } void boxplus(MTK::vectview vec, scalar scale = 1) { *this += scale * base::Map(vec.data()); } void boxminus(MTK::vectview res, const matrix& other) const { base::Map(res.data()) = *this - other; } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } void oplus(MTK::vectview vec, scalar scale = 1) { *this += scale * base::Map(vec.data()); } void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ for(int i=0; i>(std::istream &is, matrix& mat){ for(int i=0; i> mat.data()[i]; } return is; } };// @todo What if M / N = Eigen::Dynamic? /** * A simple scalar type. */ template struct Scalar { enum {DOF = 1, TYP = 5, DIM=0}; typedef _scalar scalar; scalar value; Scalar(const scalar& value = scalar(0)) : value(value) {} operator const scalar&() const { return value; } operator scalar&() { return value; } Scalar& operator=(const scalar& val) { value = val; return *this; } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void oplus(MTK::vectview vec, scalar scale=1) { value += scale * vec[0]; } void boxplus(MTK::vectview vec, scalar scale=1) { value += scale * vec[0]; } void boxminus(MTK::vectview res, const Scalar& other) const { res[0] = *this - other; } }; /** * Positive scalars. * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. */ template struct PositiveScalar { enum {DOF = 1, TYP = 6, DIM=0}; typedef _scalar scalar; scalar value; PositiveScalar(const scalar& value = scalar(1)) : value(value) { assert(value > scalar(0)); } operator const scalar&() const { return value; } PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } void boxplus(MTK::vectview vec, scalar scale = 1) { value *= std::exp(scale * vec[0]); } void boxminus(MTK::vectview res, const PositiveScalar& other) const { res[0] = std::log(*this / other); } void oplus(MTK::vectview vec, scalar scale = 1) { value *= std::exp(scale * vec[0]); } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ is >> s.value; assert(s.value > 0); return is; } }; template struct Complex : public std::complex<_scalar>{ enum {DOF = 2, TYP = 7, DIM=0}; typedef _scalar scalar; typedef std::complex Base; Complex(const Base& value) : Base(value) {} Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} template Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} void boxplus(MTK::vectview vec, scalar scale = 1) { Base::real() += scale * vec[0]; Base::imag() += scale * vec[1]; }; void boxminus(MTK::vectview res, const Complex& other) const { Complex diff = *this - other; res << diff.real(), diff.imag(); } void S2_hat(Eigen::Matrix &res) { res = Eigen::Matrix::Zero(); } void oplus(MTK::vectview vec, scalar scale = 1) { Base::real() += scale * vec[0]; Base::imag() += scale * vec[1]; }; void S2_Nx_yy(Eigen::Matrix &res) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) { std::cerr << "wrong idx for S2" << std::endl; std::exit(100); res = Eigen::Matrix::Zero(); } scalar squaredNorm() const { return std::pow(Base::real(),2) + std::pow(Base::imag(),2); } const scalar& operator()(int i) const { assert(0<=i && i<2 && "Index out of range"); return i==0 ? Base::real() : Base::imag(); } scalar& operator()(int i){ assert(0<=i && i<2 && "Index out of range"); return i==0 ? Base::real() : Base::imag(); } }; namespace internal { template struct UnalignedType >{ typedef vect type; }; } // namespace internal } // namespace MTK #endif /*VECT_H_*/ ================================================ FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp ================================================ /* * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH * All rights reserved. * * Author: Rene Wagner * Christoph Hertzberg * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Universitaet Bremen nor the DFKI GmbH * nor the names of its contributors may be used to endorse or * promote products derived from this software without specific * prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef WRAPPED_CV_MAT_HPP_ #define WRAPPED_CV_MAT_HPP_ #include #include namespace MTK { template struct cv_f_type; template<> struct cv_f_type { enum {value = CV_64F}; }; template<> struct cv_f_type { enum {value = CV_32F}; }; /** * cv_mat wraps a CvMat around an Eigen Matrix */ template class cv_mat : public matrix { typedef matrix base_type; enum {type_ = cv_f_type::value}; CvMat cv_mat_; public: cv_mat() { cv_mat_ = cvMat(rows, cols, type_, base_type::data()); } cv_mat(const cv_mat& oth) : base_type(oth) { cv_mat_ = cvMat(rows, cols, type_, base_type::data()); } template cv_mat(const Eigen::MatrixBase &value) : base_type(value) { cv_mat_ = cvMat(rows, cols, type_, base_type::data()); } template cv_mat& operator=(const Eigen::MatrixBase &value) { base_type::operator=(value); return *this; } cv_mat& operator=(const cv_mat& value) { base_type::operator=(value); return *this; } // FIXME: Maybe overloading operator& is not a good idea ... CvMat* operator&() { return &cv_mat_; } const CvMat* operator&() const { return &cv_mat_; } }; } // namespace MTK #endif /* WRAPPED_CV_MAT_HPP_ */ ================================================ FILE: FAST-LIO/include/common_lib.h ================================================ #ifndef COMMON_LIB_H #define COMMON_LIB_H #include #include #include #include #include #include #include #include #include using namespace std; using namespace Eigen; #define USE_IKFOM #define PI_M (3.14159265358) #define G_m_s2 (9.81) // 在广东的重力加速度 #define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) #define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) #define CUBE_LEN (6.0) #define LIDAR_SP_LEN (2) #define INIT_COV (1) #define NUM_MATCH_POINTS (5) #define MAX_MEAS_DIM (10000) #define VEC_FROM_ARRAY(v) v[0], v[1], v[2] #define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8] #define CONSTRAIN(v, min, max) ((v > min) ? ((v < max) ? v : max) : min) #define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols() #define STD_VEC_FROM_EIGEN(mat) vector(mat.data(), mat.data() + mat.rows() * mat.cols()) #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name)) typedef fast_lio::Pose6D Pose6D; typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; typedef vector> PointVector; typedef Vector3d V3D; typedef Matrix3d M3D; typedef Vector3f V3F; typedef Matrix3f M3F; #define MD(a, b) Matrix #define VD(a) Matrix #define MF(a, b) Matrix #define VF(a) Matrix M3D Eye3d(M3D::Identity()); M3F Eye3f(M3F::Identity()); V3D Zero3d(0, 0, 0); V3F Zero3f(0, 0, 0); struct MeasureGroup // Lidar data and imu dates for the curent process { MeasureGroup() { lidar_beg_time = 0.0; this->lidar.reset(new PointCloudXYZI()); }; double lidar_beg_time; PointCloudXYZI::Ptr lidar; deque imu; }; struct StatesGroup { StatesGroup() { this->rot_end = M3D::Identity(); this->pos_end = Zero3d; this->vel_end = Zero3d; this->bias_g = Zero3d; this->bias_a = Zero3d; this->gravity = Zero3d; this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV; this->cov.block<9, 9>(9, 9) = MD(9, 9)::Identity() * 0.00001; }; StatesGroup(const StatesGroup &b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->vel_end = b.vel_end; this->bias_g = b.bias_g; this->bias_a = b.bias_a; this->gravity = b.gravity; this->cov = b.cov; }; StatesGroup &operator=(const StatesGroup &b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->vel_end = b.vel_end; this->bias_g = b.bias_g; this->bias_a = b.bias_a; this->gravity = b.gravity; this->cov = b.cov; return *this; }; StatesGroup operator+(const Matrix &state_add) { StatesGroup a; a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0); a.vel_end = this->vel_end + state_add.block<3, 1>(6, 0); a.bias_g = this->bias_g + state_add.block<3, 1>(9, 0); a.bias_a = this->bias_a + state_add.block<3, 1>(12, 0); a.gravity = this->gravity + state_add.block<3, 1>(15, 0); a.cov = this->cov; return a; }; StatesGroup &operator+=(const Matrix &state_add) { this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); this->pos_end += state_add.block<3, 1>(3, 0); this->vel_end += state_add.block<3, 1>(6, 0); this->bias_g += state_add.block<3, 1>(9, 0); this->bias_a += state_add.block<3, 1>(12, 0); this->gravity += state_add.block<3, 1>(15, 0); return *this; }; Matrix operator-(const StatesGroup &b) { Matrix a; M3D rotd(b.rot_end.transpose() * this->rot_end); a.block<3, 1>(0, 0) = Log(rotd); a.block<3, 1>(3, 0) = this->pos_end - b.pos_end; a.block<3, 1>(6, 0) = this->vel_end - b.vel_end; a.block<3, 1>(9, 0) = this->bias_g - b.bias_g; a.block<3, 1>(12, 0) = this->bias_a - b.bias_a; a.block<3, 1>(15, 0) = this->gravity - b.gravity; return a; }; void resetpose() { this->rot_end = M3D::Identity(); this->pos_end = Zero3d; this->vel_end = Zero3d; } M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point V3D pos_end; // the estimated position at the end lidar point (world frame) V3D vel_end; // the estimated velocity at the end lidar point (world frame) V3D bias_g; // gyroscope bias V3D bias_a; // accelerator bias V3D gravity; // the estimated gravity acceleration Matrix cov; // states covariance }; template T rad2deg(T radians) { return radians * 180.0 / PI_M; } template T deg2rad(T degrees) { return degrees * PI_M / 180.0; } template auto set_pose6d(const double t, const Matrix &a, const Matrix &g, const Matrix &v, const Matrix &p, const Matrix &R) { Pose6D rot_kp; rot_kp.offset_time = t; for (int i = 0; i < 3; i++) { rot_kp.acc[i] = a(i); rot_kp.gyr[i] = g(i); rot_kp.vel[i] = v(i); rot_kp.pos[i] = p(i); for (int j = 0; j < 3; j++) rot_kp.rot[i * 3 + j] = R(i, j); } return move(rot_kp); } /* comment plane equation: Ax + By + Cz + D = 0 convert to: A/D*x + B/D*y + C/D*z = -1 solve: A0*x0 = b0 where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T normvec: normalized x0 */ template bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) { MatrixXf A(point_num, 3); MatrixXf b(point_num, 1); b.setOnes(); b *= -1.0f; for (int j = 0; j < point_num; j++) { A(j, 0) = point[j].x; A(j, 1) = point[j].y; A(j, 2) = point[j].z; } normvec = A.colPivHouseholderQr().solve(b); for (int j = 0; j < point_num; j++) { if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) { return false; } } normvec.normalize(); return true; } float calc_dist(PointType p1, PointType p2) { float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); return d; } template bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) //按照点集计算平面方程 { Matrix A; //点集的矩阵 Matrix b; //点集的值 A.setZero(); b.setOnes(); b *= -1.0f; for (int j = 0; j < NUM_MATCH_POINTS; j++) //将点集的值和矩阵放入A和b中 { A(j, 0) = point[j].x; A(j, 1) = point[j].y; A(j, 2) = point[j].z; } Matrix normvec = A.colPivHouseholderQr().solve(b); //求解点集的平面方程 T n = normvec.norm(); //求解点集的平面方程的法向量 pca_result(0) = normvec(0) / n; //求解点集的平面方程的法向量 pca_result(1) = normvec(1) / n; pca_result(2) = normvec(2) / n; pca_result(3) = 1.0 / n; for (int j = 0; j < NUM_MATCH_POINTS; j++) { //求解点集的平面方程的法向量与点集的值的积大于阈值，则代表点集不符合平面方程 if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) { return false; } } return true; } #endif ================================================ FILE: FAST-LIO/include/ikd-Tree/README.md ================================================ # ikd-Tree ikd-Tree is an incremental k-d tree for robotic applications. ================================================ FILE: FAST-LIO/include/ikd-Tree/ikd_Tree.cpp ================================================ #include "ikd_Tree.h" /* Description: ikd-Tree: an incremental k-d tree for robotic applications Author: Yixi Cai email: yixicai@connect.hku.hk */ KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) { delete_criterion_param = delete_param; balance_criterion_param = balance_param; downsample_size = box_length; Rebuild_Logger.clear(); termination_flag = false; // 初始化一系列数据互斥锁，并且创建重构树的线程 start_thread(); } KD_TREE::~KD_TREE() { stop_thread(); Delete_Storage_Disabled = true; delete_tree_nodes(&Root_Node); PointVector().swap(PCL_Storage); Rebuild_Logger.clear(); } void KD_TREE::Set_delete_criterion_param(float delete_param) //设置删除标准参数 { delete_criterion_param = delete_param; } void KD_TREE::Set_balance_criterion_param(float balance_param) //设置平衡阈值 { balance_criterion_param = balance_param; } void KD_TREE::set_downsample_param(float downsample_param) //设置下采样的参数 { downsample_size = downsample_param; } void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) //初始化kd-tree { Set_delete_criterion_param(delete_param); Set_balance_criterion_param(balance_param); set_downsample_param(box_length); } // 初始化根节点 void KD_TREE::InitTreeNode(KD_TREE_NODE *root) { root->point.x = 0.0f; //初始化根节点的坐标 root->point.y = 0.0f; root->point.z = 0.0f; root->node_range_x[0] = 0.0f; //初始化根节点的范围 root->node_range_x[1] = 0.0f; root->node_range_y[0] = 0.0f; root->node_range_y[1] = 0.0f; root->node_range_z[0] = 0.0f; root->node_range_z[1] = 0.0f; root->division_axis = 0; //初始化根节点的分割轴 root->father_ptr = nullptr; //初始化根节点的父节点指针 root->left_son_ptr = nullptr; //初始化根节点的左子指针 root->right_son_ptr = nullptr; //初始化根节点的右子指针 root->TreeSize = 0; //初始化根节点的子树大小 root->invalid_point_num = 0; //初始化根节点的无效点数 root->down_del_num = 0; //初始化根节点的下采样删除数 root->point_deleted = false; //初始化根节点的点是否被删除 root->tree_deleted = false; //初始化根节点的树是否被删除 root->need_push_down_to_left = false; //初始化根节点的是否需要向左子树下采样 root->need_push_down_to_right = false; //初始化根节点的是否需要向右子树下采样 root->point_downsample_deleted = false; //初始化根节点的点是否被下采样删除 root->working_flag = false; //初始化根节点的工作标志 // 初始化互斥锁 相当于 pthread_mutex_t root->push_down_mutex_lock = PTHREAD_MUTEX_INITIALIZER pthread_mutex_init(&(root->push_down_mutex_lock), NULL); } int KD_TREE::size() { int s = 0; if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { if (Root_Node != nullptr) { return Root_Node->TreeSize; } else { return 0; } } else { /*锁操作主要包括加锁pthread_mutex_lock()、解锁pthread_mutex_unlock()和 测试加锁 pthread_mutex_trylock()三个，不论哪种类型的锁，都不可能被两个不 同的线程同时得到，而必须等待解锁。对于普通锁和适应锁类型，解锁者可以是同进程 内任何线程；而检错锁则必须由加锁者解锁才有效，否则返回EPERM；对于嵌套锁，文 档和实现要求必须由加锁者解锁，但实验结果表明并没有这种限制，这个不同目前还没 有得到解释。在同一进程中的线程，如果加锁后没有解锁，则任何其他线程都无法再获得锁。*/ // pthread_mutex_trylock()语义与pthread_mutex_lock()类似，不同的是在锁已经被占据时返回EBUSY而不是挂起等待 if (!pthread_mutex_trylock(&working_flag_mutex)) { // 返回根节点汇总得到的有效节点个数 s = Root_Node->TreeSize; pthread_mutex_unlock(&working_flag_mutex); return s; } else { return Treesize_tmp; } } } // 获取一个包围整个树的Box边界 BoxPointType KD_TREE::tree_range() { BoxPointType range; if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { if (Root_Node != nullptr) { range.vertex_min[0] = Root_Node->node_range_x[0]; range.vertex_min[1] = Root_Node->node_range_y[0]; range.vertex_min[2] = Root_Node->node_range_z[0]; range.vertex_max[0] = Root_Node->node_range_x[1]; range.vertex_max[1] = Root_Node->node_range_y[1]; range.vertex_max[2] = Root_Node->node_range_z[1]; } else { memset(&range, 0, sizeof(range)); } } else { if (!pthread_mutex_trylock(&working_flag_mutex)) { range.vertex_min[0] = Root_Node->node_range_x[0]; range.vertex_min[1] = Root_Node->node_range_y[0]; range.vertex_min[2] = Root_Node->node_range_z[0]; range.vertex_max[0] = Root_Node->node_range_x[1]; range.vertex_max[1] = Root_Node->node_range_y[1]; range.vertex_max[2] = Root_Node->node_range_z[1]; pthread_mutex_unlock(&working_flag_mutex); } else { memset(&range, 0, sizeof(range)); } } return range; } int KD_TREE::validnum() { int s = 0; if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { if (Root_Node != nullptr) return (Root_Node->TreeSize - Root_Node->invalid_point_num); else return 0; } else { if (!pthread_mutex_trylock(&working_flag_mutex)) { s = Root_Node->TreeSize - Root_Node->invalid_point_num; pthread_mutex_unlock(&working_flag_mutex); return s; } else { return -1; } } } void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { alpha_bal = Root_Node->alpha_bal; alpha_del = Root_Node->alpha_del; return; } else { if (!pthread_mutex_trylock(&working_flag_mutex)) { alpha_bal = Root_Node->alpha_bal; alpha_del = Root_Node->alpha_del; pthread_mutex_unlock(&working_flag_mutex); return; } else { alpha_bal = alpha_bal_tmp; alpha_del = alpha_del_tmp; return; } } } void KD_TREE::start_thread() { // 初始一系列互斥锁 pthread_mutex_init(&termination_flag_mutex_lock, NULL); pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); pthread_mutex_init(&working_flag_mutex, NULL); pthread_mutex_init(&search_flag_mutex, NULL); // 创建重建树的线程，线程标识符指针：rebuild_thread， 运行函数：multi_thread_ptr pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); printf("Multi thread started \n"); } // 停止线程 void KD_TREE::stop_thread() { pthread_mutex_lock(&termination_flag_mutex_lock); termination_flag = true; pthread_mutex_unlock(&termination_flag_mutex_lock); // 等待重构树的线程结束 if (rebuild_thread) pthread_join(rebuild_thread, NULL); /*销毁一个互斥锁即意味着释放它所占用的资源，且要求锁当前处于开放状态。由于在Linux中， 互斥锁并不占用任何资源，因此LinuxThreads中的 pthread_mutex_destroy()除了检查 锁状态以外（锁定状态则返回EBUSY）没有其他动作。*/ pthread_mutex_destroy(&termination_flag_mutex_lock); pthread_mutex_destroy(&rebuild_logger_mutex_lock); pthread_mutex_destroy(&rebuild_ptr_mutex_lock); pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); pthread_mutex_destroy(&working_flag_mutex); pthread_mutex_destroy(&search_flag_mutex); } void *KD_TREE::multi_thread_ptr(void *arg) { KD_TREE *handle = (KD_TREE *)arg; handle->multi_thread_rebuild(); return nullptr; } // 构建ikd tree后，该线程一直在运行 // Algorithm 4: Parallelly Rebuild for Re-balancing void KD_TREE::multi_thread_rebuild() { bool terminated = false; // 新建虚拟头结点和重建的树的新节点 KD_TREE_NODE *father_ptr, **new_node_ptr; pthread_mutex_lock(&termination_flag_mutex_lock); // KD树初始化时termination_flag被设为false，所以后面的while循环是一直进行的 terminated = termination_flag; pthread_mutex_unlock(&termination_flag_mutex_lock); while (!terminated) { // The second thread will lock all incremental updates (i.e., points insert, re-insert, // and delete) but not queries on this sub-tree // 锁定了插入、重插入、删除功能，但不锁定查询功能 pthread_mutex_lock(&rebuild_ptr_mutex_lock); pthread_mutex_lock(&working_flag_mutex); // Rebuild_Ptr是一个tree_node类型的指针的指针 if (Rebuild_Ptr != nullptr) { /* Traverse and copy */ // 如果需要重建树的话，会先将树展平放到Rebuild_Logger里，如果这个记录器为空的话，但是又要求重建的话，那会报错 if (!Rebuild_Logger.empty()) { printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); } rebuild_flag = true; // 判断重建的子树的根节点是否为整棵树的根节点 if (*Rebuild_Ptr == Root_Node) { Treesize_tmp = Root_Node->TreeSize; Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; alpha_bal_tmp = Root_Node->alpha_bal; alpha_del_tmp = Root_Node->alpha_del; } KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); father_ptr = (*Rebuild_Ptr)->father_ptr; PointVector().swap(Rebuild_PCL_Storage); // Lock Search pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter != 0) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter = -1; pthread_mutex_unlock(&search_flag_mutex); // Lock deleted points cache pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); // 将树展平 flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); // Unlock deleted points cache pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); // Unlock Search pthread_mutex_lock(&search_flag_mutex); search_mutex_counter = 0; pthread_mutex_unlock(&search_flag_mutex); pthread_mutex_unlock(&working_flag_mutex); /* Rebuild and update missed operations*/ Operation_Logger_Type Operation; KD_TREE_NODE *new_root_node = nullptr; // 如果展平的树的节点大于0，则可以开始重建 if (int(Rebuild_PCL_Storage.size()) > 0) { // 重建树，树的根节点为new_root_node BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); // Rebuild has been done. Updates the blocked operations into the new tree pthread_mutex_lock(&working_flag_mutex); pthread_mutex_lock(&rebuild_logger_mutex_lock); int tmp_counter = 0; // 重建树完成后，将中途的更新操作（插入、重插入、删除）操作补充 while (!Rebuild_Logger.empty()) { // 读取操作，是插入？重插入？还是删除？ Operation = Rebuild_Logger.front(); max_queue_size = max(max_queue_size, Rebuild_Logger.size()); Rebuild_Logger.pop(); pthread_mutex_unlock(&rebuild_logger_mutex_lock); pthread_mutex_unlock(&working_flag_mutex); // 在新的树中操作 run_operation(&new_root_node, Operation); tmp_counter++; if (tmp_counter % 10 == 0) usleep(1); pthread_mutex_lock(&working_flag_mutex); pthread_mutex_lock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&rebuild_logger_mutex_lock); } /* Replace to original tree*/ // pthread_mutex_lock(&working_flag_mutex); pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter != 0) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter = -1; pthread_mutex_unlock(&search_flag_mutex); // 用新树替换原来的树 if (father_ptr->left_son_ptr == *Rebuild_Ptr) { father_ptr->left_son_ptr = new_root_node; } else if (father_ptr->right_son_ptr == *Rebuild_Ptr) { father_ptr->right_son_ptr = new_root_node; } else { throw "Error: Father ptr incompatible with current node\n"; } // 给新的子树重新配置虚拟头结点 if (new_root_node != nullptr) new_root_node->father_ptr = father_ptr; // 将新的头结点赋给重建的树 (*Rebuild_Ptr) = new_root_node; // 重新配置树的属性 int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; if (father_ptr == STATIC_ROOT_NODE) Root_Node = STATIC_ROOT_NODE->left_son_ptr; KD_TREE_NODE *update_root = *Rebuild_Ptr; // 将子树属性上拉到树的根节点 while (update_root != nullptr && update_root != Root_Node) { update_root = update_root->father_ptr; if (update_root->working_flag) break; if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) break; if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) break; Update(update_root); } pthread_mutex_lock(&search_flag_mutex); search_mutex_counter = 0; pthread_mutex_unlock(&search_flag_mutex); Rebuild_Ptr = nullptr; pthread_mutex_unlock(&working_flag_mutex); rebuild_flag = false; /* Delete discarded tree nodes */ delete_tree_nodes(&old_root_node); } else { pthread_mutex_unlock(&working_flag_mutex); } pthread_mutex_unlock(&rebuild_ptr_mutex_lock); pthread_mutex_lock(&termination_flag_mutex_lock); terminated = termination_flag; pthread_mutex_unlock(&termination_flag_mutex_lock); usleep(100); } printf("Rebuild thread terminated normally\n"); } void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) { switch (operation.op) { case ADD_POINT: Add_by_point(root, operation.point, false, (*root)->division_axis); break; case ADD_BOX: Add_by_range(root, operation.boxpoint, false); break; case DELETE_POINT: Delete_by_point(root, operation.point, false); break; case DELETE_BOX: Delete_by_range(root, operation.boxpoint, false, false); break; case DOWNSAMPLE_DELETE: Delete_by_range(root, operation.boxpoint, false, true); break; case PUSH_DOWN: (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; if (operation.tree_downsample_deleted) (*root)->down_del_num = (*root)->TreeSize; if (operation.tree_deleted) (*root)->invalid_point_num = (*root)->TreeSize; else (*root)->invalid_point_num = (*root)->down_del_num; (*root)->need_push_down_to_left = true; (*root)->need_push_down_to_right = true; break; default: break; } } void KD_TREE::Build(PointVector point_cloud) { if (Root_Node != nullptr) { delete_tree_nodes(&Root_Node); //如果有root需要删除以前的root } if (point_cloud.size() == 0) return; STATIC_ROOT_NODE = new KD_TREE_NODE; //初始化当前节点的属性 InitTreeNode(STATIC_ROOT_NODE); //建立kdtree，这里直接从其左子树开始构建 BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); //更新root的属性 Update(STATIC_ROOT_NODE); STATIC_ROOT_NODE->TreeSize = 0; // 将左子树直接替换根节点 Root_Node = STATIC_ROOT_NODE->left_son_ptr; } // K近邻搜索 void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, double max_dist) { // 初始化自定义的大顶堆，容量为2*k_nearest MANUAL_HEAP q(2 * k_nearest); q.clear(); // 清空Point_Distance vector().swap(Point_Distance); // 开始搜索 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { Search(Root_Node, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(Root_Node, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } int k_found = min(k_nearest, int(q.size())); // 清空Nearest_Points和Point_Distance PointVector().swap(Nearest_Points); vector().swap(Point_Distance); // 存储 for (int i = 0; i < k_found; i++) { // 优先取出最近的点 Nearest_Points.insert(Nearest_Points.begin(), q.top().point); Point_Distance.insert(Point_Distance.begin(), q.top().dist); q.pop(); } return; } // 向树中插入新的点集 int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) { // 需要插入的点的个数 int NewPointSize = PointToAdd.size(); // 树的节点个数 int tree_size = size(); BoxPointType Box_of_Point; PointType downsample_result, mid_point; // 根据输入参数判断是否需要进行降采样 bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; float min_dist, tmp_dist; int tmp_counter = 0; // 遍历点集 for (int i = 0; i < PointToAdd.size(); i++) { // 判断是否需要降采样 if (downsample_switch) { // 计算该点所属的体素 Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; // 计算该体素的中心点坐标 mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; // 清空降采样缓存 PointVector().swap(Downsample_Storage); // 在树中搜索最近邻点，且要求点都在Box中，将点存于Downsample_Storage Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); // 计算当前点于体素中心的距离 min_dist = calc_dist(PointToAdd[i], mid_point); downsample_result = PointToAdd[i]; // 遍历体素内的所有点,寻找近邻点中最接近体素中心的点 for (int index = 0; index < Downsample_Storage.size(); index++) { // 计算当前近邻点与体素中心的距离 tmp_dist = calc_dist(Downsample_Storage[index], mid_point); // 比较两个距离，判断是否需要添加该点 if (tmp_dist < min_dist) { min_dist = tmp_dist; downsample_result = Downsample_Storage[index]; } } // 如果不需要重建树 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { // 如果近邻点不止1个，或者当前点与原有的点很接近 if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) { // 删除体素内的原有点，然后插入当前点 if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, true, true); Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); tmp_counter++; } } else { // 需要重建树 if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) { Operation_Logger_Type operation_delete, operation; // 记录待删除的点 operation_delete.boxpoint = Box_of_Point; operation_delete.op = DOWNSAMPLE_DELETE; // 记录待插入的新点 operation.point = downsample_result; operation.op = ADD_POINT; pthread_mutex_lock(&working_flag_mutex); // 删除体素内的点 if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, false, true); // 添加点 Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); tmp_counter++; if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); if (Downsample_Storage.size() > 0) Rebuild_Logger.push(operation_delete); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); }; } } else { // 不需要降采样 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); } else { Operation_Logger_Type operation; operation.point = PointToAdd[i]; operation.op = ADD_POINT; pthread_mutex_lock(&working_flag_mutex); Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } } return tmp_counter; } // 以体素的形式添加点（re-insertion） void KD_TREE::Add_Point_Boxes(vector &BoxPoints) { for (int i = 0; i < BoxPoints.size(); i++) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { Add_by_range(&Root_Node, BoxPoints[i], true); } else { Operation_Logger_Type operation; operation.boxpoint = BoxPoints[i]; operation.op = ADD_BOX; pthread_mutex_lock(&working_flag_mutex); Add_by_range(&Root_Node, BoxPoints[i], false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } return; } // 删除多个点 void KD_TREE::Delete_Points(PointVector &PointToDel) { for (int i = 0; i < PointToDel.size(); i++) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { Delete_by_point(&Root_Node, PointToDel[i], true); } else { Operation_Logger_Type operation; operation.point = PointToDel[i]; operation.op = DELETE_POINT; pthread_mutex_lock(&working_flag_mutex); Delete_by_point(&Root_Node, PointToDel[i], false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } return; } // 删除多个体素的点 int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) { int tmp_counter = 0; for (int i = 0; i < BoxPoints.size(); i++) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) { tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); } else { Operation_Logger_Type operation; operation.boxpoint = BoxPoints[i]; operation.op = DELETE_BOX; pthread_mutex_lock(&working_flag_mutex); tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } return tmp_counter; } void KD_TREE::acquire_removed_points(PointVector &removed_points) { // 获取删除的点 pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); // 加锁 for (int i = 0; i < Points_deleted.size(); i++) { // 将删除的点放入removed_points中,这里的点在flatten函数中加入 removed_points.push_back(Points_deleted[i]); } for (int i = 0; i < Multithread_Points_deleted.size(); i++) { // 将多线程重建时删除的点放入removed_points中,这里的点在flatten函数中加入 removed_points.push_back(Multithread_Points_deleted[i]); } Points_deleted.clear(); Multithread_Points_deleted.clear(); pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); // 解锁 return; } // 建树的过程与构建有序二叉树的过程类似，取中间的节点作为节点值进行递归 void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) { if (l > r) return; //给**root分配内存 *root = new KD_TREE_NODE; // 初始化根节点 InitTreeNode(*root); // 相当于mid=(1+r)/2 int mid = (l + r) >> 1; // 初始化划分轴 int div_axis = 0; int i; // Find the best division Axis // 旋转最优的划分轴，这里优先选用这团点云中区间最大的那个轴 float min_value[3] = {INFINITY, INFINITY, INFINITY}; float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; float dim_range[3] = {0, 0, 0}; for (i = l; i <= r; i++) { min_value[0] = min(min_value[0], Storage[i].x); min_value[1] = min(min_value[1], Storage[i].y); min_value[2] = min(min_value[2], Storage[i].z); max_value[0] = max(max_value[0], Storage[i].x); max_value[1] = max(max_value[1], Storage[i].y); max_value[2] = max(max_value[2], Storage[i].z); } // Select the longest dimension as division axis // 选用跨度最大的维度 for (i = 0; i < 3; i++) dim_range[i] = max_value[i] - min_value[i]; for (i = 1; i < 3; i++) if (dim_range[i] > dim_range[div_axis]) div_axis = i; // Divide by the division axis and recursively build. // 给该节点标记划分轴 (*root)->division_axis = div_axis; /********************************************* * nth_element(first, nth, last, compare) 求[first, last]这个区间中第n大小的元素，如果参数加入了compare函数，就按compare函数的方式比较。 这一段就是找切割维度的中位数 nth_element(a,a+k,a+n)，函数只是把下标为k的元素放在了正确位置，对其它元素并没有排序， 当然k左边元素都小于等于它，右边元素都大于等于它，所以可以利用这个函数快速定位某个元素。 ******************************************************/ switch (div_axis) { case 0: nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); break; case 1: nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); break; case 2: nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); break; default: nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); break; } (*root)->point = Storage[mid]; KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; //这就是一般的用队列建树的流程， 属于是二叉树前序遍历过程 BuildTree(&left_son, l, mid - 1, Storage); // 处理左子树 BuildTree(&right_son, mid + 1, r, Storage); // 处理右子树 // 标记该节点的左右子树 (*root)->left_son_ptr = left_son; (*root)->right_son_ptr = right_son; //更新node的属性 Update((*root)); return; } // 重建树 void KD_TREE::Rebuild(KD_TREE_NODE **root) { KD_TREE_NODE *father_ptr; // 如果树的节点数大于阈值，则启动多线程进行重建 if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) { // pthread_mutex_trylock() 是 pthread_mutex_lock() 的非阻塞版本。 // 如果 mutex 所引用的互斥对象当前被任何线程（包括当前线程）锁定，则将立即返 // 回该调用。否则，该互斥锁将处于锁定状态，调用线程是其属主。 if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) { // true：成功加锁，fasle：其他线程已上锁 // 如果重建树的指针为空，或者当前子树的尺寸比正在重建的子树大，则将重建树的根节点设为当前根节点 if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) { Rebuild_Ptr = root; } pthread_mutex_unlock(&rebuild_ptr_mutex_lock); // 解锁之后即开始启动多线程重建 } } else { // 单线程重建 // 暂存虚拟父节点 father_ptr = (*root)->father_ptr; int size_rec = (*root)->TreeSize; PCL_Storage.clear(); // 将当前子树的节点展平，同时做被删除点的记录 flatten(*root, PCL_Storage, DELETE_POINTS_REC); // 释放被删除点的内存空间 delete_tree_nodes(root); // 基于展平的点重新构建树 BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); // 重新连接节点间的父子关系 if (*root != nullptr) (*root)->father_ptr = father_ptr; if (*root == Root_Node) STATIC_ROOT_NODE->left_son_ptr = *root; } return; } // box-wise delete int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) { if ((*root) == nullptr || (*root)->tree_deleted) return 0; (*root)->working_flag = true; // 下拉属性 Push_Down(*root); int tmp_counter = 0; // 前三行是判断给定的Box范围是否与树有交集，没有的话就不用搜索了 if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return 0; if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return 0; if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return 0; // Box包含了整一棵树的Box if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) { // 整棵树的属性都标记为删除 (*root)->tree_deleted = true; (*root)->point_deleted = true; // 标记下，下次需要下拉属性 (*root)->need_push_down_to_left = true; (*root)->need_push_down_to_right = true; // 当前子树需要被删除的点的个数 tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; (*root)->invalid_point_num = (*root)->TreeSize; if (is_downsample) { (*root)->tree_downsample_deleted = true; (*root)->point_downsample_deleted = true; (*root)->down_del_num = (*root)->TreeSize; } return tmp_counter; } // 当前节点是否被包含在给定box内 if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) { (*root)->point_deleted = true; tmp_counter += 1; if (is_downsample) (*root)->point_downsample_deleted = true; } Operation_Logger_Type delete_box_log; struct timespec Timeout; // 在左右子树中递归调用盒式删除 if (is_downsample) delete_box_log.op = DOWNSAMPLE_DELETE; else delete_box_log.op = DELETE_BOX; delete_box_log.boxpoint = boxpoint; if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) { tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); } else { pthread_mutex_lock(&working_flag_mutex); tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) { tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); } else { pthread_mutex_lock(&working_flag_mutex); tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } // 属性上拉 Update(*root); // 平衡性准则判断是否需要重建树 // 判断条件解读：重建树的指针的指针不为空，且重建树的指针为当前树的根节点，说明从上次重建后，就没有再更新这棵树的结构 // 当前树的节点个数小于多线程重建的最小点阈值，则只需要单线程重建即可 if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; // 将Rebuild_Ptr设为空，即标记需要重建子树 bool need_rebuild = allow_rebuild & Criterion_Check((*root)); if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return tmp_counter; } // 前序遍历，从树中删除指定点 void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) { // 如果目标点不存在或者已经被标记为删除，则不操作即返回 if ((*root) == nullptr || (*root)->tree_deleted) return; (*root)->working_flag = true; // 将属性下拉 Push_Down(*root); // 搜索到目标点，且目标点还未被标记删除，则标记为删除 if (same_point((*root)->point, point) && !(*root)->point_deleted) { (*root)->point_deleted = true; (*root)->invalid_point_num += 1; // 如果该树的无效点数正好等于树的总节点数，说明这棵树可以直接被删除，那么把树的删除标志设为true if ((*root)->invalid_point_num == (*root)->TreeSize) (*root)->tree_deleted = true; return; } Operation_Logger_Type delete_log; struct timespec Timeout; delete_log.op = DELETE_POINT; delete_log.point = point; // 到左子树递归查找 if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) { if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) { Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Delete_by_point(&(*root)->left_son_ptr, point, false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } else { // 到右子树递归查找 if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) { Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Delete_by_point(&(*root)->right_son_ptr, point, false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } // 将属性上拉 Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; // 平衡性准则判断是否需要重建 bool need_rebuild = allow_rebuild & Criterion_Check((*root)); if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return; } // box-wise re-insertion 重插入 // box更新，可以便利的对一整个范围的点进行更新 void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) { if ((*root) == nullptr) return; (*root)->working_flag = true; Push_Down(*root); //论文中的判断部分，判断增加的范围是否与root的range的包含关系 // 前三行是判断给定的Box范围是否与树有交集，没有的话就不用搜索了 if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return; if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return; if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return; // Box包含了整一棵树的Box if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) { // 更新树的删除标签 (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; (*root)->point_deleted = false || (*root)->point_downsample_deleted; // 告诉树，下次要进行push down了 (*root)->need_push_down_to_left = true; (*root)->need_push_down_to_right = true; (*root)->invalid_point_num = (*root)->down_del_num; return; } // box包含当前节点的点 if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) { (*root)->point_deleted = (*root)->point_downsample_deleted; } Operation_Logger_Type add_box_log; struct timespec Timeout; add_box_log.op = ADD_BOX; add_box_log.boxpoint = boxpoint; // 接着到其左右子树上进行递归重插入 if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) { Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_range(&((*root)->left_son_ptr), boxpoint, false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) { Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_range(&((*root)->right_son_ptr), boxpoint, false); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } // 重插入完成后，将子树属性上拉到其父节点 Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; // 平衡性准则判断是否需要重建子树 bool need_rebuild = allow_rebuild & Criterion_Check((*root)); // 启动重建该子树 if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return; } // 前序遍历的点级更新 // 在ik-d树上的点级更新以递归的方式实现，这类似于替罪羊k-d树。对于点级插入， // 该算法递归地从根节点向下搜索，并将新点的除法轴上的坐标与存储在树节点上的点进行比较， // 直到发现一个叶节点然后追加一个新的树节点。对于删除或重新插入一个点P，该算法会查找存 // 储该点P的树节点，并修改deleted属性。 void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) { // 如果当前节点为空的话，则新建节点 if (*root == nullptr) { // 新建节点并初始化 *root = new KD_TREE_NODE; // 注意到root是指针的指针，所以不需要返回地址即可实现父节点指向当前新节点 InitTreeNode(*root); // 配置节点的值 (*root)->point = point; // 标记该节点的划分轴 (*root)->division_axis = (father_axis + 1) % 3; // 将新的节点信息汇总给父节点 Update(*root); return; } // 当前节点不为空，说明还未到叶子节点，则递归查找 (*root)->working_flag = true; // 创建Operation_Logger_Type用于记录新插入的点，重构树的时候需要用到 Operation_Logger_Type add_log; struct timespec Timeout; // 设置动作为ADD_POINT add_log.op = ADD_POINT; add_log.point = point; // 顺便将当前节点的状态信息下拉 Push_Down(*root); //如果在左边则进行左边的比较然后添加，首先确定当前节点的划分轴，然后判断该点应该分到左子树还是右子树 if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) { if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) { Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); } else { // 线程互斥锁 pthread_mutex_lock(&working_flag_mutex); // // 递归，直到找到叶子节点再新建节点并插入树中 Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } else { // 分到右子树 if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) { Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } // 做一次pull-up, 更新root，判断是否需要进行重建 Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; // 判断子树是否平衡 bool need_rebuild = allow_rebuild & Criterion_Check((*root)); // 检验后发现不平衡的话就需要重建该子树 if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return; } // k近邻搜索，这是一个前序遍历的过程 void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist) { // 如果树都还没建，那就没必要搜索了 if (root == nullptr || root->tree_deleted) return; // 计算目标点与tree的box边界的最小距离（假设目标点不在box内），如果最小距离大于限定距离，则不搜索了 double cur_dist = calc_box_dist(root, point); if (cur_dist > max_dist) return; int retval; // The function Pushdown is applied before searching the // sub-tree rooted at node T to pass down its lazy labels. if (root->need_push_down_to_left || root->need_push_down_to_right) { retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); if (retval == 0) { Push_Down(root); pthread_mutex_unlock(&(root->push_down_mutex_lock)); } else { pthread_mutex_lock(&(root->push_down_mutex_lock)); pthread_mutex_unlock(&(root->push_down_mutex_lock)); } } // 该点的标签没有打成deteled的话，可以作为候选点 if (!root->point_deleted) { // 计算目标点与当前节点的点的距离 float dist = calc_dist(point, root->point); // 如果当前距离小于限定最大距离，则将其压入大顶堆q，大顶堆也是一个数组 // 较大的值在前，较小的值在后 if (dist <= max_dist && (q.size() < k_nearest || dist < q.top().dist)) { // 如果堆的size超过k_nearest，则pop一个 if (q.size() >= k_nearest) q.pop(); // 根据当前搜索点及其距离，构建自定义的点类型 PointType_CMP current_point{root->point, dist}; // 压入堆中 q.push(current_point); } } int cur_search_counter; // 计算目标点与左、右子树的Box边界最小距离 float dist_left_node = calc_box_dist(root->left_son_ptr, point); float dist_right_node = calc_box_dist(root->right_son_ptr, point); // 如果堆还没满，或者左右子树中存在可能点，则进一步搜索 if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) { // 左子树的Box更近，则先搜索左子树 if (dist_left_node <= dist_right_node) { // 到左子树递归搜索 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) { Search(root->left_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->left_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } if (q.size() < k_nearest || dist_right_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) { Search(root->right_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->right_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } } else { // 到右子树中递归搜索 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) { Search(root->right_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->right_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } if (q.size() < k_nearest || dist_left_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) { Search(root->left_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->left_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } } } else { if (dist_left_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) { Search(root->left_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->left_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } if (dist_right_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) { Search(root->right_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->right_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } } return; } // 前序遍历搜索，根据给定的box，从树中搜索被Box包围的点集 void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) { if (root == nullptr) return; // 现将当前节点的属性Push down一下 Push_Down(root); // 前三行是判断给定的Box范围是否与树有交集，没有的话就不用搜索了 if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) return; if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) return; if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) return; // 给定的box完全包围当前的树，则将当前树展平，将所有点放到Storage内，直接返回即可 if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) { flatten(root, Storage, NOT_RECORD); return; } // 给定的box完全包围当前节点，如果该点没有被删除，则将该点压入Storage if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) { if (!root->point_deleted) Storage.push_back(root->point); } // 到左右子树中递归搜索，直到叶子节点结束 if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) { Search_by_range(root->left_son_ptr, boxpoint, Storage); } else { pthread_mutex_lock(&search_flag_mutex); Search_by_range(root->left_son_ptr, boxpoint, Storage); pthread_mutex_unlock(&search_flag_mutex); } if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) { Search_by_range(root->right_son_ptr, boxpoint, Storage); } else { pthread_mutex_lock(&search_flag_mutex); Search_by_range(root->right_son_ptr, boxpoint, Storage); pthread_mutex_unlock(&search_flag_mutex); } return; } bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) { // 如果当前树的总节点个数小于最小的树的size，则没必要重建，返回false if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) { return false; } // 平衡性归零 float balance_evaluation = 0.0f; float delete_evaluation = 0.0f; // 新建一个节点指针存储树的根节点，root是虚拟头结点 KD_TREE_NODE *son_ptr = root->left_son_ptr; if (son_ptr == nullptr) son_ptr = root->right_son_ptr; // 无效点在树中所占的比例 delete_evaluation = float(root->invalid_point_num) / root->TreeSize; // 有效点在树中所占的比例 balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); // 论文中的平衡性判断准则 if (delete_evaluation > delete_criterion_param) { return true; } if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) { return true; } return false; } // The Pushdown function copies the labels deleted, treedeleted, and pushdown of T to its sons (but // not further offsprings) when the attribute pushdown is true. // 当属性Pushdown 为true时，Pushdown 函数将被deleted, treedeleted, // 和 pushdown的标签复制到它的子节点（但没有复制到它的的后代。 void KD_TREE::Push_Down(KD_TREE_NODE *root) { if (root == nullptr) return; // 实例化一个操作记录器 Operation_Logger_Type operation; // 操作器设置为PUSH_DOWN状态 operation.op = PUSH_DOWN; // 获取当前节点的删除标签，用于传递给子节点 operation.tree_deleted = root->tree_deleted; // 同样，获取当前节点的降采样删除标签，用于传递给子节点 operation.tree_downsample_deleted = root->tree_downsample_deleted; // 左子节点不为空，且该节点需要push down if (root->need_push_down_to_left && root->left_son_ptr != nullptr) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) { // 将当前节点的删除标签传递给左子节点 root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; else root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; // 标记下左子节点的状态更新情况，设为true意味着左子节点更新完，它的属性也要传递给后代节点 root->left_son_ptr->need_push_down_to_left = true; root->left_son_ptr->need_push_down_to_right = true; // 当前节点属性已经Push down了，所以可以将其更新push down需求设为false root->need_push_down_to_left = false; } else { pthread_mutex_lock(&working_flag_mutex); root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; else root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; root->left_son_ptr->need_push_down_to_left = true; root->left_son_ptr->need_push_down_to_right = true; if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } root->need_push_down_to_left = false; pthread_mutex_unlock(&working_flag_mutex); } } // 对于右子节点也是相同的Push down操作，传递当前节点的属性下去 if (root->need_push_down_to_right && root->right_son_ptr != nullptr) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) { root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; else root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; root->right_son_ptr->need_push_down_to_left = true; root->right_son_ptr->need_push_down_to_right = true; root->need_push_down_to_right = false; } else { pthread_mutex_lock(&working_flag_mutex); root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; else root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; root->right_son_ptr->need_push_down_to_left = true; root->right_son_ptr->need_push_down_to_right = true; if (rebuild_flag) { pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } root->need_push_down_to_right = false; pthread_mutex_unlock(&working_flag_mutex); } } return; } // Update函数的主要目的是为了更新该节点以及它子树的点的范围以及size，还有他的del，bal， // 用来判断树是否平衡，是否需要重建。这个函数再原论文中是pull-up操作（树的属性由子向父传播）。 // Pullup函数将基于T的子树的信息汇总给T的以下属性：treesize（见数据结构1，第5行）保存子树上 // 所有节点数，invalidnum 存储子树上标记为“删除”的节点数，range（见数据结构1，第7行）汇总子 // 树上所有点坐标轴的范围，其中k为点维度。 void KD_TREE::Update(KD_TREE_NODE *root) { // 该函数不需要递归，因为父节点的属性只与左右子节点相关 // 该节点的左右子节点 KD_TREE_NODE *left_son_ptr = root->left_son_ptr; KD_TREE_NODE *right_son_ptr = root->right_son_ptr; float tmp_range_x[2] = {INFINITY, -INFINITY}; float tmp_range_y[2] = {INFINITY, -INFINITY}; float tmp_range_z[2] = {INFINITY, -INFINITY}; // Update Tree Size // 左右子树皆不为空 if (left_son_ptr != nullptr && right_son_ptr != nullptr) { // 那当前树的节点数=左子树节点数+右子树节点数+1(当前节点本身) root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; // 同上，树中的无效点个数也类似计算 root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); // 降采样删除的节点个数和 root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); // 降采样删除的标志位 root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; // 删除树的标志位 root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; // 获取包围当前树的box if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) { tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); } else { // 否则的话，肯定有一边子树被标记了删除了 // 仅取左子树 if (!left_son_ptr->tree_deleted) { tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); } // 仅取右子树 if (!right_son_ptr->tree_deleted) { tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); } // 子树的box+root的点 if (!root->point_deleted) { tmp_range_x[0] = min(tmp_range_x[0], root->point.x); tmp_range_x[1] = max(tmp_range_x[1], root->point.x); tmp_range_y[0] = min(tmp_range_y[0], root->point.y); tmp_range_y[1] = max(tmp_range_y[1], root->point.y); tmp_range_z[0] = min(tmp_range_z[0], root->point.z); tmp_range_z[1] = max(tmp_range_z[1], root->point.z); } } // 仅有左子树，右子树为空 } else if (left_son_ptr != nullptr) { // 仅需要将左子树的信息汇总给root root->TreeSize = left_son_ptr->TreeSize + 1; root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; // 获取包围左子树+root的box if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) { tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); } else { if (!left_son_ptr->tree_deleted) { tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); } if (!root->point_deleted) { tmp_range_x[0] = min(tmp_range_x[0], root->point.x); tmp_range_x[1] = max(tmp_range_x[1], root->point.x); tmp_range_y[0] = min(tmp_range_y[0], root->point.y); tmp_range_y[1] = max(tmp_range_y[1], root->point.y); tmp_range_z[0] = min(tmp_range_z[0], root->point.z); tmp_range_z[1] = max(tmp_range_z[1], root->point.z); } } // 左子树为空，右子树不为空 } else if (right_son_ptr != nullptr) { // 同理，仅需要将右子树的信息汇总给root root->TreeSize = right_son_ptr->TreeSize + 1; root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; // 获取包围右子树和root的box if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) { tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); } else { if (!right_son_ptr->tree_deleted) { tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); } if (!root->point_deleted) { tmp_range_x[0] = min(tmp_range_x[0], root->point.x); tmp_range_x[1] = max(tmp_range_x[1], root->point.x); tmp_range_y[0] = min(tmp_range_y[0], root->point.y); tmp_range_y[1] = max(tmp_range_y[1], root->point.y); tmp_range_z[0] = min(tmp_range_z[0], root->point.z); tmp_range_z[1] = max(tmp_range_z[1], root->point.z); } } // 左右子树都为空 } else { root->TreeSize = 1; root->invalid_point_num = (root->point_deleted ? 1 : 0); root->down_del_num = (root->point_downsample_deleted ? 1 : 0); root->tree_downsample_deleted = root->point_downsample_deleted; root->tree_deleted = root->point_deleted; tmp_range_x[0] = root->point.x; tmp_range_x[1] = root->point.x; tmp_range_y[0] = root->point.y; tmp_range_y[1] = root->point.y; tmp_range_z[0] = root->point.z; tmp_range_z[1] = root->point.z; } // 将树的区间范围拷贝给节点属性 node_range_x = [node_range_x_min, node_range_x_max] memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); // 让子节点知道自身父节点是谁 if (left_son_ptr != nullptr) left_son_ptr->father_ptr = root; if (right_son_ptr != nullptr) right_son_ptr->father_ptr = root; // 如果当前节点为整个树的根节点，且树的节点数大于3 if (root == Root_Node && root->TreeSize > 3) { KD_TREE_NODE *son_ptr = root->left_son_ptr; if (son_ptr == nullptr) son_ptr = root->right_son_ptr; float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); root->alpha_del = float(root->invalid_point_num) / root->TreeSize; root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; } return; } // 前序遍历和后序遍历都有，将一个树的所有节点展平，按顺序放到一个容器内 void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) { if (root == nullptr) return; Push_Down(root); // 只要该点没有被删除，那么就把它记录下来， 前序遍历 if (!root->point_deleted) { Storage.push_back(root->point); } flatten(root->left_son_ptr, Storage, storage_type); flatten(root->right_son_ptr, Storage, storage_type); // 根据枚举变量storage_type判断使用什么存储动作， 后序遍历 switch (storage_type) { case NOT_RECORD: // 不操作 break; case DELETE_POINTS_REC: // 删除点的记录 // 只有该点是真正被删除，而不是被降采样删除，该点才会被记录 if (root->point_deleted && !root->point_downsample_deleted) { Points_deleted.push_back(root->point); } break; case MULTI_THREAD_REC: // 多线程重建树时的点的记录 if (root->point_deleted && !root->point_downsample_deleted) { Multithread_Points_deleted.push_back(root->point); } break; default: break; } return; } // 后序遍历，删除子树的根节点，那么也要递归删除它的后代节点 void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) { if (*root == nullptr) return; Push_Down(*root); // 先把该节点的子节点全部压下来 delete_tree_nodes(&(*root)->left_son_ptr); // 删除左子树 delete_tree_nodes(&(*root)->right_son_ptr); // 删除右子树 delete *root; *root = nullptr; return; } // 判断两点是否为相同点 bool KD_TREE::same_point(PointType a, PointType b) { return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); } // 计算两点之间的欧氏距离 float KD_TREE::calc_dist(PointType a, PointType b) { float dist = 0.0f; dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); return dist; } // 首先获取node构成的树的范围，就是一个Box。然后逐个面判断point是否在box外，如果在Box外，则 // 计算离point的面的距离之“平方”和 // 有个疑惑，这样计算的结果真的是点到Box的距离吗 float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) { if (node == nullptr) return INFINITY; float min_dist = 0.0; if (point.x < node->node_range_x[0]) min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); if (point.x > node->node_range_x[1]) min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); if (point.y < node->node_range_y[0]) min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); if (point.y > node->node_range_y[1]) min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); if (point.z < node->node_range_z[0]) min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); if (point.z > node->node_range_z[1]) min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); return min_dist; } bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } // Manual heap MANUAL_HEAP::MANUAL_HEAP(int max_capacity) { cap = max_capacity; heap = new PointType_CMP[max_capacity]; heap_size = 0; } MANUAL_HEAP::~MANUAL_HEAP() { delete[] heap; } // 弹出最大的元素，也就是堆顶 void MANUAL_HEAP::pop() { if (heap_size == 0) return; // 我们取出数组中的最后一个元素，将它放到堆的顶部，然后再修复堆属性 heap[0] = heap[heap_size - 1]; // 将size指针左移，相当于pop一个元素 heap_size--; // 修复大顶堆 MoveDown(0); return; } // 返回堆顶 PointType_CMP MANUAL_HEAP::top() { return heap[0]; } void MANUAL_HEAP::push(PointType_CMP point) { // 如果堆的size大于设定容量，则不压入 if (heap_size >= cap) return; // 将新进的元素插入到大顶堆的尾部 heap[heap_size] = point; // 维护大顶堆的内部关系 FloatUp(heap_size); heap_size++; return; } int MANUAL_HEAP::size() { return heap_size; } void MANUAL_HEAP::clear() { heap_size = 0; return; } void MANUAL_HEAP::MoveDown(int heap_index) { // 节点的左子树索引 int l = heap_index * 2 + 1; // 当前节点值 PointType_CMP tmp = heap[heap_index]; // 从顶堆遍历到最后一个节点 while (l < heap_size) { // 左右子节点比较，如果右子节点比左子节点大，则将l改为右子节点的索引 if (l + 1 < heap_size && heap[l] < heap[l + 1]) l++; // 如果父节点小于l节点，则交换值，另一边的子树的结构没有被破坏，所以不需要处理 if (tmp < heap[l]) { heap[heap_index] = heap[l]; heap_index = l; l = heap_index * 2 + 1; } else break; } heap[heap_index] = tmp; return; } // 此时最大堆已经被破坏，需要重新调整, 因加入的节点比父节点大，则新节点跟父节点调换即可。 // 调整后，新节点如果比新的父节点小，则已经调整到位，如果比新的父节点大，则 // 需要和父节点重新进行交换。至此，最大堆调整完成。 void MANUAL_HEAP::FloatUp(int heap_index) { // 找到父节点 int ancestor = (heap_index - 1) / 2; PointType_CMP tmp = heap[heap_index]; while (heap_index > 0) { // 父节点小于当前节点 if (heap[ancestor] < tmp) { // 交换两个节点值 heap[heap_index] = heap[ancestor]; // 指针移到父节点的位置 heap_index = ancestor; // 继续查找父节点 ancestor = (heap_index - 1) / 2; } else break; // 父节点大于当前节点，则调整完毕 } // 将当前点放到该位置 heap[heap_index] = tmp; return; } // manual queue void MANUAL_Q::clear() { head = 0; tail = 0; counter = 0; is_empty = true; return; } void MANUAL_Q::pop() { if (counter == 0) return; head++; head %= Q_LEN; counter--; if (counter == 0) is_empty = true; return; } Operation_Logger_Type MANUAL_Q::front() { return q[head]; } Operation_Logger_Type MANUAL_Q::back() { return q[tail]; } void MANUAL_Q::push(Operation_Logger_Type op) { q[tail] = op; counter++; if (is_empty) is_empty = false; tail++; tail %= Q_LEN; } bool MANUAL_Q::empty() { return is_empty; } int MANUAL_Q::size() { return counter; } ================================================ FILE: FAST-LIO/include/ikd-Tree/ikd_Tree.h ================================================ #pragma once #include #include #include #include #include #include #include #include #define EPSS 1e-6 #define Minimal_Unbalanced_Tree_Size 10 #define Multi_Thread_Rebuild_Point_Num 1500 #define DOWNSAMPLE_SWITCH true #define ForceRebuildPercentage 0.2 #define Q_LEN 1000000 using namespace std; typedef pcl::PointXYZINormal PointType; typedef vector> PointVector; const PointType ZeroP; struct KD_TREE_NODE { // 该节点对应的点坐标 PointType point; // 划分轴 int division_axis; int TreeSize = 1; int invalid_point_num = 0; int down_del_num = 0; bool point_deleted = false; bool tree_deleted = false; bool point_downsample_deleted = false; bool tree_downsample_deleted = false; bool need_push_down_to_left = false; bool need_push_down_to_right = false; bool working_flag = false; pthread_mutex_t push_down_mutex_lock; float node_range_x[2], node_range_y[2], node_range_z[2]; // 左右子节点 KD_TREE_NODE *left_son_ptr = nullptr; KD_TREE_NODE *right_son_ptr = nullptr; // 父节点 KD_TREE_NODE *father_ptr = nullptr; // For paper data record float alpha_del; float alpha_bal; }; struct PointType_CMP { PointType point; float dist = 0.0; PointType_CMP(PointType p = ZeroP, float d = INFINITY) { this->point = p; this->dist = d; }; bool operator<(const PointType_CMP &a) const { if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x; else return dist < a.dist; } }; struct BoxPointType { float vertex_min[3]; float vertex_max[3]; }; enum operation_set { ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN }; enum delete_point_storage_set { NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC }; struct Operation_Logger_Type { PointType point; BoxPointType boxpoint; bool tree_deleted, tree_downsample_deleted; operation_set op; }; // 自定义的循环队列 class MANUAL_Q { private: int head = 0, tail = 0, counter = 0; Operation_Logger_Type q[Q_LEN]; bool is_empty; public: void pop(); Operation_Logger_Type front(); Operation_Logger_Type back(); void clear(); void push(Operation_Logger_Type op); bool empty(); int size(); }; // 自定义的大顶堆，用于堆排序，使用堆方法依次弹出最大值，执行效率高于冒泡排序 // 参考：https://blog.csdn.net/qq_54169998/article/details/121098648 class MANUAL_HEAP { public: MANUAL_HEAP(int max_capacity = 100); ~MANUAL_HEAP(); void pop(); PointType_CMP top(); void push(PointType_CMP point); int size(); void clear(); private: PointType_CMP *heap; // 存储堆元素的数组 void MoveDown(int heap_index); void FloatUp(int heap_index); int heap_size = 0; //当前已存储的元素个数 int cap = 0; //当前的存储容量 }; class KD_TREE { private: // Multi-thread Tree Rebuild bool termination_flag = false; // 用于终止多线程重建树的标志 bool rebuild_flag = false; // 用于标志是否需要重建树的标志 pthread_t rebuild_thread; // 用于多线程重建树的线程 pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; // 用于多线程重建树的互斥锁 pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; // 用于多线程重建树的互斥锁 // queue Rebuild_Logger; MANUAL_Q Rebuild_Logger; // 用于多线程重建树的队列 PointVector Rebuild_PCL_Storage; // 用于多线程重建树的点云存储 KD_TREE_NODE **Rebuild_Ptr = nullptr; // 用于多线程重建树的树指针 int search_mutex_counter = 0; // 用于多线程重建树的互斥计数器 static void *multi_thread_ptr(void *arg); void multi_thread_rebuild(); void start_thread(); void stop_thread(); void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); // KD Tree Functions and augmented variables int Treesize_tmp = 0, Validnum_tmp = 0; // 用于多线程重建树的临时变量 float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; // 用于多线程重建树的临时变量 float delete_criterion_param = 0.5f; // 用于多线程重建树的临时变量 float balance_criterion_param = 0.7f; // 用于多线程重建树的临时变量 float downsample_size = 0.2f; // 用于多线程重建树的临时变量 bool Delete_Storage_Disabled = false; KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; PointVector Points_deleted; // 用于多线程重建树的点云存储 PointVector Downsample_Storage; // 用于多线程重建树的点云存储 PointVector Multithread_Points_deleted; // 用于多线程重建树的点云存储 void InitTreeNode(KD_TREE_NODE *root); void Test_Lock_States(KD_TREE_NODE *root); void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); void Rebuild(KD_TREE_NODE **root); int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist); // priority_queue void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); bool Criterion_Check(KD_TREE_NODE *root); void Push_Down(KD_TREE_NODE *root); void Update(KD_TREE_NODE *root); void delete_tree_nodes(KD_TREE_NODE **root); void downsample(KD_TREE_NODE **root); bool same_point(PointType a, PointType b); float calc_dist(PointType a, PointType b); float calc_box_dist(KD_TREE_NODE *node, PointType point); static bool point_cmp_x(PointType a, PointType b); static bool point_cmp_y(PointType a, PointType b); static bool point_cmp_z(PointType a, PointType b); public: KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); ~KD_TREE(); void Set_delete_criterion_param(float delete_param); void Set_balance_criterion_param(float balance_param); void set_downsample_param(float box_length); void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); int size(); int validnum(); void root_alpha(float &alpha_bal, float &alpha_del); void Build(PointVector point_cloud); void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, double max_dist = INFINITY); int Add_Points(PointVector &PointToAdd, bool downsample_on); void Add_Point_Boxes(vector &BoxPoints); void Delete_Points(PointVector &PointToDel); int Delete_Point_Boxes(vector &BoxPoints); void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); void acquire_removed_points(PointVector &removed_points); BoxPointType tree_range(); PointVector PCL_Storage; // for debug KD_TREE_NODE *Root_Node = nullptr; //设置根节点一开始为空 int max_queue_size = 0; //设置最大队列长度为0 }; ================================================ FILE: FAST-LIO/include/matplotlibcpp.h ================================================ #pragma once // Python headers must be included before any system headers, since // they define _POSIX_C_SOURCE #include #include #include #include #include #include #include #include #include // requires c++11 support #include #ifndef WITHOUT_NUMPY # define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION # include # ifdef WITH_OPENCV # include # endif // WITH_OPENCV /* * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so * define the ones we need here. */ # if CV_MAJOR_VERSION > 3 # define CV_BGR2RGB cv::COLOR_BGR2RGB # define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA # endif #endif // WITHOUT_NUMPY #if PY_MAJOR_VERSION >= 3 # define PyString_FromString PyUnicode_FromString # define PyInt_FromLong PyLong_FromLong # define PyString_FromString PyUnicode_FromString #endif namespace matplotlibcpp { namespace detail { static std::string s_backend; struct _interpreter { PyObject* s_python_function_arrow; PyObject *s_python_function_show; PyObject *s_python_function_close; PyObject *s_python_function_draw; PyObject *s_python_function_pause; PyObject *s_python_function_save; PyObject *s_python_function_figure; PyObject *s_python_function_fignum_exists; PyObject *s_python_function_plot; PyObject *s_python_function_quiver; PyObject* s_python_function_contour; PyObject *s_python_function_semilogx; PyObject *s_python_function_semilogy; PyObject *s_python_function_loglog; PyObject *s_python_function_fill; PyObject *s_python_function_fill_between; PyObject *s_python_function_hist; PyObject *s_python_function_imshow; PyObject *s_python_function_scatter; PyObject *s_python_function_boxplot; PyObject *s_python_function_subplot; PyObject *s_python_function_subplot2grid; PyObject *s_python_function_legend; PyObject *s_python_function_xlim; PyObject *s_python_function_ion; PyObject *s_python_function_ginput; PyObject *s_python_function_ylim; PyObject *s_python_function_title; PyObject *s_python_function_axis; PyObject *s_python_function_axvline; PyObject *s_python_function_axvspan; PyObject *s_python_function_xlabel; PyObject *s_python_function_ylabel; PyObject *s_python_function_gca; PyObject *s_python_function_xticks; PyObject *s_python_function_yticks; PyObject* s_python_function_margins; PyObject *s_python_function_tick_params; PyObject *s_python_function_grid; PyObject* s_python_function_cla; PyObject *s_python_function_clf; PyObject *s_python_function_errorbar; PyObject *s_python_function_annotate; PyObject *s_python_function_tight_layout; PyObject *s_python_colormap; PyObject *s_python_empty_tuple; PyObject *s_python_function_stem; PyObject *s_python_function_xkcd; PyObject *s_python_function_text; PyObject *s_python_function_suptitle; PyObject *s_python_function_bar; PyObject *s_python_function_colorbar; PyObject *s_python_function_subplots_adjust; /* For now, _interpreter is implemented as a singleton since its currently not possible to have multiple independent embedded python interpreters without patching the python source code or starting a separate process for each. http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program */ static _interpreter& get() { static _interpreter ctx; return ctx; } PyObject* safe_import(PyObject* module, std::string fname) { PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); if (!fn) throw std::runtime_error(std::string("Couldn't find required function: ") + fname); if (!PyFunction_Check(fn)) throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); return fn; } private: #ifndef WITHOUT_NUMPY # if PY_MAJOR_VERSION >= 3 void *import_numpy() { import_array(); // initialize C-API return NULL; } # else void import_numpy() { import_array(); // initialize C-API } # endif #endif _interpreter() { // optional but recommended #if PY_MAJOR_VERSION >= 3 wchar_t name[] = L"plotting"; #else char name[] = "plotting"; #endif Py_SetProgramName(name); Py_Initialize(); #ifndef WITHOUT_NUMPY import_numpy(); // initialize numpy C-API #endif PyObject* matplotlibname = PyString_FromString("matplotlib"); PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); PyObject* cmname = PyString_FromString("matplotlib.cm"); PyObject* pylabname = PyString_FromString("pylab"); if (!pyplotname || !pylabname || !matplotlibname || !cmname) { throw std::runtime_error("couldnt create string"); } PyObject* matplotlib = PyImport_Import(matplotlibname); Py_DECREF(matplotlibname); if (!matplotlib) { PyErr_Print(); throw std::runtime_error("Error loading module matplotlib!"); } // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, // or matplotlib.backends is imported for the first time if (!s_backend.empty()) { PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); } PyObject* pymod = PyImport_Import(pyplotname); Py_DECREF(pyplotname); if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } s_python_colormap = PyImport_Import(cmname); Py_DECREF(cmname); if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } PyObject* pylabmod = PyImport_Import(pylabname); Py_DECREF(pylabname); if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } s_python_function_arrow = safe_import(pymod, "arrow"); s_python_function_show = safe_import(pymod, "show"); s_python_function_close = safe_import(pymod, "close"); s_python_function_draw = safe_import(pymod, "draw"); s_python_function_pause = safe_import(pymod, "pause"); s_python_function_figure = safe_import(pymod, "figure"); s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); s_python_function_plot = safe_import(pymod, "plot"); s_python_function_quiver = safe_import(pymod, "quiver"); s_python_function_contour = safe_import(pymod, "contour"); s_python_function_semilogx = safe_import(pymod, "semilogx"); s_python_function_semilogy = safe_import(pymod, "semilogy"); s_python_function_loglog = safe_import(pymod, "loglog"); s_python_function_fill = safe_import(pymod, "fill"); s_python_function_fill_between = safe_import(pymod, "fill_between"); s_python_function_hist = safe_import(pymod,"hist"); s_python_function_scatter = safe_import(pymod,"scatter"); s_python_function_boxplot = safe_import(pymod,"boxplot"); s_python_function_subplot = safe_import(pymod, "subplot"); s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); s_python_function_legend = safe_import(pymod, "legend"); s_python_function_ylim = safe_import(pymod, "ylim"); s_python_function_title = safe_import(pymod, "title"); s_python_function_axis = safe_import(pymod, "axis"); s_python_function_axvline = safe_import(pymod, "axvline"); s_python_function_axvspan = safe_import(pymod, "axvspan"); s_python_function_xlabel = safe_import(pymod, "xlabel"); s_python_function_ylabel = safe_import(pymod, "ylabel"); s_python_function_gca = safe_import(pymod, "gca"); s_python_function_xticks = safe_import(pymod, "xticks"); s_python_function_yticks = safe_import(pymod, "yticks"); s_python_function_margins = safe_import(pymod, "margins"); s_python_function_tick_params = safe_import(pymod, "tick_params"); s_python_function_grid = safe_import(pymod, "grid"); s_python_function_xlim = safe_import(pymod, "xlim"); s_python_function_ion = safe_import(pymod, "ion"); s_python_function_ginput = safe_import(pymod, "ginput"); s_python_function_save = safe_import(pylabmod, "savefig"); s_python_function_annotate = safe_import(pymod,"annotate"); s_python_function_cla = safe_import(pymod, "cla"); s_python_function_clf = safe_import(pymod, "clf"); s_python_function_errorbar = safe_import(pymod, "errorbar"); s_python_function_tight_layout = safe_import(pymod, "tight_layout"); s_python_function_stem = safe_import(pymod, "stem"); s_python_function_xkcd = safe_import(pymod, "xkcd"); s_python_function_text = safe_import(pymod, "text"); s_python_function_suptitle = safe_import(pymod, "suptitle"); s_python_function_bar = safe_import(pymod,"bar"); s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); #ifndef WITHOUT_NUMPY s_python_function_imshow = safe_import(pymod, "imshow"); #endif s_python_empty_tuple = PyTuple_New(0); } ~_interpreter() { Py_Finalize(); } }; } // end namespace detail /// Select the backend /// /// **NOTE:** This must be called before the first plot command to have /// any effect. /// /// Mainly useful to select the non-interactive 'Agg' backend when running /// matplotlibcpp in headless mode, for example on a machine with no display. /// /// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use inline void backend(const std::string& name) { detail::s_backend = name; } inline bool annotate(std::string annotation, double x, double y) { detail::_interpreter::get(); PyObject * xy = PyTuple_New(2); PyObject * str = PyString_FromString(annotation.c_str()); PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "xy", xy); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, str); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } namespace detail { #ifndef WITHOUT_NUMPY // Type selector for numpy array conversion template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; // Sanity checks; comment them out or change the numpy type below if you're compiling on // a platform where they don't apply static_assert(sizeof(long long) == 8); template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; static_assert(sizeof(unsigned long long) == 8); template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; // TODO: add int, long, etc. template PyObject* get_array(const std::vector& v) { npy_intp vsize = v.size(); NPY_TYPES type = select_npy_type::type; if (type == NPY_NOTYPE) { size_t memsize = v.size()*sizeof(double); double* dp = static_cast(::malloc(memsize)); for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); return varray; } PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); return varray; } template PyObject* get_2darray(const std::vector<::std::vector>& v) { if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); npy_intp vsize[2] = {static_cast(v.size()), static_cast(v[0].size())}; PyArrayObject *varray = (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); double *vd_begin = static_cast(PyArray_DATA(varray)); for (const ::std::vector &v_row : v) { if (v_row.size() != static_cast(vsize[1])) throw std::runtime_error("Missmatched array size"); std::copy(v_row.begin(), v_row.end(), vd_begin); vd_begin += vsize[1]; } return reinterpret_cast(varray); } #else // fallback if we don't have numpy: copy every element of the given vector template PyObject* get_array(const std::vector& v) { PyObject* list = PyList_New(v.size()); for(size_t i = 0; i < v.size(); ++i) { PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); } return list; } #endif // WITHOUT_NUMPY // sometimes, for labels and such, we need string arrays inline PyObject * get_array(const std::vector& strings) { PyObject* list = PyList_New(strings.size()); for (std::size_t i = 0; i < strings.size(); ++i) { PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); } return list; } // not all matplotlib need 2d arrays, some prefer lists of lists template PyObject* get_listlist(const std::vector>& ll) { PyObject* listlist = PyList_New(ll.size()); for (std::size_t i = 0; i < ll.size(); ++i) { PyList_SetItem(listlist, i, get_array(ll[i])); } return listlist; } } // namespace detail /// Plot a line through the given x and y data points.. /// /// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html template bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) { assert(x.size() == y.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } // TODO - it should be possible to make this work by implementing // a non-numpy alternative for `detail::get_2darray()`. #ifndef WITHOUT_NUMPY template void plot_surface(const std::vector<::std::vector> &x, const std::vector<::std::vector> &y, const std::vector<::std::vector> &z, const std::map &keywords = std::map()) { detail::_interpreter::get(); // We lazily load the modules here the first time this function is called // because I'm not sure that we can assume "matplotlib installed" implies // "mpl_toolkits installed" on all platforms, and we don't want to require // it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { detail::_interpreter::get(); PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } mpl_toolkitsmod = PyImport_Import(mpl_toolkits); Py_DECREF(mpl_toolkits); if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } axis3dmod = PyImport_Import(axis3d); Py_DECREF(axis3d); if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } } assert(x.size() == y.size()); assert(y.size() == z.size()); // using numpy arrays PyObject *xarray = detail::get_2darray(x); PyObject *yarray = detail::get_2darray(y); PyObject *zarray = detail::get_2darray(z); // construct positional args PyObject *args = PyTuple_New(3); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); PyTuple_SetItem(args, 2, zarray); // Build up the kw args. PyObject *kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); PyObject *python_colormap_coolwarm = PyObject_GetAttrString( detail::_interpreter::get().s_python_colormap, "coolwarm"); PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject *fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); if (!fig) throw std::runtime_error("Call to figure() failed."); PyObject *gca_kwargs = PyDict_New(); PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); PyObject *gca = PyObject_GetAttrString(fig, "gca"); if (!gca) throw std::runtime_error("No gca"); Py_INCREF(gca); PyObject *axis = PyObject_Call( gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); if (!axis) throw std::runtime_error("No axis"); Py_INCREF(axis); Py_DECREF(gca); Py_DECREF(gca_kwargs); PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); if (!plot_surface) throw std::runtime_error("No surface"); Py_INCREF(plot_surface); PyObject *res = PyObject_Call(plot_surface, args, kwargs); if (!res) throw std::runtime_error("failed surface"); Py_DECREF(plot_surface); Py_DECREF(axis); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); } #endif // WITHOUT_NUMPY template void plot3(const std::vector &x, const std::vector &y, const std::vector &z, const std::map &keywords = std::map()) { detail::_interpreter::get(); // Same as with plot_surface: We lazily load the modules here the first time // this function is called because I'm not sure that we can assume "matplotlib // installed" implies "mpl_toolkits installed" on all platforms, and we don't // want to require it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { detail::_interpreter::get(); PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } mpl_toolkitsmod = PyImport_Import(mpl_toolkits); Py_DECREF(mpl_toolkits); if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } axis3dmod = PyImport_Import(axis3d); Py_DECREF(axis3d); if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } } assert(x.size() == y.size()); assert(y.size() == z.size()); PyObject *xarray = detail::get_array(x); PyObject *yarray = detail::get_array(y); PyObject *zarray = detail::get_array(z); // construct positional args PyObject *args = PyTuple_New(3); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); PyTuple_SetItem(args, 2, zarray); // Build up the kw args. PyObject *kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject *fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); if (!fig) throw std::runtime_error("Call to figure() failed."); PyObject *gca_kwargs = PyDict_New(); PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); PyObject *gca = PyObject_GetAttrString(fig, "gca"); if (!gca) throw std::runtime_error("No gca"); Py_INCREF(gca); PyObject *axis = PyObject_Call( gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); if (!axis) throw std::runtime_error("No axis"); Py_INCREF(axis); Py_DECREF(gca); Py_DECREF(gca_kwargs); PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); if (!plot3) throw std::runtime_error("No 3D line plot"); Py_INCREF(plot3); PyObject *res = PyObject_Call(plot3, args, kwargs); if (!res) throw std::runtime_error("Failed 3D line plot"); Py_DECREF(plot3); Py_DECREF(axis); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); } template bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) { assert(x.size() == y.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); // construct keyword args PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call( detail::_interpreter::get().s_python_function_stem, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template< typename Numeric > bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) { assert(x.size() == y.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); // construct keyword args PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template< typename Numeric > bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) { assert(x.size() == y1.size()); assert(x.size() == y2.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* y1array = detail::get_array(y1); PyObject* y2array = detail::get_array(y2); // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, y1array); PyTuple_SetItem(args, 2, y2array); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { PyObject* obj_x = PyFloat_FromDouble(x); PyObject* obj_y = PyFloat_FromDouble(y); PyObject* obj_end_x = PyFloat_FromDouble(end_x); PyObject* obj_end_y = PyFloat_FromDouble(end_y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); PyObject* plot_args = PyTuple_New(4); PyTuple_SetItem(plot_args, 0, obj_x); PyTuple_SetItem(plot_args, 1, obj_y); PyTuple_SetItem(plot_args, 2, obj_end_x); PyTuple_SetItem(plot_args, 3, obj_end_y); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template< typename Numeric> bool hist(const std::vector& y, long bins=10,std::string color="b", double alpha=1.0, bool cumulative=false) { detail::_interpreter::get(); PyObject* yarray = detail::get_array(y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); PyObject* plot_args = PyTuple_New(1); PyTuple_SetItem(plot_args, 0, yarray); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } #ifndef WITHOUT_NUMPY namespace detail { inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) { assert(type == NPY_UINT8 || type == NPY_FLOAT); assert(colors == 1 || colors == 3 || colors == 4); detail::_interpreter::get(); // construct args npy_intp dims[3] = { rows, columns, colors }; PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (!res) throw std::runtime_error("Call to imshow() failed"); if (out) *out = res; else Py_DECREF(res); } } // namespace detail inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) { detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); } inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) { detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); } #ifdef WITH_OPENCV void imshow(const cv::Mat &image, const std::map &keywords = {}) { // Convert underlying type of matrix, if needed cv::Mat image2; NPY_TYPES npy_type = NPY_UINT8; switch (image.type() & CV_MAT_DEPTH_MASK) { case CV_8U: image2 = image; break; case CV_32F: image2 = image; npy_type = NPY_FLOAT; break; default: image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); } // If color image, convert from BGR to RGB switch (image2.channels()) { case 3: cv::cvtColor(image2, image2, CV_BGR2RGB); break; case 4: cv::cvtColor(image2, image2, CV_BGRA2RGBA); } detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); } #endif // WITH_OPENCV #endif // WITHOUT_NUMPY template bool scatter(const std::vector& x, const std::vector& y, const double s=1.0, // The marker size in points**2 const std::map & keywords = {}) { detail::_interpreter::get(); assert(x.size() == y.size()); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); for (const auto& it : keywords) { PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); } PyObject* plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool boxplot(const std::vector>& data, const std::vector& labels = {}, const std::map & keywords = {}) { detail::_interpreter::get(); PyObject* listlist = detail::get_listlist(data); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, listlist); PyObject* kwargs = PyDict_New(); // kwargs needs the labels, if there are (the correct number of) labels if (!labels.empty() && labels.size() == data.size()) { PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); } // take care of the remaining keywords for (const auto& it : keywords) { PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool boxplot(const std::vector& data, const std::map & keywords = {}) { detail::_interpreter::get(); PyObject* vector = detail::get_array(data); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, vector); PyObject* kwargs = PyDict_New(); for (const auto& it : keywords) { PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool bar(const std::vector & x, const std::vector & y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map & keywords = {}) { detail::_interpreter::get(); PyObject * xarray = detail::get_array(x); PyObject * yarray = detail::get_array(y); PyObject * kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString( kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject * plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject * res = PyObject_Call( detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template bool bar(const std::vector & y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map & keywords = {}) { using T = typename std::remove_reference::type::value_type; detail::_interpreter::get(); std::vector x; for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } return bar(x, y, ec, ls, lw, keywords); } inline bool subplots_adjust(const std::map& keywords = {}) { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); } PyObject* plot_args = PyTuple_New(0); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template< typename Numeric> bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) { detail::_interpreter::get(); PyObject* yarray = detail::get_array(y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); PyObject* plot_args = PyTuple_New(1); PyTuple_SetItem(plot_args, 0, yarray); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool contour(const std::vector& x, const std::vector& y, const std::vector& z, const std::map& keywords = {}) { assert(x.size() == y.size() && x.size() == z.size()); PyObject* xarray = get_array(x); PyObject* yarray = get_array(y); PyObject* zarray = get_array(z); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, zarray); // construct keyword args PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) { assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* uarray = detail::get_array(u); PyObject* warray = detail::get_array(w); PyObject* plot_args = PyTuple_New(4); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, uarray); PyTuple_SetItem(plot_args, 3, warray); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call( detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject( detail::_interpreter::get().s_python_function_stem, plot_args); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* yerrarray = detail::get_array(yerr); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyDict_SetItemString(kwargs, "yerr", yerrarray); PyObject *plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); else throw std::runtime_error("Call to errorbar() failed."); return res; } template bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, yarray); PyTuple_SetItem(plot_args, 1, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool plot(const std::vector& y, const std::string& format = "") { std::vector x(y.size()); for(size_t i=0; i bool plot(const std::vector& y, const std::map& keywords) { std::vector x(y.size()); for(size_t i=0; i bool stem(const std::vector& y, const std::string& format = "") { std::vector x(y.size()); for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; return stem(x, y, format); } template void text(Numeric x, Numeric y, const std::string& s = "") { detail::_interpreter::get(); PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); if(!res) throw std::runtime_error("Call to text() failed."); Py_DECREF(args); Py_DECREF(res); } inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) { if (mappable == NULL) throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); detail::_interpreter::get(); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, mappable); PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); if(!res) throw std::runtime_error("Call to colorbar() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline long figure(long number = -1) { detail::_interpreter::get(); PyObject *res; if (number == -1) res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); else { assert(number > 0); // Make sure interpreter is initialised detail::_interpreter::get(); PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyLong_FromLong(number)); res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); Py_DECREF(args); } if(!res) throw std::runtime_error("Call to figure() failed."); PyObject* num = PyObject_GetAttrString(res, "number"); if (!num) throw std::runtime_error("Could not get number attribute of figure object"); const long figureNumber = PyLong_AsLong(num); Py_DECREF(num); Py_DECREF(res); return figureNumber; } inline bool fignum_exists(long number) { detail::_interpreter::get(); PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyLong_FromLong(number)); PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); if(!res) throw std::runtime_error("Call to fignum_exists() failed."); bool ret = PyObject_IsTrue(res); Py_DECREF(res); Py_DECREF(args); return ret; } inline void figure_size(size_t w, size_t h) { detail::_interpreter::get(); const size_t dpi = 100; PyObject* size = PyTuple_New(2); PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "figsize", size); PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple, kwargs); Py_DECREF(kwargs); if(!res) throw std::runtime_error("Call to figure_size() failed."); Py_DECREF(res); } inline void legend() { detail::_interpreter::get(); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); if(!res) throw std::runtime_error("Call to legend() failed."); Py_DECREF(res); } inline void legend(const std::map& keywords) { detail::_interpreter::get(); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); if(!res) throw std::runtime_error("Call to legend() failed."); Py_DECREF(kwargs); Py_DECREF(res); } template void ylim(Numeric left, Numeric right) { detail::_interpreter::get(); PyObject* list = PyList_New(2); PyList_SetItem(list, 0, PyFloat_FromDouble(left)); PyList_SetItem(list, 1, PyFloat_FromDouble(right)); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, list); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); if(!res) throw std::runtime_error("Call to ylim() failed."); Py_DECREF(args); Py_DECREF(res); } template void xlim(Numeric left, Numeric right) { detail::_interpreter::get(); PyObject* list = PyList_New(2); PyList_SetItem(list, 0, PyFloat_FromDouble(left)); PyList_SetItem(list, 1, PyFloat_FromDouble(right)); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, list); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); if(!res) throw std::runtime_error("Call to xlim() failed."); Py_DECREF(args); Py_DECREF(res); } inline double* xlim() { detail::_interpreter::get(); PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); PyObject* left = PyTuple_GetItem(res,0); PyObject* right = PyTuple_GetItem(res,1); double* arr = new double[2]; arr[0] = PyFloat_AsDouble(left); arr[1] = PyFloat_AsDouble(right); if(!res) throw std::runtime_error("Call to xlim() failed."); Py_DECREF(res); return arr; } inline double* ylim() { detail::_interpreter::get(); PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); PyObject* left = PyTuple_GetItem(res,0); PyObject* right = PyTuple_GetItem(res,1); double* arr = new double[2]; arr[0] = PyFloat_AsDouble(left); arr[1] = PyFloat_AsDouble(right); if(!res) throw std::runtime_error("Call to ylim() failed."); Py_DECREF(res); return arr; } template inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) { assert(labels.size() == 0 || ticks.size() == labels.size()); detail::_interpreter::get(); // using numpy array PyObject* ticksarray = detail::get_array(ticks); PyObject* args; if(labels.size() == 0) { // construct positional args args = PyTuple_New(1); PyTuple_SetItem(args, 0, ticksarray); } else { // make tuple of tick labels PyObject* labelstuple = PyTuple_New(labels.size()); for (size_t i = 0; i < labels.size(); i++) PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); // construct positional args args = PyTuple_New(2); PyTuple_SetItem(args, 0, ticksarray); PyTuple_SetItem(args, 1, labelstuple); } // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(!res) throw std::runtime_error("Call to xticks() failed"); Py_DECREF(res); } template inline void xticks(const std::vector &ticks, const std::map& keywords) { xticks(ticks, {}, keywords); } template inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) { assert(labels.size() == 0 || ticks.size() == labels.size()); detail::_interpreter::get(); // using numpy array PyObject* ticksarray = detail::get_array(ticks); PyObject* args; if(labels.size() == 0) { // construct positional args args = PyTuple_New(1); PyTuple_SetItem(args, 0, ticksarray); } else { // make tuple of tick labels PyObject* labelstuple = PyTuple_New(labels.size()); for (size_t i = 0; i < labels.size(); i++) PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); // construct positional args args = PyTuple_New(2); PyTuple_SetItem(args, 0, ticksarray); PyTuple_SetItem(args, 1, labelstuple); } // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(!res) throw std::runtime_error("Call to yticks() failed"); Py_DECREF(res); } template inline void yticks(const std::vector &ticks, const std::map& keywords) { yticks(ticks, {}, keywords); } template inline void margins(Numeric margin) { // construct positional args PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); if (!res) throw std::runtime_error("Call to margins() failed."); Py_DECREF(args); Py_DECREF(res); } template inline void margins(Numeric margin_x, Numeric margin_y) { // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); if (!res) throw std::runtime_error("Call to margins() failed."); Py_DECREF(args); Py_DECREF(res); } inline void tick_params(const std::map& keywords, const std::string axis = "both") { detail::_interpreter::get(); // construct positional args PyObject* args; args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); // construct keyword args PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (!res) throw std::runtime_error("Call to tick_params() failed"); Py_DECREF(res); } inline void subplot(long nrows, long ncols, long plot_number) { detail::_interpreter::get(); // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); if(!res) throw std::runtime_error("Call to subplot() failed."); Py_DECREF(args); Py_DECREF(res); } inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) { detail::_interpreter::get(); PyObject* shape = PyTuple_New(2); PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); PyObject* loc = PyTuple_New(2); PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); PyObject* args = PyTuple_New(4); PyTuple_SetItem(args, 0, shape); PyTuple_SetItem(args, 1, loc); PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); if(!res) throw std::runtime_error("Call to subplot2grid() failed."); Py_DECREF(shape); Py_DECREF(loc); Py_DECREF(args); Py_DECREF(res); } inline void title(const std::string &titlestr, const std::map &keywords = {}) { detail::_interpreter::get(); PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pytitlestr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); if(!res) throw std::runtime_error("Call to title() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) { detail::_interpreter::get(); PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pysuptitlestr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); if(!res) throw std::runtime_error("Call to suptitle() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void axis(const std::string &axisstr) { detail::_interpreter::get(); PyObject* str = PyString_FromString(axisstr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, str); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); if(!res) throw std::runtime_error("Call to title() failed."); Py_DECREF(args); Py_DECREF(res); } inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) { detail::_interpreter::get(); // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); } inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) { // construct positional args PyObject* args = PyTuple_New(4); PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { if (it->first == "linewidth" || it->first == "alpha") PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); else PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); } inline void xlabel(const std::string &str, const std::map &keywords = {}) { detail::_interpreter::get(); PyObject* pystr = PyString_FromString(str.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pystr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); if(!res) throw std::runtime_error("Call to xlabel() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void ylabel(const std::string &str, const std::map& keywords = {}) { detail::_interpreter::get(); PyObject* pystr = PyString_FromString(str.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pystr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); if(!res) throw std::runtime_error("Call to ylabel() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void set_zlabel(const std::string &str, const std::map& keywords = {}) { detail::_interpreter::get(); // Same as with plot_surface: We lazily load the modules here the first time // this function is called because I'm not sure that we can assume "matplotlib // installed" implies "mpl_toolkits installed" on all platforms, and we don't // want to require it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } mpl_toolkitsmod = PyImport_Import(mpl_toolkits); Py_DECREF(mpl_toolkits); if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } axis3dmod = PyImport_Import(axis3d); Py_DECREF(axis3d); if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } } PyObject* pystr = PyString_FromString(str.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pystr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject *ax = PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, detail::_interpreter::get().s_python_empty_tuple); if (!ax) throw std::runtime_error("Call to gca() failed."); Py_INCREF(ax); PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); Py_INCREF(zlabel); PyObject *res = PyObject_Call(zlabel, args, kwargs); if (!res) throw std::runtime_error("Call to set_zlabel() failed."); Py_DECREF(zlabel); Py_DECREF(ax); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); } inline void grid(bool flag) { detail::_interpreter::get(); PyObject* pyflag = flag ? Py_True : Py_False; Py_INCREF(pyflag); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pyflag); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); if(!res) throw std::runtime_error("Call to grid() failed."); Py_DECREF(args); Py_DECREF(res); } inline void show(const bool block = true) { detail::_interpreter::get(); PyObject* res; if(block) { res = PyObject_CallObject( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple); } else { PyObject *kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "block", Py_False); res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); Py_DECREF(kwargs); } if (!res) throw std::runtime_error("Call to show() failed."); Py_DECREF(res); } inline void close() { detail::_interpreter::get(); PyObject* res = PyObject_CallObject( detail::_interpreter::get().s_python_function_close, detail::_interpreter::get().s_python_empty_tuple); if (!res) throw std::runtime_error("Call to close() failed."); Py_DECREF(res); } inline void xkcd() { detail::_interpreter::get(); PyObject* res; PyObject *kwargs = PyDict_New(); res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, detail::_interpreter::get().s_python_empty_tuple, kwargs); Py_DECREF(kwargs); if (!res) throw std::runtime_error("Call to show() failed."); Py_DECREF(res); } inline void draw() { detail::_interpreter::get(); PyObject* res = PyObject_CallObject( detail::_interpreter::get().s_python_function_draw, detail::_interpreter::get().s_python_empty_tuple); if (!res) throw std::runtime_error("Call to draw() failed."); Py_DECREF(res); } template inline void pause(Numeric interval) { detail::_interpreter::get(); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); if(!res) throw std::runtime_error("Call to pause() failed."); Py_DECREF(args); Py_DECREF(res); } inline void save(const std::string& filename) { detail::_interpreter::get(); PyObject* pyfilename = PyString_FromString(filename.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pyfilename); std::cout<<"args:"<> ginput(const int numClicks = 1, const std::map& keywords = {}) { detail::_interpreter::get(); PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call( detail::_interpreter::get().s_python_function_ginput, args, kwargs); Py_DECREF(kwargs); Py_DECREF(args); if (!res) throw std::runtime_error("Call to ginput() failed."); const size_t len = PyList_Size(res); std::vector> out; out.reserve(len); for (size_t i = 0; i < len; i++) { PyObject *current = PyList_GetItem(res, i); std::array position; position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); out.push_back(position); } Py_DECREF(res); return out; } // Actually, is there any reason not to call this automatically for every plot? inline void tight_layout() { detail::_interpreter::get(); PyObject *res = PyObject_CallObject( detail::_interpreter::get().s_python_function_tight_layout, detail::_interpreter::get().s_python_empty_tuple); if (!res) throw std::runtime_error("Call to tight_layout() failed."); Py_DECREF(res); } // Support for variadic plot() and initializer lists: namespace detail { template using is_function = typename std::is_function>>::type; template struct is_callable_impl; template struct is_callable_impl { typedef is_function type; }; // a non-object is callable iff it is a function template struct is_callable_impl { struct Fallback { void operator()(); }; struct Derived : T, Fallback { }; template struct Check; template static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match template static std::false_type test( Check* ); public: typedef decltype(test(nullptr)) type; typedef decltype(&Fallback::operator()) dtype; static constexpr bool value = type::value; }; // an object is callable iff it defines operator() template struct is_callable { // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not typedef typename is_callable_impl::value, T>::type type; }; template struct plot_impl { }; template<> struct plot_impl { template bool operator()(const IterableX& x, const IterableY& y, const std::string& format) { // 2-phase lookup for distance, begin, end using std::distance; using std::begin; using std::end; auto xs = distance(begin(x), end(x)); auto ys = distance(begin(y), end(y)); assert(xs == ys && "x and y data must have the same number of elements!"); PyObject* xlist = PyList_New(xs); PyObject* ylist = PyList_New(ys); PyObject* pystring = PyString_FromString(format.c_str()); auto itx = begin(x), ity = begin(y); for(size_t i = 0; i < xs; ++i) { PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); } PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xlist); PyTuple_SetItem(plot_args, 1, ylist); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } }; template<> struct plot_impl { template bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) { if(begin(ticks) == end(ticks)) return true; // We could use additional meta-programming to deduce the correct element type of y, // but all values have to be convertible to double anyways std::vector y; for(auto x : ticks) y.push_back(f(x)); return plot_impl()(ticks,y,format); } }; } // end namespace detail // recursion stop for the above template bool plot() { return true; } template bool plot(const A& a, const B& b, const std::string& format, Args... args) { return detail::plot_impl::type>()(a,b,format) && plot(args...); } /* * This group of plot() functions is needed to support initializer lists, i.e. calling * plot( {1,2,3,4} ) */ inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { return plot(x,y,format); } inline bool plot(const std::vector& y, const std::string& format = "") { return plot(y,format); } inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { return plot(x,y,keywords); } /* * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting */ class Plot { public: // default initialization with plot label, some data and format template Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); assert(x.size() == y.size()); PyObject* kwargs = PyDict_New(); if(name != "") PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if(res) { line= PyList_GetItem(res, 0); if(line) set_data_fct = PyObject_GetAttrString(line,"set_data"); else Py_DECREF(line); Py_DECREF(res); } } // shorter initialization with name or format only // basically calls line, = plot([], []) Plot(const std::string& name = "", const std::string& format = "") : Plot(name, std::vector(), std::vector(), format) {} template bool update(const std::vector& x, const std::vector& y) { assert(x.size() == y.size()); if(set_data_fct) { PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject* res = PyObject_CallObject(set_data_fct, plot_args); if (res) Py_DECREF(res); return res; } return false; } // clears the plot but keep it available bool clear() { return update(std::vector(), std::vector()); } // definitely remove this line void remove() { if(line) { auto remove_fct = PyObject_GetAttrString(line,"remove"); PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(remove_fct, args); if (res) Py_DECREF(res); } decref(); } ~Plot() { decref(); } private: void decref() { if(line) Py_DECREF(line); if(set_data_fct) Py_DECREF(set_data_fct); } PyObject* line = nullptr; PyObject* set_data_fct = nullptr; }; } // end namespace matplotlibcpp ================================================ FILE: FAST-LIO/include/so3_math.h ================================================ #ifndef SO3_MATH_H #define SO3_MATH_H #include #include #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 template Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) { Eigen::Matrix skew_sym_mat; skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; return skew_sym_mat; } template Eigen::Matrix Exp(const Eigen::Matrix &&ang) { T ang_norm = ang.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_norm > 0.0000001) { Eigen::Matrix r_axis = ang / ang_norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_axis); /// Roderigous Tranformation return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) { T ang_vel_norm = ang_vel.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_vel_norm > 0.0000001) { Eigen::Matrix r_axis = ang_vel / ang_vel_norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_axis); T r_ang = ang_vel_norm * dt; /// Roderigous Tranformation return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) { T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (norm > 0.00001) { T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_ang); /// Roderigous Tranformation return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; } else { return Eye3; } } /* Logrithm of a Rotation Matrix */ template Eigen::Matrix Log(const Eigen::Matrix &R) { T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); } template Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) { T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); bool singular = sy < 1e-6; T x, y, z; if(!singular) { x = atan2(rot(2, 1), rot(2, 2)); y = atan2(-rot(2, 0), sy); z = atan2(rot(1, 0), rot(0, 0)); } else { x = atan2(-rot(1, 2), rot(1, 1)); y = atan2(-rot(2, 0), sy); z = 0; } Eigen::Matrix ang(x, y, z); return ang; } #endif ================================================ FILE: FAST-LIO/include/use-ikfom.hpp ================================================ #ifndef USE_IKFOM_H #define USE_IKFOM_H #include typedef MTK::vect<3, double> vect3; typedef MTK::SO3 SO3; typedef MTK::S2 S2; // S2 流形 typedef MTK::vect<1, double> vect1; typedef MTK::vect<2, double> vect2; // 定义的ieskf状态空间 MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos))((SO3, rot))((SO3, offset_R_L_I))((vect3, offset_T_L_I))((vect3, vel))((vect3, bg))((vect3, ba)) // S2流形,grav为负值 ((S2, grav))); // 定义的输入状态 MTK_BUILD_MANIFOLD(input_ikfom, ((vect3, acc))((vect3, gyro))); //定义的协方差噪声格式 //角速度(3),加速度(3),角速度偏置(3),加速度偏置(3) MTK_BUILD_MANIFOLD(process_noise_ikfom, ((vect3, ng))((vect3, na))((vect3, nbg))((vect3, nba))); // 噪声协方差初始化 MTK::get_cov::type process_noise_cov() { MTK::get_cov::type cov = MTK::get_cov::type::Zero(); MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001); // 0.03 MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); // 0.001 0.05 0.0001/out 0.01 return cov; } // double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia // vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); // fast_lio2论文公式(2), 起始这里的f就是将imu的积分方程组成矩阵形式然后再去计算 Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in) { Eigen::Matrix res = Eigen::Matrix::Zero(); // 将imu积分方程矩阵初始化为0,这里的24个对应了速度(3)，角速度(3),外参偏置T(3),外参偏置R(3)，加速度(3),角速度偏置(3),加速度偏置(3),位置(3)，与论文公式不一致 vect3 omega; in.gyro.boxminus(omega, s.bg); // 得到imu的角速度 // 加速度转到世界坐标系 vect3 a_inertial = s.rot * (in.acc - s.ba); for (int i = 0; i < 3; i++) { res(i) = s.vel[i]; //更新的速度 res(i + 3) = omega[i]; //更新的角速度 res(i + 12) = a_inertial[i] + s.grav[i]; //更新的加速度 } return res; } // 对应fast_lio2论文公式(7) Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in) { //当中的23个对应了status的维度计算，为pos(3), rot(3),offset_R_L_I(3),offset_T_L_I(3), vel(3), bg(3), ba(3), grav(2);这一块和fast-lio不同需要注意 Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); //一开始是一个R3的单位阵，代表速度转移 vect3 acc_; in.acc.boxminus(acc_, s.ba); //拿到加速度 vect3 omega; in.gyro.boxminus(omega, s.bg); //拿到角速度 cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix() * MTK::hat(acc_); //这里的-s.rot.toRotationMatrix()是因为论文中的矩阵是逆时针旋转的 cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); // 将角度转到存入的矩阵中（应该与上面颠倒了） Eigen::Matrix vec = Eigen::Matrix::Zero(); Eigen::Matrix grav_matrix; s.S2_Mx(grav_matrix, vec, 21); //将vec的2*1矩阵转为grav_matrix的3*2矩阵 cov.template block<3, 2>(12, 21) = grav_matrix; //存入到矩阵中 cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); //角速度存入 return cov; } // 对应fast_lio2论文公式(7) Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); //加速度 cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); //角速度 cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); //角速度偏置 cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); //加速度偏置 return cov; } vect3 SO3ToEuler(const SO3 &orient) //将SO3转为Euler角 { Eigen::Matrix _ang; Eigen::Vector4d q_data = orient.coeffs().transpose(); //将SO3转为4*1的矩阵 // scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; double sqw = q_data[3] * q_data[3]; double sqx = q_data[0] * q_data[0]; double sqy = q_data[1] * q_data[1]; double sqz = q_data[2] * q_data[2]; double unit = sqx + sqy + sqz + sqw; //如果归一为1，否则为校正因子 double test = q_data[3] * q_data[1] - q_data[2] * q_data[0]; if (test > 0.49999 * unit) { // 在北极点 _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI / 2, 0; double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; vect3 euler_ang(temp, 3); return euler_ang; } if (test < -0.49999 * unit) { // 在南极点 _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI / 2, 0; double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; vect3 euler_ang(temp, 3); return euler_ang; } // 否则正常计算 _ang << std::atan2(2 * q_data[0] * q_data[3] + 2 * q_data[1] * q_data[2], -sqx - sqy + sqz + sqw), std::asin(2 * test / unit), std::atan2(2 * q_data[2] * q_data[3] + 2 * q_data[1] * q_data[0], sqx - sqy - sqz + sqw); //转为弧度 double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; //转为角度 vect3 euler_ang(temp, 3); // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw return euler_ang; } #endif ================================================ FILE: FAST-LIO/launch/gdb_debug_example.launch ================================================ ================================================ FILE: FAST-LIO/launch/mapping_avia.launch ================================================ ================================================ FILE: FAST-LIO/launch/mapping_horizon.launch ================================================ ================================================ FILE: FAST-LIO/launch/mapping_ouster64.launch ================================================ ================================================ FILE: FAST-LIO/launch/mapping_ouster64_mulran.launch ================================================ ================================================ FILE: FAST-LIO/launch/mapping_velodyne.launch ================================================ ================================================ FILE: FAST-LIO/msg/Pose6D.msg ================================================ # the preintegrated Lidar states at the time of IMU measurements in a frame float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin float64[3] pos # the preintegrated position (global frame) at the Lidar origin float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin ================================================ FILE: FAST-LIO/package.xml ================================================ fast_lio 0.0.0 This is a modified version of LOAM which is original algorithm is described in the following paper: J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. claydergc BSD Ji Zhang catkin geometry_msgs nav_msgs roscpp rospy std_msgs sensor_msgs tf pcl_ros livox_ros_driver message_generation geometry_msgs nav_msgs sensor_msgs roscpp rospy std_msgs tf pcl_ros livox_ros_driver message_runtime rostest rosbag ================================================ FILE: FAST-LIO/rviz_cfg/.gitignore ================================================ ================================================ FILE: FAST-LIO/rviz_cfg/loam_livox.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /mapping1 - /mapping1/surround1 - /mapping1/currPoints1 - /mapping1/currPoints1/Autocompute Value Bounds1 - /Odometry1/Odometry1 - /Odometry1/Odometry1/Shape1 - /Odometry1/Odometry1/Covariance1 - /Odometry1/Odometry1/Covariance1/Position1 - /Odometry1/Odometry1/Covariance1/Orientation1 - /MarkerArray1/Namespaces1 Splitter Ratio: 0.6432291865348816 Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: surround Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1000 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 40 Reference Frame: Value: false - Class: rviz/Axes Enabled: false Length: 0.699999988079071 Name: Axes Radius: 0.05999999865889549 Reference Frame: Value: false - Class: rviz/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 238; 238; 236 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: false Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.15000000596046448 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 15 Min Value: -5 Value: false Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 1000 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: currPoints Position Transformer: XYZ Queue Size: 100000 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true 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/PointCloud2 Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false Enabled: true Name: mapping - Class: rviz/Group Displays: - Angle Tolerance: 0.009999999776482582 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.20000000298023224 Color: 255; 85; 0 Head Length: 0 Head Radius: 0 Shaft Length: 0.05000000074505806 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /Odometry Unreliable: false Value: true Enabled: true Name: Odometry - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: true - Alpha: 0 Buffer Length: 2 Class: rviz/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 Head Length: 0 Length: 0.30000001192092896 Line Style: Billboards Line Width: 0.20000000298023224 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 25; 255; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 Topic: /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/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 239; 41; 41 Max Intensity: 0 Min Color: 239; 41; 41 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.30000001192092896 Style: Spheres Topic: /cloud_effected Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 13.139549255371094 Min Value: -32.08251953125 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 138; 226; 52 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 138; 226; 52 Min Color: 138; 226; 52 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /MarkerArray Name: MarkerArray Namespaces: {} Queue Size: 100 Value: false Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: camera_init Frame Rate: 10 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - 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: 24.759632110595703 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0 Y: 0 Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.8303981423377991 Target Frame: body Value: Orbit (rviz) Yaw: 1.6953977346420288 Saved: ~ Window Geometry: Displays: collapsed: true Height: 1025 Hide Left Dock: true Hide Right Dock: true QMainWindow State: 000000ff00000000fd0000000400000000000001c80000034ffc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000034f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c7000000000000000000000001000001520000034ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000052fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000073d0000034f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1853 X: 67 Y: 27 ================================================ FILE: FAST-LIO/src/IMU_Processing.hpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "use-ikfom.hpp" /// *************Preconfiguration #define MAX_INI_COUNT (20) //判断点的时间是否先后颠倒 const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); }; /// *************IMU Process and undistortion class ImuProcess { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuProcess(); ~ImuProcess(); void Reset(); void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); void set_extrinsic(const V3D &transl, const M3D &rot); void set_extrinsic(const V3D &transl); void set_extrinsic(const MD(4, 4) & T); void set_gyr_cov(const V3D &scaler); void set_acc_cov(const V3D &scaler); void set_gyr_bias_cov(const V3D &b_g); void set_acc_bias_cov(const V3D &b_a); Eigen::Matrix Q; void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); ofstream fout_imu; // imu参数输出文件 V3D cov_acc; //加速度测量协方差 V3D cov_gyr; //角速度测量协方差 V3D cov_acc_scale; //加速度测量协方差 V3D cov_gyr_scale; //角速度测量协方差 V3D cov_bias_gyr; //角速度测量协方差偏置 V3D cov_bias_acc; //加速度测量协方差偏置 double first_lidar_time; //当前帧第一个点云时间 private: void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); PointCloudXYZI::Ptr cur_pcl_un_; //当前帧点云未去畸变 sensor_msgs::ImuConstPtr last_imu_; // 上一帧imu最后的一个值 deque v_imu_; // imu队列 vector IMUpose; // imu位姿 vector v_rot_pcl_; //未使用 M3D Lidar_R_wrt_IMU; // lidar到IMU的旋转外参 V3D Lidar_T_wrt_IMU; // lidar到IMU的位置外参 V3D mean_acc; //加速度均值,用于计算方差 V3D mean_gyr; //角速度均值，用于计算方差 V3D angvel_last; //上一帧角速度 V3D acc_s_last; //上一帧加速度 double start_timestamp_; //开始时间戳 double last_lidar_end_time_; //上一帧结束时间戳 int init_iter_num = 1; //初始化迭代次数 bool b_first_frame_ = true; //是否是第一帧 bool imu_need_init_ = true; //是否需要初始化imu }; ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) { init_iter_num = 1; //初始化迭代次数 Q = process_noise_cov(); //调用use-ikfom.hpp里面的process_noise_cov完成噪声协方差的初始化 cov_acc = V3D(0.1, 0.1, 0.1); //加速度测量协方差初始化 cov_gyr = V3D(0.1, 0.1, 0.1); //角速度测量协方差初始化 cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); //角速度测量协方差偏置初始化 cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); //加速度测量协方差偏置初始化 mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; //上一帧角速度初始化 Lidar_T_wrt_IMU = Zero3d; // lidar到IMU的位置外参初始化 Lidar_R_wrt_IMU = Eye3d; // lidar到IMU的旋转外参初始化 last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化 } ImuProcess::~ImuProcess() {} //重置参数 void ImuProcess::Reset() { // ROS_WARN("Reset ImuProcess"); mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; imu_need_init_ = true; //是否需要初始化imu start_timestamp_ = -1; //开始时间戳 init_iter_num = 1; //初始化迭代次数 v_imu_.clear(); // imu队列清空 IMUpose.clear(); // imu位姿清空 last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化 cur_pcl_un_.reset(new PointCloudXYZI()); //当前帧点云未去畸变初始化 } //传入外参，包含R,T void ImuProcess::set_extrinsic(const MD(4, 4) & T) { Lidar_T_wrt_IMU = T.block<3, 1>(0, 3); Lidar_R_wrt_IMU = T.block<3, 3>(0, 0); } //传入外参，包含T void ImuProcess::set_extrinsic(const V3D &transl) { Lidar_T_wrt_IMU = transl; Lidar_R_wrt_IMU.setIdentity(); } // 传入外参，包含R,T void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) { Lidar_T_wrt_IMU = transl; Lidar_R_wrt_IMU = rot; } // 传入陀螺仪角速度协方差 void ImuProcess::set_gyr_cov(const V3D &scaler) { cov_gyr_scale = scaler; } // 传入加速度计加速度协方差 void ImuProcess::set_acc_cov(const V3D &scaler) { cov_acc_scale = scaler; } // 传入陀螺仪角速度协方差偏置 void ImuProcess::set_gyr_bias_cov(const V3D &b_g) { cov_bias_gyr = b_g; } // 传入加速度计加速度协方差偏置 void ImuProcess::set_acc_bias_cov(const V3D &b_a) { cov_bias_acc = b_a; } void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N) { /** 1. 初始化重力、陀螺偏差、acc和陀螺仪协方差 /** 2. 将加速度测量值标准化为单位重力**/ //这里应该是静止初始化 V3D cur_acc, cur_gyr; if (b_first_frame_) //判断是否为第一帧 { Reset(); //重置参数 N = 1; //将迭代次数置1 b_first_frame_ = false; const auto &imu_acc = meas.imu.front()->linear_acceleration; //从common_lib.h中拿到imu初始时刻的加速度 const auto &gyr_acc = meas.imu.front()->angular_velocity; //从common_lib.h中拿到imu初始时刻的角速度 mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; //加速度测量作为初始化均值 mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; //角速度测量作为初始化均值 first_lidar_time = meas.lidar_beg_time; //将当期imu帧对应的lidar时间作为初始时间 } //计算方差 for (const auto &imu : meas.imu) //拿到所有的imu帧 { const auto &imu_acc = imu->linear_acceleration; const auto &gyr_acc = imu->angular_velocity; cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; //根据当前帧和均值差作为均值的更新 mean_acc += (cur_acc - mean_acc) / N; mean_gyr += (cur_gyr - mean_gyr) / N; //.cwiseProduct()对应系数相乘 //每次迭代之后均值都会发生变化，最后的方差公式中减的应该是最后的均值 // https://blog.csdn.net/weixin_44479136/article/details/90510374 方差迭代计算公式 //按照博客推导出来的下面方差递推公式有两种 //第一种是 cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) / (N - 1.0); cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) / (N - 1.0); //第二种是 // cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - 上一次的mean_acc) / N; // cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - 上一次的mean_gyr) / N; // cout<<"acc norm: "<::cov init_P = kf_state.get_P(); //在esekfom.hpp获得P_的协方差矩阵 init_P.setIdentity(); //将协方差矩阵置为单位阵 init_P(6, 6) = init_P(7, 7) = init_P(8, 8) = 0.00001; //将协方差矩阵的位置和旋转的协方差置为0.00001 init_P(9, 9) = init_P(10, 10) = init_P(11, 11) = 0.00001; //将协方差矩阵的速度和位姿的协方差置为0.00001 init_P(15, 15) = init_P(16, 16) = init_P(17, 17) = 0.0001; //将协方差矩阵的重力和姿态的协方差置为0.0001 init_P(18, 18) = init_P(19, 19) = init_P(20, 20) = 0.001; //将协方差矩阵的陀螺仪偏差和姿态的协方差置为0.001 init_P(21, 21) = init_P(22, 22) = 0.00001; //将协方差矩阵的lidar和imu外参位移量的协方差置为0.00001 kf_state.change_P(init_P); //将初始化协方差矩阵传入esekfom.hpp中的P_ last_imu_ = meas.imu.back(); //将最后一帧的imu数据传入last_imu_中，在UndistortPcl使用到了 } //正向传播 反向传播 去畸变，这里涉及到了Lidar的去畸变问题 void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) { auto v_imu = meas.imu; //拿到当前的imu数据 v_imu.push_front(last_imu_); //将上一帧最后尾部的imu添加到当前帧头部的imu const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); //拿到当前帧头部的imu的时间（也就是上一帧尾部的imu时间戳） const double &imu_end_time = v_imu.back()->header.stamp.toSec(); //拿到当前帧尾部的imu的时间 const double &pcl_beg_time = meas.lidar_beg_time; // pcl开始的时间戳 /*** sort point clouds by offset time ***/ // 根据点云中每个点的时间戳对点云进行重排序 pcl_out = *(meas.lidar); sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); //拿到最后一帧时间戳加上最后一帧的所需要的时间/1000得到点云的结束时间戳 // cout<<"[ IMU Process ]: Process lidar from "<header.stamp.toSec() < last_lidar_end_time_) continue; // 中值积分 angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; //通过重力数值对加速度进行一下微调 acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; //如果IMU开始时刻早于上次雷达最晚时刻(因为将上次最后一个IMU插入到此次开头了，所以会出现一次这种情况) if (head->header.stamp.toSec() < last_lidar_end_time_) { //从上次雷达时刻末尾开始传播 计算与此次IMU结尾之间的时间差 dt = tail->header.stamp.toSec() - last_lidar_end_time_; // dt = tail->header.stamp.toSec() - pcl_beg_time; } else { //两个IMU时刻之间的时间间隔 dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); } // 原始测量的中值作为更新 in.acc = acc_avr; in.gyro = angvel_avr; // 配置协方差矩阵 Q.block<3, 3>(0, 0).diagonal() = cov_gyr; Q.block<3, 3>(3, 3).diagonal() = cov_acc; Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; // IMU前向传播，每次传播的时间间隔为dt kf_state.predict(dt, Q, in); /* save the poses at each IMU measurements */ // 保存IMU预测过程的状态 imu_state = kf_state.get_x(); angvel_last = angvel_avr - imu_state.bg; //计算出来的角速度与预测的角速度的差值 acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); //计算出来的加速度与预测的加速度的差值,并转到IMU坐标系下 for (int i = 0; i < 3; i++) { acc_s_last[i] += imu_state.grav[i]; //加上重力得到世界坐标系的加速度 } double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; //后一个IMU时刻距离此次雷达开始的时间间隔 IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); //保存IMU预测过程的状态 } // 把最后一帧IMU测量也补上 // 判断雷达结束时间是否晚于IMU，最后一个IMU时刻可能早于雷达末尾 也可能晚于雷达末尾 double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; dt = note * (pcl_end_time - imu_end_time); kf_state.predict(dt, Q, in); imu_state = kf_state.get_x(); //更新IMU状态，以便于下一帧使用 last_imu_ = meas.imu.back(); //保存最后一个IMU测量，以便于下一帧使用 last_lidar_end_time_ = pcl_end_time; //保存这一帧最后一个雷达测量的结束时间，以便于下一帧使用 /*** 在处理完所有的IMU预测后，剩下的就是对激光的去畸变了 ***/ // 基于IMU预测对lidar点云去畸变 auto it_pcl = pcl_out.points.end() - 1; for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) { auto head = it_kp - 1; auto tail = it_kp; R_imu << MAT_FROM_ARRAY(head->rot); //拿到前一帧的IMU旋转矩阵 // cout<<"head imu acc: "<vel); //拿到前一帧的IMU速度 pos_imu << VEC_FROM_ARRAY(head->pos); //拿到前一帧的IMU位置 acc_imu << VEC_FROM_ARRAY(tail->acc); //拿到后一帧的IMU加速度 angvel_avr << VEC_FROM_ARRAY(tail->gyr); //拿到后一帧的IMU角速度 //点云时间需要迟于前一个IMU时刻 因为是在两个IMU时刻之间去畸变，此时默认雷达的时间戳在后一个IMU时刻之前 for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) //时间除以1000单位化为s { dt = it_pcl->curvature / double(1000) - head->offset_time; //点到IMU开始时刻的时间间隔 /*变换到“结束”帧，仅使用旋转 *注意：补偿方向与帧的移动方向相反 *所以如果我们想补偿时间戳i到帧e的一个点 * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) 其中T_ei在全局框架中表示*/ M3D R_i(R_imu * Exp(angvel_avr, dt)); //点所在时刻的旋转 V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); //点所在时刻的位置(雷达坐标系下) V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); //从点所在的世界位置-雷达末尾世界位置 //.conjugate()取旋转矩阵的共轭,rot.conjugate（）是四元数共轭，即旋转求逆 // imu_state.offset_R_L_I是从雷达到惯性的旋转矩阵 简单记为I^R_L // imu_state.offset_T_L_I是惯性系下雷达坐标系原点的位置简单记为I^t_L //下面去畸变补偿的公式这里倒推一下 // e代表end时刻 // P_compensate是点在末尾时刻在雷达系的坐标 简记为L^P_e //将右侧矩阵乘过来并加上右侧平移 //左边变为I^R_L * L^P_e + I^t_L= I^P_e 也就是end时刻点在IMU系下的坐标 //右边剩下imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei // imu_state.rot.conjugate()是结束时刻IMU到世界坐标系的旋转矩阵的逆函数 也就是(W^R_i_e)^T // T_ei展开是pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos也就是点所在时刻IMU在世界坐标系下的位置 - end时刻IMU在世界坐标系下的位置 W^t_I-W^t_I_e //现在等式两边变为 I^P_e = (W^R_i_e)^T * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + W^t_I - W^t_I_e //(W^R_i_e) * I^P_e + W^t_I_e = (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + W^t_I // 世界坐标系也无所谓时刻了 因为只有一个世界坐标系 两边变为 // W^P = R_i * I^P+ W^t_I // W^P = W^P V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I); // not accurate! // save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); if (it_pcl == pcl_out.points.begin()) break; } } } void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) { double t1, t2, t3; t1 = omp_get_wtime(); if (meas.imu.empty()) { return; }; // 拿到的当前帧的imu测量为空，则直接返回 ROS_ASSERT(meas.lidar != nullptr); if (imu_need_init_) { /// 第一个激光雷达帧 IMU_init(meas, kf_state, init_iter_num); imu_need_init_ = true; last_imu_ = meas.imu.back(); state_ikfom imu_state = kf_state.get_x(); if (init_iter_num > MAX_INI_COUNT) { cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); //在上面IMU_init()基础上乘上缩放系数 imu_need_init_ = false; cov_acc = cov_acc_scale; cov_gyr = cov_gyr_scale; ROS_INFO("IMU Initial Done"); // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out); } return; } //正向传播 反向传播 去畸变 UndistortPcl(meas, kf_state, *cur_pcl_un_); t2 = omp_get_wtime(); t3 = omp_get_wtime(); // cout<<"[ IMU Process ]: Time: "< #include #include #include #include #include #include #include #include #include #include #include "IMU_Processing.hpp" #include #include #include #include #include #include #include #include #include #include #include #include #include #include "preprocess.h" #include #define INIT_TIME (0.1) #define LASER_POINT_COV (0.001) #define MAXN (720000) #define PUBFRAME_PERIOD (20) /*** Time Log Variables ***/ // kdtree_incremental_time为kdtree建立时间，kdtree_search_time为kdtree搜索时间，kdtree_delete_time为kdtree删除时间; double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0; // T1为雷达初始时间戳，s_plot为整个流程耗时，s_plot2特征点数量,s_plot3为kdtree增量时间，s_plot4为kdtree搜索耗时，s_plot5为kdtree删除点数量 //，s_plot6为kdtree删除耗时，s_plot7为kdtree初始大小，s_plot8为kdtree结束大小,s_plot9为平均消耗时间，s_plot10为添加点数量，s_plot11为点云预处理的总时间 double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN]; // 定义全局变量，用于记录时间,match_time为匹配时间，solve_time为求解时间，solve_const_H_time为求解H矩阵时间 double match_time = 0, solve_time = 0, solve_const_H_time = 0; // kdtree_size_st为ikd-tree获得的节点数，kdtree_size_end为ikd-tree结束时的节点数，add_point_size为添加点的数量，kdtree_delete_counter为删除点的数量 int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0; // runtime_pos_log运行时的log是否开启，pcd_save_en是否保存pcd文件，time_sync_en是否同步时间 bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false; /**************************/ float res_last[100000] = {0.0}; //残差，点到面距离平方和 float DET_RANGE = 300.0f; //设置的当前雷达系中心到各个地图边缘的距离 const float MOV_THRESHOLD = 1.5f; //设置的当前雷达系中心到各个地图边缘的权重 mutex mtx_buffer; // 互斥锁 condition_variable sig_buffer; // 条件变量 string root_dir = ROOT_DIR; //设置根目录 string map_file_path, lid_topic, imu_topic; //设置地图文件路径，雷达topic，imu topic double res_mean_last = 0.05, total_residual = 0.0; //设置残差平均值，残差总和 double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; //设置雷达时间戳，imu时间戳 double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; //设置imu的角速度协方差，加速度协方差，角速度协方差偏置，加速度协方差偏置 double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; //设置滤波器的最小尺寸，地图的最小尺寸，视野角度 //设置立方体长度，视野一半的角度，视野总角度，总距离，雷达结束时间，雷达初始时间 double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0; //设置有效特征点数，时间log计数器, scan_count：接收到的激光雷达Msg的总数，publish_count：接收到的IMU的Msg的总数 int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0; //设置迭代次数，下采样的点数，最大迭代次数，有效点数 int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0; bool point_selected_surf[100000] = {0}; // 是否为平面特征点 // lidar_pushed：用于判断激光雷达数据是否从缓存队列中拿到meas中的数据, flg_EKF_inited用于判断EKF是否初始化完成 bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited; //设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据 bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; vector> pointSearchInd_surf; //每个点的索引,暂时没用到 vector cub_needrm; // ikd-tree中，地图需要移除的包围盒序列 vector Nearest_Points; //每个点的最近点序列 vector extrinT(3, 0.0); //雷达相对于IMU的外参T vector extrinR(9, 0.0); //雷达相对于IMU的外参R deque time_buffer; // 激光雷达数据时间戳缓存队列 deque lidar_buffer; //记录特征提取或间隔采样后的lidar（特征）数据 deque imu_buffer; // IMU数据缓存队列 //一些点云变量 PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); //提取地图中的特征点，IKD-tree获得 PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); //去畸变的特征 PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，lidar系 PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，w系 PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); //特征点在地图中对应点的，局部平面参数,w系 PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); // laserCloudOri是畸变纠正后降采样的单帧点云，body系 PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); //对应点法相量 PointCloudXYZI::Ptr _featsArray; // ikd-tree中，map需要移除的点云序列 //下采样的体素点云 pcl::VoxelGrid downSizeFilterSurf; //单帧内降采样使用voxel grid pcl::VoxelGrid downSizeFilterMap; //未使用 KD_TREE ikdtree; // ikd-tree类 V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于body系的X轴方向的点 V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于world系的X轴方向的点 V3D euler_cur; //当前的欧拉角 V3D position_last(Zero3d); //上一帧的位置 V3D Lidar_T_wrt_IMU(Zero3d); // T lidar to imu (imu = r * lidar + t) M3D Lidar_R_wrt_IMU(Eye3d); // R lidar to imu (imu = r * lidar + t) /*** EKF inputs and output ***/ // ESEKF操作 MeasureGroup Measures; esekfom::esekf kf; // 状态，噪声维度，输入 state_ikfom state_point; // 状态 vect3 pos_lid; // world系下lidar坐标 //输出的路径参数 nav_msgs::Path path; //包含了一系列位姿 nav_msgs::Odometry odomAftMapped; //只包含了一个位姿 geometry_msgs::Quaternion geoQuat; //四元数 geometry_msgs::PoseStamped msg_body_pose; //位姿 //激光和imu处理操作 shared_ptr p_pre(new Preprocess()); // 定义指向激光雷达数据的预处理类Preprocess的智能指针 shared_ptr p_imu(new ImuProcess()); // 定义指向IMU数据预处理类ImuProcess的智能指针 //按下ctrl+c后唤醒所有线程 void SigHandle(int sig) { flg_exit = true; ROS_WARN("catch sig %d", sig); sig_buffer.notify_all(); // 会唤醒所有等待队列中阻塞的线程 线程被唤醒后，会通过轮询方式获得锁，获得锁前也一直处理运行状态，不会被再次阻塞。 } //将fast lio2信息打印到log中 inline void dump_lio_state_to_log(FILE *fp) { V3D rot_ang(Log(state_point.rot.toRotationMatrix())); fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a fprintf(fp, "\r\n"); fflush(fp); } //把点从body系转到world系，通过ikfom的位置和姿态 void pointBodyToWorld_ikfom(PointType const *const pi, PointType *const po, state_ikfom &s) { V3D p_body(pi->x, pi->y, pi->z); //下面式子最里面的括号是从雷达到IMU坐标系 然后从IMU转到世界坐标系 V3D p_global(s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos); po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->intensity = pi->intensity; } //把点从body系转到world系 void pointBodyToWorld(PointType const *const pi, PointType *const po) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos); po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->intensity = pi->intensity; } //把点从body系转到world系 template void pointBodyToWorld(const Matrix &pi, Matrix &po) { V3D p_body(pi[0], pi[1], pi[2]); V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos); po[0] = p_global(0); po[1] = p_global(1); po[2] = p_global(2); } // 含有RGB的点云从body系转到world系 void RGBpointBodyToWorld(PointType const *const pi, PointType *const po) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos); po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->intensity = pi->intensity; } // 含有RGB的点云从Lidar系转到IMU系 void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po) { V3D p_body_lidar(pi->x, pi->y, pi->z); V3D p_body_imu(state_point.offset_R_L_I * p_body_lidar + state_point.offset_T_L_I); po->x = p_body_imu(0); po->y = p_body_imu(1); po->z = p_body_imu(2); po->intensity = pi->intensity; } //得到被剔除的点 void points_cache_collect() { PointVector points_history; ikdtree.acquire_removed_points(points_history); //返回被剔除的点 for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]); //存入到缓存中，后面没有用到该数据 } // 在拿到eskf前馈结果后，动态调整地图区域，防止地图过大而内存溢出，类似LOAM中提取局部地图的方法 BoxPointType LocalMap_Points; // ikd-tree中,局部地图的包围盒角点 bool Localmap_Initialized = false; // 局部地图是否初始化 void lasermap_fov_segment() { cub_needrm.clear(); // 清空需要移除的区域 kdtree_delete_counter = 0; kdtree_delete_time = 0.0; // X轴分界点转换到w系下，好像没有用到 pointBodyToWorld(XAxisPoint_body, XAxisPoint_world); // global系下lidar位置 V3D pos_LiD = pos_lid; //初始化局部地图包围盒角点，以为w系下lidar位置为中心,得到长宽高200*200*200的局部地图 if (!Localmap_Initialized) { // 系统起始需要初始化局部地图的大小和位置 for (int i = 0; i < 3; i++) { LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; } Localmap_Initialized = true; return; } // 各个方向上Lidar与局部地图边界的距离，或者说是lidar与立方体盒子六个面的距离 float dist_to_map_edge[3][2]; bool need_move = false; // 当前雷达系中心到各个地图边缘的距离 for (int i = 0; i < 3; i++) { dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); // 与某个方向上的边界距离（例如1.5*300m）太小，标记需要移除need_move，参考论文Fig3 if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true; } // 不需要挪动就直接退回了 if (!need_move) return; // 否则需要计算移动的距离 BoxPointType New_LocalMap_Points, tmp_boxpoints; // 新的局部地图盒子边界点 New_LocalMap_Points = LocalMap_Points; float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1))); for (int i = 0; i < 3; i++) { tmp_boxpoints = LocalMap_Points; //与包围盒最小值边界点距离 if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) { New_LocalMap_Points.vertex_max[i] -= mov_dist; New_LocalMap_Points.vertex_min[i] -= mov_dist; tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; cub_needrm.push_back(tmp_boxpoints); // 移除较远包围盒 } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) { New_LocalMap_Points.vertex_max[i] += mov_dist; New_LocalMap_Points.vertex_min[i] += mov_dist; tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; cub_needrm.push_back(tmp_boxpoints); } } LocalMap_Points = New_LocalMap_Points; points_cache_collect(); double delete_begin = omp_get_wtime(); // 使用Boxs删除指定盒内的点 if (cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); kdtree_delete_time = omp_get_wtime() - delete_begin; } // 除了AVIA类型之外的雷达点云回调函数，将数据引入到buffer当中 void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) { mtx_buffer.lock(); //加锁 scan_count++; double preprocess_start_time = omp_get_wtime(); //记录时间 if (msg->header.stamp.toSec() < last_timestamp_lidar) { ROS_ERROR("lidar loop back, clear buffer"); lidar_buffer.clear(); } PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); p_pre->process(msg, ptr); //点云预处理 lidar_buffer.push_back(ptr); //将点云放入缓冲区 time_buffer.push_back(msg->header.stamp.toSec()); //将时间放入缓冲区 last_timestamp_lidar = msg->header.stamp.toSec(); //记录最后一个时间 s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; //预处理时间 mtx_buffer.unlock(); sig_buffer.notify_all(); // 唤醒所有线程 } double timediff_lidar_wrt_imu = 0.0; //雷达时间与imu时间差 bool timediff_set_flg = false; // 时间同步flag，false表示未进行时间同步，true表示已经进行过时间同步 // 订阅器sub_pcl的回调函数：接收Livox激光雷达的点云数据，对点云数据进行预处理（特征提取、降采样、滤波），并将处理后的数据保存到激光雷达数据队列中 void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) { // 互斥锁 mtx_buffer.lock(); // 点云预处理的开始时间 double preprocess_start_time = omp_get_wtime(); // 激光雷达扫描的总次数 scan_count++; // 如果当前扫描的激光雷达数据的时间戳比上一次扫描的激光雷达数据的时间戳早，需要将激光雷达数据缓存队列清空 if (msg->header.stamp.toSec() < last_timestamp_lidar) { ROS_ERROR("lidar loop back, clear buffer"); lidar_buffer.clear(); } last_timestamp_lidar = msg->header.stamp.toSec(); // 如果不需要进行时间同步，而imu时间戳和雷达时间戳相差大于10s，则输出错误信息 if (!time_sync_en && abs(last_timestamp_imu - lidar_end_time) > 10.0) { printf("IMU and LiDAR not Synced, IMU time: %lf, lidar scan end time: %lf", last_timestamp_imu, lidar_end_time); } // time_sync_en为true时，当imu时间戳和雷达时间戳相差大于1s时，进行时间同步 if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty()) { timediff_set_flg = true; // 标记已经进行时间同步 timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu; printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu); } // 用pcl点云格式保存接收到的激光雷达数据 PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); // 对激光雷达数据进行预处理（特征提取或者降采样），其中p_pre是Preprocess类的智能指针 p_pre->process(msg, ptr); lidar_buffer.push_back(ptr); time_buffer.push_back(last_timestamp_lidar); // 点云预处理的总时间 s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; mtx_buffer.unlock(); sig_buffer.notify_all(); // 唤醒所有线程 } // 订阅器sub_imu的回调函数：接收IMU数据，将IMU数据保存到IMU数据缓存队列中 void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) { publish_count++; // cout<<"IMU got at: "<header.stamp.toSec()< 0.1 && time_sync_en) //时间同步校准 { msg->header.stamp = ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec()); //将IMU时间戳对齐到激光雷达时间戳 } double timestamp = msg->header.stamp.toSec(); // IMU时间戳 mtx_buffer.lock(); // 如果当前IMU的时间戳小于上一个时刻IMU的时间戳，则IMU数据有误，将IMU数据缓存队列清空 if (timestamp < last_timestamp_imu) { ROS_WARN("imu loop back, clear buffer"); imu_buffer.clear(); } // 将当前的IMU数据保存到IMU数据缓存队列中 last_timestamp_imu = timestamp; imu_buffer.push_back(msg); mtx_buffer.unlock(); //解锁 sig_buffer.notify_all(); // 唤醒阻塞的线程，当持有锁的线程释放锁时，这些线程中的一个会获得锁。而其余的会接着尝试获得锁 } //处理buffer中的数据，将两帧激光雷达点云数据时间内的IMU数据从缓存队列中取出，进行时间对齐，并保存到meas中 bool sync_packages(MeasureGroup &meas) { if (lidar_buffer.empty() || imu_buffer.empty()) //如果缓存队列中没有数据，则返回false { return false; } /*** push a lidar scan ***/ //如果还没有把雷达数据放到meas中的话，就执行一下操作 if (!lidar_pushed) { // 从激光雷达点云缓存队列中取出点云数据，放到meas中 meas.lidar = lidar_buffer.front(); // 如果该lidar没有点云，则返回false if (meas.lidar->points.size() <= 1) { lidar_buffer.pop_front(); return false; } // 该lidar测量起始的时间戳 meas.lidar_beg_time = time_buffer.front(); lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); //此次雷达点云结束时刻 // 成功提取到lidar测量的标志 lidar_pushed = true; } // 最新的IMU时间戳(也就是队尾的)不能早于雷达的end时间戳，因为last_timestamp_imu比较时是加了0.1的，所以要比较大于雷达的end时间戳 if (last_timestamp_imu < lidar_end_time) { return false; } // 压入imu数据，并从imu缓冲区弹出 double imu_time = imu_buffer.front()->header.stamp.toSec(); meas.imu.clear(); // 拿出lidar_beg_time到lidar_end_time之间的所有IMU数据 while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) //如果imu缓存队列中的数据时间戳小于雷达结束时间戳，则将该数据放到meas中,代表了这一帧中的imu数据 { imu_time = imu_buffer.front()->header.stamp.toSec(); //获取imu数据的时间戳 if (imu_time > lidar_end_time) break; meas.imu.push_back(imu_buffer.front()); //将imu数据放到meas中 imu_buffer.pop_front(); } lidar_buffer.pop_front(); //将lidar数据弹出 time_buffer.pop_front(); //将时间戳弹出 lidar_pushed = false; //将lidar_pushed置为false，代表lidar数据已经被放到meas中了 return true; } int process_increments = 0; void map_incremental() //地图的增量更新，主要完成对ikd-tree的地图建立 { PointVector PointToAdd; //需要加入到ikd-tree中的点云 PointVector PointNoNeedDownsample; //加入ikd-tree时，不需要降采样的点云 PointToAdd.reserve(feats_down_size); //构建的地图点 PointNoNeedDownsample.reserve(feats_down_size); //构建的地图点，不需要降采样的点云 //根据点与所在包围盒中心点的距离，分类是否需要降采样 for (int i = 0; i < feats_down_size; i++) { // 转换到世界坐标系下 pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); // 判断是否有关键点需要加到地图中 if (!Nearest_Points[i].empty() && flg_EKF_inited) { const PointVector &points_near = Nearest_Points[i]; //获取附近的点云 bool need_add = true; //是否需要加入到地图中 BoxPointType Box_of_Point; //点云所在的包围盒 PointType downsample_result, mid_point; //降采样结果，中点 // filter_size_map_min是地图体素降采样的栅格边长，设为0.1m // mid_point即为该特征点所属的栅格的中心点坐标 mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; // 当前点与box中心的距离 float dist = calc_dist(feats_down_world->points[i], mid_point); //判断最近点在x、y、z三个方向上，与中心的距离，判断是否加入时需要降采样 if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min) { //若三个方向距离都大于地图栅格半轴长，无需降采样 PointNoNeedDownsample.push_back(feats_down_world->points[i]); continue; } //判断当前点的 NUM_MATCH_POINTS 个邻近点与包围盒中心的范围 for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i++) { if (points_near.size() < NUM_MATCH_POINTS) //若邻近点数小于NUM_MATCH_POINTS，则直接跳出，添加到PointToAdd中 break; // 如果存在邻近点到中心的距离小于当前点到中心的距离，则不需要添加当前点 if (calc_dist(points_near[readd_i], mid_point) < dist) { need_add = false; break; } } if (need_add) PointToAdd.push_back(feats_down_world->points[i]); //加入到PointToAdd中 } else { PointToAdd.push_back(feats_down_world->points[i]); //如果周围没有点或者没有初始化EKF，则加入到PointToAdd中 } } double st_time = omp_get_wtime(); //记录起始时间 add_point_size = ikdtree.Add_Points(PointToAdd, true); //加入点时需要降采样 ikdtree.Add_Points(PointNoNeedDownsample, false); //加入点时不需要降采样 add_point_size = PointToAdd.size() + PointNoNeedDownsample.size(); //计算总共加入ikd-tree的点的数量 kdtree_incremental_time = omp_get_wtime() - st_time; // kdtree建立时间更新 } PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); //创建一个点云用于存储等待发布的点云 PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); //创建一个点云用于存储等待保存的点云 void publish_frame_world(const ros::Publisher &pubLaserCloudFull) { if (scan_pub_en) // 设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据 { PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); //判断是否需要降采样 int size = laserCloudFullRes->points.size(); //获取待转换点云的大小 PointCloudXYZI::Ptr laserCloudWorld( new PointCloudXYZI(size, 1)); //创建一个点云用于存储转换到世界坐标系的点云 for (int i = 0; i < size; i++) { RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); } sensor_msgs::PointCloud2 laserCloudmsg; pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.frame_id = "camera_init"; pubLaserCloudFull.publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } //把结果压入到pcd中 /**************** save map ****************/ /* 1. make sure you have enough memories /* 2. pcd save will largely influence the real-time performences **/ if (pcd_save_en) { int size = feats_undistort->points.size(); PointCloudXYZI::Ptr laserCloudWorld( new PointCloudXYZI(size, 1)); for (int i = 0; i < size; i++) { RGBpointBodyToWorld(&feats_undistort->points[i], &laserCloudWorld->points[i]); //转换到世界坐标系 } *pcl_wait_save += *laserCloudWorld; //把结果压入到pcd中 } } //把去畸变的雷达系下的点云转到IMU系 void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body) { int size = feats_undistort->points.size(); PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); //创建一个点云用于存储转换到IMU系的点云 for (int i = 0; i < size; i++) { RGBpointBodyLidarToIMU(&feats_undistort->points[i], &laserCloudIMUBody->points[i]); //转换到IMU坐标系 } sensor_msgs::PointCloud2 laserCloudmsg; pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.frame_id = "body"; pubLaserCloudFull_body.publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } //发布雷达信息 void publish_frame_lidar(const ros::Publisher &pubLaserCloudFull_lidar) { sensor_msgs::PointCloud2 laserCloudmsg; pcl::toROSMsg(*feats_undistort, laserCloudmsg); //转换到ROS消息格式,并直接发布信息 laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.frame_id = "lidar"; pubLaserCloudFull_lidar.publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } //把起作用的特征点转到地图中 void publish_effect_world(const ros::Publisher &pubLaserCloudEffect) { PointCloudXYZI::Ptr laserCloudWorld( new PointCloudXYZI(effct_feat_num, 1)); //创建一个点云用于存储转换到世界坐标系的点云,从h_share_model获得 for (int i = 0; i < effct_feat_num; i++) { RGBpointBodyToWorld(&laserCloudOri->points[i], &laserCloudWorld->points[i]); } sensor_msgs::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudFullRes3.header.frame_id = "camera_init"; pubLaserCloudEffect.publish(laserCloudFullRes3); } // 发布ikd-tree地图 void publish_map(const ros::Publisher &pubLaserCloudMap) { sensor_msgs::PointCloud2 laserCloudMap; pcl::toROSMsg(*featsFromMap, laserCloudMap); laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudMap.header.frame_id = "camera_init"; pubLaserCloudMap.publish(laserCloudMap); } // 设置输出的t,q，在publish_odometry，publish_path调用 template void set_posestamp(T &out) { out.pose.position.x = state_point.pos(0); //将eskf求得的位置传入 out.pose.position.y = state_point.pos(1); out.pose.position.z = state_point.pos(2); out.pose.orientation.x = geoQuat.x; //将eskf求得的姿态传入 out.pose.orientation.y = geoQuat.y; out.pose.orientation.z = geoQuat.z; out.pose.orientation.w = geoQuat.w; } //发布里程计 void publish_odometry(const ros::Publisher &pubOdomAftMapped) { odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "body"; odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); // ros::Time().fromSec(lidar_end_time); set_posestamp(odomAftMapped.pose); pubOdomAftMapped.publish(odomAftMapped); auto P = kf.get_P(); for (int i = 0; i < 6; i++) { int k = i < 3 ? i + 3 : i - 3; //设置协方差 P里面先是旋转后是位置 这个POSE里面先是位置后是旋转 所以对应的协方差要对调一下 odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3); odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4); odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5); odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0); odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1); odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2); } static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, odomAftMapped.pose.pose.position.y, odomAftMapped.pose.pose.position.z)); q.setW(odomAftMapped.pose.pose.orientation.w); q.setX(odomAftMapped.pose.pose.orientation.x); q.setY(odomAftMapped.pose.pose.orientation.y); q.setZ(odomAftMapped.pose.pose.orientation.z); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "body")); //发布tf变换 } //每隔10个发布一下位姿 void publish_path(const ros::Publisher pubPath) { set_posestamp(msg_body_pose); msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); msg_body_pose.header.frame_id = "camera_init"; /*** if path is too large, the rvis will crash ***/ static int jjj = 0; jjj++; if (jjj % 10 == 0) { path.poses.push_back(msg_body_pose); pubPath.publish(path); } } //计算残差信息 void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct &ekfom_data) { double match_start = omp_get_wtime(); //计算匹配的开始时间 laserCloudOri->clear(); //将body系的有效点云存储清空 corr_normvect->clear(); //将对应的法向量清空 total_residual = 0.0; /** 最接近曲面搜索和残差计算 **/ #ifdef MP_EN omp_set_num_threads(MP_PROC_NUM); #pragma omp parallel for #endif //对降采样后的每个特征点进行残差计算 for (int i = 0; i < feats_down_size; i++) { PointType &point_body = feats_down_body->points[i]; //获取降采样后的每个特征点 PointType &point_world = feats_down_world->points[i]; //获取降采样后的每个特征点的世界坐标 /* transform to world frame */ //将点转换至世界坐标系下 V3D p_body(point_body.x, point_body.y, point_body.z); V3D p_global(s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos); //将点转换至世界坐标系下,从而来计算残差 point_world.x = p_global(0); point_world.y = p_global(1); point_world.z = p_global(2); point_world.intensity = point_body.intensity; vector pointSearchSqDis(NUM_MATCH_POINTS); auto &points_near = Nearest_Points[i]; if (ekfom_data.converge) //如果收敛了 { /** Find the closest surfaces in the map **/ //在已构造的地图上查找特征点的最近邻 ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis); //如果最近邻的点数小于NUM_MATCH_POINTS或者最近邻的点到特征点的距离大于5m，则认为该点不是有效点 point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true; } if (!point_selected_surf[i]) //如果该点不是有效点 continue; VF(4) pabcd; //平面点信息 point_selected_surf[i] = false; //将该点设置为无效点，用来计算是否为平面点 //拟合平面方程ax+by+cz+d=0并求解点到平面距离 if (esti_plane(pabcd, points_near, 0.1f)) //找平面点法向量寻找，common_lib.h中的函数 { float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); //计算点到平面的距离 float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); //计算残差 if (s > 0.9) //如果残差大于阈值，则认为该点是有效点 { point_selected_surf[i] = true; //再次回复为有效点 normvec->points[i].x = pabcd(0); //将法向量存储至normvec normvec->points[i].y = pabcd(1); normvec->points[i].z = pabcd(2); normvec->points[i].intensity = pd2; //将点到平面的距离存储至normvec的intensit中 res_last[i] = abs(pd2); //将残差存储至res_last } } } effct_feat_num = 0; //有效特征点数 for (int i = 0; i < feats_down_size; i++) { //根据point_selected_surf状态判断哪些点是可用的 if (point_selected_surf[i]) { // body点存到laserCloudOri中 laserCloudOri->points[effct_feat_num] = feats_down_body->points[i]; //将降采样后的每个特征点存储至laserCloudOri //拟合平面点存到corr_normvect中 corr_normvect->points[effct_feat_num] = normvec->points[i]; total_residual += res_last[i]; //计算总残差 effct_feat_num++; //有效特征点数加1 } } res_mean_last = total_residual / effct_feat_num; //计算残差平均值 match_time += omp_get_wtime() - match_start; //返回从匹配开始时候所经过的时间 double solve_start_ = omp_get_wtime(); //下面是solve求解的时间 // 测量雅可比矩阵H和测量向量的计算 H=J*P*J' ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //测量雅可比矩阵H，论文中的23 ekfom_data.h.resize(effct_feat_num); //测量向量h //求观测值与误差的雅克比矩阵，如论文式14以及式12、13 for (int i = 0; i < effct_feat_num; i++) { // 拿到的有效点的坐标 const PointType &laser_p = laserCloudOri->points[i]; V3D point_this_be(laser_p.x, laser_p.y, laser_p.z); M3D point_be_crossmat; //计算点的反对称矩阵 // 从点值转换到叉乘矩阵 point_be_crossmat << SKEW_SYM_MATRX(point_this_be); // 转换到IMU坐标系下 V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I; // offset_R_L_I，offset_T_L_I为IMU的旋转姿态和位移,此时转到了IMU坐标系下 M3D point_crossmat; point_crossmat << SKEW_SYM_MATRX(point_this); //计算imu中点的反对称矩阵 // 得到对应的曲面/角的法向量 const PointType &norm_p = corr_normvect->points[i]; V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); // 计算测量雅可比矩阵H，见fatlio v1的论文公式(14)，求导这部分和LINS相同：https://zhuanlan.zhihu.com/p/258972164 //FAST-LIO2的特别之处 // 1.在IESKF中，状态更新可以看成是一个优化问题，即对位姿状态先验 x_bk_bk+1 的偏差，以及基于观测模型引入的残差函数f 的优化问题。 // 2.LINS的特别之处在于，将LOAM的后端优化放在了IESKF的更新过程中实现，也就是用IESKF的迭代更新过程代替了LOAM的高斯牛顿法。 V3D C(s.rot.conjugate() * norm_vec); // R^-1 * 法向量, s.rot.conjugate（）是四元数共轭，即旋转求逆 V3D A(point_crossmat * C); // imu坐标系的点坐标的反对称点乘C V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //带be的是激光雷达原始坐标系的点云，不带be的是imu坐标系的点坐标 ekfom_data.h_x.block<1, 12>(i, 0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); // 测量:到最近表面/角落的距离 ekfom_data.h(i) = -norm_p.intensity; //点到面的距离 } solve_time += omp_get_wtime() - solve_start_; //返回从solve开始时候所经过的时间 } // FAST_LIO2主函数 int main(int argc, char **argv) { /*****************************初始化：读取参数、定义变量以及赋值*************************/ // 初始化ros节点，节点名为laserMapping ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; // 从参数服务器读取参数值赋给变量（包括launch文件和launch读取的yaml文件中的参数） nh.param("publish/scan_publish_en", scan_pub_en, true); // 是否发布当前正在扫描的点云的topic nh.param("publish/dense_publish_en", dense_pub_en, true); // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic， nh.param("publish/scan_bodyframe_pub_en", scan_body_pub_en, true); // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，需要该变量和上一个变量同时为true才发布 nh.param("max_iteration", NUM_MAX_ITERATIONS, 4); // 卡尔曼滤波的最大迭代次数 nh.param("map_file_path", map_file_path, ""); // 地图保存路径 nh.param("common/lid_topic", lid_topic, "/livox/lidar"); // 激光雷达点云topic名称 nh.param("common/imu_topic", imu_topic, "/livox/imu"); // IMU的topic名称 nh.param("common/time_sync_en", time_sync_en, false); // 是否需要时间同步，只有当外部未进行时间同步时设为true nh.param("filter_size_corner", filter_size_corner_min, 0.5); // VoxelGrid降采样时的体素大小 nh.param("filter_size_surf", filter_size_surf_min, 0.5); // VoxelGrid降采样时的体素大小 nh.param("filter_size_map", filter_size_map_min, 0.5); // VoxelGrid降采样时的体素大小 nh.param("cube_side_length", cube_len, 200); // 地图的局部区域的长度（FastLio2论文中有解释） nh.param("mapping/det_range", DET_RANGE, 300.f); // 激光雷达的最大探测范围 nh.param("mapping/fov_degree", fov_deg, 180); // 激光雷达的视场角 nh.param("mapping/gyr_cov", gyr_cov, 0.1); // IMU陀螺仪的协方差 nh.param("mapping/acc_cov", acc_cov, 0.1); // IMU加速度计的协方差 nh.param("mapping/b_gyr_cov", b_gyr_cov, 0.0001); // IMU陀螺仪偏置的协方差 nh.param("mapping/b_acc_cov", b_acc_cov, 0.0001); // IMU加速度计偏置的协方差 nh.param("preprocess/blind", p_pre->blind, 0.01); // 最小距离阈值，即过滤掉0～blind范围内的点云 nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); // 激光雷达的类型 nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); // 激光雷达扫描的线数（livox avia为6线） nh.param("point_filter_num", p_pre->point_filter_num, 2); // 采样间隔，即每隔point_filter_num个点取1个点 nh.param("feature_extract_enable", p_pre->feature_enabled, false); // 是否提取特征点（FAST_LIO2默认不进行特征点提取） nh.param("runtime_pos_log_enable", runtime_pos_log, 0); // 是否输出调试log信息 nh.param("pcd_save/pcd_save_en", pcd_save_en, false); // 是否将点云地图保存到PCD文件 nh.param>("mapping/extrinsic_T", extrinT, vector()); // 雷达相对于IMU的外参T（即雷达在IMU坐标系中的坐标） nh.param>("mapping/extrinsic_R", extrinR, vector()); // 雷达相对于IMU的外参R cout << "p_pre->lidar_type " << p_pre->lidar_type << endl; // 初始化path的header（包括时间戳和帧id），path用于保存odemetry的路径 path.header.stamp = ros::Time::now(); path.header.frame_id = "camera_init"; /*** variables definition ***/ /** 变量定义 * effect_feat_num （后面的代码中没有用到该变量） * frame_num 雷达总帧数 * deltaT （后面的代码中没有用到该变量） * deltaR （后面的代码中没有用到该变量） * aver_time_consu 每帧平均的处理总时间 * aver_time_icp 每帧中icp的平均时间 * aver_time_match 每帧中匹配的平均时间 * aver_time_incre 每帧中ikd-tree增量处理的平均时间 * aver_time_solve 每帧中计算的平均时间 * aver_time_const_H_time 每帧中计算的平均时间（当H恒定时） * flg_EKF_converged （后面的代码中没有用到该变量） * EKF_stop_flg （后面的代码中没有用到该变量） * FOV_DEG （后面的代码中没有用到该变量） * HALF_FOV_COS （后面的代码中没有用到该变量） * _featsArray （后面的代码中没有用到该变量） */ int effect_feat_num = 0, frame_num = 0; double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0; bool flg_EKF_converged, EKF_stop_flg = 0; //这里没用到 FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); HALF_FOV_COS = cos((FOV_DEG)*0.5 * PI_M / 180.0); _featsArray.reset(new PointCloudXYZI()); // 将数组point_selected_surf内元素的值全部设为true，数组point_selected_surf用于选择平面点 memset(point_selected_surf, true, sizeof(point_selected_surf)); // 将数组res_last内元素的值全部设置为-1000.0f，数组res_last用于平面拟合中 memset(res_last, -1000.0f, sizeof(res_last)); // VoxelGrid滤波器参数，即进行滤波时的创建的体素边长为filter_size_surf_min downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); // VoxelGrid滤波器参数，即进行滤波时的创建的体素边长为filter_size_map_min downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); // 重复操作 没有必要 memset(point_selected_surf, true, sizeof(point_selected_surf)); memset(res_last, -1000.0f, sizeof(res_last)); // 从雷达到IMU的外参R和T Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT); // 相对IMU的外参 Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR); // 设置IMU的参数，对p_imu进行初始化，其中p_imu为ImuProcess的智能指针（ImuProcess是进行IMU处理的类） p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); double epsi[23] = {0.001}; fill(epsi, epsi + 23, 0.001); //从epsi填充到epsi+22 也就是全部数组置0.001 // 将函数地址传入kf对象中，用于接收特定于系统的模型及其差异 // 作为一个维数变化的特征矩阵进行测量。 // 通过一个函数（h_dyn_share_in）同时计算测量（z）、估计测量（h）、偏微分矩阵（h_x，h_v）和噪声协方差（R）。 kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); /*** debug record ***/ // 将调试log输出到文件中 FILE *fp; string pos_log_dir = root_dir + "/Log/pos_log.txt"; fp = fopen(pos_log_dir.c_str(), "w"); ofstream fout_pre, fout_out, fout_dbg; fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), ios::out); fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), ios::out); fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"), ios::out); if (fout_pre && fout_out) cout << "~~~~" << ROOT_DIR << " file opened" << endl; else cout << "~~~~" << ROOT_DIR << " doesn't exist" << endl; /*** ROS subscribe initialization ***/ // ROS订阅器和发布器的定义和初始化 // 雷达点云的订阅器sub_pcl，订阅点云的topic ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : nh.subscribe(lid_topic, 200000, standard_pcl_cbk); // IMU的订阅器sub_imu，订阅IMU的topic ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); // 发布当前正在扫描的点云，topic名字为/cloud_registered ros::Publisher pubLaserCloudFull = nh.advertise("/cloud_registered", 100000); // 发布经过运动畸变校正到IMU坐标系的点云，topic名字为/cloud_registered_body ros::Publisher pubLaserCloudFull_body = nh.advertise("/cloud_registered_body", 100000); // 发布雷达的点云，topic名字为/cloud_registered_lidar ros::Publisher pubLaserCloudFull_lidar = nh.advertise("/cloud_registered_lidar", 100000); // 后面的代码中没有用到 ros::Publisher pubLaserCloudEffect = nh.advertise("/cloud_effected", 100000); // 后面的代码中没有用到 ros::Publisher pubLaserCloudMap = nh.advertise("/Laser_map", 100000); // 发布当前里程计信息，topic名字为/Odometry ros::Publisher pubOdomAftMapped = nh.advertise("/Odometry", 100000); // 发布里程计总的路径，topic名字为/path ros::Publisher pubPath = nh.advertise("/path", 100000); //------------------------------------------------------------------------------------------------------ // 中断处理函数，如果有中断信号（比如Ctrl+C），则执行第二个参数里面的SigHandle函数 signal(SIGINT, SigHandle); // 设置ROS程序主循环每次运行的时间至少为0.0002秒（5000Hz） ros::Rate rate(5000); bool status = ros::ok(); // 程序主循环 while (status) { // 如果有中断产生，则结束主循环 if (flg_exit) break; // ROS消息回调处理函数，放在ROS的主循环中 ros::spinOnce(); // 将激光雷达点云数据和IMU数据从缓存队列中取出，进行时间对齐，并保存到Measures中 if (sync_packages(Measures)) { // 激光雷达第一次扫描 if (flg_reset) { ROS_WARN("reset when rosbag play back"); p_imu->Reset(); flg_reset = false; Measures.imu.clear(); //判断状态，并把imu的数据清空 continue; } double t0, t1, t2, t3, t4, t5, match_start, solve_start, svd_time; match_time = 0; kdtree_search_time = 0.0; solve_time = 0; solve_const_H_time = 0; svd_time = 0; t0 = omp_get_wtime(); // 对IMU数据进行预处理，其中包含了点云畸变处理 前向传播 反向传播 p_imu->Process(Measures, kf, feats_undistort); // 获取kf预测的全局状态（imu） state_point = kf.get_x(); //世界系下雷达坐标系的位置 //下面式子的意义是W^p_L = W^p_I + W^R_I * I^t_L pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; if (feats_undistort->empty() || (feats_undistort == NULL)) //如果点云数据为空，则代表了激光雷达没有完成去畸变，此时还不能初始化成功 { first_lidar_time = Measures.lidar_beg_time; //记录第一次扫描的时间 p_imu->first_lidar_time = first_lidar_time; //将第一帧的时间传给imu作为当前帧的第一个点云时间 // cout<<"FAST-LIO not ready"<points.size(); //记录滤波后的点云数量 /*** initialize the map kdtree ***/ // 构建kd树 if (ikdtree.Root_Node == nullptr) { if (feats_down_size > 5) { // 设置ikd tree的降采样参数 ikdtree.set_downsample_param(filter_size_map_min); feats_down_world->resize(feats_down_size); //将下采样得到的地图点大小于body系大小一致 for (int i = 0; i < feats_down_size; i++) { pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); //将下采样得到的地图点转换为世界坐标系下的点云 } // 组织ikd tree ikdtree.Build(feats_down_world->points); //构建ikd树 } continue; } // 获取ikd tree中的有效节点数，无效点就是被打了deleted标签的点 int featsFromMapNum = ikdtree.validnum(); // 获取Ikd tree中的节点数 kdtree_size_st = ikdtree.size(); // cout<<"[ mapping ]: In num: "<points.size()<<" downsamp "<resize(feats_down_size); feats_down_world->resize(feats_down_size); // 外参，旋转矩阵转欧拉角 V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); fout_pre << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose() << " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << endl; //输出预测的结果 if (0) // If you need to see map point, change to "if(1)" { // 释放PCL_Storage的内存 PointVector().swap(ikdtree.PCL_Storage); // 把树展平用于展示 ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); featsFromMap->clear(); featsFromMap->points = ikdtree.PCL_Storage; } pointSearchInd_surf.resize(feats_down_size); //搜索索引 Nearest_Points.resize(feats_down_size); //将降采样处理后的点云用于搜索最近点 int rematch_num = 0; bool nearest_search_en = true; // t2 = omp_get_wtime(); /*** 迭代状态估计 ***/ double t_update_start = omp_get_wtime(); double solve_H_time = 0; //迭代卡尔曼滤波更新，更新地图信息 kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); state_point = kf.get_x(); euler_cur = SO3ToEuler(state_point.rot); pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; geoQuat.x = state_point.rot.coeffs()[0]; geoQuat.y = state_point.rot.coeffs()[1]; geoQuat.z = state_point.rot.coeffs()[2]; geoQuat.w = state_point.rot.coeffs()[3]; double t_update_end = omp_get_wtime(); /******* 发布里程计 *******/ publish_odometry(pubOdomAftMapped); /*** 向映射kdtree添加特性点 ***/ t3 = omp_get_wtime(); map_incremental(); t5 = omp_get_wtime(); /******* 发布轨迹和点 *******/ publish_path(pubPath); if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); if (scan_pub_en && scan_body_pub_en) { publish_frame_body(pubLaserCloudFull_body); publish_frame_lidar(pubLaserCloudFull_lidar); } // publish_effect_world(pubLaserCloudEffect); // publish_map(pubLaserCloudMap); /*** Debug 参数 ***/ if (runtime_pos_log) { frame_num++; kdtree_size_end = ikdtree.size(); aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t_update_end - t_update_start) / frame_num; aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num; aver_time_incre = aver_time_incre * (frame_num - 1) / frame_num + (kdtree_incremental_time) / frame_num; aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time + solve_H_time) / frame_num; aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_time / frame_num; T1[time_log_counter] = Measures.lidar_beg_time; s_plot[time_log_counter] = t5 - t0; //整个流程总时间 s_plot2[time_log_counter] = feats_undistort->points.size(); //特征点数量 s_plot3[time_log_counter] = kdtree_incremental_time; // kdtree增量时间 s_plot4[time_log_counter] = kdtree_search_time; // kdtree搜索耗时 s_plot5[time_log_counter] = kdtree_delete_counter; // kdtree删除点数量 s_plot6[time_log_counter] = kdtree_delete_time; // kdtree删除耗时 s_plot7[time_log_counter] = kdtree_size_st; // kdtree初始大小 s_plot8[time_log_counter] = kdtree_size_end; // kdtree结束大小 s_plot9[time_log_counter] = aver_time_consu; //平均消耗时间 s_plot10[time_log_counter] = add_point_size; //添加点数量 time_log_counter++; printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_const_H_time); ext_euler = SO3ToEuler(state_point.offset_R_L_I); fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose() << " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << " " << feats_undistort->points.size() << endl; dump_lio_state_to_log(fp); } } status = ros::ok(); rate.sleep(); } /**************** save map ****************/ /* 1. make sure you have enough memories /* 2. pcd save will largely influence the real-time performences **/ if (pcl_wait_save->size() > 0 && pcd_save_en) { string file_name = string("scans.pcd"); string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); pcl::PCDWriter pcd_writer; cout << "current scan saved to /PCD/" << file_name << endl; pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); } fout_out.close(); fout_pre.close(); if (runtime_pos_log) { vector t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; FILE *fp2; string log_dir = root_dir + "/Log/fast_lio_time_log.csv"; fp2 = fopen(log_dir.c_str(), "w"); fprintf(fp2, "time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n"); for (int i = 0; i < time_log_counter; i++) { fprintf(fp2, "%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n", T1[i], s_plot[i], int(s_plot2[i]), s_plot3[i], s_plot4[i], int(s_plot5[i]), s_plot6[i], int(s_plot7[i]), int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]); t.push_back(T1[i]); s_vec.push_back(s_plot9[i]); s_vec2.push_back(s_plot3[i] + s_plot6[i]); s_vec3.push_back(s_plot4[i]); s_vec5.push_back(s_plot[i]); } fclose(fp2); } return 0; } ================================================ FILE: FAST-LIO/src/preprocess.cpp ================================================ #include "preprocess.h" #define RETURN0 0x00 #define RETURN0AND1 0x10 Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1) { inf_bound = 10; // 有效点集合,大于10m则是盲区 N_SCANS = 6; //多线激光雷达的线数 group_size = 8; // 8个点为一组 disA = 0.01; // 点集合的距离阈值,判断是否为平面 disB = 0.1; // 点集合的距离阈值,判断是否为平面 p2l_ratio = 225; // 点到线的距离阈值，需要大于这个值才能判断组成面 limit_maxmid = 6.25; // 中点到左侧的距离变化率范围 limit_midmin = 6.25; // 中点到右侧的距离变化率范围 limit_maxmin = 3.24; // 左侧到右侧的距离变化率范围 jump_up_limit = 170.0; jump_down_limit = 8.0; cos160 = 160.0; edgea = 2; //点与点距离超过两倍则认为遮挡 edgeb = 0.1; //点与点距离超过0.1m则认为遮挡 smallp_intersect = 172.5; smallp_ratio = 1.2; //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面 given_offset_time = false; //是否提供时间偏移 jump_up_limit = cos(jump_up_limit / 180 * M_PI); //角度大于170度的点跳过，认为在 jump_down_limit = cos(jump_down_limit / 180 * M_PI); //角度小于8度的点跳过 cos160 = cos(cos160 / 180 * M_PI); //夹角限制 smallp_intersect = cos(smallp_intersect / 180 * M_PI); //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面 } Preprocess::~Preprocess() {} void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) { feature_enabled = feat_en; //是否提取特征点 lidar_type = lid_type; //雷达的种类 blind = bld; //最小距离阈值，即过滤掉0～blind范围内的点云 point_filter_num = pfilt_num; //采样间隔，即每隔point_filter_num个点取1个点 } /** * @brief Livox激光雷达点云预处理函数 * * @param msg livox激光雷达点云数据，格式为livox_ros_driver::CustomMsg * @param pcl_out 输出处理后的点云数据，格式为pcl::PointCloud */ void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { avia_handler(msg); *pcl_out = pl_surf; } void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { switch (lidar_type) { case OUST64: oust64_handler(msg); break; case VELO16: velodyne_handler(msg); break; default: printf("Error LiDAR Type"); break; } *pcl_out = pl_surf; } /** * @brief 对Livox激光雷达点云数据进行预处理 * * @param msg livox激光雷达点云数据，格式为livox_ros_driver::CustomMsg */ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) { // 清除之前的点云缓存 pl_surf.clear(); // 清除之前的平面点云缓存 pl_corn.clear(); // 清除之前的角点云缓存 pl_full.clear(); // 清除之前的全点云缓存 double t1 = omp_get_wtime(); // 后面没用到 int plsize = msg->point_num; // 一帧中的点云总个数 // cout<<"plsie: "<points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { pl_full[i].x = msg->points[i].x; // 点云x轴坐标 pl_full[i].y = msg->points[i].y; // 点云y轴坐标 pl_full[i].z = msg->points[i].z; // 点云z轴坐标 pl_full[i].intensity = msg->points[i].reflectivity; // 点云强度 pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // 使用曲率作为每个激光点的时间 bool is_new = false; // 只有当当前点和上一点的间距足够大（>1e-7），才将当前点认为是有用的点，分别加入到对应line的pl_buff队列中 if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) { pl_buff[msg->points[i].line].push_back(pl_full[i]); // 将当前点加入到对应line的pl_buff队列中 } } } static int count = 0; static double time = 0.0; count++; double t0 = omp_get_wtime(); // 对每个line中的激光雷达分别进行处理 for (int j = 0; j < N_SCANS; j++) { // 如果该line中的点云过小，则继续处理下一条line if (pl_buff[j].size() <= 5) continue; pcl::PointCloud &pl = pl_buff[j]; plsize = pl.size(); vector &types = typess[j]; types.clear(); types.resize(plsize); plsize--; for (uint i = 0; i < plsize; i++) { types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; // 计算每个点到机器人本身的距离 vx = pl[i].x - pl[i + 1].x; vy = pl[i].y - pl[i + 1].y; vz = pl[i].z - pl[i + 1].z; types[i].dista = vx * vx + vy * vy + vz * vz; // 计算两个间隔点的距离 } //因为i最后一个点没有i+1了所以就单独求了一个range，没有distance types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y; give_feature(pl, types); //给特征 // pl_surf += pl; } time += omp_get_wtime() - t0; printf("Feature extraction time: %lf \n", time / count); } else { // 分别对每个点云进行处理 for (uint i = 1; i < plsize; i++) { // 只取线数在0~N_SCANS内并且回波次序为0或者1的点云 if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { valid_num++; // 有效的点云数 // 等间隔降采样 if (valid_num % point_filter_num == 0) { pl_full[i].x = msg->points[i].x; // 点云x轴坐标 pl_full[i].y = msg->points[i].y; // 点云y轴坐标 pl_full[i].z = msg->points[i].z; // 点云z轴坐标 pl_full[i].intensity = msg->points[i].reflectivity; // 点云强度 pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points // 只有当当前点和上一点的间距足够大（>1e-7），并且在最小距离阈值之外，才将当前点认为是有用的点，加入到pl_surf队列中 if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7) && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind)) { pl_surf.push_back(pl_full[i]); } } } } } } void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.size(); pl_corn.reserve(plsize); pl_surf.reserve(plsize); if (feature_enabled) { for (int i = 0; i < N_SCANS; i++) { pl_buff[i].clear(); pl_buff[i].reserve(plsize); } for (uint i = 0; i < plsize; i++) { double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < blind) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; //这里好像没有用 因为atan2()返回在-pi到pi之间 if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].t / 1e6; if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } } for (int j = 0; j < N_SCANS; j++) { PointCloudXYZI &pl = pl_buff[j]; int linesize = pl.size(); vector &types = typess[j]; types.clear(); types.resize(linesize); linesize--; for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); vx = pl[i].x - pl[i + 1].x; vy = pl[i].y - pl[i + 1].y; vz = pl[i].z - pl[i + 1].z; types[i].dista = vx * vx + vy * vy + vz * vz; } types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); give_feature(pl, types); } } else { double time_stamp = msg->header.stamp.toSec(); // cout << "===================================" << endl; // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); for (int i = 0; i < pl_orig.points.size(); i++) { if (i % point_filter_num != 0) continue; double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < blind) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; // Normal结构体表示给定点所在样本曲面上的法线方向,以及对应曲率的测量值 added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].t / 1e6; pl_surf.points.push_back(added_pt); } } // pub_func(pl_surf, pub_full, msg->header.stamp); // pub_func(pl_surf, pub_corn, msg->header.stamp); } #define MAX_LINE_NUM 64 void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point double omega_l = 3.61; // scan angular velocity float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time if (pl_orig.points[plsize - 1].time > 0) //假如提供了每个点的时间戳 { given_offset_time = true; //提供时间偏移 } else //没有提供每个点的时间戳 { given_offset_time = false; memset(is_first, true, sizeof(is_first)); double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_end = yaw_first; int layer_first = pl_orig.points[0].ring; for (uint i = plsize - 1; i > 0; i--) { if (pl_orig.points[i].ring == layer_first) { yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; //寻找终止yaw角 break; } } } if (feature_enabled) { for (int i = 0; i < N_SCANS; i++) { pl_buff[i].clear(); pl_buff[i].reserve(plsize); } for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; int layer = pl_orig.points[i].ring; if (layer >= N_SCANS) continue; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms if (!given_offset_time) //假如没有提供时间 { double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) //给每条线第一个赋予yaw角 时间归0 { // printf("layer: %d; is first: %d", layer, is_first[layer]); yaw_fp[layer] = yaw_angle; is_first[layer] = false; added_pt.curvature = 0.0; yaw_last[layer] = yaw_angle; //对每一条线最后一个点的时间赋值 time_last[layer] = added_pt.curvature; //对每一条线最后一个点的时间赋值 continue; } //对每个点计算时间 if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } //新得到的点的时间不能早于之前的最后一个点的时间 if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } pl_buff[layer].points.push_back(added_pt); } for (int j = 0; j < N_SCANS; j++) { PointCloudXYZI &pl = pl_buff[j]; int linesize = pl.size(); if (linesize < 2) continue; //点太少跳过 vector &types = typess[j]; types.clear(); types.resize(linesize); linesize--; for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); vx = pl[i].x - pl[i + 1].x; vy = pl[i].y - pl[i + 1].y; vz = pl[i].z - pl[i + 1].z; types[i].dista = vx * vx + vy * vy + vz * vz; } types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); give_feature(pl, types); } } else { for (int i = 0; i < plsize; i++) { PointType added_pt; // cout<<"!!!!!!"< blind) { pl_surf.points.push_back(added_pt); // printf("time mode: %d time: %d \n", given_offset_time, pl_orig.points[i].t); } } } } // pub_func(pl_surf, pub_full, msg->header.stamp); // pub_func(pl_surf, pub_surf, msg->header.stamp); // pub_func(pl_surf, pub_corn, msg->header.stamp); } /** * @brief 对于每条line的点云提取特征 * * @param pl pcl格式的点云 输入进来一条扫描线上的点 * @param types 点云的其他属性 */ void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) { int plsize = pl.size(); //单条线的点数 int plsize2; if (plsize == 0) { printf("something wrong\n"); return; } uint head = 0; //不能在盲区 从这条线非盲区的点开始 while (types[head].range < blind) { head++; } // Surf // group_size默认等于8 plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; //判断当前点后面是否还有8个点 够的话就逐渐减少 Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); //当前平面的法向量 Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); //上一个平面的法向量 uint i_nex = 0, i2; // i2为当前点的下一个点 uint last_i = 0; // last_i为上一个点的保存的索引 uint last_i_nex = 0; // last_i_nex为上一个点的下一个点的索引 int last_state = 0; //为1代表上个状态为平面 否则为0 //判断面点 int plane_type; // 拿到8个点用于判断平面 for (uint i = head; i < plsize2; i++) { if (types[i].range < blind) // 在盲区范围内的点不做处理 { continue; } i2 = i; //更新i2 plane_type = plane_judge(pl, types, i, i_nex, curr_direct); //求得平面，并返回类型0 1 2 if (plane_type == 1) //返回1一般默认是平面 { //设置确定的平面点和可能的平面点 for (uint j = i; j <= i_nex; j++) { if (j != i && j != i_nex) { //把起始点和终止点之间的所有点设置为确定的平面点 types[j].ftype = Real_Plane; } else { //把起始点和终止点设置为可能的平面点 types[j].ftype = Poss_Plane; } } // if(last_state==1 && fabs(last_direct.sum())>0.5) //最开始last_state=0直接跳过 //之后last_state=1 //如果之前状态是平面则判断当前点是处于两平面边缘的点还是较为平坦的平面的点 if (last_state == 1 && last_direct.norm() > 0.1) { double mod = last_direct.transpose() * curr_direct; if (mod > -0.707 && mod < 0.707) { //修改ftype，两个面法向量夹角在45度和135度之间 认为是两平面边缘上的点 types[i].ftype = Edge_Plane; } else { //否则认为是真正的平面点 types[i].ftype = Real_Plane; } } i = i_nex - 1; last_state = 1; } else // if(plane_type == 2) { // plane_type=0或2的时候 i = i_nex; last_state = 0; //设置为不是平面点 } // else if(plane_type == 0) // { // if(last_state == 1) // { // uint i_nex_tem; // uint j; // for(j=last_i+1; j<=last_i_nex; j++) // { // uint i_nex_tem2 = i_nex_tem; // Eigen::Vector3d curr_direct2; // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); // if(ttem != 1) // { // i_nex_tem = i_nex_tem2; // break; // } // curr_direct = curr_direct2; // } // if(j == last_i+1) // { // last_state = 0; // } // else // { // for(uint k=last_i_nex; k<=i_nex_tem; k++) // { // if(k != i_nex_tem) // { // types[k].ftype = Real_Plane; // } // else // { // types[k].ftype = Poss_Plane; // } // } // i = i_nex_tem-1; // i_nex = i_nex_tem; // i2 = j-1; // last_state = 1; // } // } // } last_i = i2; //更新last_i last_i_nex = i_nex; //更新last_i_nex last_direct = curr_direct; //更新last_direct } //判断边缘点 plsize2 = plsize > 3 ? plsize - 3 : 0; //如果剩下的点数小于3则不判断边缘点，否则计算哪些点是边缘点 for (uint i = head + 3; i < plsize2; i++) { //点不能在盲区 或者 点必须属于正常点和可能的平面点 if (types[i].range < blind || types[i].ftype >= Real_Plane) { continue; } //该点与前后点的距离不能挨的太近 if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { continue; } Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); //当前点组成的向量 Eigen::Vector3d vecs[2]; for (int j = 0; j < 2; j++) { int m = -1; if (j == 1) { m = 1; } //若当前的前/后一个点在盲区内（4m) if (types[i + m].range < blind) { if (types[i].range > inf_bound) //若其大于10m { types[i].edj[j] = Nr_inf; //赋予该点Nr_inf(跳变较远) } else { types[i].edj[j] = Nr_blind; //赋予该点Nr_blind(在盲区) } continue; } vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z); vecs[j] = vecs[j] - vec_a; //前/后点指向当前点的向量 //若雷达坐标系原点为O 当前点为A 前/后一点为M和N //则下面OA点乘MA/（|OA|*|MA|） //得到的是cos角OAM的大小 types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); //角MAN的cos值 //前一个点是正常点 && 下一个点在激光线上 && 当前点与后一个点的距离大于0.0225m && 当前点与后一个点的距离大于当前点与前一个点距离的四倍 //这种边缘点像是7字形这种的边缘？ if (types[i].angle[j] < jump_up_limit) // cos(170) { types[i].edj[j] = Nr_180; // M在OA延长线上 } else if (types[i].angle[j] > jump_down_limit) // cos(8) { types[i].edj[j] = Nr_zero; // M在OA上 } } types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista) { if (types[i].intersect > cos160) //角MAN要小于160度 不然就平行于激光了 { if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } } } //与上面类似 //前一个点在激光束上 && 后一个点正常 && 前一个点与当前点的距离大于0.0225m && 前一个点与当前点的距离大于当前点与后一个点距离的四倍 else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista) { if (types[i].intersect > cos160) { if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } } } //前面的是正常点 && (当前点到中心距离>10m并且后点在盲区) else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) { if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } } //(当前点到中心距离>10m并且前点在盲区) && 后面的是正常点 else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) { if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } } //前后点都不是正常点 else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) { if (types[i].ftype == Nor) { types[i].ftype = Wire; //程序中应该没使用 当成空间中的小线段或者无用点了 } } } plsize2 = plsize - 1; double ratio; //继续找平面点 for (uint i = head + 1; i < plsize2; i++) { //前面、当前、之后三个点都需要不在盲区内 if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) { continue; } //前面和当前 当前和之后的点与点之间距离都不能太近 if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { continue; } //还剩下来一些正常点继续找平面点 if (types[i].ftype == Nor) { //求点与点间距的比例 大间距/小间距 if (types[i - 1].dista > types[i].dista) { ratio = types[i - 1].dista / types[i].dista; } else { ratio = types[i].dista / types[i - 1].dista; } //如果夹角大于172.5度 && 间距比例<1.2 if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) { //前后三个点认为是平面点 if (types[i - 1].ftype == Nor) { types[i - 1].ftype = Real_Plane; } if (types[i + 1].ftype == Nor) { types[i + 1].ftype = Real_Plane; } types[i].ftype = Real_Plane; } } } //存储平面点 int last_surface = -1; for (uint j = head; j < plsize; j++) { //可能的平面点和确定的平面点 if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) { if (last_surface == -1) { last_surface = j; } //通常连着好几个都是面点 //必须在采样间隔上的平面点才使用（这里的是无差别滤波 从每次新找到面点开始每几个点才取一个） if (j == uint(last_surface + point_filter_num - 1)) { PointType ap; ap.x = pl[j].x; ap.y = pl[j].y; ap.z = pl[j].z; ap.intensity = pl[j].intensity; ap.curvature = pl[j].curvature; pl_surf.push_back(ap); last_surface = -1; } } else { //跳变较大的边缘边的点 位于平面边缘的点 if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) { pl_corn.push_back(pl[j]); } //假如上次找到的面点被无差别滤波掉了，而此时已经到了边缘 if (last_surface != -1) { PointType ap; //取上次面点到此次边缘线之间的所有点的重心当作一个面点存储进去 for (uint k = last_surface; k < j; k++) { ap.x += pl[k].x; ap.y += pl[k].y; ap.z += pl[k].z; ap.intensity += pl[k].intensity; ap.curvature += pl[k].curvature; } ap.x /= (j - last_surface); ap.y /= (j - last_surface); ap.z /= (j - last_surface); ap.intensity /= (j - last_surface); ap.curvature /= (j - last_surface); pl_surf.push_back(ap); } last_surface = -1; } } } // 发布topic指令，该函数暂时没用到 void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) { pl.height = 1; pl.width = pl.size(); sensor_msgs::PointCloud2 output; pcl::toROSMsg(pl, output); output.header.frame_id = "livox"; output.header.stamp = ct; } //平面判断 int Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) { double group_dis = disA * types[i_cur].range + disB; // 0.01*sqrt(x^2+y^2)+0.1 基本上可以近似看成是0.1 100m的时候才到0.2 group_dis = group_dis * group_dis; // i_nex = i_cur; double two_dis; vector disarr; //前后点距离数组 disarr.reserve(20); //距离小 点与点之间较近 先取够8个点 for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) { if (types[i_nex].range < blind) { curr_direct.setZero(); //距离雷达原点太小了将法向量设置为零向量 return 2; } disarr.push_back(types[i_nex].dista); //存储当前点与后一个点的距离 } //看看后续的点有没有满足条件的 for (;;) { if ((i_cur >= pl.size()) || (i_nex >= pl.size())) //索引超出所有点的个数直接BREAK break; if (types[i_nex].range < blind) { curr_direct.setZero(); //距离雷达原点太小了将法向量设置为零向量 return 2; } //最后的i_nex点到i_cur点的距离 vx = pl[i_nex].x - pl[i_cur].x; vy = pl[i_nex].y - pl[i_cur].y; vz = pl[i_nex].z - pl[i_cur].z; two_dis = vx * vx + vy * vy + vz * vz; if (two_dis >= group_dis) //距离i_cur点太远了就直接break { break; } disarr.push_back(types[i_nex].dista); //存储当前点与后一个点的距离 i_nex++; // i_nex点加一 } double leng_wid = 0; double v1[3], v2[3]; for (uint j = i_cur + 1; j < i_nex; j++) { if ((j >= pl.size()) || (i_cur >= pl.size())) break; //假设i_cur点为A j点为B i_nex点为C //向量AB v1[0] = pl[j].x - pl[i_cur].x; v1[1] = pl[j].y - pl[i_cur].y; v1[2] = pl[j].z - pl[i_cur].z; //向量AB叉乘向量AC v2[0] = v1[1] * vz - vy * v1[2]; v2[1] = v1[2] * vx - v1[0] * vz; v2[2] = v1[0] * vy - vx * v1[1]; //物理意义是组成的ABC组成的平行四边形面积的平方(为|AC|*h，其中为B到线AC的距离) double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; if (lw > leng_wid) { leng_wid = lw; //寻找最大面积的平方(也就是寻找距离AC最远的B) } } //|AC|*|AC|/(|AC|*|AC|*h*h)<225 //也就是h>1/15 B点到AC的距离要大于0.06667m //太近了不好拟合一个平面 if ((two_dis * two_dis / leng_wid) < p2l_ratio) { curr_direct.setZero(); //太近了法向量直接设置为0 return 0; } //把两点之间的距离 按从大到小排个顺序 uint disarrsize = disarr.size(); for (uint j = 0; j < disarrsize - 1; j++) { for (uint k = j + 1; k < disarrsize; k++) { if (disarr[j] < disarr[k]) { leng_wid = disarr[j]; disarr[j] = disarr[k]; disarr[k] = leng_wid; } } } //这里可能最近的点还是太近了 if (disarr[disarr.size() - 2] < 1e-16) { curr_direct.setZero(); return 0; } //目前还不太懂为什么给AVIA单独弄了一种，其实我觉得全都可以采用上面这种判定方式（虽然FAST-LIO默认不开特征提取） if (lidar_type == AVIA) { //点与点之间距离变化太大的时候 可能与激光束是平行的 就也舍弃了 double dismax_mid = disarr[0] / disarr[disarrsize / 2]; double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) { curr_direct.setZero(); return 0; } } else { double dismax_min = disarr[0] / disarr[disarrsize - 2]; if (dismax_min >= limit_maxmin) { curr_direct.setZero(); return 0; } } curr_direct << vx, vy, vz; curr_direct.normalize(); //法向量归一化 return 1; } //边缘判断 bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) { if (nor_dir == 0) { if (types[i - 1].range < blind || types[i - 2].range < blind) //前两个点不能在盲区 { return false; } } else if (nor_dir == 1) { if (types[i + 1].range < blind || types[i + 2].range < blind) //后两个点不能在盲区 { return false; } } //下面分别对i-2 i-1和i i+1两种情况时点与点间距进行了判断 double d1 = types[i + nor_dir - 1].dista; double d2 = types[i + 3 * nor_dir - 2].dista; double d; //将大小间距进行调换 大在前 小在后 if (d1 < d2) { d = d1; d1 = d2; d2 = d; } d1 = sqrt(d1); d2 = sqrt(d2); if (d1 > edgea * d2 || (d1 - d2) > edgeb) { //假如间距太大 可能是被遮挡，就不把它当作边缘点 return false; } return true; } ================================================ FILE: FAST-LIO/src/preprocess.h ================================================ #include #include #include #include using namespace std; #define IS_VALID(a) ((abs(a) > 1e8) ? true : false) typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; // 枚举类型：表示支持的雷达类型 enum LID_TYPE { AVIA = 1, VELO16, OUST64 }; //{1, 2, 3} // 枚举类型：表示特征点的类型 enum Feature { Nor, // 正常点 Poss_Plane, // 可能的平面点 Real_Plane, // 确定的平面点 Edge_Jump, // 有跨越的边 Edge_Plane, // 边上的平面点 Wire, // 线段 这个也许当了无效点？也就是空间中的小线段？ ZeroPoint // 无效点 程序中未使用 }; // 枚举类型：位置标识 enum Surround { Prev, // 前一个 Next // 后一个 }; // 枚举类型：表示有跨越边的类型 enum E_jump { Nr_nor, // 正常 Nr_zero, // 0 Nr_180, // 180 Nr_inf, // 无穷大 跳变较远？ Nr_blind // 在盲区？ }; // orgtype类：用于存储激光雷达点的一些其他属性 struct orgtype { double range; // 点云在xy平面离雷达中心的距离 double dista; // 当前点与后一个点之间的距离 //假设雷达原点为O 前一个点为M 当前点为A 后一个点为N double angle[2]; // 这个是角OAM和角OAN的cos值 double intersect; // 这个是角MAN的cos值 E_jump edj[2]; // 前后两点的类型 Feature ftype; // 点类型 // 构造函数 orgtype() { range = 0; edj[Prev] = Nr_nor; edj[Next] = Nr_nor; ftype = Nor; //默认为正常点 intersect = 2; } }; // velodyne数据结构 namespace velodyne_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; // 4D点坐标类型 float intensity; // 强度 float time; // 时间 uint16_t ring; // 点所属的圈数 EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐 }; } // namespace velodyne_ros // 注册velodyne_ros的Point类型 POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(uint16_t, ring, ring)) // ouster数据结构 namespace ouster_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; // 4D点坐标类型 float intensity; // 强度 uint32_t t; // 时间 uint16_t reflectivity; // 反射率 uint8_t ring; // 点所属的圈数 uint16_t ambient; // 没用到 uint32_t range; // 距离 EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐 }; } // namespace ouster_ros // clang-format off // 注册ouster的Point类型 POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) (std::uint8_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) // Preproscess类：用于对激光雷达点云数据进行预处理 class Preprocess { public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW Preprocess(); // 构造函数 ~Preprocess(); // 析构函数 void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对Livox自定义Msg格式的激光雷达数据进行处理 void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对ros的Msg格式的激光雷达数据进行处理 void set(bool feat_en, int lid_type, double bld, int pfilt_num); // sensor_msgs::PointCloud2::ConstPtr pointcloud; PointCloudXYZI pl_full, pl_corn, pl_surf; // 全部点、边缘点、平面点 PointCloudXYZI pl_buff[128]; //maximum 128 line lidar vector typess[128]; //maximum 128 line lidar int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; // 雷达类型、采样间隔、扫描线数、扫描频率 double blind; // 最小距离阈值(盲区) bool feature_enabled, given_offset_time; // 是否提取特征、是否进行时间偏移 ros::Publisher pub_full, pub_surf, pub_corn; // 发布全部点、发布平面点、发布边缘点 private: void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); // 用于对Livox激光雷达数据进行处理 void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对ouster激光雷达数据进行处理 void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对velodyne激光雷达数据进行处理 void give_feature(PointCloudXYZI &pl, vector &types); void pub_func(PointCloudXYZI &pl, const ros::Time &ct); int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); // 没有用到 bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); int group_size; double disA, disB, inf_bound; double limit_maxmid, limit_midmin, limit_maxmin; double p2l_ratio; double jump_up_limit, jump_down_limit; double cos160; double edgea, edgeb; double smallp_intersect, smallp_ratio; double vx, vy, vz; }; ================================================ FILE: README.md ================================================ # FAST_LIO2_Noted ## 中文注释说明 本项目是基于[FAST_LIO_SLAM](https://github.com/GDUT-Kyle/FAST_LIO_SLAM)项目扩展来的，主要讲述了Fast-LIO2主程序代码、esikf更新代码和ikd-tree算法，因此基于自己的理解和思考，在代码里做了相关的中文注释。注释部分难免有误，欢迎大家批评指正。 ## News - ``Aug 2021``: The Livox-lidar tests and corresponding launch files will be uploaded soon. Currenty only Ouster lidar tutorial videos had been made. ## What is FAST_LIO_SLAM? Integration of 1. [FAST-LIO2](https://github.com/hku-mars/FAST_LIO) (Odometry): A computationally efficient and robust LiDAR-inertial odometry (LIO) package 2. [SC-PGO](https://github.com/gisbi-kim/SC-A-LOAM) (Loop detection and Pose-graph Optimization): [Scan Context](https://github.com/irapkaist/scancontext)-based Loop detection and GTSAM-based Pose-graph optimization ## Features - An easy-to-use plug-and-play LiDAR SLAM - FAST-LIO2 and SC-PGO run separately (see below How to use? tab). - SC-PGO takes odometry and lidar point cloud topics from the FAST-LIO2 node. - Finally, an optimized map is made within the SC-PGO node. ## How to use? - The below commands and the launch files are made for playing the [MulRan dataset](https://sites.google.com/view/mulran-pr/home), but applicable for livox lidars in the same way (you could easily make your own launch files). ``` # terminal 1: run FAST-LIO2 mkdir -p ~/catkin_fastlio_slam/src cd ~/catkin_fastlio_slam/src git clone https://github.com/gisbi-kim/FAST_LIO_SLAM.git git clone https://github.com/Livox-SDK/livox_ros_driver cd .. catkin_make source devel/setup.bash roslaunch fast_lio mapping_ouster64_mulran.launch # setting for MulRan dataset # open the other terminal tab: run SC-PGO cd ~/catkin_fastlio_slam source devel/setup.bash roslaunch aloam_velodyne fastlio_ouster64.launch # setting for MulRan dataset # open the other terminal tab # run file_player_mulran (for the details, refer here https://github.com/irapkaist/file_player_mulran) ``` ## Utility - We support keyframe scan saver (as in .pcd) and provide a script reconstructs a point cloud map by merging the saved scans using the optimized poses. See [here](https://github.com/gisbi-kim/FAST_LIO_SLAM/blob/bf975560741c425f71811c864af5d35aa880c797/SC-PGO/utils/python/makeMergedMap.py#L7). ## Example results - [Tutorial video 1](https://youtu.be/nu8j4yaBMnw) (using KAIST 03 sequence of [MulRan dataset](https://sites.google.com/view/mulran-pr/dataset)) - Example result captures

- [download the KAIST03 pcd map](https://www.dropbox.com/s/w599ozdg7h6215q/KAIST03.pcd?dl=0) made by FAST-LIO-SLAM, 500MB - [Example Video 2](https://youtu.be/94mC05PesvQ) (Riverside 02 sequence of [MulRan dataset](https://sites.google.com/view/mulran-pr/dataset)) - Example result captures

- [download the Riverisde02 pcd map](https://www.dropbox.com/s/1aolth7ry4odxo4/Riverside02.pcd?dl=0) made by FAST-LIO-SLAM, 400MB ## Acknowledgements - Thanks for [FAST_LIO](https://github.com/hku-mars/FAST_LIO) authors. ================================================ FILE: SC-PGO/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(aloam_velodyne) set(CMAKE_BUILD_TYPE "Release") # set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs sensor_msgs roscpp rospy rosbag std_msgs image_transport cv_bridge tf ) #find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) find_package(OpenCV REQUIRED) find_package(Ceres REQUIRED) find_package(OpenMP REQUIRED) find_package(GTSAM REQUIRED QUIET) include_directories( include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} ) catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs DEPENDS EIGEN3 PCL INCLUDE_DIRS include ) add_executable(ascanRegistration src/scanRegistration.cpp) target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(alaserOdometry src/laserOdometry.cpp) target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) add_executable(alaserMapping src/laserMapping.cpp) target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) add_executable(alaserPGO src/laserPosegraphOptimization.cpp include/scancontext/Scancontext.cpp ) target_compile_options(alaserPGO PRIVATE ${OpenMP_CXX_FLAGS} ) target_link_libraries(alaserPGO ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam ) add_executable(kittiHelper src/kittiHelper.cpp) target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) ================================================ FILE: SC-PGO/README.md ================================================ # SC-A-LOAM ## What is SC-A-LOAM? - A real-time LiDAR SLAM package that integrates A-LOAM and ScanContext. - **A-LOAM** for odometry (i.e., consecutive motion estimation) - **ScanContext** for coarse global localization that can deal with big drifts (i.e., place recognition as kidnapped robot problem without initial pose) - and iSAM2 of GTSAM is used for pose-graph optimization. - This package aims to show ScanContext's handy applicability. - The only things a user should do is just to include `Scancontext.h`, call `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`. ## Features 1. A strong place recognition and loop closing - We integrated ScanContext as a loop detector into A-LOAM, and ISAM2-based pose-graph optimization is followed. (see https://youtu.be/okML_zNadhY?t=313 to enjoy the drift-closing moment) 2. A modular implementation - The only difference from A-LOAM is the addition of the `laserPosegraphOptimization.cpp` file. In the new file, we subscribe the point cloud topic and odometry topic (as a result of A-LOAM, published from `laserMapping.cpp`). That is, our implementation is generic to any front-end odometry methods. Thus, our pose-graph optimization module (i.e., `laserPosegraphOptimization.cpp`) can easily be integrated with any odometry algorithms such as non-LOAM family or even other sensors (e.g., visual odometry). -

3. (optional) Altitude stabilization using consumer-level GPS - To make a result more trustworthy, we supports GPS (consumer-level price, such as U-Blox EVK-7P)-based altitude stabilization. The LOAM family of methods are known to be susceptible to z-errors in outdoors. We used the robust loss for only the altitude term. For the details, see the variable `robustGPSNoise` in the `laserPosegraphOptimization.cpp` file. ## Prerequisites (dependencies) - We mainly depend on ROS, Ceres (for A-LOAM), and GTSAM (for pose-graph optimization). - For the details to install the prerequisites, please follow the A-LOAM and LIO-SAM repositiory. - The below examples are done under ROS melodic (ubuntu 18) and GTSAM version 4.x. ## How to use? - First, install the abovementioned dependencies, and follow below lines. ``` mkdir -p ~/catkin_scaloam_ws/src cd ~/catkin_scaloam_ws/src git clone https://github.com/gisbi-kim/SC-A-LOAM.git cd ../ catkin_make source ~/catkin_scaloam_ws/devel/setup.bash roslaunch aloam_velodyne aloam_mulran.launch # for MulRan dataset setting ``` ## Example Results ### Riverside 01, MulRan dataset - The MulRan dataset provides lidar scans (Ouster OS1-64, horizontally mounted, 10Hz) and consumer level gps (U-Blox EVK-7P, 4Hz) data. - About how to use (publishing data) data: see here https://github.com/irapkaist/file_player_mulran - example videos on Riverside 01 sequence. 1. with consumer level GPS-based altitude stabilization: https://youtu.be/FwAVX5TVm04 2. without the z stabilization: https://youtu.be/okML_zNadhY - example result:

### KITTI 05 - For KITTI (HDL-64 sensor), run using the command ``` roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch # for KITTI dataset setting ``` - To publish KITTI scans, you can use mini-kitti publisher, a simple python script: https://github.com/gisbi-kim/mini-kitti-publisher - example video (no GPS used here): https://youtu.be/hk3Xx8SKkv4 - example result:

### Indoor - ScanContext also works at indoor environments (use smaller sc_max_radius value). - example video: https://youtu.be/Uv6_BRmxJho - example result:

### For Livox LiDAR - Scan Context also works for Livox LiDAR data - In this example, Scan Context is integrated with FAST-LIO (https://github.com/hku-mars/FAST_LIO). - Note: No additional integration effort is required. A user just run seperately FAST-LIO node and SC-A-LOAM's posegraphoptimization.cpp node! - example video (tutoial and results): https://youtu.be/Fw9S6D6HozA - example result:

### For Navtech Radar - Scan Context also works for Navtech Radar data! - For the details, please see - https://github.com/gisbi-kim/navtech-radar-slam - used the pose-graph optimization node of this repository (SC-A-LOAM) - [example video](https://www.youtube.com/watch?v=avtIQ8fesgU&t=128s) ## Utilities ### Data saver and Map construction - Similar to the [SC-LIO-SAM's saver utility](https://github.com/gisbi-kim/SC-LIO-SAM#applications), we support pose and scan saver per keyframes. Using these saved data, the map (within ROI) can be constructed offline. See the `utils/python/makeMergedMap.py` and [this tutorial](https://youtu.be/jmR3DH_A4Co). - Below is the example results of MulRan dataset KAIST 03's merged map, visualized using CloudCompare ([download the map data here](https://www.dropbox.com/sh/96jrpx3x6hh316j/AACb07kGbocnQWMIpksmU6MQa?dl=0)).

- A user also can remove dynamic points using these saved keyframe poses and scans. See [this tutorial](https://www.youtube.com/watch?v=UiYYrPMcIRU) and our [Removert project](https://github.com/irapkaist/removert). ## Acknowledgements - Thanks to LOAM, A-LOAM, and LIO-SAM code authors. The major codes in this repository are borrowed from their efforts. ## Maintainer - please contact me through `paulgkim@kaist.ac.kr` ## TODO - Delayed RS loop closings - SLAM with multi-session localization - More examples on other datasets (KITTI, complex urban dataset, etc.) ================================================ FILE: SC-PGO/docker/Dockerfile ================================================ FROM ros:kinetic-perception ENV CERES_VERSION="1.12.0" ENV PCL_VERSION="1.8.0" ENV CATKIN_WS=/root/catkin_ws # setup processors number used to compile library RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \ # Install dependencies apt-get update && apt-get install -y \ cmake \ libatlas-base-dev \ libeigen3-dev \ libgoogle-glog-dev \ libsuitesparse-dev \ python-catkin-tools \ ros-${ROS_DISTRO}-cv-bridge \ ros-${ROS_DISTRO}-image-transport \ ros-${ROS_DISTRO}-message-filters \ ros-${ROS_DISTRO}-tf && \ rm -rf /var/lib/apt/lists/* && \ # Build and install Ceres git clone https://ceres-solver.googlesource.com/ceres-solver && \ cd ceres-solver && \ git checkout tags/${CERES_VERSION} && \ mkdir build && cd build && \ cmake .. && \ make -j${USE_PROC} install && \ cd ../.. && \ rm -rf ./ceres-solver && \ # Build and install pcl git clone https://github.com/PointCloudLibrary/pcl.git && \ cd pcl && \ git checkout tags/pcl-${PCL_VERSION} && \ mkdir build && cd build && \ cmake .. && \ make -j${USE_PROC} install && \ cd ../.. && \ rm -rf ./pcl && \ # Setup catkin workspace mkdir -p $CATKIN_WS/src/A-LOAM/ # WORKDIR $CATKIN_WS/src # Copy A-LOAM COPY ./ $CATKIN_WS/src/A-LOAM/ # use the following line if you only have this dockerfile # RUN git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git # Build A-LOAM WORKDIR $CATKIN_WS ENV TERM xterm ENV PYTHONIOENCODING UTF-8 RUN catkin config \ --extend /opt/ros/$ROS_DISTRO \ --cmake-args \ -DCMAKE_BUILD_TYPE=Release && \ catkin build && \ sed -i '/exec "$@"/i \ source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh ================================================ FILE: SC-PGO/docker/Makefile ================================================ all: help help: @echo "" @echo "-- Help Menu" @echo "" @echo " 1. make build - build all images" # @echo " 1. make pull - pull all images" @echo " 1. make clean - remove all images" @echo "" build: @docker build --tag ros:aloam -f ./Dockerfile .. clean: @docker rmi -f ros:aloam ================================================ FILE: SC-PGO/docker/run.sh ================================================ #!/bin/bash trap : SIGTERM SIGINT function abspath() { # generate absolute path from relative path # $1 : relative filename # return : absolute path if [ -d "$1" ]; then # dir (cd "$1"; pwd) elif [ -f "$1" ]; then # file if [[ $1 = /* ]]; then echo "$1" elif [[ $1 == */* ]]; then echo "$(cd "${1%/*}"; pwd)/${1##*/}" else echo "$(pwd)/$1" fi fi } if [ "$#" -ne 1 ]; then echo "Usage: $0 LIDAR_SCAN_NUMBER" >&2 exit 1 fi roscore & ROSCORE_PID=$! sleep 1 rviz -d ../rviz_cfg/aloam_velodyne.rviz & RVIZ_PID=$! A_LOAM_DIR=$(abspath "..") if [ "$1" -eq 16 ]; then docker run \ -it \ --rm \ --net=host \ -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ ros:aloam \ /bin/bash -c \ "cd /root/catkin_ws/; \ catkin config \ --cmake-args \ -DCMAKE_BUILD_TYPE=Release; \ catkin build; \ source devel/setup.bash; \ roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch rviz:=false" elif [ "$1" -eq "32" ]; then docker run \ -it \ --rm \ --net=host \ -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ ros:aloam \ /bin/bash -c \ "cd /root/catkin_ws/; \ catkin config \ --cmake-args \ -DCMAKE_BUILD_TYPE=Release; \ catkin build; \ source devel/setup.bash; \ roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch rviz:=false" elif [ "$1" -eq "64" ]; then docker run \ -it \ --rm \ --net=host \ -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ ros:aloam \ /bin/bash -c \ "cd /root/catkin_ws/; \ catkin config \ --cmake-args \ -DCMAKE_BUILD_TYPE=Release; \ catkin build; \ source devel/setup.bash; \ roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch rviz:=false" fi wait $ROSCORE_PID wait $RVIZ_PID if [[ $? -gt 128 ]] then kill $ROSCORE_PID kill $RVIZ_PID fi ================================================ FILE: SC-PGO/include/aloam_velodyne/common.h ================================================ // This is an advanced implementation of the algorithm described in the following paper: // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. // Modifier: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk // Copyright 2013, Ji Zhang, Carnegie Mellon University // Further contributions copyright (c) 2016, Southwest Research Institute // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // 1. Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // 2. Redistributions in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // 3. Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived from this // software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #pragma once #include #include typedef pcl::PointXYZI PointType; inline double rad2deg(double radians) { return radians * 180.0 / M_PI; } inline double deg2rad(double degrees) { return degrees * M_PI / 180.0; } struct Pose6D { double x; double y; double z; double roll; double pitch; double yaw; }; ================================================ FILE: SC-PGO/include/aloam_velodyne/tic_toc.h ================================================ // Author: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk #pragma once #include #include #include class TicToc { public: TicToc() { tic(); } void tic() { start = std::chrono::system_clock::now(); } double toc() { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; return elapsed_seconds.count() * 1000; } private: std::chrono::time_point start, end; }; class TicTocV2 { public: TicTocV2() { tic(); } TicTocV2( bool _disp ) { disp_ = _disp; tic(); } void tic() { start = std::chrono::system_clock::now(); } void toc( std::string _about_task ) { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; double elapsed_ms = elapsed_seconds.count() * 1000; if( disp_ ) { std::cout.precision(3); // 10 for sec, 3 for ms std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; } } private: std::chrono::time_point start, end; bool disp_ = false; }; ================================================ FILE: SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h ================================================ /*********************************************************************** * Software License Agreement (BSD License) * * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *************************************************************************/ #pragma once #include "scancontext/nanoflann.hpp" #include // ===== This example shows how to use nanoflann with these types of containers: ======= //typedef std::vector > my_vector_of_vectors_t; //typedef std::vector my_vector_of_vectors_t; // This requires #include // ===================================================================================== /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. * The i'th vector represents a point in the state space. * * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. * \tparam num_t The type of the point coordinates (typically, double or float). * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) */ template struct KDTreeVectorOfVectorsAdaptor { typedef KDTreeVectorOfVectorsAdaptor self_t; typedef typename Distance::template traits::distance_t metric_t; typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. /// Constructor: takes a const ref to the vector of vectors object with the data points KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) { assert(mat.size() != 0 && mat[0].size() != 0); const size_t dims = mat[0].size(); if (DIM>0 && static_cast(dims) != DIM) throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); index->buildIndex(); } ~KDTreeVectorOfVectorsAdaptor() { delete index; } const VectorOfVectorsType &m_data; /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). * Note that this is a short-cut method for index->findNeighbors(). * The user can also call index->... methods as desired. * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. */ inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); } /** @name Interface expected by KDTreeSingleIndexAdaptor * @{ */ const self_t & derived() const { return *this; } self_t & derived() { return *this; } // Must return the number of data points inline size_t kdtree_get_point_count() const { return m_data.size(); } // Returns the dim'th component of the idx'th point in the class: inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { return m_data[idx][dim]; } // Optional bounding-box computation: return false to default to a standard bbox computation loop. // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) template bool kdtree_get_bbox(BBOX & /*bb*/) const { return false; } /** @} */ }; // end of KDTreeVectorOfVectorsAdaptor ================================================ FILE: SC-PGO/include/scancontext/Scancontext.cpp ================================================ #include "scancontext/Scancontext.h" // namespace SC2 // { void coreImportTest (void) { cout << "scancontext lib is successfully imported." << endl; } // coreImportTest float rad2deg(float radians) { return radians * 180.0 / M_PI; } float deg2rad(float degrees) { return degrees * M_PI / 180.0; } float xy2theta( const float & _x, const float & _y ) { if ( (_x >= 0) & (_y >= 0)) return (180/M_PI) * atan(_y / _x); if ( (_x < 0) & (_y >= 0)) return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); if ( (_x < 0) & (_y < 0)) return 180 + ( (180/M_PI) * atan(_y / _x) ); if ( (_x >= 0) & (_y < 0)) return 360 - ( (180/M_PI) * atan((-_y) / _x) ); } // xy2theta MatrixXd circshift( MatrixXd &_mat, int _num_shift ) { // shift columns to right direction assert(_num_shift >= 0); if( _num_shift == 0 ) { MatrixXd shifted_mat( _mat ); return shifted_mat; // Early return } MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) { int new_location = (col_idx + _num_shift) % _mat.cols(); shifted_mat.col(new_location) = _mat.col(col_idx); } return shifted_mat; } // circshift std::vector eig2stdvec( MatrixXd _eigmat ) { std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); return vec; } // eig2stdvec double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) { int num_eff_cols = 0; // i.e., to exclude all-nonzero sector double sum_sector_similarity = 0; for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) { VectorXd col_sc1 = _sc1.col(col_idx); VectorXd col_sc2 = _sc2.col(col_idx); if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) ) continue; // don't count this sector pair. double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); sum_sector_similarity = sum_sector_similarity + sector_similarity; num_eff_cols = num_eff_cols + 1; } double sc_sim = sum_sector_similarity / num_eff_cols; return 1.0 - sc_sim; } // distDirectSC int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) { int argmin_vkey_shift = 0; double min_veky_diff_norm = 10000000; for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) { MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); MatrixXd vkey_diff = _vkey1 - vkey2_shifted; double cur_diff_norm = vkey_diff.norm(); if( cur_diff_norm < min_veky_diff_norm ) { argmin_vkey_shift = shift_idx; min_veky_diff_norm = cur_diff_norm; } } return argmin_vkey_shift; } // fastAlignUsingVkey std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) { // 1. fast align using variant key (not in original IROS18) MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range std::vector shift_idx_search_space { argmin_vkey_shift }; for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) { shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); } std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); // 2. fast columnwise diff int argmin_shift = 0; double min_sc_dist = 10000000; for ( int num_shift: shift_idx_search_space ) { MatrixXd sc2_shifted = circshift(_sc2, num_shift); double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); if( cur_sc_dist < min_sc_dist ) { argmin_shift = num_shift; min_sc_dist = cur_sc_dist; } } return make_pair(min_sc_dist, argmin_shift); } // distanceBtnScanContext MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) { TicTocV2 t_making_desc; int num_pts_scan_down = _scan_down.points.size(); // main const int NO_POINT = -1000; MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); SCPointType pt; float azim_angle, azim_range; // wihtin 2d plane int ring_idx, sctor_idx; for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++) { pt.x = _scan_down.points[pt_idx].x; pt.y = _scan_down.points[pt_idx].y; pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). // xyz to ring, sector azim_range = sqrt(pt.x * pt.x + pt.y * pt.y); azim_angle = xy2theta(pt.x, pt.y); // if range is out of roi, pass if( azim_range > PC_MAX_RADIUS ) continue; ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); // taking maximum z if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin } // reset no points to zero (for cosine dist later) for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) if( desc(row_idx, col_idx) == NO_POINT ) desc(row_idx, col_idx) = 0; t_making_desc.toc("PolarContext making"); return desc; } // SCManager::makeScancontext MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) { /* * summary: rowwise mean vector */ Eigen::MatrixXd invariant_key(_desc.rows(), 1); for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) { Eigen::MatrixXd curr_row = _desc.row(row_idx); invariant_key(row_idx, 0) = curr_row.mean(); } return invariant_key; } // SCManager::makeRingkeyFromScancontext MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) { /* * summary: columnwise mean vector */ Eigen::MatrixXd variant_key(1, _desc.cols()); for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) { Eigen::MatrixXd curr_col = _desc.col(col_idx); variant_key(0, col_idx) = curr_col.mean(); } return variant_key; } // SCManager::makeSectorkeyFromScancontext const Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void) { return polarcontexts_.back(); } void SCManager::saveScancontextAndKeys( Eigen::MatrixXd _scd ) { Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( _scd ); Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( _scd ); std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); polarcontexts_.push_back( _scd ); polarcontext_invkeys_.push_back( ringkey ); polarcontext_vkeys_.push_back( sectorkey ); polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); } // SCManager::makeAndSaveScancontextAndKeys void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ) { Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); polarcontexts_.push_back( sc ); polarcontext_invkeys_.push_back( ringkey ); polarcontext_vkeys_.push_back( sectorkey ); polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); } // SCManager::makeAndSaveScancontextAndKeys void SCManager::setSCdistThres(double _new_thres) { SC_DIST_THRES = _new_thres; } // SCManager::setThres void SCManager::setMaximumRadius(double _max_r) { PC_MAX_RADIUS = _max_r; } // SCManager::setMaximumRadius std::pair SCManager::detectLoopClosureIDBetweenSession (std::vector& _curr_key, Eigen::MatrixXd& _curr_desc) { int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") auto& curr_key = _curr_key; auto& curr_desc = _curr_desc; // current observation (query) // step 0: if first, construct the tree in batch if( ! is_tree_batch_made ) // run only once { polarcontext_invkeys_to_search_.clear(); polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() ) ; polarcontext_tree_batch_.reset(); polarcontext_tree_batch_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); is_tree_batch_made = true; // for running this block only once } double min_dist = 10000000; // init with somthing large int nn_align = 0; int nn_idx = 0; // step 1: knn search std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); polarcontext_tree_batch_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); // error here // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) TicTocV2 t_calc_dist; for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) { MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); double candidate_dist = sc_dist_result.first; int candidate_align = sc_dist_result.second; if( candidate_dist < min_dist ) { min_dist = candidate_dist; nn_align = candidate_align; nn_idx = candidate_indexes[candidate_iter_idx]; } } t_calc_dist.toc("Distance calc"); // step 3: similarity threshold if( min_dist < SC_DIST_THRES ) loop_id = nn_idx; // To do: return also nn_align (i.e., yaw diff) float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); std::pair result {loop_id, yaw_diff_rad}; return result; } // SCManager::detectLoopClosureIDBetweenSession std::pair SCManager::detectLoopClosureID ( void ) { int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) auto curr_desc = polarcontexts_.back(); // current observation (query) /* * step 1: candidates from ringkey tree_ */ if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) { std::pair result {loop_id, 0.0}; return result; // Early return } // tree_ reconstruction (not mandatory to make everytime) if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost { TicTocV2 t_tree_construction; polarcontext_invkeys_to_search_.clear(); polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; polarcontext_tree_.reset(); polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor) t_tree_construction.toc("Tree construction"); } tree_making_period_conter = tree_making_period_conter + 1; double min_dist = 10000000; // init with somthing large int nn_align = 0; int nn_idx = 0; // knn search std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); TicTocV2 t_tree_search; nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); t_tree_search.toc("Tree search"); /* * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) */ TicTocV2 t_calc_dist; for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) { MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); double candidate_dist = sc_dist_result.first; int candidate_align = sc_dist_result.second; if( candidate_dist < min_dist ) { min_dist = candidate_dist; nn_align = candidate_align; nn_idx = candidate_indexes[candidate_iter_idx]; } } t_calc_dist.toc("Distance calc"); /* * loop threshold check */ if( min_dist < SC_DIST_THRES ) { loop_id = nn_idx; // std::cout.precision(3); cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; // cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; } else { std::cout.precision(3); cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; // cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; } // To do: return also nn_align (i.e., yaw diff) float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); std::pair result {loop_id, yaw_diff_rad}; return result; } // SCManager::detectLoopClosureID // } // namespace SC2 ================================================ FILE: SC-PGO/include/scancontext/Scancontext.h ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "scancontext/nanoflann.hpp" #include "scancontext/KDTreeVectorOfVectorsAdaptor.h" #include "aloam_velodyne/tic_toc.h" using namespace Eigen; using namespace nanoflann; using std::cout; using std::endl; using std::make_pair; using std::atan2; using std::cos; using std::sin; using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) using KeyMat = std::vector >; using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; // namespace SC2 // { void coreImportTest ( void ); // sc param-independent helper functions float xy2theta( const float & _x, const float & _y ); MatrixXd circshift( MatrixXd &_mat, int _num_shift ); std::vector eig2stdvec( MatrixXd _eigmat ); class SCManager { public: SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) // User-side API void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw // for ltslam // User-side API for multi-session void saveScancontextAndKeys( Eigen::MatrixXd _scd ); std::pair detectLoopClosureIDBetweenSession ( std::vector& curr_key, Eigen::MatrixXd& curr_desc); const Eigen::MatrixXd& getConstRefRecentSCD(void); public: // hyper parameters () const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); // tree const int NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper) // loop thres const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) double SC_DIST_THRES = 0.2; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 // config const int TREE_MAKING_PERIOD_ = 30; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N. int tree_making_period_conter = 0; // setter void setSCdistThres(double _new_thres); void setMaximumRadius(double _max_r); // data std::vector polarcontexts_timestamp_; // optional. std::vector polarcontexts_; std::vector polarcontext_invkeys_; std::vector polarcontext_vkeys_; KeyMat polarcontext_invkeys_mat_; KeyMat polarcontext_invkeys_to_search_; std::unique_ptr polarcontext_tree_; bool is_tree_batch_made = false; std::unique_ptr polarcontext_tree_batch_; }; // SCManager // } // namespace SC2 ================================================ FILE: SC-PGO/include/scancontext/nanoflann.hpp ================================================ /*********************************************************************** * Software License Agreement (BSD License) * * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). * All rights reserved. * * THE BSD LICENSE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *************************************************************************/ /** \mainpage nanoflann C++ API documentation * nanoflann is a C++ header-only library for building KD-Trees, mostly * optimized for 2D or 3D point clouds. * * nanoflann does not require compiling or installing, just an * #include in your code. * * See: * - C++ API organized by modules * - Online README * - Doxygen * documentation */ #ifndef NANOFLANN_HPP_ #define NANOFLANN_HPP_ #include #include #include #include // for abs() #include // for fwrite() #include // for abs() #include #include // std::reference_wrapper #include #include /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ #define NANOFLANN_VERSION 0x132 // Avoid conflicting declaration of min/max macros in windows headers #if !defined(NOMINMAX) && \ (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) #define NOMINMAX #ifdef max #undef max #undef min #endif #endif namespace nanoflann { /** @addtogroup nanoflann_grp nanoflann C++ library for ANN * @{ */ /** the PI constant (required to avoid MSVC missing symbols) */ template T pi_const() { return static_cast(3.14159265358979323846); } /** * Traits if object is resizable and assignable (typically has a resize | assign * method) */ template struct has_resize : std::false_type {}; template struct has_resize().resize(1), 0)> : std::true_type {}; template struct has_assign : std::false_type {}; template struct has_assign().assign(1, 0), 0)> : std::true_type {}; /** * Free function to resize a resizable object */ template inline typename std::enable_if::value, void>::type resize(Container &c, const size_t nElements) { c.resize(nElements); } /** * Free function that has no effects on non resizable containers (e.g. * std::array) It raises an exception if the expected size does not match */ template inline typename std::enable_if::value, void>::type resize(Container &c, const size_t nElements) { if (nElements != c.size()) throw std::logic_error("Try to change the size of a std::array."); } /** * Free function to assign to a container */ template inline typename std::enable_if::value, void>::type assign(Container &c, const size_t nElements, const T &value) { c.assign(nElements, value); } /** * Free function to assign to a std::array */ template inline typename std::enable_if::value, void>::type assign(Container &c, const size_t nElements, const T &value) { for (size_t i = 0; i < nElements; i++) c[i] = value; } /** @addtogroup result_sets_grp Result set classes * @{ */ template class KNNResultSet { public: typedef _DistanceType DistanceType; typedef _IndexType IndexType; typedef _CountType CountType; private: IndexType *indices; DistanceType *dists; CountType capacity; CountType count; public: inline KNNResultSet(CountType capacity_) : indices(0), dists(0), capacity(capacity_), count(0) {} inline void init(IndexType *indices_, DistanceType *dists_) { indices = indices_; dists = dists_; count = 0; if (capacity) dists[capacity - 1] = (std::numeric_limits::max)(); } inline CountType size() const { return count; } inline bool full() const { return count == capacity; } /** * Called during search to add an element matching the criteria. * @return true if the search should be continued, false if the results are * sufficient */ inline bool addPoint(DistanceType dist, IndexType index) { CountType i; for (i = count; i > 0; --i) { #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same // distance, the one with the lowest-index will be // returned first. if ((dists[i - 1] > dist) || ((dist == dists[i - 1]) && (indices[i - 1] > index))) { #else if (dists[i - 1] > dist) { #endif if (i < capacity) { dists[i] = dists[i - 1]; indices[i] = indices[i - 1]; } } else break; } if (i < capacity) { dists[i] = dist; indices[i] = index; } if (count < capacity) count++; // tell caller that the search shall continue return true; } inline DistanceType worstDist() const { return dists[capacity - 1]; } }; /** operator "<" for std::sort() */ struct IndexDist_Sorter { /** PairType will be typically: std::pair */ template inline bool operator()(const PairType &p1, const PairType &p2) const { return p1.second < p2.second; } }; /** * A result-set class used when performing a radius based search. */ template class RadiusResultSet { public: typedef _DistanceType DistanceType; typedef _IndexType IndexType; public: const DistanceType radius; std::vector> &m_indices_dists; inline RadiusResultSet( DistanceType radius_, std::vector> &indices_dists) : radius(radius_), m_indices_dists(indices_dists) { init(); } inline void init() { clear(); } inline void clear() { m_indices_dists.clear(); } inline size_t size() const { return m_indices_dists.size(); } inline bool full() const { return true; } /** * Called during search to add an element matching the criteria. * @return true if the search should be continued, false if the results are * sufficient */ inline bool addPoint(DistanceType dist, IndexType index) { if (dist < radius) m_indices_dists.push_back(std::make_pair(index, dist)); return true; } inline DistanceType worstDist() const { return radius; } /** * Find the worst result (furtherest neighbor) without copying or sorting * Pre-conditions: size() > 0 */ std::pair worst_item() const { if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " "an empty list of results."); typedef typename std::vector>::const_iterator DistIt; DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); return *it; } }; /** @} */ /** @addtogroup loadsave_grp Load/save auxiliary functions * @{ */ template void save_value(FILE *stream, const T &value, size_t count = 1) { fwrite(&value, sizeof(value), count, stream); } template void save_value(FILE *stream, const std::vector &value) { size_t size = value.size(); fwrite(&size, sizeof(size_t), 1, stream); fwrite(&value[0], sizeof(T), size, stream); } template void load_value(FILE *stream, T &value, size_t count = 1) { size_t read_cnt = fread(&value, sizeof(value), count, stream); if (read_cnt != count) { throw std::runtime_error("Cannot read from file"); } } template void load_value(FILE *stream, std::vector &value) { size_t size; size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); if (read_cnt != 1) { throw std::runtime_error("Cannot read from file"); } value.resize(size); read_cnt = fread(&value[0], sizeof(T), size, stream); if (read_cnt != size) { throw std::runtime_error("Cannot read from file"); } } /** @} */ /** @addtogroup metric_grp Metric (distance) classes * @{ */ struct Metric {}; /** Manhattan distance functor (generic version, optimized for * high-dimensionality data sets). Corresponding distance traits: * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) * (e.g. float, double, int64_t) */ template struct L1_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const { DistanceType result = DistanceType(); const T *last = a + size; const T *lastgroup = last - 3; size_t d = 0; /* Process 4 items with each loop for efficiency. */ while (a < lastgroup) { const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); result += diff0 + diff1 + diff2 + diff3; a += 4; if ((worst_dist > 0) && (result > worst_dist)) { return result; } } /* Process last 0-3 components. Not needed for standard vector lengths. */ while (a < last) { result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); } return result; } template inline DistanceType accum_dist(const U a, const V b, const size_t) const { return std::abs(a - b); } }; /** Squared Euclidean distance functor (generic version, optimized for * high-dimensionality data sets). Corresponding distance traits: * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) * (e.g. float, double, int64_t) */ template struct L2_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const { DistanceType result = DistanceType(); const T *last = a + size; const T *lastgroup = last - 3; size_t d = 0; /* Process 4 items with each loop for efficiency. */ while (a < lastgroup) { const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; a += 4; if ((worst_dist > 0) && (result > worst_dist)) { return result; } } /* Process last 0-3 components. Not needed for standard vector lengths. */ while (a < last) { const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); result += diff0 * diff0; } return result; } template inline DistanceType accum_dist(const U a, const V b, const size_t) const { return (a - b) * (a - b); } }; /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality * datasets, like 2D or 3D point clouds) Corresponding distance traits: * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, * float, uint8_t) \tparam _DistanceType Type of distance variables (must be * signed) (e.g. float, double, int64_t) */ template struct L2_Simple_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size) const { DistanceType result = DistanceType(); for (size_t i = 0; i < size; ++i) { const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); result += diff * diff; } return result; } template inline DistanceType accum_dist(const U a, const V b, const size_t) const { return (a - b) * (a - b); } }; /** SO2 distance functor * Corresponding distance traits: nanoflann::metric_SO2 * \tparam T Type of the elements (e.g. double, float) * \tparam _DistanceType Type of distance variables (must be signed) (e.g. * float, double) orientation is constrained to be in [-pi, pi] */ template struct SO2_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size) const { return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1); } /** Note: this assumes that input angles are already in the range [-pi,pi] */ template inline DistanceType accum_dist(const U a, const V b, const size_t) const { DistanceType result = DistanceType(); DistanceType PI = pi_const(); result = b - a; if (result > PI) result -= 2 * PI; else if (result < -PI) result += 2 * PI; return result; } }; /** SO3 distance functor (Uses L2_Simple) * Corresponding distance traits: nanoflann::metric_SO3 * \tparam T Type of the elements (e.g. double, float) * \tparam _DistanceType Type of distance variables (must be signed) (e.g. * float, double) */ template struct SO3_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; L2_Simple_Adaptor distance_L2_Simple; SO3_Adaptor(const DataSource &_data_source) : distance_L2_Simple(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size) const { return distance_L2_Simple.evalMetric(a, b_idx, size); } template inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { return distance_L2_Simple.accum_dist(a, b, idx); } }; /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ struct metric_L1 : public Metric { template struct traits { typedef L1_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ struct metric_L2 : public Metric { template struct traits { typedef L2_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ struct metric_L2_Simple : public Metric { template struct traits { typedef L2_Simple_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ struct metric_SO2 : public Metric { template struct traits { typedef SO2_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ struct metric_SO3 : public Metric { template struct traits { typedef SO3_Adaptor distance_t; }; }; /** @} */ /** @addtogroup param_grp Parameter structs * @{ */ /** Parameters (see README.md) */ struct KDTreeSingleIndexAdaptorParams { KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) : leaf_max_size(_leaf_max_size) {} size_t leaf_max_size; }; /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ struct SearchParams { /** Note: The first argument (checks_IGNORED_) is ignored, but kept for * compatibility with the FLANN interface */ SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} int checks; //!< Ignored parameter (Kept for compatibility with the FLANN //!< interface). float eps; //!< search for eps-approximate neighbours (default: 0) bool sorted; //!< only for radius search, require neighbours sorted by //!< distance (default: true) }; /** @} */ /** @addtogroup memalloc_grp Memory allocation * @{ */ /** * Allocates (using C's malloc) a generic type T. * * Params: * count = number of instances to allocate. * Returns: pointer (of type T*) to memory buffer */ template inline T *allocate(size_t count = 1) { T *mem = static_cast(::malloc(sizeof(T) * count)); return mem; } /** * Pooled storage allocator * * The following routines allow for the efficient allocation of storage in * small chunks from a specified pool. Rather than allowing each structure * to be freed individually, an entire pool of storage is freed at once. * This method has two advantages over just using malloc() and free(). First, * it is far more efficient for allocating small objects, as there is * no overhead for remembering all the information needed to free each * object or consolidating fragmented memory. Second, the decision about * how long to keep an object is made at the time of allocation, and there * is no need to track down all the objects to free them. * */ const size_t WORDSIZE = 16; const size_t BLOCKSIZE = 8192; class PooledAllocator { /* We maintain memory alignment to word boundaries by requiring that all allocations be in multiples of the machine wordsize. */ /* Size of machine word in bytes. Must be power of 2. */ /* Minimum number of bytes requested at a time from the system. Must be * multiple of WORDSIZE. */ size_t remaining; /* Number of bytes left in current block of storage. */ void *base; /* Pointer to base of current block of storage. */ void *loc; /* Current location in block to next allocate memory. */ void internal_init() { remaining = 0; base = NULL; usedMemory = 0; wastedMemory = 0; } public: size_t usedMemory; size_t wastedMemory; /** Default constructor. Initializes a new pool. */ PooledAllocator() { internal_init(); } /** * Destructor. Frees all the memory allocated in this pool. */ ~PooledAllocator() { free_all(); } /** Frees all allocated memory chunks */ void free_all() { while (base != NULL) { void *prev = *(static_cast(base)); /* Get pointer to prev block. */ ::free(base); base = prev; } internal_init(); } /** * Returns a pointer to a piece of new memory of the given size in bytes * allocated from the pool. */ void *malloc(const size_t req_size) { /* Round size up to a multiple of wordsize. The following expression only works for WORDSIZE that is a power of 2, by masking last bits of incremented size to zero. */ const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); /* Check whether a new block must be allocated. Note that the first word of a block is reserved for a pointer to the previous block. */ if (size > remaining) { wastedMemory += remaining; /* Allocate new storage. */ const size_t blocksize = (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) ? size + sizeof(void *) + (WORDSIZE - 1) : BLOCKSIZE; // use the standard C malloc to allocate memory void *m = ::malloc(blocksize); if (!m) { fprintf(stderr, "Failed to allocate memory.\n"); return NULL; } /* Fill first word of new block with pointer to previous block. */ static_cast(m)[0] = base; base = m; size_t shift = 0; // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & // (WORDSIZE-1))) & (WORDSIZE-1); remaining = blocksize - sizeof(void *) - shift; loc = (static_cast(m) + sizeof(void *) + shift); } void *rloc = loc; loc = static_cast(loc) + size; remaining -= size; usedMemory += size; return rloc; } /** * Allocates (using this pool) a generic type T. * * Params: * count = number of instances to allocate. * Returns: pointer (of type T*) to memory buffer */ template T *allocate(const size_t count = 1) { T *mem = static_cast(this->malloc(sizeof(T) * count)); return mem; } }; /** @} */ /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff * @{ */ /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors * when DIM=-1. Fixed size version for a generic DIM: */ template struct array_or_vector_selector { typedef std::array container_t; }; /** Dynamic size version */ template struct array_or_vector_selector<-1, T> { typedef std::vector container_t; }; /** @} */ /** kd-tree base-class * * Contains the member functions common to the classes KDTreeSingleIndexAdaptor * and KDTreeSingleIndexDynamicAdaptor_. * * \tparam Derived The name of the class which inherits this class. * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use, these are all classes derived * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for * 3D points) \tparam IndexType Will be typically size_t or int */ template class KDTreeBaseClass { public: /** Frees the previously-built index. Automatically called within * buildIndex(). */ void freeIndex(Derived &obj) { obj.pool.free_all(); obj.root_node = NULL; obj.m_size_at_index_build = 0; } typedef typename Distance::ElementType ElementType; typedef typename Distance::DistanceType DistanceType; /*--------------------- Internal Data Structures --------------------------*/ struct Node { /** Union used because a node can be either a LEAF node or a non-leaf node, * so both data fields are never used simultaneously */ union { struct leaf { IndexType left, right; //!< Indices of points in leaf node } lr; struct nonleaf { int divfeat; //!< Dimension used for subdivision. DistanceType divlow, divhigh; //!< The values used for subdivision. } sub; } node_type; Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) }; typedef Node *NodePtr; struct Interval { ElementType low, high; }; /** * Array of indices to vectors in the dataset. */ std::vector vind; NodePtr root_node; size_t m_leaf_max_size; size_t m_size; //!< Number of current points in the dataset size_t m_size_at_index_build; //!< Number of points in the dataset when the //!< index was built int dim; //!< Dimensionality of each data point /** Define "BoundingBox" as a fixed-size or variable-size container depending * on "DIM" */ typedef typename array_or_vector_selector::container_t BoundingBox; /** Define "distance_vector_t" as a fixed-size or variable-size container * depending on "DIM" */ typedef typename array_or_vector_selector::container_t distance_vector_t; /** The KD-tree used to find neighbours */ BoundingBox root_bbox; /** * Pooled memory allocator. * * Using a pooled memory allocator is more efficient * than allocating memory directly when there is a large * number small of memory allocations. */ PooledAllocator pool; /** Returns number of points in dataset */ size_t size(const Derived &obj) const { return obj.m_size; } /** Returns the length of each point in the dataset */ size_t veclen(const Derived &obj) { return static_cast(DIM > 0 ? DIM : obj.dim); } /// Helper accessor to the dataset points: inline ElementType dataset_get(const Derived &obj, size_t idx, int component) const { return obj.dataset.kdtree_get_pt(idx, component); } /** * Computes the inde memory usage * Returns: memory used by the index */ size_t usedMemory(Derived &obj) { return obj.pool.usedMemory + obj.pool.wastedMemory + obj.dataset.kdtree_get_point_count() * sizeof(IndexType); // pool memory and vind array memory } void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, int element, ElementType &min_elem, ElementType &max_elem) { min_elem = dataset_get(obj, ind[0], element); max_elem = dataset_get(obj, ind[0], element); for (IndexType i = 1; i < count; ++i) { ElementType val = dataset_get(obj, ind[i], element); if (val < min_elem) min_elem = val; if (val > max_elem) max_elem = val; } } /** * Create a tree node that subdivides the list of vecs from vind[first] * to vind[last]. The routine is called recursively on each sublist. * * @param left index of the first vector * @param right index of the last vector */ NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, BoundingBox &bbox) { NodePtr node = obj.pool.template allocate(); // allocate memory /* If too few exemplars remain, then make this a leaf node. */ if ((right - left) <= static_cast(obj.m_leaf_max_size)) { node->child1 = node->child2 = NULL; /* Mark as leaf node. */ node->node_type.lr.left = left; node->node_type.lr.right = right; // compute bounding-box of leaf points for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { bbox[i].low = dataset_get(obj, obj.vind[left], i); bbox[i].high = dataset_get(obj, obj.vind[left], i); } for (IndexType k = left + 1; k < right; ++k) { for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) bbox[i].low = dataset_get(obj, obj.vind[k], i); if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) bbox[i].high = dataset_get(obj, obj.vind[k], i); } } } else { IndexType idx; int cutfeat; DistanceType cutval; middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, bbox); node->node_type.sub.divfeat = cutfeat; BoundingBox left_bbox(bbox); left_bbox[cutfeat].high = cutval; node->child1 = divideTree(obj, left, left + idx, left_bbox); BoundingBox right_bbox(bbox); right_bbox[cutfeat].low = cutval; node->child2 = divideTree(obj, left + idx, right, right_bbox); node->node_type.sub.divlow = left_bbox[cutfeat].high; node->node_type.sub.divhigh = right_bbox[cutfeat].low; for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); } } return node; } void middleSplit_(Derived &obj, IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox) { const DistanceType EPS = static_cast(0.00001); ElementType max_span = bbox[0].high - bbox[0].low; for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { ElementType span = bbox[i].high - bbox[i].low; if (span > max_span) { max_span = span; } } ElementType max_spread = -1; cutfeat = 0; for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { ElementType span = bbox[i].high - bbox[i].low; if (span > (1 - EPS) * max_span) { ElementType min_elem, max_elem; computeMinMax(obj, ind, count, i, min_elem, max_elem); ElementType spread = max_elem - min_elem; ; if (spread > max_spread) { cutfeat = i; max_spread = spread; } } } // split in the middle DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; ElementType min_elem, max_elem; computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); if (split_val < min_elem) cutval = min_elem; else if (split_val > max_elem) cutval = max_elem; else cutval = split_val; IndexType lim1, lim2; planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); if (lim1 > count / 2) index = lim1; else if (lim2 < count / 2) index = lim2; else index = count / 2; } /** * Subdivide the list of points by a plane perpendicular on axe corresponding * to the 'cutfeat' dimension at 'cutval' position. * * On return: * dataset[ind[0..lim1-1]][cutfeat]cutval */ void planeSplit(Derived &obj, IndexType *ind, const IndexType count, int cutfeat, DistanceType &cutval, IndexType &lim1, IndexType &lim2) { /* Move vector indices for left subtree to front of list. */ IndexType left = 0; IndexType right = count - 1; for (;;) { while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) ++left; while (right && left <= right && dataset_get(obj, ind[right], cutfeat) >= cutval) --right; if (left > right || !right) break; // "!right" was added to support unsigned Index types std::swap(ind[left], ind[right]); ++left; --right; } /* If either list is empty, it means that all remaining features * are identical. Split in the middle to maintain a balanced tree. */ lim1 = left; right = count - 1; for (;;) { while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) ++left; while (right && left <= right && dataset_get(obj, ind[right], cutfeat) > cutval) --right; if (left > right || !right) break; // "!right" was added to support unsigned Index types std::swap(ind[left], ind[right]); ++left; --right; } lim2 = left; } DistanceType computeInitialDistances(const Derived &obj, const ElementType *vec, distance_vector_t &dists) const { assert(vec); DistanceType distsq = DistanceType(); for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { if (vec[i] < obj.root_bbox[i].low) { dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); distsq += dists[i]; } if (vec[i] > obj.root_bbox[i].high) { dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); distsq += dists[i]; } } return distsq; } void save_tree(Derived &obj, FILE *stream, NodePtr tree) { save_value(stream, *tree); if (tree->child1 != NULL) { save_tree(obj, stream, tree->child1); } if (tree->child2 != NULL) { save_tree(obj, stream, tree->child2); } } void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { tree = obj.pool.template allocate(); load_value(stream, *tree); if (tree->child1 != NULL) { load_tree(obj, stream, tree->child1); } if (tree->child2 != NULL) { load_tree(obj, stream, tree->child2); } } /** Stores the index in a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when * loading the index object it must be constructed associated to the same * source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ void saveIndex_(Derived &obj, FILE *stream) { save_value(stream, obj.m_size); save_value(stream, obj.dim); save_value(stream, obj.root_bbox); save_value(stream, obj.m_leaf_max_size); save_value(stream, obj.vind); save_tree(obj, stream, obj.root_node); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the * index object must be constructed associated to the same source of data * points used while building the index. See the example: * examples/saveload_example.cpp \sa loadIndex */ void loadIndex_(Derived &obj, FILE *stream) { load_value(stream, obj.m_size); load_value(stream, obj.dim); load_value(stream, obj.root_bbox); load_value(stream, obj.m_leaf_max_size); load_value(stream, obj.vind); load_tree(obj, stream, obj.root_node); } }; /** @addtogroup kdtrees_grp KD-tree classes and adaptors * @{ */ /** kd-tree static index * * Contains the k-d trees and other information for indexing a set of points * for nearest-neighbor matching. * * The class "DatasetAdaptor" must provide the following interface (can be * non-virtual, inlined methods): * * \code * // Must return the number of data poins * inline size_t kdtree_get_point_count() const { ... } * * * // Must return the dim'th component of the idx'th point in the class: * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } * * // Optional bounding-box computation: return false to default to a standard * bbox computation loop. * // Return true if the BBOX was already computed by the class and returned * in "bb" so it can be avoided to redo it again. * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const * { * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits * ... * return true; * } * * \endcode * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ template class KDTreeSingleIndexAdaptor : public KDTreeBaseClass< KDTreeSingleIndexAdaptor, Distance, DatasetAdaptor, DIM, IndexType> { public: /** Deleted copy constructor*/ KDTreeSingleIndexAdaptor( const KDTreeSingleIndexAdaptor &) = delete; /** * The dataset used by this index */ const DatasetAdaptor &dataset; //!< The source of our data const KDTreeSingleIndexAdaptorParams index_params; Distance distance; typedef typename nanoflann::KDTreeBaseClass< nanoflann::KDTreeSingleIndexAdaptor, Distance, DatasetAdaptor, DIM, IndexType> BaseClassRef; typedef typename BaseClassRef::ElementType ElementType; typedef typename BaseClassRef::DistanceType DistanceType; typedef typename BaseClassRef::Node Node; typedef Node *NodePtr; typedef typename BaseClassRef::Interval Interval; /** Define "BoundingBox" as a fixed-size or variable-size container depending * on "DIM" */ typedef typename BaseClassRef::BoundingBox BoundingBox; /** Define "distance_vector_t" as a fixed-size or variable-size container * depending on "DIM" */ typedef typename BaseClassRef::distance_vector_t distance_vector_t; /** * KDTree constructor * * Refer to docs in README.md or online in * https://github.com/jlblancoc/nanoflann * * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 * for 3D points) is determined by means of: * - The \a DIM template parameter if >0 (highest priority) * - Otherwise, the \a dimensionality parameter of this constructor. * * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms = KDTreeSingleIndexAdaptorParams()) : dataset(inputData), index_params(params), distance(inputData) { BaseClassRef::root_node = NULL; BaseClassRef::m_size = dataset.kdtree_get_point_count(); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; BaseClassRef::dim = dimensionality; if (DIM > 0) BaseClassRef::dim = DIM; BaseClassRef::m_leaf_max_size = params.leaf_max_size; // Create a permutable array of indices to the input vectors. init_vind(); } /** * Builds the index */ void buildIndex() { BaseClassRef::m_size = dataset.kdtree_get_point_count(); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; init_vind(); this->freeIndex(*this); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; if (BaseClassRef::m_size == 0) return; computeBoundingBox(BaseClassRef::root_bbox); BaseClassRef::root_node = this->divideTree(*this, 0, BaseClassRef::m_size, BaseClassRef::root_bbox); // construct the tree } /** \name Query methods * @{ */ /** * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored * inside the result object. * * Params: * result = the result object in which the indices of the * nearest-neighbors are stored vec = the vector for which to search the * nearest neighbors * * \tparam RESULTSET Should be any ResultSet * \return True if the requested neighbors could be found. * \sa knnSearch, radiusSearch */ template bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const { assert(vec); if (this->size(*this) == 0) return false; if (!BaseClassRef::root_node) throw std::runtime_error( "[nanoflann] findNeighbors() called before building the index."); float epsError = 1 + searchParams.eps; distance_vector_t dists; // fixed or variable-sized container (depending on DIM) auto zero = static_cast(0); assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), zero); // Fill it with zeros. DistanceType distsq = this->computeInitialDistances(*this, vec, dists); searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither // used nor returned to the user. return result.full(); } /** * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. * Their indices are stored inside the result object. \sa radiusSearch, * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility * with the original FLANN interface. \return Number `N` of valid points in * the result set. Only the first `N` entries in `out_indices` and * `out_distances_sq` will be valid. Return may be less than `num_closest` * only if the number of elements in the tree is less than `num_closest`. */ size_t knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); return resultSet.size(); } /** * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. * The output is given as a vector of pairs, of which the first element is a * point index and the second the corresponding distance. Previous contents of * \a IndicesDists are cleared. * * If searchParams.sorted==true, the output list is sorted by ascending * distances. * * For a better performance, it is advisable to do a .reserve() on the vector * if you have any wild guess about the number of expected matches. * * \sa knnSearch, findNeighbors, radiusSearchCustomCallback * \return The number of points within the given radius (i.e. indices.size() * or dists.size() ) */ size_t radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector> &IndicesDists, const SearchParams &searchParams) const { RadiusResultSet resultSet(radius, IndicesDists); const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); if (searchParams.sorted) std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); return nFound; } /** * Just like radiusSearch() but with a custom callback class for each point * found in the radius of the query. See the source of RadiusResultSet<> as a * start point for your own classes. \sa radiusSearch */ template size_t radiusSearchCustomCallback( const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams = SearchParams()) const { this->findNeighbors(resultSet, query_point, searchParams); return resultSet.size(); } /** @} */ public: /** Make sure the auxiliary list \a vind has the same size than the current * dataset, and re-generate if size has changed. */ void init_vind() { // Create a permutable array of indices to the input vectors. BaseClassRef::m_size = dataset.kdtree_get_point_count(); if (BaseClassRef::vind.size() != BaseClassRef::m_size) BaseClassRef::vind.resize(BaseClassRef::m_size); for (size_t i = 0; i < BaseClassRef::m_size; i++) BaseClassRef::vind[i] = i; } void computeBoundingBox(BoundingBox &bbox) { resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dataset.kdtree_get_bbox(bbox)) { // Done! It was implemented in derived class } else { const size_t N = dataset.kdtree_get_point_count(); if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but " "no data points found."); for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); } for (size_t k = 1; k < N; ++k) { for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { if (this->dataset_get(*this, k, i) < bbox[i].low) bbox[i].low = this->dataset_get(*this, k, i); if (this->dataset_get(*this, k, i) > bbox[i].high) bbox[i].high = this->dataset_get(*this, k, i); } } } } /** * Performs an exact search in the tree starting from a node. * \tparam RESULTSET Should be any ResultSet * \return true if the search should be continued, false if the results are * sufficient */ template bool searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const { /* If this is a leaf node, then do check and return. */ if ((node->child1 == NULL) && (node->child2 == NULL)) { // count_leaf += (node->lr.right-node->lr.left); // Removed since was // neither used nor returned to the user. DistanceType worst_dist = result_set.worstDist(); for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { const IndexType index = BaseClassRef::vind[i]; // reorder... : i; DistanceType dist = distance.evalMetric( vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dist < worst_dist) { if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { // the resultset doesn't want to receive any more points, we're done // searching! return false; } } } return true; } /* Which child branch should be taken first? */ int idx = node->node_type.sub.divfeat; ElementType val = vec[idx]; DistanceType diff1 = val - node->node_type.sub.divlow; DistanceType diff2 = val - node->node_type.sub.divhigh; NodePtr bestChild; NodePtr otherChild; DistanceType cut_dist; if ((diff1 + diff2) < 0) { bestChild = node->child1; otherChild = node->child2; cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); } else { bestChild = node->child2; otherChild = node->child1; cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); } /* Call recursively to search next level down. */ if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { // the resultset doesn't want to receive any more points, we're done // searching! return false; } DistanceType dst = dists[idx]; mindistsq = mindistsq + cut_dist - dst; dists[idx] = cut_dist; if (mindistsq * epsError <= result_set.worstDist()) { if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError)) { // the resultset doesn't want to receive any more points, we're done // searching! return false; } } dists[idx] = dst; return true; } public: /** Stores the index in a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when * loading the index object it must be constructed associated to the same * source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the * index object must be constructed associated to the same source of data * points used while building the index. See the example: * examples/saveload_example.cpp \sa loadIndex */ void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } }; // class KDTree /** kd-tree dynamic index * * Contains the k-d trees and other information for indexing a set of points * for nearest-neighbor matching. * * The class "DatasetAdaptor" must provide the following interface (can be * non-virtual, inlined methods): * * \code * // Must return the number of data poins * inline size_t kdtree_get_point_count() const { ... } * * // Must return the dim'th component of the idx'th point in the class: * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } * * // Optional bounding-box computation: return false to default to a standard * bbox computation loop. * // Return true if the BBOX was already computed by the class and returned * in "bb" so it can be avoided to redo it again. * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const * { * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits * ... * return true; * } * * \endcode * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ template class KDTreeSingleIndexDynamicAdaptor_ : public KDTreeBaseClass, Distance, DatasetAdaptor, DIM, IndexType> { public: /** * The dataset used by this index */ const DatasetAdaptor &dataset; //!< The source of our data KDTreeSingleIndexAdaptorParams index_params; std::vector &treeIndex; Distance distance; typedef typename nanoflann::KDTreeBaseClass< nanoflann::KDTreeSingleIndexDynamicAdaptor_, Distance, DatasetAdaptor, DIM, IndexType> BaseClassRef; typedef typename BaseClassRef::ElementType ElementType; typedef typename BaseClassRef::DistanceType DistanceType; typedef typename BaseClassRef::Node Node; typedef Node *NodePtr; typedef typename BaseClassRef::Interval Interval; /** Define "BoundingBox" as a fixed-size or variable-size container depending * on "DIM" */ typedef typename BaseClassRef::BoundingBox BoundingBox; /** Define "distance_vector_t" as a fixed-size or variable-size container * depending on "DIM" */ typedef typename BaseClassRef::distance_vector_t distance_vector_t; /** * KDTree constructor * * Refer to docs in README.md or online in * https://github.com/jlblancoc/nanoflann * * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 * for 3D points) is determined by means of: * - The \a DIM template parameter if >0 (highest priority) * - Otherwise, the \a dimensionality parameter of this constructor. * * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ KDTreeSingleIndexDynamicAdaptor_( const int dimensionality, const DatasetAdaptor &inputData, std::vector &treeIndex_, const KDTreeSingleIndexAdaptorParams ¶ms = KDTreeSingleIndexAdaptorParams()) : dataset(inputData), index_params(params), treeIndex(treeIndex_), distance(inputData) { BaseClassRef::root_node = NULL; BaseClassRef::m_size = 0; BaseClassRef::m_size_at_index_build = 0; BaseClassRef::dim = dimensionality; if (DIM > 0) BaseClassRef::dim = DIM; BaseClassRef::m_leaf_max_size = params.leaf_max_size; } /** Assignment operator definiton */ KDTreeSingleIndexDynamicAdaptor_ operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); std::swap(index_params, tmp.index_params); std::swap(treeIndex, tmp.treeIndex); std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); std::swap(BaseClassRef::m_size_at_index_build, tmp.BaseClassRef::m_size_at_index_build); std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); return *this; } /** * Builds the index */ void buildIndex() { BaseClassRef::m_size = BaseClassRef::vind.size(); this->freeIndex(*this); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; if (BaseClassRef::m_size == 0) return; computeBoundingBox(BaseClassRef::root_bbox); BaseClassRef::root_node = this->divideTree(*this, 0, BaseClassRef::m_size, BaseClassRef::root_bbox); // construct the tree } /** \name Query methods * @{ */ /** * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored * inside the result object. * * Params: * result = the result object in which the indices of the * nearest-neighbors are stored vec = the vector for which to search the * nearest neighbors * * \tparam RESULTSET Should be any ResultSet * \return True if the requested neighbors could be found. * \sa knnSearch, radiusSearch */ template bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const { assert(vec); if (this->size(*this) == 0) return false; if (!BaseClassRef::root_node) return false; float epsError = 1 + searchParams.eps; // fixed or variable-sized container (depending on DIM) distance_vector_t dists; // Fill it with zeros. assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), static_cast(0)); DistanceType distsq = this->computeInitialDistances(*this, vec, dists); searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither // used nor returned to the user. return result.full(); } /** * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. * Their indices are stored inside the result object. \sa radiusSearch, * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility * with the original FLANN interface. \return Number `N` of valid points in * the result set. Only the first `N` entries in `out_indices` and * `out_distances_sq` will be valid. Return may be less than `num_closest` * only if the number of elements in the tree is less than `num_closest`. */ size_t knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); return resultSet.size(); } /** * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. * The output is given as a vector of pairs, of which the first element is a * point index and the second the corresponding distance. Previous contents of * \a IndicesDists are cleared. * * If searchParams.sorted==true, the output list is sorted by ascending * distances. * * For a better performance, it is advisable to do a .reserve() on the vector * if you have any wild guess about the number of expected matches. * * \sa knnSearch, findNeighbors, radiusSearchCustomCallback * \return The number of points within the given radius (i.e. indices.size() * or dists.size() ) */ size_t radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector> &IndicesDists, const SearchParams &searchParams) const { RadiusResultSet resultSet(radius, IndicesDists); const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); if (searchParams.sorted) std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); return nFound; } /** * Just like radiusSearch() but with a custom callback class for each point * found in the radius of the query. See the source of RadiusResultSet<> as a * start point for your own classes. \sa radiusSearch */ template size_t radiusSearchCustomCallback( const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams = SearchParams()) const { this->findNeighbors(resultSet, query_point, searchParams); return resultSet.size(); } /** @} */ public: void computeBoundingBox(BoundingBox &bbox) { resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dataset.kdtree_get_bbox(bbox)) { // Done! It was implemented in derived class } else { const size_t N = BaseClassRef::m_size; if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but " "no data points found."); for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { bbox[i].low = bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[0], i); } for (size_t k = 1; k < N; ++k) { for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); } } } } /** * Performs an exact search in the tree starting from a node. * \tparam RESULTSET Should be any ResultSet */ template void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const { /* If this is a leaf node, then do check and return. */ if ((node->child1 == NULL) && (node->child2 == NULL)) { // count_leaf += (node->lr.right-node->lr.left); // Removed since was // neither used nor returned to the user. DistanceType worst_dist = result_set.worstDist(); for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { const IndexType index = BaseClassRef::vind[i]; // reorder... : i; if (treeIndex[index] == -1) continue; DistanceType dist = distance.evalMetric( vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dist < worst_dist) { if (!result_set.addPoint( static_cast(dist), static_cast( BaseClassRef::vind[i]))) { // the resultset doesn't want to receive any more points, we're done // searching! return; // false; } } } return; } /* Which child branch should be taken first? */ int idx = node->node_type.sub.divfeat; ElementType val = vec[idx]; DistanceType diff1 = val - node->node_type.sub.divlow; DistanceType diff2 = val - node->node_type.sub.divhigh; NodePtr bestChild; NodePtr otherChild; DistanceType cut_dist; if ((diff1 + diff2) < 0) { bestChild = node->child1; otherChild = node->child2; cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); } else { bestChild = node->child2; otherChild = node->child1; cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); } /* Call recursively to search next level down. */ searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); DistanceType dst = dists[idx]; mindistsq = mindistsq + cut_dist - dst; dists[idx] = cut_dist; if (mindistsq * epsError <= result_set.worstDist()) { searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); } dists[idx] = dst; } public: /** Stores the index in a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when * loading the index object it must be constructed associated to the same * source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the * index object must be constructed associated to the same source of data * points used while building the index. See the example: * examples/saveload_example.cpp \sa loadIndex */ void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } }; /** kd-tree dynaimic index * * class to create multiple static index and merge their results to behave as * single dynamic index as proposed in Logarithmic Approach. * * Example of usage: * examples/dynamic_pointcloud_example.cpp * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ template class KDTreeSingleIndexDynamicAdaptor { public: typedef typename Distance::ElementType ElementType; typedef typename Distance::DistanceType DistanceType; protected: size_t m_leaf_max_size; size_t treeCount; size_t pointCount; /** * The dataset used by this index */ const DatasetAdaptor &dataset; //!< The source of our data std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which //!< point at idx is stored. treeIndex[idx]=-1 //!< means that point has been removed. KDTreeSingleIndexAdaptorParams index_params; int dim; //!< Dimensionality of each data point typedef KDTreeSingleIndexDynamicAdaptor_ index_container_t; std::vector index; public: /** Get a const ref to the internal list of indices; the number of indices is * adapted dynamically as the dataset grows in size. */ const std::vector &getAllIndices() const { return index; } private: /** finds position of least significant unset bit */ int First0Bit(IndexType num) { int pos = 0; while (num & 1) { num = num >> 1; pos++; } return pos; } /** Creates multiple empty trees to handle dynamic support */ void init() { typedef KDTreeSingleIndexDynamicAdaptor_ my_kd_tree_t; std::vector index_( treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); index = index_; } public: Distance distance; /** * KDTree constructor * * Refer to docs in README.md or online in * https://github.com/jlblancoc/nanoflann * * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 * for 3D points) is determined by means of: * - The \a DIM template parameter if >0 (highest priority) * - Otherwise, the \a dimensionality parameter of this constructor. * * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ KDTreeSingleIndexDynamicAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms = KDTreeSingleIndexAdaptorParams(), const size_t maximumPointCount = 1000000000U) : dataset(inputData), index_params(params), distance(inputData) { treeCount = static_cast(std::log2(maximumPointCount)); pointCount = 0U; dim = dimensionality; treeIndex.clear(); if (DIM > 0) dim = DIM; m_leaf_max_size = params.leaf_max_size; init(); const size_t num_initial_points = dataset.kdtree_get_point_count(); if (num_initial_points > 0) { addPoints(0, num_initial_points - 1); } } /** Deleted copy constructor*/ KDTreeSingleIndexDynamicAdaptor( const KDTreeSingleIndexDynamicAdaptor &) = delete; /** Add points to the set, Inserts all points from [start, end] */ void addPoints(IndexType start, IndexType end) { size_t count = end - start + 1; treeIndex.resize(treeIndex.size() + count); for (IndexType idx = start; idx <= end; idx++) { int pos = First0Bit(pointCount); index[pos].vind.clear(); treeIndex[pointCount] = pos; for (int i = 0; i < pos; i++) { for (int j = 0; j < static_cast(index[i].vind.size()); j++) { index[pos].vind.push_back(index[i].vind[j]); if (treeIndex[index[i].vind[j]] != -1) treeIndex[index[i].vind[j]] = pos; } index[i].vind.clear(); index[i].freeIndex(index[i]); } index[pos].vind.push_back(idx); index[pos].buildIndex(); pointCount++; } } /** Remove a point from the set (Lazy Deletion) */ void removePoint(size_t idx) { if (idx >= pointCount) return; treeIndex[idx] = -1; } /** * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored * inside the result object. * * Params: * result = the result object in which the indices of the * nearest-neighbors are stored vec = the vector for which to search the * nearest neighbors * * \tparam RESULTSET Should be any ResultSet * \return True if the requested neighbors could be found. * \sa knnSearch, radiusSearch */ template bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const { for (size_t i = 0; i < treeCount; i++) { index[i].findNeighbors(result, &vec[0], searchParams); } return result.full(); } }; /** An L2-metric KD-tree adaptor for working with data directly stored in an * Eigen Matrix, without duplicating the data storage. Each row in the matrix * represents a point in the state space. * * Example of usage: * \code * Eigen::Matrix mat; * // Fill out "mat"... * * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf * ); mat_index.index->buildIndex(); mat_index.index->... \endcode * * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality * for the points in the data set, allowing more compiler optimizations. \tparam * Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. */ template struct KDTreeEigenMatrixAdaptor { typedef KDTreeEigenMatrixAdaptor self_t; typedef typename MatrixType::Scalar num_t; typedef typename MatrixType::Index IndexType; typedef typename Distance::template traits::distance_t metric_t; typedef KDTreeSingleIndexAdaptor index_t; index_t *index; //! The kd-tree index for the user to call its methods as //! usual with any other FLANN index. /// Constructor: takes a const ref to the matrix object with the data points KDTreeEigenMatrixAdaptor(const size_t dimensionality, const std::reference_wrapper &mat, const int leaf_max_size = 10) : m_data_matrix(mat) { const auto dims = mat.get().cols(); if (size_t(dims) != dimensionality) throw std::runtime_error( "Error: 'dimensionality' must match column count in data matrix"); if (DIM > 0 && int(dims) != DIM) throw std::runtime_error( "Data set dimensionality does not match the 'DIM' template argument"); index = new index_t(static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); index->buildIndex(); } public: /** Deleted copy constructor */ KDTreeEigenMatrixAdaptor(const self_t &) = delete; ~KDTreeEigenMatrixAdaptor() { delete index; } const std::reference_wrapper m_data_matrix; /** Query for the \a num_closest closest points to a given point (entered as * query_point[0:dim-1]). Note that this is a short-cut method for * index->findNeighbors(). The user can also call index->... methods as * desired. \note nChecks_IGNORED is ignored but kept for compatibility with * the original FLANN interface. */ inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); } /** @name Interface expected by KDTreeSingleIndexAdaptor * @{ */ const self_t &derived() const { return *this; } self_t &derived() { return *this; } // Must return the number of data points inline size_t kdtree_get_point_count() const { return m_data_matrix.get().rows(); } // Returns the dim'th component of the idx'th point in the class: inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { return m_data_matrix.get().coeff(idx, IndexType(dim)); } // Optional bounding-box computation: return false to default to a standard // bbox computation loop. // Return true if the BBOX was already computed by the class and returned in // "bb" so it can be avoided to redo it again. Look at bb.size() to find out // the expected dimensionality (e.g. 2 or 3 for point clouds) template bool kdtree_get_bbox(BBOX & /*bb*/) const { return false; } /** @} */ }; // end of KDTreeEigenMatrixAdaptor /** @} */ /** @} */ // end of grouping } // namespace nanoflann #endif /* NANOFLANN_HPP_ */ ================================================ FILE: SC-PGO/launch/aloam_mulran.launch ================================================ ================================================ FILE: SC-PGO/launch/aloam_velodyne_HDL_32.launch ================================================ ================================================ FILE: SC-PGO/launch/aloam_velodyne_HDL_64.launch ================================================ ================================================ FILE: SC-PGO/launch/aloam_velodyne_VLP_16.launch ================================================ ================================================ FILE: SC-PGO/launch/fastlio_ouster64.launch ================================================ ================================================ FILE: SC-PGO/launch/kitti_helper.launch ================================================ ================================================ FILE: SC-PGO/package.xml ================================================ aloam_velodyne 0.0.0 This is an advanced implentation of LOAM. LOAM is described in the following paper: J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. qintong BSD Ji Zhang catkin geometry_msgs nav_msgs roscpp rospy std_msgs rosbag sensor_msgs tf image_transport geometry_msgs nav_msgs sensor_msgs roscpp rospy std_msgs rosbag tf image_transport ================================================ FILE: SC-PGO/rviz_cfg/aloam_velodyne.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /odometry1/odomPath1 - /pgoOdom1/Shape1 - /pgoMap1 Splitter Ratio: 0.440559446811676 Tree Height: 787 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: pgoMap - Class: rviz/Views Expanded: - /Current View1 Name: pgo Views Splitter Ratio: 0.5 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 160 Reference Frame: Value: false - Class: rviz/Axes Enabled: true Length: 1 Name: Axes Radius: 0.10000000149011612 Reference Frame: camera_init Value: true - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: gtPathlc Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /path_gt Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 170; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: gtPathLidar Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /path_gt_lidar Unreliable: false Value: true Enabled: false Name: GT - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 204; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 0.20000000298023224 Name: odomPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /laser_odom_path Unreliable: false 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/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 63.10047912597656 Min Color: 0; 0; 0 Min Intensity: -0.0067862942814826965 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Flat Squares Topic: /velodyne_cloud_2 Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false Enabled: true Name: odometry - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 4; 69; 246 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 0.30000001192092896 Name: mappingPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /aft_mapped_path Unreliable: false 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/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 50.14332962036133 Min Color: 0; 0; 0 Min Intensity: -0.04392780363559723 Name: allMapCloud Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.20000000298023224 Style: Points Topic: /laser_cloud_map Unreliable: false Use Fixed Frame: true Use rainbow: false Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 15 Min Color: 0; 0; 0 Min Intensity: 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1 Size (m): 0.05000000074505806 Style: Squares Topic: /laser_cloud_surround Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 50.141441345214844 Min Color: 0; 0; 0 Min Intensity: -0.024373823776841164 Name: currPoints Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1.5 Size (m): 0.10000000149011612 Style: Squares Topic: /velodyne_cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1.5 Axes Radius: 0.5 Color: 255; 25; 0 Head Length: 0.10000000149011612 Head Radius: 2 Shaft Length: 0.10000000149011612 Shaft Radius: 20 Value: Axes Topic: /aft_mapped_to_init Unreliable: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false Enabled: true Name: mapping - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Billboards Line Width: 0.4000000059604645 Name: pgoPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: Axes Radius: 0.10000000149011612 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /aft_pgo_path Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: pgoOdom Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 2 Axes Radius: 0.699999988079071 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /aft_pgo_odom Unreliable: false Value: true - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Max Intensity: 300 Min Color: 0; 0; 0 Min Intensity: 0 Name: pgoMap Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 2 Size (m): 0.4000000059604645 Style: Points Topic: /aft_pgo_map Unreliable: false Use Fixed Frame: true Use rainbow: true 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/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 20.06364631652832 Min Color: 0; 0; 0 Min Intensity: 3.003571033477783 Name: loop scan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.5 Style: Spheres Topic: /loop_scan_local Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 32.84956741333008 Min Value: -16.756437301635742 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 20.100271224975586 Min Color: 0; 0; 0 Min Intensity: 3.0008726119995117 Name: loop submap Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 2 Size (m): 0.20000000298023224 Style: Spheres Topic: /loop_submap_local Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false Enabled: true Global Options: Background Color: 255; 255; 255 Default Light: true Fixed Frame: camera_init Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - 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: 638.897705078125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 191.27947998046875 Y: 248.57839965820312 Z: -12.774103164672852 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.5697963237762451 Target Frame: aft_pgo Value: Orbit (rviz) Yaw: 3.1635923385620117 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1016 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd0000000400000000000001af0000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002ef000000c30000000000000000fb0000001200700067006f00200056006900650077007300000002db000000d7000000a400ffffff000000010000010f00000267fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000002670000000000fffffffaffffffff0100000002fb0000000a005600690065007700730000000000ffffffff0000010000fffffffb0000000a0056006900650077007300000006710000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007530000003efc0100000002fb0000000800540069006d0065000000000000000753000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000059e0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1875 X: 1965 Y: 27 pgo Views: collapsed: false ================================================ FILE: SC-PGO/src/kittiHelper.cpp ================================================ // Author: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include std::vector read_lidar_data(const std::string lidar_data_path) { std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary); lidar_data_file.seekg(0, std::ios::end); const size_t num_elements = lidar_data_file.tellg() / sizeof(float); lidar_data_file.seekg(0, std::ios::beg); std::vector lidar_data_buffer(num_elements); lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), num_elements*sizeof(float)); return lidar_data_buffer; } int main(int argc, char** argv) { ros::init(argc, argv, "kitti_helper"); ros::NodeHandle n("~"); std::string dataset_folder, sequence_number, output_bag_file; n.getParam("dataset_folder", dataset_folder); n.getParam("sequence_number", sequence_number); std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n'; bool to_bag; n.getParam("to_bag", to_bag); if (to_bag) n.getParam("output_bag_file", output_bag_file); int publish_delay; n.getParam("publish_delay", publish_delay); publish_delay = publish_delay <= 0 ? 1 : publish_delay; ros::Publisher pub_laser_cloud = n.advertise("/velodyne_points", 2); image_transport::ImageTransport it(n); image_transport::Publisher pub_image_left = it.advertise("/image_left", 2); image_transport::Publisher pub_image_right = it.advertise("/image_right", 2); ros::Publisher pubOdomGT = n.advertise ("/odometry_gt", 5); nav_msgs::Odometry odomGT; odomGT.header.frame_id = "camera_init"; odomGT.child_frame_id = "/ground_truth"; ros::Publisher pubPathGT = n.advertise ("/path_gt", 5); nav_msgs::Path pathGT; pathGT.header.frame_id = "camera_init"; std::string timestamp_path = "sequences/" + sequence_number + "/times.txt"; std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in); std::string ground_truth_path = "results/" + sequence_number + ".txt"; std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in); rosbag::Bag bag_out; if (to_bag) bag_out.open(output_bag_file, rosbag::bagmode::Write); Eigen::Matrix3d R_transform; R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0; Eigen::Quaterniond q_transform(R_transform); std::string line; std::size_t line_num = 0; ros::Rate r(10.0 / publish_delay); while (std::getline(timestamp_file, line) && ros::ok()) { float timestamp = stof(line); std::stringstream left_image_path, right_image_path; left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png"; cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE); right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png"; cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE); std::getline(ground_truth_file, line); std::stringstream pose_stream(line); std::string s; Eigen::Matrix gt_pose; for (std::size_t i = 0; i < 3; ++i) { for (std::size_t j = 0; j < 4; ++j) { std::getline(pose_stream, s, ' '); gt_pose(i, j) = stof(s); } } Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>()); Eigen::Quaterniond q = q_transform * q_w_i; q.normalize(); Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>(); odomGT.header.stamp = ros::Time().fromSec(timestamp); odomGT.pose.pose.orientation.x = q.x(); odomGT.pose.pose.orientation.y = q.y(); odomGT.pose.pose.orientation.z = q.z(); odomGT.pose.pose.orientation.w = q.w(); odomGT.pose.pose.position.x = t(0); odomGT.pose.pose.position.y = t(1); odomGT.pose.pose.position.z = t(2); pubOdomGT.publish(odomGT); geometry_msgs::PoseStamped poseGT; poseGT.header = odomGT.header; poseGT.pose = odomGT.pose.pose; pathGT.header.stamp = odomGT.header.stamp; pathGT.poses.push_back(poseGT); pubPathGT.publish(pathGT); // read lidar point cloud std::stringstream lidar_data_path; lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" << std::setfill('0') << std::setw(6) << line_num << ".bin"; std::vector lidar_data = read_lidar_data(lidar_data_path.str()); std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n"; std::vector lidar_points; std::vector lidar_intensities; pcl::PointCloud laser_cloud; for (std::size_t i = 0; i < lidar_data.size(); i += 4) { lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]); lidar_intensities.push_back(lidar_data[i+3]); pcl::PointXYZI point; point.x = lidar_data[i]; point.y = lidar_data[i + 1]; point.z = lidar_data[i + 2]; point.intensity = lidar_data[i + 3]; laser_cloud.push_back(point); } sensor_msgs::PointCloud2 laser_cloud_msg; pcl::toROSMsg(laser_cloud, laser_cloud_msg); laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp); laser_cloud_msg.header.frame_id = "camera_init"; pub_laser_cloud.publish(laser_cloud_msg); sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg(); sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg(); pub_image_left.publish(image_left_msg); pub_image_right.publish(image_right_msg); if (to_bag) { bag_out.write("/image_left", ros::Time::now(), image_left_msg); bag_out.write("/image_right", ros::Time::now(), image_right_msg); bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg); bag_out.write("/path_gt", ros::Time::now(), pathGT); bag_out.write("/odometry_gt", ros::Time::now(), odomGT); } line_num ++; r.sleep(); } bag_out.close(); std::cout << "Done \n"; return 0; } ================================================ FILE: SC-PGO/src/laserMapping.cpp ================================================ // This is an advanced implementation of the algorithm described in the following paper: // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. // Modifier: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk // Copyright 2013, Ji Zhang, Carnegie Mellon University // Further contributions copyright (c) 2016, Southwest Research Institute // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // 1. Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // 2. Redistributions in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // 3. Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived from this // software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "lidarFactor.hpp" #include "aloam_velodyne/common.h" #include "aloam_velodyne/tic_toc.h" int frameCount = 0; double timeLaserCloudCornerLast = 0; double timeLaserCloudSurfLast = 0; double timeLaserCloudFullRes = 0; double timeLaserOdometry = 0; int laserCloudCenWidth = 10; int laserCloudCenHeight = 10; int laserCloudCenDepth = 5; const int laserCloudWidth = 21; const int laserCloudHeight = 21; const int laserCloudDepth = 11; const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851 int laserCloudValidInd[125]; int laserCloudSurroundInd[125]; // input: from odom pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); // ouput: all visualble cube points pcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud()); // surround points in map to build tree pcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud()); pcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud()); //input & output: points in one frame. local --> global pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); // points in every cube pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum]; pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum]; //kd-tree pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN()); pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; Eigen::Map q_w_curr(parameters); Eigen::Map t_w_curr(parameters + 4); // wmap_T_odom * odom_T_curr = wmap_T_curr; // transformation between odom's world and map's world frame Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0); Eigen::Vector3d t_wmap_wodom(0, 0, 0); Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0); Eigen::Vector3d t_wodom_curr(0, 0, 0); std::queue cornerLastBuf; std::queue surfLastBuf; std::queue fullResBuf; std::queue odometryBuf; std::mutex mBuf; pcl::VoxelGrid downSizeFilterCorner; pcl::VoxelGrid downSizeFilterSurf; std::vector pointSearchInd; std::vector pointSearchSqDis; PointType pointOri, pointSel; ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath; ros::Publisher pubLaserCloudFullResLocal; nav_msgs::Path laserAfterMappedPath; // set initial guess void transformAssociateToMap() { q_w_curr = q_wmap_wodom * q_wodom_curr; t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; } void transformUpdate() { q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; } void pointAssociateToMap(PointType const *const pi, PointType *const po) { Eigen::Vector3d point_curr(pi->x, pi->y, pi->z); Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr; po->x = point_w.x(); po->y = point_w.y(); po->z = point_w.z(); po->intensity = pi->intensity; //po->intensity = 1.0; } void pointAssociateTobeMapped(PointType const *const pi, PointType *const po) { Eigen::Vector3d point_w(pi->x, pi->y, pi->z); Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr); po->x = point_curr.x(); po->y = point_curr.y(); po->z = point_curr.z(); po->intensity = pi->intensity; } void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2) { mBuf.lock(); cornerLastBuf.push(laserCloudCornerLast2); mBuf.unlock(); } void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2) { mBuf.lock(); surfLastBuf.push(laserCloudSurfLast2); mBuf.unlock(); } void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) { mBuf.lock(); fullResBuf.push(laserCloudFullRes2); mBuf.unlock(); } //receive odomtry void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry) { mBuf.lock(); odometryBuf.push(laserOdometry); mBuf.unlock(); // high frequence publish Eigen::Quaterniond q_wodom_curr; Eigen::Vector3d t_wodom_curr; q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x; q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y; q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z; q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w; t_wodom_curr.x() = laserOdometry->pose.pose.position.x; t_wodom_curr.y() = laserOdometry->pose.pose.position.y; t_wodom_curr.z() = laserOdometry->pose.pose.position.z; Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr; Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; nav_msgs::Odometry odomAftMapped; odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "/aft_mapped"; odomAftMapped.header.stamp = laserOdometry->header.stamp; odomAftMapped.pose.pose.orientation.x = q_w_curr.x(); odomAftMapped.pose.pose.orientation.y = q_w_curr.y(); odomAftMapped.pose.pose.orientation.z = q_w_curr.z(); odomAftMapped.pose.pose.orientation.w = q_w_curr.w(); odomAftMapped.pose.pose.position.x = t_w_curr.x(); odomAftMapped.pose.pose.position.y = t_w_curr.y(); odomAftMapped.pose.pose.position.z = t_w_curr.z(); pubOdomAftMappedHighFrec.publish(odomAftMapped); } void process() { while(1) { while (!cornerLastBuf.empty() && !surfLastBuf.empty() && !fullResBuf.empty() && !odometryBuf.empty()) { mBuf.lock(); while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) odometryBuf.pop(); if (odometryBuf.empty()) { mBuf.unlock(); break; } while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) surfLastBuf.pop(); if (surfLastBuf.empty()) { mBuf.unlock(); break; } while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) fullResBuf.pop(); if (fullResBuf.empty()) { mBuf.unlock(); break; } timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec(); timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec(); timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec(); timeLaserOdometry = odometryBuf.front()->header.stamp.toSec(); if (timeLaserCloudCornerLast != timeLaserOdometry || timeLaserCloudSurfLast != timeLaserOdometry || timeLaserCloudFullRes != timeLaserOdometry) { //printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry); //printf("unsync messeage!"); mBuf.unlock(); break; } laserCloudCornerLast->clear(); pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast); cornerLastBuf.pop(); laserCloudSurfLast->clear(); pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast); surfLastBuf.pop(); laserCloudFullRes->clear(); pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes); fullResBuf.pop(); q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x; q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y; q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z; q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w; t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x; t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y; t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z; odometryBuf.pop(); while(!cornerLastBuf.empty()) { cornerLastBuf.pop(); //printf("drop lidar frame in mapping for real time performance \n"); } mBuf.unlock(); TicToc t_whole; transformAssociateToMap(); TicToc t_shift; int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth; int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight; int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth; if (t_w_curr.x() + 25.0 < 0) centerCubeI--; if (t_w_curr.y() + 25.0 < 0) centerCubeJ--; if (t_w_curr.z() + 25.0 < 0) centerCubeK--; while (centerCubeI < 3) { for (int j = 0; j < laserCloudHeight; j++) { for (int k = 0; k < laserCloudDepth; k++) { int i = laserCloudWidth - 1; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; for (; i >= 1; i--) { laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeI++; laserCloudCenWidth++; } while (centerCubeI >= laserCloudWidth - 3) { for (int j = 0; j < laserCloudHeight; j++) { for (int k = 0; k < laserCloudDepth; k++) { int i = 0; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; for (; i < laserCloudWidth - 1; i++) { laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeI--; laserCloudCenWidth--; } while (centerCubeJ < 3) { for (int i = 0; i < laserCloudWidth; i++) { for (int k = 0; k < laserCloudDepth; k++) { int j = laserCloudHeight - 1; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; for (; j >= 1; j--) { laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeJ++; laserCloudCenHeight++; } while (centerCubeJ >= laserCloudHeight - 3) { for (int i = 0; i < laserCloudWidth; i++) { for (int k = 0; k < laserCloudDepth; k++) { int j = 0; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; for (; j < laserCloudHeight - 1; j++) { laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeJ--; laserCloudCenHeight--; } while (centerCubeK < 3) { for (int i = 0; i < laserCloudWidth; i++) { for (int j = 0; j < laserCloudHeight; j++) { int k = laserCloudDepth - 1; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; for (; k >= 1; k--) { laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeK++; laserCloudCenDepth++; } while (centerCubeK >= laserCloudDepth - 3) { for (int i = 0; i < laserCloudWidth; i++) { for (int j = 0; j < laserCloudHeight; j++) { int k = 0; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; for (; k < laserCloudDepth - 1; k++) { laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeK--; laserCloudCenDepth--; } int laserCloudValidNum = 0; int laserCloudSurroundNum = 0; for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) { for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) { for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) { if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth) { laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudValidNum++; laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudSurroundNum++; } } } } laserCloudCornerFromMap->clear(); laserCloudSurfFromMap->clear(); for (int i = 0; i < laserCloudValidNum; i++) { *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]]; *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]]; } int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size(); int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size(); pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud()); downSizeFilterCorner.setInputCloud(laserCloudCornerLast); downSizeFilterCorner.filter(*laserCloudCornerStack); int laserCloudCornerStackNum = laserCloudCornerStack->points.size(); pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud()); downSizeFilterSurf.setInputCloud(laserCloudSurfLast); downSizeFilterSurf.filter(*laserCloudSurfStack); int laserCloudSurfStackNum = laserCloudSurfStack->points.size(); //printf("map prepare time %f ms\n", t_shift.toc()); //printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum); if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50) { TicToc t_opt; TicToc t_tree; kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap); //printf("build tree time %f ms \n", t_tree.toc()); for (int iterCount = 0; iterCount < 2; iterCount++) { //ceres::LossFunction *loss_function = NULL; ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization(); ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); problem.AddParameterBlock(parameters, 4, q_parameterization); problem.AddParameterBlock(parameters + 4, 3); TicToc t_data; int corner_num = 0; for (int i = 0; i < laserCloudCornerStackNum; i++) { pointOri = laserCloudCornerStack->points[i]; //double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z; pointAssociateToMap(&pointOri, &pointSel); kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); if (pointSearchSqDis[4] < 1.0) { std::vector nearCorners; Eigen::Vector3d center(0, 0, 0); for (int j = 0; j < 5; j++) { Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x, laserCloudCornerFromMap->points[pointSearchInd[j]].y, laserCloudCornerFromMap->points[pointSearchInd[j]].z); center = center + tmp; nearCorners.push_back(tmp); } center = center / 5.0; Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); for (int j = 0; j < 5; j++) { Eigen::Matrix tmpZeroMean = nearCorners[j] - center; covMat = covMat + tmpZeroMean * tmpZeroMean.transpose(); } Eigen::SelfAdjointEigenSolver saes(covMat); // if is indeed line feature // note Eigen library sort eigenvalues in increasing order Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) { Eigen::Vector3d point_on_line = center; Eigen::Vector3d point_a, point_b; point_a = 0.1 * unit_direction + point_on_line; point_b = -0.1 * unit_direction + point_on_line; ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0); problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); corner_num++; } } /* else if(pointSearchSqDis[4] < 0.01 * sqrtDis) { Eigen::Vector3d center(0, 0, 0); for (int j = 0; j < 5; j++) { Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x, laserCloudCornerFromMap->points[pointSearchInd[j]].y, laserCloudCornerFromMap->points[pointSearchInd[j]].z); center = center + tmp; } center = center / 5.0; Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center); problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); } */ } int surf_num = 0; for (int i = 0; i < laserCloudSurfStackNum; i++) { pointOri = laserCloudSurfStack->points[i]; //double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z; pointAssociateToMap(&pointOri, &pointSel); kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); Eigen::Matrix matA0; Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones(); if (pointSearchSqDis[4] < 1.0) { for (int j = 0; j < 5; j++) { matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x; matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y; matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z; ////printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2)); } // find the norm of plane Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); double negative_OA_dot_norm = 1 / norm.norm(); norm.normalize(); // Here n(pa, pb, pc) is unit norm of plane bool planeValid = true; for (int j = 0; j < 5; j++) { // if OX * n > 0.2, then plane is not fit well if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x + norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y + norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2) { planeValid = false; break; } } Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); if (planeValid) { ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm); problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); surf_num++; } } /* else if(pointSearchSqDis[4] < 0.01 * sqrtDis) { Eigen::Vector3d center(0, 0, 0); for (int j = 0; j < 5; j++) { Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x, laserCloudSurfFromMap->points[pointSearchInd[j]].y, laserCloudSurfFromMap->points[pointSearchInd[j]].z); center = center + tmp; } center = center / 5.0; Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center); problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); } */ } ////printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num); ////printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num); //printf("mapping data assosiation time %f ms \n", t_data.toc()); TicToc t_solver; ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; options.max_num_iterations = 4; options.minimizer_progress_to_stdout = false; options.check_gradients = false; options.gradient_check_relative_precision = 1e-4; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); //printf("mapping solver time %f ms \n", t_solver.toc()); ////printf("time %f \n", timeLaserOdometry); ////printf("corner factor num %d surf factor num %d\n", corner_num, surf_num); ////printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2], // parameters[4], parameters[5], parameters[6]); } //printf("mapping optimization time %f \n", t_opt.toc()); } else { ROS_WARN("time Map corner and surf num are not enough"); } transformUpdate(); TicToc t_add; for (int i = 0; i < laserCloudCornerStackNum; i++) { pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel); int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; if (pointSel.x + 25.0 < 0) cubeI--; if (pointSel.y + 25.0 < 0) cubeJ--; if (pointSel.z + 25.0 < 0) cubeK--; if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth) { int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; laserCloudCornerArray[cubeInd]->push_back(pointSel); } } for (int i = 0; i < laserCloudSurfStackNum; i++) { pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel); int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; if (pointSel.x + 25.0 < 0) cubeI--; if (pointSel.y + 25.0 < 0) cubeJ--; if (pointSel.z + 25.0 < 0) cubeK--; if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth) { int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; laserCloudSurfArray[cubeInd]->push_back(pointSel); } } //printf("add points time %f ms\n", t_add.toc()); TicToc t_filter; for (int i = 0; i < laserCloudValidNum; i++) { int ind = laserCloudValidInd[i]; pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud()); downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]); downSizeFilterCorner.filter(*tmpCorner); laserCloudCornerArray[ind] = tmpCorner; pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud()); downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]); downSizeFilterSurf.filter(*tmpSurf); laserCloudSurfArray[ind] = tmpSurf; } //printf("filter time %f ms \n", t_filter.toc()); TicToc t_pub; //publish surround map for every 5 frame if (frameCount % 5 == 0) { laserCloudSurround->clear(); for (int i = 0; i < laserCloudSurroundNum; i++) { int ind = laserCloudSurroundInd[i]; *laserCloudSurround += *laserCloudCornerArray[ind]; *laserCloudSurround += *laserCloudSurfArray[ind]; } sensor_msgs::PointCloud2 laserCloudSurround3; pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3); laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry); laserCloudSurround3.header.frame_id = "camera_init"; pubLaserCloudSurround.publish(laserCloudSurround3); } if (frameCount % 20 == 0) { pcl::PointCloud laserCloudMap; for (int i = 0; i < 4851; i++) { laserCloudMap += *laserCloudCornerArray[i]; laserCloudMap += *laserCloudSurfArray[i]; } sensor_msgs::PointCloud2 laserCloudMsg; pcl::toROSMsg(laserCloudMap, laserCloudMsg); laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry); laserCloudMsg.header.frame_id = "camera_init"; pubLaserCloudMap.publish(laserCloudMsg); } sensor_msgs::PointCloud2 laserCloudFullRes3Local; pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3Local); laserCloudFullRes3Local.header.stamp = ros::Time().fromSec(timeLaserOdometry); laserCloudFullRes3Local.header.frame_id = "camera_init"; pubLaserCloudFullResLocal.publish(laserCloudFullRes3Local); int laserCloudFullResNum = laserCloudFullRes->points.size(); for (int i = 0; i < laserCloudFullResNum; i++) { pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); } sensor_msgs::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry); laserCloudFullRes3.header.frame_id = "camera_init"; pubLaserCloudFullRes.publish(laserCloudFullRes3); //printf("mapping pub time %f ms \n", t_pub.toc()); //printf("whole mapping time %f ms ++++++++++\n", t_whole.toc()); nav_msgs::Odometry odomAftMapped; odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "/aft_mapped"; odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); odomAftMapped.pose.pose.orientation.x = q_w_curr.x(); odomAftMapped.pose.pose.orientation.y = q_w_curr.y(); odomAftMapped.pose.pose.orientation.z = q_w_curr.z(); odomAftMapped.pose.pose.orientation.w = q_w_curr.w(); odomAftMapped.pose.pose.position.x = t_w_curr.x(); odomAftMapped.pose.pose.position.y = t_w_curr.y(); odomAftMapped.pose.pose.position.z = t_w_curr.z(); pubOdomAftMapped.publish(odomAftMapped); // double roll, pitch, yaw; // geometry_msgs::Quaternion quat = odomAftMapped.pose.pose.orientation; // tf::Matrix3x3(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(roll, pitch, yaw); // std::cout << "pub - roll, pitch, yaw (deg): " << rad2deg(roll) << ", " << rad2deg(pitch) << ", " << rad2deg(yaw) << std::endl; geometry_msgs::PoseStamped laserAfterMappedPose; laserAfterMappedPose.header = odomAftMapped.header; laserAfterMappedPose.pose = odomAftMapped.pose.pose; laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp; laserAfterMappedPath.header.frame_id = "camera_init"; laserAfterMappedPath.poses.push_back(laserAfterMappedPose); pubLaserAfterMappedPath.publish(laserAfterMappedPath); static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; transform.setOrigin(tf::Vector3(t_w_curr(0), t_w_curr(1), t_w_curr(2))); q.setW(q_w_curr.w()); q.setX(q_w_curr.x()); q.setY(q_w_curr.y()); q.setZ(q_w_curr.z()); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "/aft_mapped")); frameCount++; } std::chrono::milliseconds dura(2); std::this_thread::sleep_for(dura); } } int main(int argc, char **argv) { ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; float lineRes = 0; float planeRes = 0; nh.param("mapping_line_resolution", lineRes, 0.4); nh.param("mapping_plane_resolution", planeRes, 0.8); //printf("line resolution %f plane resolution %f \n", lineRes, planeRes); downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes); downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes); ros::Subscriber subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler); ros::Subscriber subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler); ros::Subscriber subLaserOdometry = nh.subscribe("/laser_odom_to_init", 100, laserOdometryHandler); ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_3", 100, laserCloudFullResHandler); pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 100); pubLaserCloudMap = nh.advertise("/laser_cloud_map", 100); pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_registered", 100); pubLaserCloudFullResLocal = nh.advertise("/velodyne_cloud_registered_local", 100); pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 100); pubOdomAftMappedHighFrec = nh.advertise("/aft_mapped_to_init_high_frec", 100); pubLaserAfterMappedPath = nh.advertise("/aft_mapped_path", 100); for (int i = 0; i < laserCloudNum; i++) { laserCloudCornerArray[i].reset(new pcl::PointCloud()); laserCloudSurfArray[i].reset(new pcl::PointCloud()); } std::thread mapping_process{process}; ros::spin(); return 0; } ================================================ FILE: SC-PGO/src/laserOdometry.cpp ================================================ // This is an advanced implementation of the algorithm described in the following paper: // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. // Modifier: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk // Copyright 2013, Ji Zhang, Carnegie Mellon University // Further contributions copyright (c) 2016, Southwest Research Institute // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // 1. Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // 2. Redistributions in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // 3. Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived from this // software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "aloam_velodyne/common.h" #include "aloam_velodyne/tic_toc.h" #include "lidarFactor.hpp" #define DISTORTION 0 int corner_correspondence = 0, plane_correspondence = 0; constexpr double SCAN_PERIOD = 0.1; constexpr double DISTANCE_SQ_THRESHOLD = 25; constexpr double NEARBY_SCAN = 2.5; int skipFrameNum = 5; bool systemInited = false; double timeCornerPointsSharp = 0; double timeCornerPointsLessSharp = 0; double timeSurfPointsFlat = 0; double timeSurfPointsLessFlat = 0; double timeLaserCloudFullRes = 0; pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN()); pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN()); pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); int laserCloudCornerLastNum = 0; int laserCloudSurfLastNum = 0; // Transformation from current frame to world frame Eigen::Quaterniond q_w_curr(1, 0, 0, 0); Eigen::Vector3d t_w_curr(0, 0, 0); // q_curr_last(x, y, z, w), t_curr_last double para_q[4] = {0, 0, 0, 1}; double para_t[3] = {0, 0, 0}; Eigen::Map q_last_curr(para_q); Eigen::Map t_last_curr(para_t); std::queue cornerSharpBuf; std::queue cornerLessSharpBuf; std::queue surfFlatBuf; std::queue surfLessFlatBuf; std::queue fullPointsBuf; std::mutex mBuf; // undistort lidar point void TransformToStart(PointType const *const pi, PointType *const po) { //interpolation ratio double s; if (DISTORTION) s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; else s = 1.0; //s = 1; Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr); Eigen::Vector3d t_point_last = s * t_last_curr; Eigen::Vector3d point(pi->x, pi->y, pi->z); Eigen::Vector3d un_point = q_point_last * point + t_point_last; po->x = un_point.x(); po->y = un_point.y(); po->z = un_point.z(); po->intensity = pi->intensity; } // transform all lidar points to the start of the next frame void TransformToEnd(PointType const *const pi, PointType *const po) { // undistort point first pcl::PointXYZI un_point_tmp; TransformToStart(pi, &un_point_tmp); Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z); Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr); po->x = point_end.x(); po->y = point_end.y(); po->z = point_end.z(); //Remove distortion time info po->intensity = int(pi->intensity); } void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2) { mBuf.lock(); cornerSharpBuf.push(cornerPointsSharp2); mBuf.unlock(); } void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2) { mBuf.lock(); cornerLessSharpBuf.push(cornerPointsLessSharp2); mBuf.unlock(); } void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2) { mBuf.lock(); surfFlatBuf.push(surfPointsFlat2); mBuf.unlock(); } void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2) { mBuf.lock(); surfLessFlatBuf.push(surfPointsLessFlat2); mBuf.unlock(); } //receive all point cloud void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) { mBuf.lock(); fullPointsBuf.push(laserCloudFullRes2); mBuf.unlock(); } int main(int argc, char **argv) { ros::init(argc, argv, "laserOdometry"); ros::NodeHandle nh; nh.param("mapping_skip_frame", skipFrameNum, 2); //printf("Mapping %d Hz \n", 10 / skipFrameNum); ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler); ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler); ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler); ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler); ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler); ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100); ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100); ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100); ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_to_init", 100); ros::Publisher pubLaserPath = nh.advertise("/laser_odom_path", 100); nav_msgs::Path laserPath; int frameCount = 0; ros::Rate rate(100); while (ros::ok()) { ros::spinOnce(); if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() && !surfFlatBuf.empty() && !surfLessFlatBuf.empty() && !fullPointsBuf.empty()) { timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec(); timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec(); timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec(); timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec(); timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec(); if (timeCornerPointsSharp != timeLaserCloudFullRes || timeCornerPointsLessSharp != timeLaserCloudFullRes || timeSurfPointsFlat != timeLaserCloudFullRes || timeSurfPointsLessFlat != timeLaserCloudFullRes) { //printf("unsync messeage!"); ROS_BREAK(); } mBuf.lock(); cornerPointsSharp->clear(); pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp); cornerSharpBuf.pop(); cornerPointsLessSharp->clear(); pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp); cornerLessSharpBuf.pop(); surfPointsFlat->clear(); pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat); surfFlatBuf.pop(); surfPointsLessFlat->clear(); pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat); surfLessFlatBuf.pop(); laserCloudFullRes->clear(); pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes); fullPointsBuf.pop(); mBuf.unlock(); TicToc t_whole; // initializing if (!systemInited) { systemInited = true; std::cout << "Initialization finished \n"; } else { int cornerPointsSharpNum = cornerPointsSharp->points.size(); int surfPointsFlatNum = surfPointsFlat->points.size(); TicToc t_opt; for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter) { corner_correspondence = 0; plane_correspondence = 0; //ceres::LossFunction *loss_function = NULL; ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization(); ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); problem.AddParameterBlock(para_q, 4, q_parameterization); problem.AddParameterBlock(para_t, 3); pcl::PointXYZI pointSel; std::vector pointSearchInd; std::vector pointSearchSqDis; TicToc t_data; // find correspondence for corner features for (int i = 0; i < cornerPointsSharpNum; ++i) { TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); int closestPointInd = -1, minPointInd2 = -1; if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) { closestPointInd = pointSearchInd[0]; int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; // search in the direction of increasing scan line for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j) { // if in the same scan line, continue if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) continue; // if not in nearby scans, end the loop if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) break; double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z); if (pointSqDis < minPointSqDis2) { // find nearer point minPointSqDis2 = pointSqDis; minPointInd2 = j; } } // search in the direction of decreasing scan line for (int j = closestPointInd - 1; j >= 0; --j) { // if in the same scan line, continue if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID) continue; // if not in nearby scans, end the loop if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) break; double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z); if (pointSqDis < minPointSqDis2) { // find nearer point minPointSqDis2 = pointSqDis; minPointInd2 = j; } } } if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid { Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, cornerPointsSharp->points[i].y, cornerPointsSharp->points[i].z); Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, laserCloudCornerLast->points[closestPointInd].y, laserCloudCornerLast->points[closestPointInd].z); Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, laserCloudCornerLast->points[minPointInd2].y, laserCloudCornerLast->points[minPointInd2].z); double s; if (DISTORTION) s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD; else s = 1.0; ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s); problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); corner_correspondence++; } } // find correspondence for plane features for (int i = 0; i < surfPointsFlatNum; ++i) { TransformToStart(&(surfPointsFlat->points[i]), &pointSel); kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) { closestPointInd = pointSearchInd[0]; // get closest point's scan ID int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity); double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD; // search in the direction of increasing scan line for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j) { // if not in nearby scans, end the loop if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) break; double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z); // if in the same or lower scan line if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; } // if in the higher scan line else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3) { minPointSqDis3 = pointSqDis; minPointInd3 = j; } } // search in the direction of decreasing scan line for (int j = closestPointInd - 1; j >= 0; --j) { // if not in nearby scans, end the loop if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) break; double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z); // if in the same or higher scan line if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; } else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) { // find nearer point minPointSqDis3 = pointSqDis; minPointInd3 = j; } } if (minPointInd2 >= 0 && minPointInd3 >= 0) { Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, surfPointsFlat->points[i].y, surfPointsFlat->points[i].z); Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, laserCloudSurfLast->points[closestPointInd].y, laserCloudSurfLast->points[closestPointInd].z); Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, laserCloudSurfLast->points[minPointInd2].y, laserCloudSurfLast->points[minPointInd2].z); Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, laserCloudSurfLast->points[minPointInd3].y, laserCloudSurfLast->points[minPointInd3].z); double s; if (DISTORTION) s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD; else s = 1.0; ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s); problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); plane_correspondence++; } } } ////printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence); //printf("data association time %f ms \n", t_data.toc()); if ((corner_correspondence + plane_correspondence) < 10) { //printf("less correspondence! *************************************************\n"); } TicToc t_solver; ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; options.max_num_iterations = 4; options.minimizer_progress_to_stdout = false; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); //printf("solver time %f ms \n", t_solver.toc()); } //printf("optimization twice time %f \n", t_opt.toc()); t_w_curr = t_w_curr + q_w_curr * t_last_curr; q_w_curr = q_w_curr * q_last_curr; } TicToc t_pub; // publish odometry nav_msgs::Odometry laserOdometry; laserOdometry.header.frame_id = "camera_init"; laserOdometry.child_frame_id = "/laser_odom"; laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserOdometry.pose.pose.orientation.x = q_w_curr.x(); laserOdometry.pose.pose.orientation.y = q_w_curr.y(); laserOdometry.pose.pose.orientation.z = q_w_curr.z(); laserOdometry.pose.pose.orientation.w = q_w_curr.w(); laserOdometry.pose.pose.position.x = t_w_curr.x(); laserOdometry.pose.pose.position.y = t_w_curr.y(); laserOdometry.pose.pose.position.z = t_w_curr.z(); pubLaserOdometry.publish(laserOdometry); geometry_msgs::PoseStamped laserPose; laserPose.header = laserOdometry.header; laserPose.pose = laserOdometry.pose.pose; laserPath.header.stamp = laserOdometry.header.stamp; laserPath.poses.push_back(laserPose); laserPath.header.frame_id = "camera_init"; pubLaserPath.publish(laserPath); // transform corner features and plane features to the scan end point if (0) { int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); for (int i = 0; i < cornerPointsLessSharpNum; i++) { TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); } int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); for (int i = 0; i < surfPointsLessFlatNum; i++) { TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); } int laserCloudFullResNum = laserCloudFullRes->points.size(); for (int i = 0; i < laserCloudFullResNum; i++) { TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); } } pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; cornerPointsLessSharp = laserCloudCornerLast; laserCloudCornerLast = laserCloudTemp; laserCloudTemp = surfPointsLessFlat; surfPointsLessFlat = laserCloudSurfLast; laserCloudSurfLast = laserCloudTemp; laserCloudCornerLastNum = laserCloudCornerLast->points.size(); laserCloudSurfLastNum = laserCloudSurfLast->points.size(); // std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n'; kdtreeCornerLast->setInputCloud(laserCloudCornerLast); kdtreeSurfLast->setInputCloud(laserCloudSurfLast); if (frameCount % skipFrameNum == 0) { frameCount = 0; sensor_msgs::PointCloud2 laserCloudCornerLast2; pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudCornerLast2.header.frame_id = "/camera"; pubLaserCloudCornerLast.publish(laserCloudCornerLast2); sensor_msgs::PointCloud2 laserCloudSurfLast2; pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudSurfLast2.header.frame_id = "/camera"; pubLaserCloudSurfLast.publish(laserCloudSurfLast2); sensor_msgs::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudFullRes3.header.frame_id = "/camera"; pubLaserCloudFullRes.publish(laserCloudFullRes3); } //printf("publication time %f ms \n", t_pub.toc()); //printf("whole laserOdometry time %f ms \n", t_whole.toc()); if(t_whole.toc() > 100) ROS_WARN("odometry process over 100ms"); frameCount++; } rate.sleep(); } return 0; } ================================================ FILE: SC-PGO/src/laserPosegraphOptimization.cpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "aloam_velodyne/common.h" #include "aloam_velodyne/tic_toc.h" #include "scancontext/Scancontext.h" using namespace gtsam; using std::cout; using std::endl; double keyframeMeterGap; double keyframeDegGap, keyframeRadGap; double translationAccumulated = 1000000.0; // large value means must add the first given frame. double rotaionAccumulated = 1000000.0; // large value means must add the first given frame. bool isNowKeyFrame = false; Pose6D odom_pose_prev {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // init Pose6D odom_pose_curr {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // init pose is zero std::queue odometryBuf; std::queue fullResBuf; std::queue gpsBuf; std::queue > scLoopICPBuf; std::mutex mBuf; std::mutex mKF; double timeLaserOdometry = 0.0; double timeLaser = 0.0; pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); pcl::PointCloud::Ptr laserCloudMapAfterPGO(new pcl::PointCloud()); std::vector::Ptr> keyframeLaserClouds; std::vector keyframePoses; std::vector keyframePosesUpdated; std::vector keyframeTimes; int recentIdxUpdated = 0; gtsam::NonlinearFactorGraph gtSAMgraph; bool gtSAMgraphMade = false; gtsam::Values initialEstimate; gtsam::ISAM2 *isam; gtsam::Values isamCurrentEstimate; noiseModel::Diagonal::shared_ptr priorNoise; noiseModel::Diagonal::shared_ptr odomNoise; noiseModel::Base::shared_ptr robustLoopNoise; noiseModel::Base::shared_ptr robustGPSNoise; pcl::VoxelGrid downSizeFilterScancontext; SCManager scManager; double scDistThres, scMaximumRadius; pcl::VoxelGrid downSizeFilterICP; std::mutex mtxICP; std::mutex mtxPosegraph; std::mutex mtxRecentPose; pcl::PointCloud::Ptr laserCloudMapPGO(new pcl::PointCloud()); pcl::VoxelGrid downSizeFilterMapPGO; bool laserCloudMapPGORedraw = true; bool useGPS = true; // bool useGPS = false; sensor_msgs::NavSatFix::ConstPtr currGPS; bool hasGPSforThisKF = false; bool gpsOffsetInitialized = false; double gpsAltitudeInitOffset = 0.0; double recentOptimizedX = 0.0; double recentOptimizedY = 0.0; ros::Publisher pubMapAftPGO, pubOdomAftPGO, pubPathAftPGO; ros::Publisher pubLoopScanLocal, pubLoopSubmapLocal; ros::Publisher pubOdomRepubVerifier; std::string save_directory; std::string pgKITTIformat, pgScansDirectory; std::string odomKITTIformat; std::fstream pgTimeSaveStream; std::string padZeros(int val, int num_digits = 6) { std::ostringstream out; out << std::internal << std::setfill('0') << std::setw(num_digits) << val; return out.str(); } gtsam::Pose3 Pose6DtoGTSAMPose3(const Pose6D& p) { return gtsam::Pose3( gtsam::Rot3::RzRyRx(p.roll, p.pitch, p.yaw), gtsam::Point3(p.x, p.y, p.z) ); } // Pose6DtoGTSAMPose3 void saveOdometryVerticesKITTIformat(std::string _filename) { // ref from gtsam's original code "dataset.cpp" std::fstream stream(_filename.c_str(), std::fstream::out); for(const auto& _pose6d: keyframePoses) { gtsam::Pose3 pose = Pose6DtoGTSAMPose3(_pose6d); Point3 t = pose.translation(); Rot3 R = pose.rotation(); auto col1 = R.column(1); // Point3 auto col2 = R.column(2); // Point3 auto col3 = R.column(3); // Point3 stream << col1.x() << " " << col2.x() << " " << col3.x() << " " << t.x() << " " << col1.y() << " " << col2.y() << " " << col3.y() << " " << t.y() << " " << col1.z() << " " << col2.z() << " " << col3.z() << " " << t.z() << std::endl; } } void saveOptimizedVerticesKITTIformat(gtsam::Values _estimates, std::string _filename) { using namespace gtsam; // ref from gtsam's original code "dataset.cpp" std::fstream stream(_filename.c_str(), std::fstream::out); for(const auto& key_value: _estimates) { auto p = dynamic_cast*>(&key_value.value); if (!p) continue; const Pose3& pose = p->value(); Point3 t = pose.translation(); Rot3 R = pose.rotation(); auto col1 = R.column(1); // Point3 auto col2 = R.column(2); // Point3 auto col3 = R.column(3); // Point3 stream << col1.x() << " " << col2.x() << " " << col3.x() << " " << t.x() << " " << col1.y() << " " << col2.y() << " " << col3.y() << " " << t.y() << " " << col1.z() << " " << col2.z() << " " << col3.z() << " " << t.z() << std::endl; } } void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &_laserOdometry) { mBuf.lock(); odometryBuf.push(_laserOdometry); mBuf.unlock(); } // laserOdometryHandler void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &_laserCloudFullRes) { mBuf.lock(); fullResBuf.push(_laserCloudFullRes); mBuf.unlock(); } // laserCloudFullResHandler void gpsHandler(const sensor_msgs::NavSatFix::ConstPtr &_gps) { if(useGPS) { mBuf.lock(); gpsBuf.push(_gps); mBuf.unlock(); } } // gpsHandler void initNoises( void ) { gtsam::Vector priorNoiseVector6(6); priorNoiseVector6 << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12; priorNoise = noiseModel::Diagonal::Variances(priorNoiseVector6); gtsam::Vector odomNoiseVector6(6); // odomNoiseVector6 << 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4; odomNoiseVector6 << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4; odomNoise = noiseModel::Diagonal::Variances(odomNoiseVector6); double loopNoiseScore = 0.5; // constant is ok... gtsam::Vector robustNoiseVector6(6); // gtsam::Pose3 factor has 6 elements (6D) robustNoiseVector6 << loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore; robustLoopNoise = gtsam::noiseModel::Robust::Create( gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure is okay but Cauchy is empirically good. gtsam::noiseModel::Diagonal::Variances(robustNoiseVector6) ); double bigNoiseTolerentToXY = 1000000000.0; // 1e9 double gpsAltitudeNoiseScore = 250.0; // if height is misaligned after loop clsosing, use this value bigger gtsam::Vector robustNoiseVector3(3); // gps factor has 3 elements (xyz) robustNoiseVector3 << bigNoiseTolerentToXY, bigNoiseTolerentToXY, gpsAltitudeNoiseScore; // means only caring altitude here. (because LOAM-like-methods tends to be asymptotically flyging) robustGPSNoise = gtsam::noiseModel::Robust::Create( gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure is okay but Cauchy is empirically good. gtsam::noiseModel::Diagonal::Variances(robustNoiseVector3) ); } // initNoises Pose6D getOdom(nav_msgs::Odometry::ConstPtr _odom) { auto tx = _odom->pose.pose.position.x; auto ty = _odom->pose.pose.position.y; auto tz = _odom->pose.pose.position.z; double roll, pitch, yaw; geometry_msgs::Quaternion quat = _odom->pose.pose.orientation; tf::Matrix3x3(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(roll, pitch, yaw); return Pose6D{tx, ty, tz, roll, pitch, yaw}; } // getOdom Pose6D diffTransformation(const Pose6D& _p1, const Pose6D& _p2) { Eigen::Affine3f SE3_p1 = pcl::getTransformation(_p1.x, _p1.y, _p1.z, _p1.roll, _p1.pitch, _p1.yaw); Eigen::Affine3f SE3_p2 = pcl::getTransformation(_p2.x, _p2.y, _p2.z, _p2.roll, _p2.pitch, _p2.yaw); Eigen::Matrix4f SE3_delta0 = SE3_p1.matrix().inverse() * SE3_p2.matrix(); Eigen::Affine3f SE3_delta; SE3_delta.matrix() = SE3_delta0; float dx, dy, dz, droll, dpitch, dyaw; pcl::getTranslationAndEulerAngles (SE3_delta, dx, dy, dz, droll, dpitch, dyaw); // std::cout << "delta : " << dx << ", " << dy << ", " << dz << ", " << droll << ", " << dpitch << ", " << dyaw << std::endl; return Pose6D{double(abs(dx)), double(abs(dy)), double(abs(dz)), double(abs(droll)), double(abs(dpitch)), double(abs(dyaw))}; } // SE3Diff pcl::PointCloud::Ptr local2global(const pcl::PointCloud::Ptr &cloudIn, const Pose6D& tf) { pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); int cloudSize = cloudIn->size(); cloudOut->resize(cloudSize); Eigen::Affine3f transCur = pcl::getTransformation(tf.x, tf.y, tf.z, tf.roll, tf.pitch, tf.yaw); int numberOfCores = 16; #pragma omp parallel for num_threads(numberOfCores) for (int i = 0; i < cloudSize; ++i) { const auto &pointFrom = cloudIn->points[i]; cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3); cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3); cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3); cloudOut->points[i].intensity = pointFrom.intensity; } return cloudOut; } void pubPath( void ) { // pub odom and path nav_msgs::Odometry odomAftPGO; nav_msgs::Path pathAftPGO; pathAftPGO.header.frame_id = "camera_init"; mKF.lock(); // for (int node_idx=0; node_idx < int(keyframePosesUpdated.size()) - 1; node_idx++) // -1 is just delayed visualization (because sometimes mutexed while adding(push_back) a new one) for (int node_idx=0; node_idx < recentIdxUpdated; node_idx++) // -1 is just delayed visualization (because sometimes mutexed while adding(push_back) a new one) { const Pose6D& pose_est = keyframePosesUpdated.at(node_idx); // upodated poses // const gtsam::Pose3& pose_est = isamCurrentEstimate.at(node_idx); nav_msgs::Odometry odomAftPGOthis; odomAftPGOthis.header.frame_id = "camera_init"; odomAftPGOthis.child_frame_id = "/aft_pgo"; odomAftPGOthis.header.stamp = ros::Time().fromSec(keyframeTimes.at(node_idx)); odomAftPGOthis.pose.pose.position.x = pose_est.x; odomAftPGOthis.pose.pose.position.y = pose_est.y; odomAftPGOthis.pose.pose.position.z = pose_est.z; odomAftPGOthis.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(pose_est.roll, pose_est.pitch, pose_est.yaw); odomAftPGO = odomAftPGOthis; geometry_msgs::PoseStamped poseStampAftPGO; poseStampAftPGO.header = odomAftPGOthis.header; poseStampAftPGO.pose = odomAftPGOthis.pose.pose; pathAftPGO.header.stamp = odomAftPGOthis.header.stamp; pathAftPGO.header.frame_id = "camera_init"; pathAftPGO.poses.push_back(poseStampAftPGO); } mKF.unlock(); pubOdomAftPGO.publish(odomAftPGO); // last pose pubPathAftPGO.publish(pathAftPGO); // poses static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; transform.setOrigin(tf::Vector3(odomAftPGO.pose.pose.position.x, odomAftPGO.pose.pose.position.y, odomAftPGO.pose.pose.position.z)); q.setW(odomAftPGO.pose.pose.orientation.w); q.setX(odomAftPGO.pose.pose.orientation.x); q.setY(odomAftPGO.pose.pose.orientation.y); q.setZ(odomAftPGO.pose.pose.orientation.z); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, odomAftPGO.header.stamp, "camera_init", "/aft_pgo")); } // pubPath void updatePoses(void) { mKF.lock(); for (int node_idx=0; node_idx < int(isamCurrentEstimate.size()); node_idx++) { Pose6D& p =keyframePosesUpdated[node_idx]; p.x = isamCurrentEstimate.at(node_idx).translation().x(); p.y = isamCurrentEstimate.at(node_idx).translation().y(); p.z = isamCurrentEstimate.at(node_idx).translation().z(); p.roll = isamCurrentEstimate.at(node_idx).rotation().roll(); p.pitch = isamCurrentEstimate.at(node_idx).rotation().pitch(); p.yaw = isamCurrentEstimate.at(node_idx).rotation().yaw(); } mKF.unlock(); mtxRecentPose.lock(); const gtsam::Pose3& lastOptimizedPose = isamCurrentEstimate.at(int(isamCurrentEstimate.size())-1); recentOptimizedX = lastOptimizedPose.translation().x(); recentOptimizedY = lastOptimizedPose.translation().y(); recentIdxUpdated = int(keyframePosesUpdated.size()) - 1; mtxRecentPose.unlock(); } // updatePoses void runISAM2opt(void) { // called when a variable added isam->update(gtSAMgraph, initialEstimate); isam->update(); gtSAMgraph.resize(0); initialEstimate.clear(); isamCurrentEstimate = isam->calculateEstimate(); updatePoses(); } pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, gtsam::Pose3 transformIn) { pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); PointType *pointFrom; int cloudSize = cloudIn->size(); cloudOut->resize(cloudSize); Eigen::Affine3f transCur = pcl::getTransformation( transformIn.translation().x(), transformIn.translation().y(), transformIn.translation().z(), transformIn.rotation().roll(), transformIn.rotation().pitch(), transformIn.rotation().yaw() ); int numberOfCores = 8; // TODO move to yaml #pragma omp parallel for num_threads(numberOfCores) for (int i = 0; i < cloudSize; ++i) { pointFrom = &cloudIn->points[i]; cloudOut->points[i].x = transCur(0,0) * pointFrom->x + transCur(0,1) * pointFrom->y + transCur(0,2) * pointFrom->z + transCur(0,3); cloudOut->points[i].y = transCur(1,0) * pointFrom->x + transCur(1,1) * pointFrom->y + transCur(1,2) * pointFrom->z + transCur(1,3); cloudOut->points[i].z = transCur(2,0) * pointFrom->x + transCur(2,1) * pointFrom->y + transCur(2,2) * pointFrom->z + transCur(2,3); cloudOut->points[i].intensity = pointFrom->intensity; } return cloudOut; } // transformPointCloud void loopFindNearKeyframesCloud( pcl::PointCloud::Ptr& nearKeyframes, const int& key, const int& submap_size, const int& root_idx) { // extract and stacking near keyframes (in global coord) nearKeyframes->clear(); for (int i = -submap_size; i <= submap_size; ++i) { int keyNear = key + i; if (keyNear < 0 || keyNear >= int(keyframeLaserClouds.size()) ) continue; mKF.lock(); *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]); mKF.unlock(); } if (nearKeyframes->empty()) return; // downsample near keyframes pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); downSizeFilterICP.setInputCloud(nearKeyframes); downSizeFilterICP.filter(*cloud_temp); *nearKeyframes = *cloud_temp; } // loopFindNearKeyframesCloud std::optional doICPVirtualRelative( int _loop_kf_idx, int _curr_kf_idx ) { // parse pointclouds int historyKeyframeSearchNum = 25; // enough. ex. [-25, 25] covers submap length of 50x1 = 50m if every kf gap is 1m pcl::PointCloud::Ptr cureKeyframeCloud(new pcl::PointCloud()); pcl::PointCloud::Ptr targetKeyframeCloud(new pcl::PointCloud()); loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx); // use same root of loop kf idx loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum, _loop_kf_idx); // loop verification sensor_msgs::PointCloud2 cureKeyframeCloudMsg; pcl::toROSMsg(*cureKeyframeCloud, cureKeyframeCloudMsg); cureKeyframeCloudMsg.header.frame_id = "camera_init"; pubLoopScanLocal.publish(cureKeyframeCloudMsg); sensor_msgs::PointCloud2 targetKeyframeCloudMsg; pcl::toROSMsg(*targetKeyframeCloud, targetKeyframeCloudMsg); targetKeyframeCloudMsg.header.frame_id = "camera_init"; pubLoopSubmapLocal.publish(targetKeyframeCloudMsg); // ICP Settings pcl::IterativeClosestPoint icp; icp.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter icp.setMaximumIterations(100); icp.setTransformationEpsilon(1e-6); icp.setEuclideanFitnessEpsilon(1e-6); icp.setRANSACIterations(0); // Align pointclouds icp.setInputSource(cureKeyframeCloud); icp.setInputTarget(targetKeyframeCloud); pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); icp.align(*unused_result); float loopFitnessScoreThreshold = 0.3; // user parameter but fixed low value is safe. if (icp.hasConverged() == false || icp.getFitnessScore() > loopFitnessScoreThreshold) { std::cout << "[SC loop] ICP fitness test failed (" << icp.getFitnessScore() << " > " << loopFitnessScoreThreshold << "). Reject this SC loop." << std::endl; return std::nullopt; } else { std::cout << "[SC loop] ICP fitness test passed (" << icp.getFitnessScore() << " < " << loopFitnessScoreThreshold << "). Add this SC loop." << std::endl; } // Get pose transformation float x, y, z, roll, pitch, yaw; Eigen::Affine3f correctionLidarFrame; correctionLidarFrame = icp.getFinalTransformation(); pcl::getTranslationAndEulerAngles (correctionLidarFrame, x, y, z, roll, pitch, yaw); gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); return poseFrom.between(poseTo); } // doICPVirtualRelative void process_pg() { while(1) { while ( !odometryBuf.empty() && !fullResBuf.empty() ) { // // pop and check keyframe is or not // mBuf.lock(); while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < fullResBuf.front()->header.stamp.toSec()) odometryBuf.pop(); if (odometryBuf.empty()) { mBuf.unlock(); break; } // Time equal check timeLaserOdometry = odometryBuf.front()->header.stamp.toSec(); timeLaser = fullResBuf.front()->header.stamp.toSec(); // TODO laserCloudFullRes->clear(); pcl::PointCloud::Ptr thisKeyFrame(new pcl::PointCloud()); pcl::fromROSMsg(*fullResBuf.front(), *thisKeyFrame); fullResBuf.pop(); Pose6D pose_curr = getOdom(odometryBuf.front()); odometryBuf.pop(); // find nearest gps double eps = 0.1; // find a gps topioc arrived within eps second while (!gpsBuf.empty()) { auto thisGPS = gpsBuf.front(); auto thisGPSTime = thisGPS->header.stamp.toSec(); if( abs(thisGPSTime - timeLaserOdometry) < eps ) { currGPS = thisGPS; hasGPSforThisKF = true; break; } else { hasGPSforThisKF = false; } gpsBuf.pop(); } mBuf.unlock(); // // Early reject by counting local delta movement (for equi-spereated kf drop) // odom_pose_prev = odom_pose_curr; odom_pose_curr = pose_curr; Pose6D dtf = diffTransformation(odom_pose_prev, odom_pose_curr); // dtf means delta_transform double delta_translation = sqrt(dtf.x*dtf.x + dtf.y*dtf.y + dtf.z*dtf.z); // note: absolute value. translationAccumulated += delta_translation; rotaionAccumulated += (dtf.roll + dtf.pitch + dtf.yaw); // sum just naive approach. if( translationAccumulated > keyframeMeterGap || rotaionAccumulated > keyframeRadGap ) { isNowKeyFrame = true; translationAccumulated = 0.0; // reset rotaionAccumulated = 0.0; // reset } else { isNowKeyFrame = false; } if( ! isNowKeyFrame ) continue; if( !gpsOffsetInitialized ) { if(hasGPSforThisKF) { // if the very first frame gpsAltitudeInitOffset = currGPS->altitude; gpsOffsetInitialized = true; } } // // Save data and Add consecutive node // pcl::PointCloud::Ptr thisKeyFrameDS(new pcl::PointCloud()); downSizeFilterScancontext.setInputCloud(thisKeyFrame); downSizeFilterScancontext.filter(*thisKeyFrameDS); mKF.lock(); keyframeLaserClouds.push_back(thisKeyFrameDS); keyframePoses.push_back(pose_curr); keyframePosesUpdated.push_back(pose_curr); // init keyframeTimes.push_back(timeLaserOdometry); scManager.makeAndSaveScancontextAndKeys(*thisKeyFrameDS); laserCloudMapPGORedraw = true; mKF.unlock(); const int prev_node_idx = keyframePoses.size() - 2; const int curr_node_idx = keyframePoses.size() - 1; // becuase cpp starts with 0 (actually this index could be any number, but for simple implementation, we follow sequential indexing) if( ! gtSAMgraphMade /* prior node */) { const int init_node_idx = 0; gtsam::Pose3 poseOrigin = Pose6DtoGTSAMPose3(keyframePoses.at(init_node_idx)); // auto poseOrigin = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(0.0, 0.0, 0.0)); mtxPosegraph.lock(); { // prior factor gtSAMgraph.add(gtsam::PriorFactor(init_node_idx, poseOrigin, priorNoise)); initialEstimate.insert(init_node_idx, poseOrigin); // runISAM2opt(); } mtxPosegraph.unlock(); gtSAMgraphMade = true; cout << "posegraph prior node " << init_node_idx << " added" << endl; } else /* consecutive node (and odom factor) after the prior added */ { // == keyframePoses.size() > 1 gtsam::Pose3 poseFrom = Pose6DtoGTSAMPose3(keyframePoses.at(prev_node_idx)); gtsam::Pose3 poseTo = Pose6DtoGTSAMPose3(keyframePoses.at(curr_node_idx)); mtxPosegraph.lock(); { // odom factor gtSAMgraph.add(gtsam::BetweenFactor(prev_node_idx, curr_node_idx, poseFrom.between(poseTo), odomNoise)); // gps factor if(hasGPSforThisKF) { double curr_altitude_offseted = currGPS->altitude - gpsAltitudeInitOffset; mtxRecentPose.lock(); gtsam::Point3 gpsConstraint(recentOptimizedX, recentOptimizedY, curr_altitude_offseted); // in this example, only adjusting altitude (for x and y, very big noises are set) mtxRecentPose.unlock(); gtSAMgraph.add(gtsam::GPSFactor(curr_node_idx, gpsConstraint, robustGPSNoise)); cout << "GPS factor added at node " << curr_node_idx << endl; } initialEstimate.insert(curr_node_idx, poseTo); // runISAM2opt(); } mtxPosegraph.unlock(); if(curr_node_idx % 100 == 0) cout << "posegraph odom node " << curr_node_idx << " added." << endl; } // if want to print the current graph, use gtSAMgraph.print("\nFactor Graph:\n"); // save utility std::string curr_node_idx_str = padZeros(curr_node_idx); pcl::io::savePCDFileBinary(pgScansDirectory + curr_node_idx_str + ".pcd", *thisKeyFrame); // scan pgTimeSaveStream << timeLaser << std::endl; // path } // ps. // scan context detector is running in another thread (in constant Hz, e.g., 1 Hz) // pub path and point cloud in another thread // wait (must required for running the while loop) std::chrono::milliseconds dura(2); std::this_thread::sleep_for(dura); } } // process_pg void performSCLoopClosure(void) { if( int(keyframePoses.size()) < scManager.NUM_EXCLUDE_RECENT) // do not try too early return; auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff int SCclosestHistoryFrameID = detectResult.first; if( SCclosestHistoryFrameID != -1 ) { const int prev_node_idx = SCclosestHistoryFrameID; const int curr_node_idx = keyframePoses.size() - 1; // because cpp starts 0 and ends n-1 cout << "Loop detected! - between " << prev_node_idx << " and " << curr_node_idx << "" << endl; mBuf.lock(); scLoopICPBuf.push(std::pair(prev_node_idx, curr_node_idx)); // addding actual 6D constraints in the other thread, icp_calculation. mBuf.unlock(); } } // performSCLoopClosure void process_lcd(void) { float loopClosureFrequency = 1.0; // can change ros::Rate rate(loopClosureFrequency); while (ros::ok()) { rate.sleep(); performSCLoopClosure(); // performRSLoopClosure(); // TODO } } // process_lcd void process_icp(void) { while(1) { while ( !scLoopICPBuf.empty() ) { if( scLoopICPBuf.size() > 30 ) { ROS_WARN("Too many loop clousre candidates to be ICPed is waiting ... Do process_lcd less frequently (adjust loopClosureFrequency)"); } mBuf.lock(); std::pair loop_idx_pair = scLoopICPBuf.front(); scLoopICPBuf.pop(); mBuf.unlock(); const int prev_node_idx = loop_idx_pair.first; const int curr_node_idx = loop_idx_pair.second; auto relative_pose_optional = doICPVirtualRelative(prev_node_idx, curr_node_idx); if(relative_pose_optional) { gtsam::Pose3 relative_pose = relative_pose_optional.value(); mtxPosegraph.lock(); gtSAMgraph.add(gtsam::BetweenFactor(prev_node_idx, curr_node_idx, relative_pose, robustLoopNoise)); // runISAM2opt(); mtxPosegraph.unlock(); } } // wait (must required for running the while loop) std::chrono::milliseconds dura(2); std::this_thread::sleep_for(dura); } } // process_icp void process_viz_path(void) { float hz = 10.0; ros::Rate rate(hz); while (ros::ok()) { rate.sleep(); if(recentIdxUpdated > 1) { pubPath(); } } } void process_isam(void) { float hz = 1; ros::Rate rate(hz); while (ros::ok()) { rate.sleep(); if( gtSAMgraphMade ) { mtxPosegraph.lock(); runISAM2opt(); cout << "running isam2 optimization ..." << endl; mtxPosegraph.unlock(); saveOptimizedVerticesKITTIformat(isamCurrentEstimate, pgKITTIformat); // pose saveOdometryVerticesKITTIformat(odomKITTIformat); // pose } } } void pubMap(void) { int SKIP_FRAMES = 2; // sparse map visulalization to save computations int counter = 0; laserCloudMapPGO->clear(); mKF.lock(); // for (int node_idx=0; node_idx < int(keyframePosesUpdated.size()); node_idx++) { for (int node_idx=0; node_idx < recentIdxUpdated; node_idx++) { if(counter % SKIP_FRAMES == 0) { *laserCloudMapPGO += *local2global(keyframeLaserClouds[node_idx], keyframePosesUpdated[node_idx]); } counter++; } mKF.unlock(); downSizeFilterMapPGO.setInputCloud(laserCloudMapPGO); downSizeFilterMapPGO.filter(*laserCloudMapPGO); sensor_msgs::PointCloud2 laserCloudMapPGOMsg; pcl::toROSMsg(*laserCloudMapPGO, laserCloudMapPGOMsg); laserCloudMapPGOMsg.header.frame_id = "camera_init"; pubMapAftPGO.publish(laserCloudMapPGOMsg); } void process_viz_map(void) { float vizmapFrequency = 0.1; // 0.1 means run onces every 10s ros::Rate rate(vizmapFrequency); while (ros::ok()) { rate.sleep(); if(recentIdxUpdated > 1) { pubMap(); } } } // pointcloud_viz int main(int argc, char **argv) { ros::init(argc, argv, "laserPGO"); ros::NodeHandle nh; nh.param("save_directory", save_directory, "/"); // pose assignment every k m move pgKITTIformat = save_directory + "optimized_poses.txt"; odomKITTIformat = save_directory + "odom_poses.txt"; pgTimeSaveStream = std::fstream(save_directory + "times.txt", std::fstream::out); pgTimeSaveStream.precision(std::numeric_limits::max_digits10); pgScansDirectory = save_directory + "Scans/"; auto unused = system((std::string("exec rm -r ") + pgScansDirectory).c_str()); unused = system((std::string("mkdir -p ") + pgScansDirectory).c_str()); nh.param("keyframe_meter_gap", keyframeMeterGap, 2.0); // pose assignment every k m move nh.param("keyframe_deg_gap", keyframeDegGap, 10.0); // pose assignment every k deg rot keyframeRadGap = deg2rad(keyframeDegGap); nh.param("sc_dist_thres", scDistThres, 0.2); nh.param("sc_max_radius", scMaximumRadius, 80.0); // 80 is recommended for outdoor, and lower (ex, 20, 40) values are recommended for indoor ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; parameters.relinearizeSkip = 1; isam = new ISAM2(parameters); initNoises(); scManager.setSCdistThres(scDistThres); scManager.setMaximumRadius(scMaximumRadius); float filter_size = 0.4; downSizeFilterScancontext.setLeafSize(filter_size, filter_size, filter_size); downSizeFilterICP.setLeafSize(filter_size, filter_size, filter_size); double mapVizFilterSize; nh.param("mapviz_filter_size", mapVizFilterSize, 0.4); // pose assignment every k frames downSizeFilterMapPGO.setLeafSize(mapVizFilterSize, mapVizFilterSize, mapVizFilterSize); ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_registered_local", 100, laserCloudFullResHandler); ros::Subscriber subLaserOdometry = nh.subscribe("/aft_mapped_to_init", 100, laserOdometryHandler); ros::Subscriber subGPS = nh.subscribe("/gps/fix", 100, gpsHandler); pubOdomAftPGO = nh.advertise("/aft_pgo_odom", 100); pubOdomRepubVerifier = nh.advertise("/repub_odom", 100); pubPathAftPGO = nh.advertise("/aft_pgo_path", 100); pubMapAftPGO = nh.advertise("/aft_pgo_map", 100); pubLoopScanLocal = nh.advertise("/loop_scan_local", 100); pubLoopSubmapLocal = nh.advertise("/loop_submap_local", 100); std::thread posegraph_slam {process_pg}; // pose graph construction std::thread lc_detection {process_lcd}; // loop closure detection std::thread icp_calculation {process_icp}; // loop constraint calculation via icp std::thread isam_update {process_isam}; // if you want to call less isam2 run (for saving redundant computations and no real-time visulization is required), uncommment this and comment all the above runisam2opt when node is added. std::thread viz_map {process_viz_map}; // visualization - map (low frequency because it is heavy) std::thread viz_path {process_viz_path}; // visualization - path (high frequency) ros::spin(); return 0; } ================================================ FILE: SC-PGO/src/lidarFactor.hpp ================================================ // Author: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk #include #include #include #include #include #include #include struct LidarEdgeFactor { LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_, double s_) : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {} template bool operator()(const T *q, const T *t, T *residual) const { Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; q_last_curr = q_identity.slerp(T(s), q_last_curr); Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; Eigen::Matrix lp; lp = q_last_curr * cp + t_last_curr; Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); Eigen::Matrix de = lpa - lpb; residual[0] = nu.x() / de.norm(); residual[1] = nu.y() / de.norm(); residual[2] = nu.z() / de.norm(); return true; } static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, const Eigen::Vector3d last_point_b_, const double s_) { return (new ceres::AutoDiffCostFunction< LidarEdgeFactor, 3, 4, 3>( new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); } Eigen::Vector3d curr_point, last_point_a, last_point_b; double s; }; struct LidarPlaneFactor { LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_) : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_) { ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); ljm_norm.normalize(); } template bool operator()(const T *q, const T *t, T *residual) const { Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; q_last_curr = q_identity.slerp(T(s), q_last_curr); Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; Eigen::Matrix lp; lp = q_last_curr * cp + t_last_curr; residual[0] = (lp - lpj).dot(ljm); return true; } static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, const double s_) { return (new ceres::AutoDiffCostFunction< LidarPlaneFactor, 1, 4, 3>( new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_))); } Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; Eigen::Vector3d ljm_norm; double s; }; struct LidarPlaneNormFactor { LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_) {} template bool operator()(const T *q, const T *t, T *residual) const { Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; Eigen::Matrix point_w; point_w = q_w_curr * cp + t_w_curr; Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); return true; } static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, const double negative_OA_dot_norm_) { return (new ceres::AutoDiffCostFunction< LidarPlaneNormFactor, 1, 4, 3>( new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); } Eigen::Vector3d curr_point; Eigen::Vector3d plane_unit_norm; double negative_OA_dot_norm; }; struct LidarDistanceFactor { LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) : curr_point(curr_point_), closed_point(closed_point_){} template bool operator()(const T *q, const T *t, T *residual) const { Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; Eigen::Matrix point_w; point_w = q_w_curr * cp + t_w_curr; residual[0] = point_w.x() - T(closed_point.x()); residual[1] = point_w.y() - T(closed_point.y()); residual[2] = point_w.z() - T(closed_point.z()); return true; } static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_) { return (new ceres::AutoDiffCostFunction< LidarDistanceFactor, 3, 4, 3>( new LidarDistanceFactor(curr_point_, closed_point_))); } Eigen::Vector3d curr_point; Eigen::Vector3d closed_point; }; ================================================ FILE: SC-PGO/src/scanRegistration.cpp ================================================ // This is an advanced implementation of the algorithm described in the following paper: // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. // Modifier: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk // Copyright 2013, Ji Zhang, Carnegie Mellon University // Further contributions copyright (c) 2016, Southwest Research Institute // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // 1. Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // 2. Redistributions in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // 3. Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived from this // software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include #include #include "aloam_velodyne/common.h" #include "aloam_velodyne/tic_toc.h" #include #include #include #include #include #include #include #include #include #include #include #include using std::atan2; using std::cos; using std::sin; std::string LIDAR_TYPE; const double scanPeriod = 0.1; const int systemDelay = 0; int systemInitCount = 0; bool systemInited = false; int N_SCANS = 0; float cloudCurvature[400000]; int cloudSortInd[400000]; int cloudNeighborPicked[400000]; int cloudLabel[400000]; bool comp (int i,int j) { return (cloudCurvature[i] pubEachScan; bool PUB_EACH_LINE = false; double MINIMUM_RANGE = 0.1; template void removeClosedPointCloud(const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, float thres) { if (&cloud_in != &cloud_out) { cloud_out.header = cloud_in.header; cloud_out.points.resize(cloud_in.points.size()); } size_t j = 0; for (size_t i = 0; i < cloud_in.points.size(); ++i) { if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres) continue; cloud_out.points[j] = cloud_in.points[i]; j++; } if (j != cloud_in.points.size()) { cloud_out.points.resize(j); } cloud_out.height = 1; cloud_out.width = static_cast(j); cloud_out.is_dense = true; } void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) { if (!systemInited) { systemInitCount++; if (systemInitCount >= systemDelay) { systemInited = true; } else return; } TicToc t_whole; TicToc t_prepare; std::vector scanStartInd(N_SCANS, 0); std::vector scanEndInd(N_SCANS, 0); pcl::PointCloud laserCloudIn; pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); std::vector indices; pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); int cloudSize = laserCloudIn.points.size(); float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; if (endOri - startOri > 3 * M_PI) { endOri -= 2 * M_PI; } else if (endOri - startOri < M_PI) { endOri += 2 * M_PI; } ////printf("end Ori %f\n", endOri); bool halfPassed = false; int count = cloudSize; PointType point; std::vector> laserCloudScans(N_SCANS); for (int i = 0; i < cloudSize; i++) { point.x = laserCloudIn.points[i].x; point.y = laserCloudIn.points[i].y; point.z = laserCloudIn.points[i].z; float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; int scanID = 0; if (LIDAR_TYPE == "VLP16" && N_SCANS == 16) { scanID = int((angle + 15) / 2 + 0.5); if (scanID > (N_SCANS - 1) || scanID < 0) { count--; continue; } } else if (LIDAR_TYPE == "HDL32" && N_SCANS == 32) { scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); if (scanID > (N_SCANS - 1) || scanID < 0) { count--; continue; } } // HDL64 (e.g., KITTI) else if (LIDAR_TYPE == "HDL64" && N_SCANS == 64) { if (angle >= -8.83) scanID = int((2 - angle) * 3.0 + 0.5); else scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); // use [0 50] > 50 remove outlies if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0) { count--; continue; } } // Ouster OS1-64 (e.g., MulRan) else if (LIDAR_TYPE == "OS1-64" && N_SCANS == 64) { scanID = int((angle + 22.5) / 2 + 0.5); // ouster os1-64 vfov is [-22.5, 22.5] see https://ouster.com/products/os1-lidar-sensor/ if (scanID > (N_SCANS - 1) || scanID < 0) { count--; continue; } } else { //printf("wrong scan number\n"); ROS_BREAK(); } ////printf("angle %f scanID %d \n", angle, scanID); float ori = -atan2(point.y, point.x); if (!halfPassed) { if (ori < startOri - M_PI / 2) { ori += 2 * M_PI; } else if (ori > startOri + M_PI * 3 / 2) { ori -= 2 * M_PI; } if (ori - startOri > M_PI) { halfPassed = true; } } else { ori += 2 * M_PI; if (ori < endOri - M_PI * 3 / 2) { ori += 2 * M_PI; } else if (ori > endOri + M_PI / 2) { ori -= 2 * M_PI; } } float relTime = (ori - startOri) / (endOri - startOri); point.intensity = scanID + scanPeriod * relTime; laserCloudScans[scanID].push_back(point); } cloudSize = count; //printf("points size %d \n", cloudSize); pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); for (int i = 0; i < N_SCANS; i++) { scanStartInd[i] = laserCloud->size() + 5; *laserCloud += laserCloudScans[i]; scanEndInd[i] = laserCloud->size() - 6; } //printf("prepare time %f \n", t_prepare.toc()); for (int i = 5; i < cloudSize - 5; i++) { float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x; float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y; float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z; cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; cloudSortInd[i] = i; cloudNeighborPicked[i] = 0; cloudLabel[i] = 0; } TicToc t_pts; pcl::PointCloud cornerPointsSharp; pcl::PointCloud cornerPointsLessSharp; pcl::PointCloud surfPointsFlat; pcl::PointCloud surfPointsLessFlat; float t_q_sort = 0; for (int i = 0; i < N_SCANS; i++) { if( scanEndInd[i] - scanStartInd[i] < 6) continue; pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); for (int j = 0; j < 6; j++) { int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; TicToc t_tmp; std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); t_q_sort += t_tmp.toc(); int largestPickedNum = 0; for (int k = ep; k >= sp; k--) { int ind = cloudSortInd[k]; if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1) { largestPickedNum++; if (largestPickedNum <= 2) { cloudLabel[ind] = 2; cornerPointsSharp.push_back(laserCloud->points[ind]); cornerPointsLessSharp.push_back(laserCloud->points[ind]); } else if (largestPickedNum <= 20) { cloudLabel[ind] = 1; cornerPointsLessSharp.push_back(laserCloud->points[ind]); } else { break; } cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; l++) { float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { break; } cloudNeighborPicked[ind + l] = 1; } for (int l = -1; l >= -5; l--) { float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { break; } cloudNeighborPicked[ind + l] = 1; } } } int smallestPickedNum = 0; for (int k = sp; k <= ep; k++) { int ind = cloudSortInd[k]; if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1) { cloudLabel[ind] = -1; surfPointsFlat.push_back(laserCloud->points[ind]); smallestPickedNum++; if (smallestPickedNum >= 4) { break; } cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; l++) { float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { break; } cloudNeighborPicked[ind + l] = 1; } for (int l = -1; l >= -5; l--) { float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { break; } cloudNeighborPicked[ind + l] = 1; } } } for (int k = sp; k <= ep; k++) { if (cloudLabel[k] <= 0) { surfPointsLessFlatScan->push_back(laserCloud->points[k]); } } } pcl::PointCloud surfPointsLessFlatScanDS; pcl::VoxelGrid downSizeFilter; downSizeFilter.setInputCloud(surfPointsLessFlatScan); downSizeFilter.setLeafSize(0.2, 0.2, 0.2); downSizeFilter.filter(surfPointsLessFlatScanDS); surfPointsLessFlat += surfPointsLessFlatScanDS; } //printf("sort q time %f \n", t_q_sort); //printf("seperate points time %f \n", t_pts.toc()); sensor_msgs::PointCloud2 laserCloudOutMsg; pcl::toROSMsg(*laserCloud, laserCloudOutMsg); laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; laserCloudOutMsg.header.frame_id = "camera_init"; pubLaserCloud.publish(laserCloudOutMsg); sensor_msgs::PointCloud2 cornerPointsSharpMsg; pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; cornerPointsSharpMsg.header.frame_id = "camera_init"; pubCornerPointsSharp.publish(cornerPointsSharpMsg); sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; cornerPointsLessSharpMsg.header.frame_id = "camera_init"; pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); sensor_msgs::PointCloud2 surfPointsFlat2; pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; surfPointsFlat2.header.frame_id = "camera_init"; pubSurfPointsFlat.publish(surfPointsFlat2); sensor_msgs::PointCloud2 surfPointsLessFlat2; pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; surfPointsLessFlat2.header.frame_id = "camera_init"; pubSurfPointsLessFlat.publish(surfPointsLessFlat2); // std::cout << "published ... " << std::endl; // pub each scam if(PUB_EACH_LINE) { for(int i = 0; i< N_SCANS; i++) { sensor_msgs::PointCloud2 scanMsg; pcl::toROSMsg(laserCloudScans[i], scanMsg); scanMsg.header.stamp = laserCloudMsg->header.stamp; scanMsg.header.frame_id = "camera_init"; pubEachScan[i].publish(scanMsg); } } //printf("scan registration time %f ms *************\n", t_whole.toc()); if(t_whole.toc() > 100) ROS_WARN("scan registration process over 100ms"); } int main(int argc, char **argv) { ros::init(argc, argv, "scanRegistration"); ros::NodeHandle nh; nh.param("scan_line", N_SCANS, 16); nh.param("lidar_type", LIDAR_TYPE, "KITTI"); nh.param("minimum_range", MINIMUM_RANGE, 0.1); //printf("scan line number %d \n", N_SCANS); if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64) { //printf("only support velodyne with 16, 32 or 64 scan line!"); return 0; } ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, laserCloudHandler); pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100); pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100); pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100); pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100); pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100); pubRemovePoints = nh.advertise("/laser_remove_points", 100); if(PUB_EACH_LINE) { for(int i = 0; i < N_SCANS; i++) { ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100); pubEachScan.push_back(tmp); } } ros::spin(); return 0; } ================================================ FILE: SC-PGO/utils/python/makeMergedMap.py ================================================ import os import sys import time import copy from io import StringIO import pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git from pypcd import pypcd import numpy as np from numpy import linalg as LA import open3d as o3d from pypcdMyUtils import * jet_table = np.load('jet_table.npy') bone_table = np.load('bone_table.npy') color_table = jet_table color_table_len = color_table.shape[0] ########################## # User only consider this block ########################## data_dir = "/home/user/Documents/catkin2021/catkin_fastlio2/data/" # should end with / scan_idx_range_to_stack = [0, 200] # if you want a whole map, use [0, len(scan_files)] node_skip = 1 num_points_in_a_scan = 150000 # for reservation (save faster) // e.g., use 150000 for 128 ray lidars, 100000 for 64 ray lidars, 30000 for 16 ray lidars, if error occured, use the larger value. is_live_vis = False # recommend to use false is_o3d_vis = True intensity_color_max = 200 is_near_removal = True thres_near_removal = 2 # meter (to remove platform-myself structure ghost points) ########################## # scan_dir = data_dir + "Scans" scan_files = os.listdir(scan_dir) scan_files.sort() poses = [] f = open(data_dir+"optimized_poses.txt", 'r') while True: line = f.readline() if not line: break pose_SE3 = np.asarray([float(i) for i in line.split()]) pose_SE3 = np.vstack( (np.reshape(pose_SE3, (3, 4)), np.asarray([0,0,0,1])) ) poses.append(pose_SE3) f.close() # assert (scan_idx_range_to_stack[1] > scan_idx_range_to_stack[0]) print("Merging scans from", scan_idx_range_to_stack[0], "to", scan_idx_range_to_stack[1]) # if(is_live_vis): vis = o3d.visualization.Visualizer() vis.create_window('Map', visible = True) nodes_count = 0 pcd_combined_for_vis = o3d.geometry.PointCloud() pcd_combined_for_save = None # The scans from 000000.pcd should be prepared if it is not used (because below code indexing is designed in a naive way) # manually reserve memory for fast write num_all_points_expected = int(num_points_in_a_scan * np.round((scan_idx_range_to_stack[1] - scan_idx_range_to_stack[0])/node_skip)) np_xyz_all = np.empty([num_all_points_expected, 3]) np_intensity_all = np.empty([num_all_points_expected, 1]) curr_count = 0 for node_idx in range(len(scan_files)): if(node_idx < scan_idx_range_to_stack[0] or node_idx >= scan_idx_range_to_stack[1]): continue nodes_count = nodes_count + 1 if( nodes_count % node_skip is not 0): if(node_idx is not scan_idx_range_to_stack[0]): # to ensure the vis init continue print("read keyframe scan idx", node_idx) scan_pose = poses[node_idx] scan_path = os.path.join(scan_dir, scan_files[node_idx]) scan_pcd = o3d.io.read_point_cloud(scan_path) scan_xyz_local = copy.deepcopy(np.asarray(scan_pcd.points)) scan_pypcd_with_intensity = pypcd.PointCloud.from_path(scan_path) scan_intensity = scan_pypcd_with_intensity.pc_data['intensity'] scan_intensity_colors_idx = np.round( (color_table_len-1) * np.minimum( 1, np.maximum(0, scan_intensity / intensity_color_max) ) ) scan_intensity_colors = color_table[scan_intensity_colors_idx.astype(int)] scan_pcd_global = scan_pcd.transform(scan_pose) # global coord, note that this is not deepcopy scan_pcd_global.colors = o3d.utility.Vector3dVector(scan_intensity_colors) scan_xyz = np.asarray(scan_pcd_global.points) scan_intensity = np.expand_dims(scan_intensity, axis=1) scan_ranges = LA.norm(scan_xyz_local, axis=1) if(is_near_removal): eff_idxes = np.where (scan_ranges > thres_near_removal) scan_xyz = scan_xyz[eff_idxes[0], :] scan_intensity = scan_intensity[eff_idxes[0], :] scan_pcd_global = scan_pcd_global.select_by_index(eff_idxes[0]) if(is_o3d_vis): pcd_combined_for_vis += scan_pcd_global # open3d pointcloud class append is fast if is_live_vis: if(node_idx is scan_idx_range_to_stack[0]): # to ensure the vis init vis.add_geometry(pcd_combined_for_vis) vis.update_geometry(pcd_combined_for_vis) vis.poll_events() vis.update_renderer() # save np_xyz_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_xyz np_intensity_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_intensity curr_count = curr_count + scan_xyz.shape[0] print(curr_count) # if(is_o3d_vis): print("draw the merged map.") o3d.visualization.draw_geometries([pcd_combined_for_vis]) # save ply having intensity np_xyz_all = np_xyz_all[0:curr_count, :] np_intensity_all = np_intensity_all[0:curr_count, :] np_xyzi_all = np.hstack( (np_xyz_all, np_intensity_all) ) xyzi = make_xyzi_point_cloud(np_xyzi_all) map_name = data_dir + "map_" + str(scan_idx_range_to_stack[0]) + "_to_" + str(scan_idx_range_to_stack[1]) + "_with_intensity.pcd" xyzi.save_pcd(map_name, compression='binary_compressed') print("intensity map is save (path:", map_name, ")") # save rgb colored points # map_name = data_dir + "map_" + str(scan_idx_range_to_stack[0]) + "_to_" + str(scan_idx_range_to_stack[1]) + ".pcd" # o3d.io.write_point_cloud(map_name, pcd_combined_for_vis) # print("the map is save (path:", map_name, ")") ================================================ FILE: SC-PGO/utils/python/pypcdMyUtils.py ================================================ import numpy as np import pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git from pypcd import pypcd def make_xyzi_point_cloud(xyzl, label_type='f'): """ Make XYZL point cloud from numpy array. TODO i labels? """ md = {'version': .7, 'fields': ['x', 'y', 'z', 'intensity'], 'count': [1, 1, 1, 1], 'width': len(xyzl), 'height': 1, 'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 'points': len(xyzl), 'data': 'ASCII'} if label_type.lower() == 'f': md['size'] = [4, 4, 4, 4] md['type'] = ['F', 'F', 'F', 'F'] elif label_type.lower() == 'u': md['size'] = [4, 4, 4, 1] md['type'] = ['F', 'F', 'F', 'U'] else: raise ValueError('label type must be F or U') # TODO use .view() xyzl = xyzl.astype(np.float32) dt = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.float32)]) pc_data = np.rec.fromarrays([xyzl[:, 0], xyzl[:, 1], xyzl[:, 2], xyzl[:, 3]], dtype=dt) pc = pypcd.PointCloud(md, pc_data) return pc ================================================ FILE: SC-PGO/utils/sample_data/KAIST03/optimized_poses.txt ================================================ 1 -8.3363e-15 -7.00722e-10 -1.25902e-17 8.33638e-15 1 1.06392e-10 7.48517e-18 7.00722e-10 -1.06392e-10 1 -2.90588e-12 0.999995 -0.00183662 0.00242372 -0.830723 0.00183814 0.999998 -0.000626489 -0.0702528 -0.00242256 0.000630942 0.999997 0.028506 0.999955 -0.00619254 0.00712886 -2.0279 0.00621328 0.999977 -0.00289042 -0.0890035 -0.0071108 0.00293459 0.99997 0.0611351 0.999896 -0.0101634 0.0102549 -3.23604 0.0101777 0.999947 -0.00135147 -0.094529 -0.0102406 0.0014557 0.999947 0.0943558 0.999925 -0.0105464 0.00615898 -4.43987 0.0105666 0.999939 -0.00325993 -0.111933 -0.00612422 0.00332476 0.999976 0.112778 0.999922 -0.0116081 0.00466198 -5.45798 0.0116165 0.999931 -0.00177551 -0.129743 -0.00464105 0.00182953 0.999988 0.149339 0.999908 -0.0131164 0.00351053 -6.57744 0.0131274 0.999909 -0.00312253 -0.154525 -0.00346926 0.00316832 0.999989 0.16777 0.999879 -0.0153974 0.00217493 -7.812 0.0153993 0.999881 -0.000849054 -0.157814 -0.0021616 0.000882443 0.999997 0.201606 0.999857 -0.0163638 0.0041903 -9.13307 0.0163833 0.999855 -0.00466874 -0.194597 -0.00411329 0.00473672 0.99998 0.235488 0.999829 -0.0177863 0.00496165 -10.5694 0.0178 0.999838 -0.00272271 -0.203606 -0.00491242 0.00281056 0.999984 0.267049 0.99982 -0.018772 0.00264324 -11.5779 0.018783 0.999815 -0.00422143 -0.229772 -0.00256351 0.00427032 0.999988 0.292312 0.999811 -0.0194355 0.000538649 -12.6198 0.0194381 0.999797 -0.00522362 -0.256755 -0.000437016 0.0052331 0.999986 0.309011 0.999788 -0.0205736 0.00112994 -13.7042 0.0205779 0.999781 -0.00390498 -0.268662 -0.00104935 0.0039274 0.999992 0.335767 0.999785 -0.0206704 0.00141779 -14.8214 0.0206772 0.999774 -0.0050044 -0.297563 -0.00131403 0.00503264 0.999986 0.362394 0.999758 -0.0219373 -0.00176261 -15.982 0.0219281 0.999747 -0.005058 -0.321281 0.00187312 0.00501812 0.999986 0.37218 0.999764 -0.0213675 -0.00404201 -17.1689 0.021348 0.999761 -0.00478866 -0.337659 0.00414336 0.00470124 0.99998 0.406462 0.999803 -0.0196226 -0.00289677 -18.3763 0.0196064 0.999793 -0.00551055 -0.359554 0.0030043 0.00545267 0.999981 0.424904 0.999839 -0.0178471 -0.00186246 -19.6187 0.0178382 0.99983 -0.00465797 -0.376691 0.00194528 0.004624 0.999987 0.439104 0.99987 -0.01585 -0.00300788 -20.9112 0.0158355 0.999863 -0.00479842 -0.386074 0.00308352 0.00475017 0.999984 0.464757 0.999895 -0.0142657 -0.00267759 -22.2296 0.0142546 0.99989 -0.00409914 -0.405231 0.00273578 0.00406054 0.999988 0.492367 0.999922 -0.0122191 -0.00273686 -23.5692 0.0122053 0.999913 -0.0050065 -0.412837 0.00279779 0.00497271 0.999984 0.509906 0.999928 -0.0102724 -0.00615312 -24.9569 0.0102525 0.999942 -0.0032604 -0.417687 0.00618626 0.00319709 0.999976 0.527312 0.99994 -0.00833045 -0.00717445 -26.3628 0.0083144 0.999963 -0.00226466 -0.414567 0.00719305 0.00220488 0.999972 0.551214 0.999959 -0.00647444 -0.00627222 -27.7871 0.00646454 0.999978 -0.00159864 -0.422627 0.00628243 0.00155803 0.999979 0.57781 0.999979 -0.00423741 -0.00480929 -29.2351 0.00422703 0.999989 -0.00216671 -0.423229 0.00481841 0.00214633 0.999986 0.605058 0.999975 -0.0019234 -0.00677742 -30.7076 0.00190565 0.999995 -0.00262539 -0.423579 0.00678243 0.00261241 0.999974 0.619955 0.999941 0.000347003 -0.0108292 -32.2167 -0.000400607 0.999988 -0.00494819 -0.409744 0.0108274 0.00495223 0.999929 0.650678 0.999965 0.00250141 -0.00799228 -33.7485 -0.00255279 0.999976 -0.00642575 -0.416179 0.00797602 0.00644593 0.999947 0.665858 0.999956 0.00478632 -0.00803003 -35.281 -0.00482304 0.999978 -0.00456042 -0.401822 0.00800802 0.00459895 0.999957 0.679959 0.999912 0.00560473 -0.0120096 -36.8471 -0.00563733 0.999981 -0.00268286 -0.377095 0.0119943 0.00275033 0.999924 0.692276 0.999869 0.00454994 -0.0155517 -38.4331 -0.0045754 0.999988 -0.00160219 -0.362438 0.0155443 0.00167314 0.999878 0.715622 0.999912 0.00388503 -0.0126591 -40.0408 -0.00390938 0.999991 -0.00189978 -0.359124 0.0126516 0.00194911 0.999918 0.733842 0.99992 0.00325335 -0.0122313 -41.6595 -0.003262 0.999994 -0.000686914 -0.340403 0.012229 0.000726757 0.999925 0.74452 0.999902 0.0026678 -0.0137442 -43.2728 -0.00266631 0.999996 0.000127089 -0.333153 0.0137445 -9.04307e-05 0.999906 0.77104 0.999849 0.00162624 -0.0173283 -44.9106 -0.0016182 0.999999 0.000478112 -0.334345 0.017329 -0.000449999 0.99985 0.774508 0.999732 0.00152652 -0.0230993 -46.5311 -0.00148958 0.999998 0.00161646 -0.332199 0.0231017 -0.00158162 0.999732 0.784287 0.999811 0.000871191 -0.0194398 -48.1657 -0.00086688 1 0.00023019 -0.330082 0.01944 -0.000213295 0.999811 0.80549 0.999921 0.000115175 -0.0126007 -49.7812 -0.000115823 1 -5.07525e-05 -0.317745 0.0126007 5.22079e-05 0.999921 0.807006 0.999819 -0.000221692 -0.0190245 -51.4 0.000245069 0.999999 0.00122644 -0.310961 0.0190242 -0.00123088 0.999818 0.823888 0.999735 -0.000259385 -0.0230115 -52.9964 0.000300172 0.999998 0.00176903 -0.307364 0.023011 -0.00177546 0.999734 0.832678 0.99978 -0.000954219 -0.0209399 -54.5679 0.00100568 0.999996 0.00244721 -0.310612 0.0209375 -0.00246774 0.999778 0.842094 0.999844 -0.00117056 -0.0176046 -56.1247 0.0012017 0.999998 0.0017586 -0.307098 0.0176025 -0.00177948 0.999843 0.861565 0.999859 -0.000800726 -0.0167805 -57.6587 0.000818031 0.999999 0.00102441 -0.305357 0.0167797 -0.00103799 0.999859 0.865338 0.999744 -0.000808112 -0.0226065 -59.2103 0.000849993 0.999998 0.00184307 -0.296377 0.022605 -0.00186181 0.999743 0.873276 0.999708 -0.000589724 -0.0241482 -60.7201 0.000652981 0.999996 0.00261173 -0.299686 0.0241466 -0.00262673 0.999705 0.880218 0.999797 -0.000181453 -0.020128 -62.2009 0.000213049 0.999999 0.0015676 -0.299851 0.0201277 -0.00157158 0.999796 0.902987 0.999874 0.00134807 -0.0158233 -63.675 -0.00136805 0.999998 -0.00125157 -0.299722 0.0158216 0.00127306 0.999874 0.912534 0.999817 0.0029464 -0.0189069 -65.153 -0.00297809 0.999994 -0.0016481 -0.296431 0.0189019 0.0017041 0.99982 0.914659 0.999778 0.00497033 -0.0204849 -66.6121 -0.00501685 0.999985 -0.00222026 -0.281654 0.0204736 0.00232253 0.999788 0.924416 0.999791 0.00664969 -0.0193123 -68.0477 -0.00669024 0.999976 -0.00203595 -0.273236 0.0192983 0.00216473 0.999811 0.938519 0.999728 0.00843194 -0.0217442 -69.4976 -0.0085178 0.999956 -0.00385885 -0.266942 0.0217107 0.00404301 0.999756 0.944458 0.999667 0.00981497 -0.0238651 -70.9531 -0.00994006 0.999937 -0.00512875 -0.249896 0.0238133 0.00536426 0.999702 0.944498 0.999678 0.0106371 -0.0230457 -72.3951 -0.0107674 0.999927 -0.00554096 -0.230012 0.022985 0.00578732 0.999719 0.953214 0.999686 0.0115917 -0.0222375 -73.8336 -0.0117632 0.999902 -0.00759634 -0.218305 0.0221472 0.00785554 0.999724 0.949802 0.999542 0.0122646 -0.0276782 -75.2809 -0.0124518 0.999901 -0.00659876 -0.204613 0.0275945 0.00694037 0.999595 0.937946 0.999289 0.0131764 -0.0353224 -76.7449 -0.0133179 0.999904 -0.00377472 -0.173019 0.0352693 0.00424245 0.999369 0.923618 0.999396 0.0142741 -0.0316745 -78.1845 -0.0143916 0.99989 -0.00348446 -0.143875 0.0316213 0.0039382 0.999492 0.934749 0.999744 0.0151394 -0.0168026 -79.6347 -0.0152334 0.999869 -0.00548034 -0.12423 0.0167175 0.0057349 0.999844 0.942736 0.999742 0.0153425 -0.016749 -81.0827 -0.0154513 0.99986 -0.00638551 -0.104081 0.0166487 0.00664265 0.999839 0.945526 0.999562 0.0163348 -0.0246726 -82.5563 -0.0165855 0.999813 -0.00999054 -0.0851321 0.0245048 0.0103954 0.999646 0.952955 0.999807 0.0173985 -0.00908848 -83.9991 -0.017448 0.999833 -0.00539563 -0.0524397 0.00899309 0.00555317 0.999944 0.95772 0.999785 0.0184078 0.00957443 -85.4304 -0.0184586 0.999816 0.00524604 -0.00380463 -0.0094761 -0.00542164 0.99994 0.993929 0.999357 0.020509 -0.0294099 -86.9353 -0.0204722 0.999789 0.0015506 0.0249635 0.0294355 -0.000947518 0.999566 0.996213 0.997568 0.0221877 -0.0660789 -88.4381 -0.0224619 0.999742 -0.00340991 0.0587651 0.0659862 0.00488588 0.997809 0.982381 0.999178 0.0259119 -0.031181 -89.8861 -0.0261289 0.999637 -0.00657216 0.0967307 0.0309994 0.00738148 0.999492 0.966956 0.99947 0.0291681 -0.0144853 -91.3476 -0.0292821 0.999541 -0.00772404 0.139726 0.0142533 0.00814411 0.999865 0.954579 0.999129 0.0329718 -0.025583 -92.8651 -0.0331741 0.999421 -0.00752205 0.206921 0.0253201 0.00836419 0.999644 0.939938 0.999034 0.0359342 -0.0252883 -94.3741 -0.0360788 0.999335 -0.00528484 0.275734 0.0250816 0.00619211 0.999666 0.948756 0.998963 0.0389727 -0.0235172 -95.911 -0.0390272 0.999236 -0.00186254 0.339928 0.0234267 0.00277842 0.999722 0.934362 0.998672 0.0431516 -0.0281437 -97.4494 -0.0432238 0.999063 -0.00196104 0.418702 0.0280327 0.00317491 0.999602 0.951712 0.998551 0.0463483 -0.0273324 -99.0277 -0.0464583 0.998914 -0.00340187 0.486907 0.0271451 0.00466676 0.999621 0.937415 0.99842 0.0497074 -0.0261861 -100.594 -0.0497074 0.998764 0.000652842 0.593596 0.0261862 0.000649831 0.999657 0.927809 0.998255 0.0538298 -0.0242822 -102.189 -0.0538708 0.998547 -0.00104 0.683397 0.0241909 0.00234628 0.999705 0.940103 0.998132 0.0574794 -0.0207196 -103.805 -0.057587 0.99833 -0.00463275 0.780015 0.0204187 0.00581727 0.999775 0.938506 0.997817 0.0612176 -0.0247621 -105.431 -0.0613484 0.998106 -0.00455795 0.878749 0.0244362 0.00606712 0.999683 0.932503 0.997267 0.0655989 -0.0340016 -107.075 -0.0658057 0.99782 -0.00499991 0.999029 0.0335995 0.00722375 0.999409 0.924833 0.997146 0.070005 -0.028261 -108.723 -0.070252 0.997498 -0.00784277 1.11844 0.0276412 0.00980578 0.99957 0.917284 0.997065 0.0745653 -0.0173652 -110.377 -0.0747404 0.997156 -0.00966361 1.23452 0.0165952 0.0109331 0.999803 0.915406 0.996619 0.07891 -0.0228737 -112.06 -0.0791569 0.996811 -0.010097 1.38238 0.022004 0.0118735 0.999687 0.919478 0.99592 0.0845322 -0.0315781 -113.756 -0.0847843 0.996377 -0.00673051 1.53402 0.0308947 0.00938037 0.999479 0.902298 0.995569 0.0890701 -0.0301616 -115.435 -0.0892573 0.995996 -0.00491529 1.70281 0.029603 0.00758565 0.999533 0.913445 0.995534 0.0928534 -0.017045 -117.155 -0.0930093 0.995628 -0.0085908 1.87636 0.0161728 0.0101378 0.999818 0.900303 0.995219 0.0960647 -0.0176372 -118.861 -0.0961738 0.99535 -0.00544 2.04344 0.0170326 0.00711023 0.99983 0.91538 0.994907 0.0987931 -0.0199952 -120.59 -0.098924 0.995079 -0.00566581 2.21767 0.0193371 0.00761496 0.999784 0.929805 0.99469 0.101442 -0.017331 -122.305 -0.101583 0.994799 -0.00746538 2.38726 0.0164835 0.00918628 0.999822 0.934681 0.994116 0.104519 -0.0284424 -124.035 -0.104659 0.994502 -0.00348936 2.58676 0.0279214 0.00644559 0.999589 0.941475 0.993476 0.108013 -0.0365965 -125.761 -0.108193 0.994125 -0.00297087 2.7741 0.0360606 0.00691098 0.999326 0.920828 0.993425 0.110759 -0.0289659 -127.473 -0.110853 0.993835 -0.00164337 2.9697 0.0286054 0.00484351 0.999579 0.902153 0.993109 0.113923 -0.0274958 -129.201 -0.114063 0.993467 -0.00356539 3.17278 0.02691 0.00667708 0.999616 0.9133 0.992617 0.116772 -0.0328094 -130.956 -0.116939 0.993134 -0.00320765 3.38911 0.0322096 0.00702067 0.999456 0.888669 0.992239 0.11977 -0.033415 -132.714 -0.119822 0.992795 0.000460446 3.61678 0.0332294 0.00354697 0.999441 0.872709 0.992066 0.122383 -0.028778 -134.474 -0.122393 0.992481 0.00140582 3.84292 0.0287337 0.00212758 0.999585 0.865437 0.991704 0.124554 -0.0317668 -136.238 -0.12454 0.992212 0.00241063 4.07511 0.0318196 0.00156561 0.999492 0.848593 0.99144 0.126005 -0.0342116 -138.007 -0.126006 0.992027 0.00211798 4.30708 0.0342057 0.00221102 0.999412 0.841349 0.991388 0.126305 -0.0345918 -139.792 -0.126323 0.991988 0.0016587 4.54241 0.0345242 0.00272535 0.9994 0.815614 0.991518 0.126177 -0.031157 -141.553 -0.126245 0.991999 -0.000207109 4.75593 0.0308815 0.00413875 0.999514 0.809015 0.991632 0.125632 -0.0297032 -143.344 -0.125656 0.992073 0.00107425 4.99671 0.0296027 0.00266711 0.999558 0.786039 0.991662 0.125438 -0.0295425 -145.107 -0.125485 0.992095 0.000250888 5.21772 0.0293404 0.00345835 0.999563 0.7879 0.991706 0.124514 -0.0318558 -146.895 -0.124593 0.992208 -0.000501944 5.45087 0.0315451 0.00446679 0.999492 0.771089 0.991735 0.124416 -0.0313499 -148.661 -0.124524 0.992215 -0.00152673 5.67618 0.0309159 0.00541792 0.999507 0.757635 0.991793 0.124064 -0.0308984 -150.433 -0.124143 0.992264 -0.000645294 5.90309 0.0305793 0.00447581 0.999522 0.742313 0.99185 0.123685 -0.0305918 -152.191 -0.123765 0.992311 -0.000742914 6.12408 0.0302647 0.00452306 0.999532 0.729167 0.991974 0.123088 -0.0289285 -153.93 -0.123083 0.992394 0.00193891 6.36114 0.0289472 0.00163727 0.99958 0.728292 0.991903 0.12278 -0.0324487 -155.665 -0.122722 0.992434 0.00376523 6.58626 0.0326655 0.000247433 0.999466 0.726314 0.991986 0.12233 -0.0316018 -157.383 -0.122247 0.992489 0.00457166 6.79597 0.0319237 -0.000671817 0.99949 0.721669 0.992124 0.121754 -0.0294291 -159.09 -0.121673 0.99256 0.00453419 7.01175 0.0297622 -0.000917742 0.999557 0.709524 0.992158 0.121249 -0.0303541 -160.781 -0.121173 0.992622 0.00433076 7.22294 0.0306552 -0.000618715 0.99953 0.707036 0.992067 0.121275 -0.0330855 -162.487 -0.121182 0.992619 0.00479841 7.42594 0.0334232 -0.000750964 0.999441 0.685673 0.992059 0.120792 -0.0350592 -164.154 -0.120667 0.992677 0.00565842 7.6346 0.035486 -0.001383 0.999369 0.676517 0.992157 0.120351 -0.0337657 -165.824 -0.120254 0.992731 0.00489276 7.83691 0.0341091 -0.000793932 0.999418 0.661173 0.992223 0.120029 -0.0329739 -167.457 -0.119914 0.992769 0.00543476 8.05219 0.0333878 -0.00143845 0.999441 0.650302 0.992255 0.119927 -0.0323805 -169.104 -0.119796 0.992781 0.00596926 8.25903 0.0328627 -0.00204398 0.999458 0.640254 0.992373 0.119774 -0.0291536 -170.714 -0.119628 0.992796 0.00671345 8.46111 0.0297477 -0.00317466 0.999552 0.634852 0.992368 0.119741 -0.0294539 -172.33 -0.11961 0.992802 0.00619613 8.65842 0.0299838 -0.00262587 0.999547 0.634673 0.992366 0.119075 -0.0320977 -173.929 -0.119009 0.992885 0.00395425 8.85001 0.0323402 -0.000104144 0.999477 0.620622 0.992343 0.118945 -0.0332858 -175.503 -0.118915 0.9929 0.00286992 9.03977 0.0333908 0.00111024 0.999442 0.612208 0.9924 0.118754 -0.0322349 -177.081 -0.118782 0.99292 0.00107009 9.2316 0.0321337 0.00276695 0.99948 0.603588 0.992543 0.118363 -0.0291414 -178.643 -0.118401 0.992966 0.000418498 9.42476 0.0289859 0.00303499 0.999575 0.598992 0.992669 0.117833 -0.0268929 -180.2 -0.11792 0.993022 -0.00166422 9.60193 0.0265091 0.00482324 0.999637 0.595815 0.99276 0.116881 -0.0276743 -181.778 -0.117097 0.9931 -0.00632259 9.7846 0.0267444 0.00951741 0.999597 0.604232 0.99316 0.116324 -0.0100813 -183.326 -0.116385 0.993188 -0.00563842 9.96727 0.00935675 0.00677316 0.999933 0.596649 0.993161 0.116454 -0.00833313 -184.874 -0.116413 0.993187 0.00519763 10.1811 0.00888164 -0.004192 0.999952 0.618373 0.991811 0.117158 -0.0508449 -186.493 -0.117012 0.993113 0.00584664 10.3727 0.0511797 0.000150699 0.998689 0.594474 0.991042 0.118093 -0.0623744 -188.102 -0.117815 0.993002 0.00812732 10.5714 0.0628977 -0.00070587 0.99802 0.55988 0.992696 0.119787 -0.0143321 -189.649 -0.119823 0.992794 -0.00166512 10.7535 0.0140294 0.00337028 0.999896 0.559824 0.99265 0.120289 -0.0133062 -191.24 -0.120264 0.992738 0.00262257 10.9637 0.013525 -0.00100303 0.999908 0.547221 0.991853 0.121993 -0.0366889 -192.873 -0.121724 0.992519 0.00947316 11.1837 0.0375701 -0.00493005 0.999282 0.538003 0.991834 0.122269 -0.0362758 -194.49 -0.122109 0.992495 0.0065844 11.3654 0.0368086 -0.00210102 0.99932 0.539339 0.992266 0.121506 -0.0253978 -196.117 -0.121342 0.992579 0.0079145 11.575 0.026171 -0.00477146 0.999646 0.496028 0.991982 0.122189 -0.0322593 -197.77 -0.121889 0.992481 0.0111199 11.7982 0.0333755 -0.0070987 0.999418 0.502764 0.992253 0.122031 -0.0233065 -199.419 -0.121793 0.99249 0.011386 11.9904 0.0245209 -0.00845921 0.999664 0.48661 0.992337 0.121922 -0.020071 -201.063 -0.12171 0.9925 0.0114402 12.1927 0.0213153 -0.00890972 0.999733 0.484511 0.992095 0.12184 -0.030044 -202.736 -0.121514 0.992512 0.0124572 12.4115 0.0313368 -0.008708 0.999471 0.491007 0.991976 0.122372 -0.0317575 -204.414 -0.122133 0.992469 0.00937437 12.6016 0.0326655 -0.00542051 0.999452 0.481848 0.992033 0.122027 -0.031307 -206.058 -0.121795 0.992513 0.00920538 12.8067 0.0321959 -0.00531899 0.999467 0.46152 0.991955 0.122597 -0.0315619 -207.704 -0.12239 0.992446 0.00842133 13.0082 0.0323559 -0.00449072 0.999466 0.451409 0.992001 0.122463 -0.0305932 -209.321 -0.122259 0.992462 0.00848739 13.2081 0.031402 -0.00467922 0.999496 0.449392 0.992023 0.12258 -0.0294203 -210.928 -0.12237 0.992445 0.00883814 13.4132 0.0302814 -0.00516748 0.999528 0.42843 0.991958 0.122591 -0.0314636 -212.536 -0.122317 0.992435 0.0105046 13.6184 0.0325133 -0.00657161 0.99945 0.425296 0.991918 0.123026 -0.0310217 -214.114 -0.122758 0.992382 0.0103979 13.8199 0.0320646 -0.00650568 0.999465 0.425745 0.992052 0.123029 -0.0263956 -215.676 -0.122828 0.992386 0.00910528 14.0125 0.0273149 -0.0057908 0.99961 0.414303 0.991936 0.123044 -0.0303789 -217.23 -0.122778 0.992379 0.0104665 14.2267 0.0314352 -0.00665219 0.999484 0.39692 0.991821 0.12313 -0.0336283 -218.777 -0.122826 0.992368 0.0109569 14.4252 0.0347207 -0.00673685 0.999374 0.396948 0.992192 0.123467 -0.0176259 -220.288 -0.123331 0.992328 0.00861423 14.6069 0.0185543 -0.00637315 0.999808 0.407073 0.992285 0.123844 -0.00581851 -222.534 -0.123738 0.992186 0.016003 14.9078 0.00775492 -0.0151596 0.999855 0.43348 0.990539 0.124342 -0.0580606 -224.085 -0.123779 0.992222 0.0132146 15.0876 0.0592521 -0.00590291 0.998226 0.395898 0.990184 0.125663 -0.0611905 -225.585 -0.12494 0.992043 0.0155195 15.2965 0.0626538 -0.00772205 0.998005 0.373201 0.991888 0.126282 -0.0145452 -227.064 -0.126138 0.991958 0.0104267 15.4757 0.0157449 -0.00850743 0.99984 0.374841 0.991908 0.125641 -0.0182214 -228.591 -0.125472 0.992045 0.010152 15.6673 0.0193519 -0.00778361 0.999782 0.340973 0.991344 0.125749 -0.0377345 -230.131 -0.12528 0.992015 0.0145575 15.8722 0.0392638 -0.0097041 0.999182 0.342853 0.991539 0.125797 -0.0320304 -231.662 -0.125349 0.99199 0.0156319 16.0861 0.0337403 -0.0114846 0.999365 0.333547 0.991818 0.125729 -0.0221061 -233.189 -0.125488 0.992023 0.0119426 16.2804 0.0234313 -0.00907083 0.999684 0.337007 0.991881 0.125487 -0.0206147 -234.734 -0.12533 0.992076 0.00876579 16.4573 0.0215514 -0.00611098 0.999749 0.335418 0.991767 0.125363 -0.026111 -236.294 -0.125221 0.992104 0.00702582 16.6577 0.0267856 -0.00369833 0.999634 0.331449 0.991578 0.125252 -0.0329442 -237.842 -0.125099 0.992122 0.00668042 16.845 0.0335214 -0.00250288 0.999435 0.324724 0.991681 0.125059 -0.0304805 -239.388 -0.124955 0.992148 0.00528435 17.0447 0.030902 -0.0014317 0.999521 0.31093 0.991826 0.124419 -0.0283044 -240.939 -0.124281 0.992225 0.00658446 17.2449 0.0289036 -0.00301294 0.999578 0.302349 0.991758 0.124015 -0.0321862 -242.519 -0.123821 0.992272 0.00798127 17.4517 0.0329272 -0.00393017 0.99945 0.293686 0.991793 0.124023 -0.0310524 -244.091 -0.123775 0.992262 0.00979914 17.6499 0.0320274 -0.00587521 0.99947 0.278568 0.991912 0.123849 -0.0277931 -245.656 -0.123555 0.992264 0.0120592 17.8562 0.0290716 -0.00852764 0.999541 0.281243 0.992047 0.123649 -0.0235297 -247.222 -0.123433 0.992298 0.0104384 18.0496 0.0246391 -0.007451 0.999669 0.275111 0.992075 0.122994 -0.0256957 -248.774 -0.122761 0.992381 0.0104432 18.2455 0.0267843 -0.00720599 0.999615 0.280011 0.992109 0.122241 -0.0278865 -251.072 -0.121993 0.992476 0.0104217 18.5327 0.0289506 -0.00693752 0.999557 0.273992 0.992203 0.121366 -0.0283516 -252.585 -0.121095 0.992579 0.0110801 18.7211 0.029486 -0.0075605 0.999537 0.268395 0.992357 0.119947 -0.0289956 -254.091 -0.119629 0.99274 0.0124657 18.916 0.0302803 -0.00890177 0.999502 0.262983 0.992399 0.119176 -0.0306724 -255.561 -0.118808 0.992824 0.0135882 19.0924 0.0320717 -0.0098408 0.999437 0.258587 0.992647 0.117779 -0.0279123 -257.059 -0.117435 0.992986 0.0136796 19.2731 0.0293277 -0.0103011 0.999517 0.253928 0.992828 0.116842 -0.025301 -258.527 -0.116515 0.99309 0.0140331 19.4559 0.0267658 -0.0109846 0.999581 0.250439 0.992847 0.115988 -0.02832 -259.979 -0.115601 0.993183 0.0149455 19.6218 0.0298604 -0.0115648 0.999487 0.247921 0.992909 0.115309 -0.0289059 -261.424 -0.114914 0.993263 0.0149655 19.7841 0.0304368 -0.0115377 0.99947 0.252547 0.993261 0.11447 -0.018147 -262.853 -0.114231 0.993359 0.0137251 19.9398 0.0195976 -0.0115596 0.999741 0.259768 0.993332 0.113535 0.020021 -264.225 -0.113902 0.993324 0.0182741 20.1201 -0.0178126 -0.0204327 0.999633 0.293088 0.993476 0.113427 0.0117953 -265.596 -0.1137 0.993177 0.0259161 20.3114 -0.00877522 -0.0270882 0.999595 0.328834 0.991125 0.114912 -0.0668265 -267.781 -0.113425 0.993215 0.0256511 20.5619 0.0693207 -0.0178437 0.997435 0.28832 0.991395 0.116796 -0.0591128 -269.18 -0.115347 0.992948 0.0273594 20.7277 0.0618914 -0.0203055 0.997876 0.269978 0.99329 0.115549 -0.00481963 -271.273 -0.115479 0.993225 0.0129679 20.9624 0.0062854 -0.0123243 0.999904 0.272501 0.993407 0.114113 -0.0109572 -272.706 -0.113935 0.993364 0.015721 21.1423 0.0126785 -0.0143689 0.999816 0.249537 0.99331 0.112854 -0.0244721 -274.858 -0.112451 0.993507 0.0172571 21.3874 0.0262607 -0.0143897 0.999552 0.276924 0.993731 0.110457 -0.0172447 -276.302 -0.11014 0.993747 0.0183414 21.5569 0.0191628 -0.0163271 0.999683 0.271332 0.994169 0.106453 -0.0172 -278.467 -0.106143 0.994187 0.0180659 21.7926 0.0190232 -0.0161349 0.999689 0.291327 0.994587 0.103151 -0.0124754 -279.924 -0.10296 0.994571 0.0150691 21.9445 0.0139621 -0.013703 0.999809 0.312072 0.994933 0.0996653 -0.0132044 -281.369 -0.0994764 0.994938 0.0142613 22.0889 0.0145589 -0.0128755 0.999811 0.320162 0.995322 0.0947114 -0.0190983 -283.572 -0.0944843 0.995448 0.0124646 22.2951 0.0201919 -0.0106018 0.99974 0.335876 0.995647 0.0911624 -0.0194132 -285.048 -0.0909222 0.995774 0.0129148 22.4348 0.0205085 -0.0110935 0.999728 0.330833 0.99612 0.086032 -0.0185393 -287.252 -0.085799 0.996227 0.0130187 22.6231 0.0195894 -0.0113776 0.999743 0.347658 0.996428 0.0827384 -0.0169215 -288.746 -0.08252 0.996502 0.013223 22.7555 0.0179564 -0.0117794 0.999769 0.351238 0.996704 0.0793154 -0.0170345 -290.24 -0.079115 0.996792 0.0121347 22.8702 0.0179423 -0.010747 0.999781 0.365914 0.996924 0.0764242 -0.0173818 -291.727 -0.0762468 0.997032 0.0106498 22.9878 0.0181441 -0.00929169 0.999792 0.373562 0.997068 0.0744627 -0.0176507 -293.241 -0.0742688 0.997173 0.0113982 23.1001 0.0184495 -0.0100539 0.999779 0.38145 0.997187 0.0722935 -0.0197999 -294.753 -0.0720514 0.99732 0.0126794 23.2318 0.0206635 -0.0112171 0.999724 0.379582 0.997415 0.0696214 -0.0177846 -296.259 -0.0694268 0.997523 0.0113362 23.3273 0.0185297 -0.0100722 0.999778 0.40126 0.997816 0.0655916 -0.0078275 -297.772 -0.065507 0.997796 0.0106131 23.4244 0.00850637 -0.0100772 0.999913 0.408713 0.998022 0.0612822 -0.0140167 -299.294 -0.0611327 0.998071 0.0108567 23.5278 0.0146549 -0.00997834 0.999843 0.434359 0.998405 0.0543221 -0.0153821 -301.596 -0.0541754 0.998483 0.00980358 23.6469 0.0158913 -0.00895462 0.999834 0.452444 0.998699 0.0501117 -0.00941879 -303.107 -0.0500152 0.998696 0.010215 23.7309 0.0099184 -0.00973061 0.999903 0.470583 0.998844 0.0457875 -0.0146501 -304.624 -0.0456591 0.998917 0.00898384 23.7922 0.0150456 -0.00830454 0.999852 0.489392 0.998994 0.041596 -0.0167828 -306.146 -0.0414479 0.999099 0.00907872 23.8509 0.0171453 -0.00837397 0.999818 0.503157 0.999261 0.0353986 -0.0149486 -308.38 -0.0352588 0.999333 0.00951748 23.9377 0.0152755 -0.00898339 0.999843 0.519086 0.999339 0.032083 -0.0170778 -309.845 -0.0318856 0.999423 0.0117104 23.9807 0.0174436 -0.0111581 0.999786 0.54051 0.999374 0.0325961 -0.0137871 -311.318 -0.0324848 0.999438 0.00822217 24.0343 0.0140474 -0.00776915 0.999871 0.571499 0.999463 0.0325578 -0.00362284 -312.773 -0.0325323 0.999447 0.00688715 24.0747 0.00384507 -0.00676559 0.99997 0.580735 0.999426 0.0337409 0.00317153 -314.226 -0.0337603 0.99941 0.00628574 24.1304 -0.00295758 -0.0063892 0.999975 0.631288 0.999048 0.0338399 0.0275287 -315.656 -0.0340602 0.999391 0.00757278 24.1883 -0.0272557 -0.00850321 0.999592 0.688883 0.999149 0.0341748 -0.0230865 -317.906 -0.0339573 0.999376 0.00974616 24.2729 0.0234051 -0.00895391 0.999686 0.727219 0.996778 0.0356215 -0.0718694 -319.403 -0.0347965 0.999314 0.0126997 24.3334 0.0722725 -0.0101579 0.997333 0.712549 0.998814 0.0362965 -0.0324477 -320.811 -0.0360788 0.999322 0.00727137 24.3844 0.0326896 -0.00609208 0.999447 0.720855 0.999277 0.0359145 0.0124745 -322.244 -0.0359837 0.999338 0.00536951 24.4345 -0.0122734 -0.00581451 0.999908 0.717384 0.999338 0.0360582 -0.00480962 -323.739 -0.0360456 0.999347 0.00266275 24.4884 0.00490249 -0.00248762 0.999985 0.745015 0.999344 0.0359061 -0.00477896 -325.199 -0.0359091 0.999355 -0.000535161 24.523 0.00475666 0.000706418 0.999988 0.785787 0.999398 0.0345842 0.00260977 -326.665 -0.0345831 0.999402 -0.000455498 24.5737 -0.00262396 0.00036497 0.999996 0.809089 0.999416 0.0329969 -0.00885217 -328.166 -0.0329951 0.999455 0.00034663 24.6288 0.00885879 -5.435e-05 0.999961 0.821041 0.999404 0.0307483 -0.0156852 -329.681 -0.0307251 0.999526 0.00171711 24.6696 0.0157306 -0.00123415 0.999876 0.840807 0.999529 0.0278157 -0.0129283 -331.195 -0.0277803 0.99961 0.00290796 24.7093 0.0130042 -0.00254744 0.999912 0.854768 0.999628 0.025064 -0.0107502 -332.714 -0.0250167 0.999677 0.00451136 24.7583 0.0108598 -0.00424075 0.999932 0.860931 0.999685 0.0214082 -0.0130691 -334.259 -0.0212855 0.999729 0.00945807 24.8004 0.0132681 -0.00917691 0.99987 0.878492 0.999754 0.0193549 -0.0107998 -335.774 -0.0192323 0.999751 0.0113454 24.8283 0.0110167 -0.0111349 0.999877 0.902481 0.999865 0.016302 -0.002158 -337.322 -0.0162776 0.999808 0.0108846 24.8489 0.00233503 -0.010848 0.999938 0.94135 0.99991 0.0134455 0.000242166 -338.884 -0.013447 0.99988 0.00762899 24.8639 -0.000139561 -0.00763156 0.999971 0.974294 0.999938 0.0107175 -0.00294769 -340.458 -0.0107083 0.999938 0.00311295 24.8739 0.00298087 -0.00308119 0.999991 1.00528 0.999943 0.00748803 -0.00760445 -342.058 -0.00747993 0.999971 0.0010934 24.8858 0.00761242 -0.00103645 0.99997 1.02713 0.999949 0.00674783 -0.00744796 -343.647 -0.00675169 0.999977 -0.000492853 24.8894 0.00744446 0.000543115 0.999972 1.05027 0.999966 0.00711658 -0.00416401 -345.258 -0.00711885 0.999975 -0.00052981 24.9105 0.00416013 0.000559434 0.999991 1.07351 0.999969 0.00715773 -0.00313537 -346.859 -0.00715833 0.999974 -0.000178208 24.9228 0.00313401 0.000200647 0.999995 1.10211 0.999972 0.0066059 -0.00352603 -348.463 -0.00661195 0.999977 -0.0017082 24.9359 0.00351467 0.00173147 0.999992 1.13696 0.999973 0.00547456 -0.00499298 -350.107 -0.00547823 0.999985 -0.000722 24.9572 0.00498895 0.000749333 0.999987 1.15834 0.999947 0.00428413 -0.00931055 -351.75 -0.00427025 0.99999 0.00151038 24.9625 0.00931692 -0.00147054 0.999956 1.18744 0.999955 0.00337015 -0.00883567 -353.394 -0.00335562 0.999993 0.00165904 24.969 0.0088412 -0.00162931 0.99996 1.22001 0.999998 0.00180156 -0.000276485 -355.033 -0.00180112 0.999997 0.00159525 24.9853 0.000279358 -0.00159474 0.999999 1.24683 0.999995 0.0010883 -0.00300914 -356.693 -0.00108433 0.999999 0.0013196 24.9801 0.00301057 -0.00131633 0.999995 1.28233 0.999985 0.00116212 -0.00534317 -358.358 -0.00115392 0.999998 0.00153745 24.9897 0.00534494 -0.00153127 0.999985 1.30919 0.999981 0.00126454 -0.00599619 -360.03 -0.00124863 0.999996 0.00265531 24.9996 0.00599952 -0.00264777 0.999978 1.33971 0.999996 0.00162416 -0.00221741 -362.552 -0.00161769 0.999994 0.00291317 24.9928 0.00222213 -0.00290957 0.999993 1.3892 0.999999 0.00154992 0.00045083 -364.244 -0.00155064 0.999998 0.00159133 24.9923 -0.000448363 -0.00159203 0.999999 1.42672 0.999993 0.00113021 -0.00369196 -365.954 -0.00112606 0.999999 0.00112517 24.9849 0.00369323 -0.00112101 0.999993 1.4574 0.999964 0.000717984 -0.00847995 -367.672 -0.000684094 0.999992 0.00399871 24.9941 0.00848275 -0.00399277 0.999956 1.48392 0.999992 0.000501015 -0.00404284 -369.388 -0.000478437 0.999984 0.00558358 25.0053 0.00404557 -0.0055816 0.999976 1.52179 0.999966 -0.000744611 0.00823438 -371.985 0.000710324 0.999991 0.004166 25.0068 -0.00823741 -0.00416 0.999957 1.5866 0.99912 -0.00209773 0.0418951 -373.689 0.00166531 0.999945 0.0103539 25.0105 -0.0419145 -0.010275 0.999068 1.66983 0.999836 -0.00404687 0.0176491 -375.424 0.00385705 0.999934 0.0107762 25.0189 -0.0176916 -0.0107064 0.999786 1.71778 0.998072 -0.00597873 -0.061786 -377.247 0.00657316 0.999934 0.00942201 25.0136 0.0617255 -0.00980997 0.998045 1.73625 0.999092 -0.00613622 -0.0421535 -378.989 0.00644498 0.999953 0.00719271 25.0265 0.0421074 -0.00745786 0.999085 1.73088 0.999703 -0.00839686 0.0228636 -380.685 0.00822845 0.999938 0.00745003 24.9911 -0.0229248 -0.00725968 0.999711 1.73901 0.999937 -0.0108018 -0.00320669 -382.471 0.0108298 0.999902 0.0088452 24.9797 0.00311083 -0.00887937 0.999956 1.77609 0.999815 -0.011473 -0.0154249 -384.276 0.0116358 0.999877 0.0105059 24.9661 0.0153025 -0.0106834 0.999826 1.80764 0.999912 -0.0131695 0.00178745 -386.044 0.0131501 0.999859 0.0104405 24.9466 -0.0019247 -0.010416 0.999944 1.84899 0.99988 -0.0150213 0.00386299 -387.842 0.0149843 0.999843 0.00945967 24.909 -0.00400448 -0.00940065 0.999948 1.88981 0.999823 -0.0169568 -0.00816032 -389.645 0.0170288 0.999816 0.00884207 24.8932 0.00800889 -0.00897946 0.999928 1.92126 0.999791 -0.0183374 -0.00903012 -391.43 0.0184277 0.99978 0.0100164 24.865 0.00884445 -0.0101808 0.999909 1.96259 0.999802 -0.0198753 -7.35591e-05 -393.191 0.019875 0.999749 0.0103619 24.8312 -0.000132406 -0.0103614 0.999946 2.00315 0.999772 -0.0213224 -0.000840548 -394.94 0.0213296 0.999725 0.00972783 24.7995 0.000632896 -0.00974354 0.999952 2.03618 0.999711 -0.0230015 -0.00698048 -396.663 0.0230735 0.99968 0.0104069 24.7669 0.00673887 -0.010565 0.999921 2.07835 0.999669 -0.0250164 -0.00595651 -398.364 0.0251007 0.999579 0.0145301 24.738 0.00559051 -0.0146748 0.999877 2.11192 0.999559 -0.0293499 0.00446018 -400.891 0.0292838 0.99947 0.014216 24.6586 -0.00487506 -0.0140792 0.999889 2.19029 0.999463 -0.0327213 0.00179952 -402.571 0.0326927 0.999366 0.0141175 24.6068 -0.00226032 -0.0140511 0.999899 2.22944 0.999344 -0.0361325 -0.00237448 -404.264 0.036162 0.99925 0.0138746 24.5534 0.00187137 -0.0139514 0.999901 2.26809 0.999213 -0.0396638 -0.000578243 -405.915 0.039668 0.999108 0.0144918 24.488 2.92785e-06 -0.0145033 0.999895 2.31017 0.999061 -0.0433202 0.000627004 -407.574 0.0433048 0.998936 0.0158876 24.4093 -0.00131459 -0.0158456 0.999874 2.34711 0.998894 -0.0470158 0.000881507 -409.233 0.0469968 0.998778 0.0153136 24.3348 -0.00160041 -0.0152552 0.999882 2.40771 0.998675 -0.050748 0.00850776 -410.905 0.0506006 0.998579 0.0167305 24.2473 -0.00934471 -0.0162779 0.999824 2.42701 0.998353 -0.0536283 0.0203961 -412.538 0.0530534 0.998206 0.0277547 24.1892 -0.0218479 -0.0266269 0.999407 2.51655 0.998356 -0.0569406 0.00654711 -414.193 0.0567966 0.998177 0.0204065 24.093 -0.00769713 -0.0200011 0.99977 2.59033 0.997889 -0.0598227 -0.025283 -415.891 0.0601861 0.998091 0.0138629 23.986 0.0244054 -0.0153554 0.999584 2.59048 0.997898 -0.0602312 -0.0239182 -417.542 0.0605607 0.998076 0.0132953 23.8728 0.0230714 -0.0147158 0.999626 2.62206 0.998006 -0.0623757 0.00965961 -419.199 0.0622406 0.997967 0.0137139 23.7644 -0.0104954 -0.0130853 0.999859 2.65202 0.997846 -0.0652549 0.00676818 -420.879 0.0651565 0.997779 0.0138556 23.6738 -0.00765729 -0.0133847 0.999881 2.68078 0.99771 -0.0671161 -0.00840026 -422.56 0.0672263 0.997645 0.0136061 23.566 0.0074673 -0.0141396 0.999872 2.71634 0.997503 -0.0699163 -0.00993947 -424.249 0.0700603 0.997431 0.0149566 23.444 0.00886822 -0.0156156 0.999839 2.74576 0.997278 -0.0736053 -0.00437253 -425.93 0.073674 0.997112 0.0184484 23.3292 0.00300201 -0.0187203 0.99982 2.77558 0.997013 -0.0767602 -0.0085983 -427.622 0.0769142 0.996851 0.0192947 23.2091 0.00709016 -0.0198984 0.999777 2.80678 0.996762 -0.0798951 -0.00906196 -429.326 0.0800606 0.996596 0.0196643 23.0795 0.00746003 -0.0203261 0.999766 2.83588 0.996485 -0.0833804 0.00808622 -431.004 0.0831903 0.996301 0.0215301 22.9357 -0.0098515 -0.0207818 0.999735 2.8877 0.996135 -0.0873145 0.00953256 -432.679 0.0871271 0.996024 0.0185651 22.7942 -0.0111157 -0.0176628 0.999782 2.94392 0.995865 -0.0905794 -0.00688027 -434.363 0.0906639 0.995795 0.0131597 22.6397 0.00565934 -0.0137291 0.99989 2.99397 0.995543 -0.0939748 -0.00798856 -435.99 0.0940711 0.995485 0.0126778 22.4876 0.00676109 -0.0133728 0.999888 3.0261 0.995274 -0.0970315 -0.00391727 -437.614 0.0970723 0.995204 0.0120941 22.3249 0.00272497 -0.0124172 0.999919 3.05746 0.994922 -0.100503 -0.00532267 -439.216 0.100567 0.994838 0.0135457 22.1713 0.0039338 -0.0140122 0.999894 3.09097 0.994635 -0.103327 -0.0048854 -440.79 0.103381 0.994563 0.0124997 21.9971 0.00356728 -0.0129377 0.99991 3.14556 0.994252 -0.107017 0.00324716 -442.345 0.106965 0.994173 0.0133735 21.8351 -0.00465943 -0.0129493 0.999905 3.1756 0.993926 -0.110047 -0.00118604 -443.901 0.110053 0.993822 0.014374 21.6774 -0.000403111 -0.0144172 0.999896 3.20961 0.993636 -0.112396 -0.00743879 -445.415 0.112483 0.993574 0.0125875 21.4946 0.0059762 -0.0133441 0.999893 3.27473 0.992959 -0.115558 0.0260445 -446.881 0.114958 0.993093 0.0234552 21.3193 -0.0285751 -0.020296 0.999386 3.31987 0.992149 -0.119147 0.0380009 -448.345 0.118099 0.992586 0.0287182 21.1813 -0.0411408 -0.0240049 0.998865 3.41143 0.992395 -0.122471 -0.0123884 -449.886 0.122678 0.992289 0.01766 20.9944 0.01013 -0.0190455 0.999767 3.42424 0.99066 -0.125178 -0.0540618 -451.392 0.126044 0.99194 0.0129222 20.806 0.0520085 -0.0196157 0.998454 3.4295 0.99142 -0.127729 -0.0277887 -452.836 0.128156 0.991653 0.0141472 20.6101 0.0257498 -0.0175871 0.999514 3.44591 0.991337 -0.130722 0.0127382 -454.279 0.130494 0.991297 0.0173394 20.4167 -0.014894 -0.0155269 0.999769 3.47451 0.991017 -0.133652 0.00479564 -455.78 0.133564 0.990919 0.0154659 20.2232 -0.00681915 -0.0146864 0.999869 3.48631 0.990621 -0.135755 -0.0155321 -457.287 0.136008 0.990569 0.0165781 20.0239 0.013135 -0.0185351 0.999742 3.52295 0.990335 -0.138498 -0.00747949 -458.78 0.138605 0.99021 0.0165128 19.8068 0.00511929 -0.0173898 0.999836 3.56263 0.989863 -0.142021 0.000730939 -460.272 0.141984 0.989701 0.0182296 19.6029 -0.0033124 -0.0179411 0.999834 3.5776 0.989493 -0.144337 -0.00841168 -461.805 0.144462 0.989368 0.0168132 19.3835 0.00589546 -0.0178518 0.999823 3.61746 0.989081 -0.147219 -0.00673779 -463.326 0.147311 0.988959 0.0160876 19.1528 0.00429499 -0.0169045 0.999848 3.65328 0.988605 -0.150536 0.000126158 -464.829 0.150508 0.988441 0.0181892 18.9269 -0.00286283 -0.017963 0.999835 3.68432 0.988216 -0.15299 -0.00474664 -466.356 0.153049 0.988074 0.0169156 18.6935 0.00210213 -0.0174427 0.999846 3.71605 0.987711 -0.15612 -0.00732036 -467.881 0.156216 0.987606 0.015224 18.4415 0.00485286 -0.0161805 0.999857 3.75466 0.987234 -0.159223 -0.00403668 -469.396 0.159267 0.987104 0.016123 18.2054 0.00141747 -0.0165601 0.999862 3.78426 0.986656 -0.162648 -0.00742736 -470.938 0.162746 0.986549 0.0153124 17.9565 0.00483693 -0.0163169 0.999855 3.81963 0.986179 -0.165423 -0.00933281 -472.446 0.165546 0.986097 0.0144005 17.6972 0.00682089 -0.0157464 0.999853 3.85484 0.985667 -0.168563 -0.00688651 -473.94 0.168653 0.985549 0.0157881 17.4386 0.00412571 -0.0167232 0.999852 3.87724 0.985156 -0.171383 -0.00974861 -475.415 0.171522 0.985052 0.0159004 17.1778 0.00687784 -0.0173365 0.999826 3.91292 0.984731 -0.173993 -0.00552635 -476.86 0.174042 0.984686 0.0101759 16.9109 0.00367119 -0.0109823 0.999933 3.97832 0.983948 -0.177555 0.0179082 -478.287 0.177247 0.98401 0.0175235 16.6566 -0.0207332 -0.014068 0.999686 4.00114 0.983184 -0.180238 0.0293952 -479.676 0.179427 0.983366 0.028247 16.4248 -0.0339974 -0.0224977 0.999169 4.08167 0.983107 -0.182546 -0.0133053 -481.143 0.1828 0.982918 0.0213654 16.1524 0.00917782 -0.0234367 0.999683 4.07436 0.980755 -0.185566 -0.060703 -482.586 0.186655 0.982343 0.0127385 15.8727 0.0572674 -0.0238239 0.998075 4.10863 0.981491 -0.18851 -0.0337597 -483.962 0.189122 0.981824 0.0159313 15.6011 0.0301428 -0.0220211 0.999303 4.09528 0.9816 -0.190948 -0.00101975 -485.344 0.190923 0.981352 0.0222653 15.337 -0.00325079 -0.0220503 0.999752 4.11682 0.980974 -0.194022 -0.00670505 -486.785 0.194113 0.980815 0.0179511 15.0482 0.00309351 -0.0189111 0.999816 4.12737 0.980423 -0.196594 -0.0110386 -488.211 0.196767 0.98029 0.0177261 14.7605 0.00733622 -0.0195511 0.999782 4.15907 0.980079 -0.198553 -0.00461182 -489.656 0.198606 0.979892 0.0191813 14.4682 0.000710584 -0.0197151 0.999805 4.19317 0.979832 -0.199825 -0.000347604 -491.087 0.199804 0.979699 0.0163755 14.1725 -0.00293169 -0.0161147 0.999866 4.23624 0.979732 -0.200307 0.00166166 -492.548 0.200264 0.979639 0.0142058 13.8633 -0.00447335 -0.0135851 0.999898 4.2703 0.979613 -0.200893 -0.000859963 -494.024 0.200885 0.979508 0.0144447 13.5631 -0.0020595 -0.0143229 0.999895 4.29934 0.979484 -0.201468 -0.00455299 -495.493 0.201515 0.979368 0.0151511 13.2646 0.00140659 -0.0157578 0.999875 4.33921 0.979334 -0.202247 -0.000860852 -496.956 0.202238 0.979231 0.0143333 12.9624 -0.0020559 -0.0142112 0.999897 4.38383 0.979079 -0.203479 0.000658527 -498.426 0.203447 0.978972 0.0149285 12.6557 -0.00368232 -0.0144822 0.999888 4.41698 0.978789 -0.204831 -0.00412626 -499.925 0.20487 0.978684 0.0143525 12.342 0.00109847 -0.0148934 0.999888 4.45473 0.978369 -0.206756 -0.00679873 -501.429 0.206835 0.978268 0.0145165 12.0319 0.00364961 -0.0156087 0.999872 4.48359 0.977683 -0.209978 -0.00676722 -502.93 0.210063 0.977551 0.0163805 11.7179 0.00317576 -0.0174364 0.999843 4.51559 0.976835 -0.213913 -0.00586324 -504.431 0.213981 0.976706 0.0160159 11.3867 0.00230066 -0.0168995 0.999855 4.54842 0.975941 -0.217899 -0.00766515 -505.973 0.218002 0.975795 0.0173071 11.0452 0.00370842 -0.0185617 0.999821 4.57937 0.975271 -0.220878 -0.00775076 -507.503 0.22098 0.975133 0.0168127 10.6925 0.00384446 -0.0181097 0.999829 4.61173 0.97481 -0.222948 -0.00631018 -509.027 0.223027 0.974644 0.0180894 10.3445 0.00211718 -0.0190411 0.999816 4.64217 0.974374 -0.224868 -0.00555402 -510.567 0.224932 0.974204 0.0182222 9.9859 0.00131316 -0.0190045 0.999819 4.67387 0.973957 -0.226634 -0.00674811 -512.131 0.226723 0.973773 0.0190416 9.62828 0.00225567 -0.0200756 0.999796 4.70934 0.973627 -0.228051 -0.0065124 -513.689 0.228136 0.973438 0.0192931 9.26871 0.00193961 -0.02027 0.999793 4.74904 0.973179 -0.229986 -0.00538274 -515.255 0.230048 0.972972 0.020077 8.9029 0.000619828 -0.0207768 0.999784 4.78346 0.972667 -0.231936 -0.0111831 -516.819 0.232118 0.972494 0.0193891 8.52566 0.00637849 -0.0214549 0.999749 4.80938 0.972091 -0.234081 -0.0156312 -518.389 0.234358 0.971962 0.0191573 8.15214 0.0107085 -0.022286 0.999694 4.8412 0.971597 -0.236317 -0.0124222 -519.917 0.236526 0.971431 0.019451 7.7758 0.00747072 -0.0218367 0.999734 4.8742 0.970776 -0.239568 -0.0141759 -521.471 0.239854 0.970511 0.0240648 7.40223 0.00799269 -0.0267617 0.99961 4.88494 0.969934 -0.242483 -0.020759 -522.983 0.24297 0.969697 0.0255551 7.03653 0.0139333 -0.0298305 0.999458 4.92512 0.969111 -0.24617 -0.0149948 -524.476 0.246504 0.968753 0.0274528 6.6606 0.00776817 -0.030301 0.999511 4.95601 0.968292 -0.249671 -0.00866353 -525.937 0.249814 0.967953 0.0256895 6.27954 0.00197196 -0.0270392 0.999632 5.01543 0.967294 -0.253654 0.00108046 -527.408 0.253589 0.967128 0.0188698 5.88292 -0.00583134 -0.0179787 0.999821 5.06092 0.966024 -0.258452 -0.000644474 -528.86 0.258419 0.965855 0.0185564 5.49122 -0.00417347 -0.0180925 0.999828 5.07034 0.964948 -0.262125 -0.012901 -530.298 0.262294 0.964889 0.0138212 5.10058 0.00882513 -0.0167206 0.999821 5.1291 0.963852 -0.266354 -0.00678136 -531.715 0.266413 0.963804 0.0103056 4.68526 0.00379097 -0.0117397 0.999924 5.15046 0.962632 -0.2707 -0.00780061 -533.107 0.270789 0.962529 0.0145469 4.30818 0.00357045 -0.0161157 0.999864 5.16305 0.961624 -0.27389 -0.0162081 -534.495 0.274149 0.961541 0.0167642 3.92396 0.0109932 -0.0205642 0.999728 5.20527 0.960774 -0.277311 -0.00352643 -535.85 0.277322 0.960547 0.0209982 3.51244 -0.00243571 -0.0211525 0.999773 5.233 0.959115 -0.281684 0.0274224 -537.173 0.280622 0.959104 0.0370274 3.12796 -0.036731 -0.0278182 0.998938 5.305 0.958211 -0.285202 0.0221716 -538.498 0.284309 0.958041 0.036403 2.74847 -0.0316235 -0.0285781 0.999091 5.39307 0.957412 -0.287511 -0.0264488 -539.87 0.287989 0.957493 0.0164291 2.32743 0.020601 -0.0233464 0.999515 5.39865 0.955872 -0.289327 -0.0509813 -541.231 0.290091 0.956965 0.00811624 1.91447 0.0464391 -0.0225473 0.998667 5.40879 0.956687 -0.290631 -0.0168588 -542.52 0.290895 0.956618 0.0161848 1.50671 0.0114236 -0.020388 0.999727 5.43185 0.956323 -0.292282 0.00423253 -543.781 0.292101 0.956081 0.0242262 1.12493 -0.0111275 -0.0219318 0.999698 5.45068 0.955986 -0.293348 -0.00613928 -545.077 0.293412 0.955769 0.0203817 0.715623 -0.000111194 -0.0212859 0.999773 5.47409 0.955482 -0.29483 -0.0113602 -546.361 0.295015 0.955246 0.0216866 0.329321 0.00445791 -0.0240725 0.9997 5.51189 0.955184 -0.295934 -0.00677795 -547.609 0.29601 0.954862 0.0248357 -0.0531755 -0.000877734 -0.025729 0.999669 5.55248 0.954812 -0.297202 -0.00238965 -548.851 0.297172 0.954521 0.0240344 -0.434957 -0.00486211 -0.0236585 0.999708 5.59611 0.954534 -0.298096 -0.00202905 -550.077 0.298064 0.954276 0.0226848 -0.825695 -0.00482597 -0.0222582 0.999741 5.63397 0.954842 -0.297097 -0.0031115 -551.28 0.297096 0.954618 0.0209507 -1.19385 -0.0032541 -0.0209291 0.999776 5.67818 0.956152 -0.292857 -0.00280867 -552.441 0.292859 0.955989 0.0178618 -1.55029 -0.0025459 -0.0179012 0.999837 5.71949 0.959234 -0.282603 -0.0022356 -554.142 0.282604 0.959124 0.014677 -2.04533 -0.00200355 -0.0147105 0.99989 5.7735 0.963125 -0.269029 -0.0036235 -555.234 0.269054 0.963054 0.0116775 -2.3355 0.000348045 -0.0122218 0.999925 5.81416 0.968333 -0.249621 -0.00450682 -556.272 0.249655 0.968279 0.0104243 -2.58562 0.00176174 -0.0112193 0.999936 5.83516 0.974282 -0.225206 -0.00754552 -557.293 0.225279 0.974232 0.0109827 -2.78229 0.00487774 -0.0124 0.999911 5.86326 0.981069 -0.193442 -0.00911801 -558.279 0.193527 0.981048 0.00960963 -2.95233 0.0070863 -0.0111923 0.999912 5.88346 0.990939 -0.133699 -0.0128398 -559.669 0.133865 0.990913 0.0131 -3.07363 0.0109717 -0.0147001 0.999832 5.90675 0.998271 -0.058429 -0.0063626 -560.955 0.0585666 0.997991 0.0241691 -3.07188 0.00493764 -0.0245 0.999688 5.90786 0.999669 0.0254658 -0.00351937 -562.197 -0.0253044 0.998876 0.0400893 -2.93936 0.00453631 -0.039987 0.99919 5.9534 0.993004 0.117839 -0.00756438 -563.393 -0.117467 0.992336 0.0383389 -2.7061 0.0120242 -0.0371821 0.999236 5.97949 0.974706 0.223242 0.0105725 -564.501 -0.223492 0.973584 0.0467428 -2.33504 0.000141785 -0.0479233 0.998851 5.9894 0.939917 0.341378 0.00412856 -565.585 -0.34123 0.938985 0.0432448 -1.85167 0.0108862 -0.0420553 0.999056 6.01205 0.884893 0.465787 0.0026189 -566.524 -0.465344 0.883778 0.0489058 -1.19777 0.0204651 -0.0444951 0.9988 6.00338 0.804142 0.594436 -0.0011034 -567.399 -0.593739 0.803286 0.0469576 -0.411059 0.0287997 -0.0371055 0.998896 6.01442 0.700909 0.713163 -0.0112221 -568.138 -0.71208 0.700572 0.0462676 0.521578 0.0408583 -0.0244383 0.998866 5.96777 0.576968 0.816744 -0.00613842 -568.67 -0.815579 0.576518 0.0495798 1.57283 0.0440329 -0.0235996 0.998751 5.95505 0.436223 0.899828 -0.0043381 -569.099 -0.89876 0.435931 0.0468523 2.73517 0.0440501 -0.0165392 0.998892 5.9003 0.298779 0.954296 0.00702123 -569.36 -0.953483 0.2982 0.0441295 4.03339 0.0400189 -0.0198796 0.999001 5.86555 0.185142 0.982536 0.0186022 -569.517 -0.982093 0.18432 0.0389841 5.40804 0.0348745 -0.0254867 0.999067 5.85971 0.107087 0.993998 0.0223829 -569.628 -0.994167 0.10676 0.0153007 6.82479 0.0128193 -0.0238909 0.999632 5.84556 0.0568807 0.997738 0.0358127 -569.645 -0.998381 0.0568321 0.0023744 8.26614 0.000333725 -0.0358897 0.999356 5.8592 0.0340687 0.998891 0.0324893 -569.668 -0.999249 0.0334451 0.0195486 9.29129 0.0184403 -0.0331309 0.999281 5.86258 0.020081 0.999208 0.0343641 -569.659 -0.998665 0.0184102 0.0482631 10.3712 0.0475922 -0.0352874 0.998243 5.81523 0.00872091 0.999253 0.0376609 -569.669 -0.998117 0.00641208 0.0609971 11.4229 0.06071 -0.0381219 0.997427 5.82827 -0.000528818 0.999531 0.0306121 -569.67 -0.998955 -0.00192712 0.0456666 12.4884 0.0457042 -0.030556 0.998488 5.81438 -0.00583638 0.999317 0.0364798 -569.649 -0.999675 -0.0067365 0.0246006 13.5652 0.0248296 -0.0363244 0.999032 5.78498 -0.008557 0.99941 0.0332672 -569.637 -0.999685 -0.00933443 0.0232846 14.6851 0.0235814 -0.0330575 0.999175 5.79833 -0.0090841 0.999567 0.0279702 -569.623 -0.999494 -0.00992878 0.0302099 15.8635 0.0304745 -0.0276817 0.999152 5.76935 -0.00847925 0.999417 0.0330872 -569.586 -0.999494 -0.00948485 0.0303548 17.0294 0.0306509 -0.032813 0.998991 5.76495 -0.00851587 0.999557 0.0285141 -569.587 -0.999576 -0.00930334 0.0275992 18.2183 0.0278522 -0.028267 0.999212 5.7727 -0.00764479 0.999602 0.0271591 -569.584 -0.999554 -0.00842293 0.028653 19.4326 0.0288703 -0.0269279 0.99922 5.74969 -0.00629257 0.999575 0.0284597 -569.549 -0.99949 -0.00717788 0.0311131 20.6684 0.0313041 -0.0282494 0.999111 5.73573 -0.00600962 0.99958 0.0283646 -569.547 -0.99953 -0.00685757 0.0298929 21.9189 0.0300749 -0.0281716 0.999151 5.72749 -0.00431959 0.999557 0.029447 -569.532 -0.999585 -0.00515428 0.0283288 23.1786 0.028468 -0.0293125 0.999165 5.71121 -0.00390141 0.999389 0.0347368 -569.516 -0.999586 -0.00488823 0.0283689 24.4848 0.0285213 -0.0346117 0.998994 5.68965 -0.00438531 0.999279 0.0377196 -569.488 -0.999497 -0.00556496 0.0312261 25.7796 0.0314134 -0.0375637 0.9988 5.69587 -0.00528407 0.999148 0.0409228 -569.466 -0.999471 -0.00659003 0.031844 27.1421 0.0320865 -0.0407329 0.998655 5.67661 -0.00656149 0.998982 0.0446304 -569.435 -0.999517 -0.00790755 0.0300509 28.489 0.0303732 -0.0444116 0.998551 5.66492 -0.00746806 0.999004 0.0440015 -569.429 -0.999452 -0.00887622 0.0318945 29.8687 0.0322533 -0.0437392 0.998522 5.65118 -0.00830232 0.999059 0.0425751 -569.409 -0.999374 -0.00975361 0.0339943 31.2601 0.0343775 -0.0422663 0.998515 5.64205 -0.0087094 0.998969 0.0445546 -569.372 -0.999535 -0.00999956 0.0288165 32.6622 0.0292323 -0.0442829 0.998591 5.61968 -0.009803 0.998935 0.0450878 -569.364 -0.999608 -0.0109729 0.0257734 34.0849 0.0262407 -0.0448175 0.99865 5.62194 -0.00819397 0.998833 0.0475926 -569.324 -0.99965 -0.0093799 0.0247487 35.5603 0.0251662 -0.0473732 0.99856 5.60805 -0.00585685 0.998715 0.0503437 -569.312 -0.999701 -0.00704373 0.0234305 37.7691 0.023755 -0.0501914 0.998457 5.61709 -0.00386109 0.99853 0.0540658 -569.288 -0.999715 -0.00512906 0.0233332 39.2626 0.0235762 -0.0539603 0.998265 5.60283 -0.00205045 0.998405 0.0564151 -569.276 -0.999724 -0.0033673 0.0232571 40.7639 0.02341 -0.0563518 0.998136 5.60232 -0.000979697 0.998281 0.0586058 -569.268 -0.999736 -0.00232398 0.0228739 42.2915 0.0229708 -0.0585679 0.998019 5.60001 0.000470918 0.998012 0.0630236 -569.256 -0.999689 -0.00110168 0.0249155 43.8243 0.0249354 -0.0630157 0.997701 5.59313 0.00190888 0.997558 0.069821 -569.227 -0.999617 -2.42101e-05 0.027675 45.3999 0.0276091 -0.0698471 0.997176 5.57612 0.00175273 0.997124 0.0757622 -569.214 -0.999608 -0.000370284 0.0279989 47.7606 0.0279464 -0.0757816 0.996733 5.5758 0.00138971 0.997023 0.0770944 -569.186 -0.999594 -0.000809963 0.0284935 50.1632 0.0284712 -0.0771027 0.996617 5.56548 0.000401221 0.997419 0.0717953 -569.2 -0.999609 -0.00160771 0.0279214 51.7795 0.0279648 -0.0717784 0.997028 5.57443 -0.000300058 0.997955 0.0639164 -569.187 -0.999599 -0.00210924 0.0282399 54.2297 0.028317 -0.0638823 0.997556 5.55475 -0.00162515 0.998311 0.0580721 -569.202 -0.999368 -0.00368282 0.0353437 55.8707 0.0354979 -0.057978 0.997687 5.55402 -0.00213686 0.998426 0.0560476 -569.2 -0.999352 -0.00414664 0.0357668 57.4812 0.0359429 -0.0559348 0.997787 5.53521 -0.00281132 0.998491 0.0548372 -569.194 -0.999334 -0.00480068 0.0361797 59.1019 0.0363884 -0.0546989 0.99784 5.50467 -0.00314832 0.998511 0.0544512 -569.17 -0.999338 -0.00511454 0.0360082 60.6897 0.0362331 -0.0543018 0.997867 5.49974 -0.00304208 0.998716 0.0505697 -569.159 -0.999382 -0.00480702 0.0348162 62.286 0.0350146 -0.0504325 0.998113 5.47716 -0.00265636 0.998594 0.0529479 -569.141 -0.999281 -0.00465268 0.037616 63.8639 0.0378095 -0.0528099 0.997889 5.4463 -0.00334561 0.998366 0.0570514 -569.132 -0.999291 -0.00547744 0.0372515 65.4113 0.0375032 -0.0568863 0.997676 5.43985 -0.00120276 0.998184 0.0602243 -569.107 -0.99936 -0.00335279 0.0356119 67.6602 0.0357492 -0.060143 0.997549 5.43156 0.000466317 0.998112 0.0614261 -569.106 -0.999162 -0.0020494 0.0408859 69.1078 0.0409345 -0.0613937 0.997274 5.41105 0.00193039 0.998525 0.0542651 -569.094 -0.999096 -0.000378313 0.0425026 71.1551 0.0424604 -0.0542981 0.997622 5.42983 0.000128079 0.998847 0.0480168 -569.121 -0.999719 -0.00101076 0.0236925 72.4593 0.0237137 -0.0480064 0.998565 5.41558 -0.000149868 0.998797 0.0490394 -569.091 -0.999672 -0.00140574 0.0255759 73.7114 0.025614 -0.0490195 0.998469 5.38827 -0.000292454 0.999033 0.0439647 -569.09 -0.999246 -0.00199935 0.0387853 74.9357 0.0388356 -0.0439202 0.99828 5.42011 -0.000623864 0.999226 0.0393222 -569.105 -0.999567 -0.00178005 0.0293747 76.129 0.029422 -0.0392869 0.998795 5.38673 0.000708978 0.998948 0.0458583 -569.088 -0.999701 -0.000412138 0.0244334 77.2892 0.0244265 -0.0458619 0.998649 5.38867 0.00157682 0.999154 0.0410848 -569.102 -0.999607 0.000425672 0.0280124 78.4414 0.0279713 -0.0411129 0.998763 5.39579 0.00145432 0.999219 0.0394871 -569.119 -0.999727 0.000532428 0.0233471 79.6002 0.0233079 -0.0395103 0.998947 5.39842 0.00187372 0.998945 0.0458771 -569.112 -0.999992 0.00203528 -0.00347515 80.7188 -0.00356485 -0.0458703 0.998941 5.41831 0.00362176 0.998865 0.0474867 -569.081 -0.999993 0.00366815 -0.000889692 81.8697 -0.00106287 -0.0474832 0.998871 5.40973 0.00489831 0.998893 0.046787 -569.087 -0.998691 0.00250443 0.0510876 83.0956 0.0509138 -0.046976 0.997598 5.33536 0.00567105 0.99856 0.0533404 -569.073 -0.997517 0.00190426 0.0704053 84.2621 0.0702023 -0.0536072 0.996091 5.36023 0.00227974 0.998477 0.0551159 -569.066 -0.999449 0.00045052 0.0331783 85.4058 0.0331029 -0.0551611 0.997929 5.3979 0.00102431 0.998596 0.0529569 -569.065 -0.999935 0.000420893 0.0114043 86.603 0.011366 -0.0529652 0.998532 5.33017 0.000594137 0.998464 0.0554088 -569.049 -0.999555 -0.00106019 0.0298225 87.8637 0.0298354 -0.0554019 0.998018 5.30705 -0.000137221 0.998553 0.0537818 -569.046 -0.999373 -0.00204086 0.0353423 89.1018 0.0354009 -0.0537432 0.997927 5.33692 -0.00173706 0.998509 0.0545628 -569.029 -0.999738 -0.00297908 0.0226901 90.9812 0.0228188 -0.0545091 0.998253 5.30066 -0.00294448 0.998567 0.0534257 -569.032 -0.999591 -0.00445946 0.0282596 92.2584 0.0284574 -0.0533206 0.998172 5.30775 -0.00399261 0.998744 0.0499492 -569.027 -0.999646 -0.00530028 0.0260751 94.2206 0.0263071 -0.0498274 0.998411 5.29864 -0.00448519 0.998886 0.0469723 -569.022 -0.999672 -0.00566293 0.02497 95.5219 0.0252081 -0.0468449 0.998584 5.3025 -0.00407959 0.998894 0.0468358 -569.008 -0.999618 -0.00535413 0.0271198 96.855 0.0273406 -0.0467073 0.998534 5.27776 -0.00402063 0.99885 0.047781 -568.992 -0.999468 -0.00556077 0.0321443 98.8265 0.032373 -0.0476263 0.99834 5.27604 -0.00310727 0.998685 0.0511642 -568.968 -0.99951 -0.00469508 0.0309429 100.131 0.0311424 -0.051043 0.998211 5.25621 -0.00274282 0.99854 0.0539468 -568.962 -0.999481 -0.00446856 0.031895 101.418 0.0320895 -0.0538314 0.998034 5.2686 -0.00160611 0.998612 0.0526473 -568.958 -0.99946 -0.00333099 0.0326916 102.699 0.0328216 -0.0525664 0.998078 5.25192 -0.000744003 0.998701 0.0509406 -568.958 -0.99947 -0.00240058 0.0324663 103.969 0.0325464 -0.0508894 0.998174 5.25693 0.000289676 0.998885 0.0472006 -568.96 -0.999466 -0.00125365 0.0326642 105.219 0.032687 -0.0471849 0.998351 5.24572 0.0018436 0.998987 0.0449548 -568.968 -0.999439 0.00033718 0.0334944 106.467 0.0334453 -0.0449913 0.998427 5.23278 0.0029523 0.999074 0.0429246 -568.979 -0.999676 0.00186359 0.0253812 108.293 0.0252777 -0.0429856 0.998756 5.23391 0.00232153 0.998833 0.0482432 -568.972 -0.999977 0.00262523 -0.00623267 109.464 -0.00635204 -0.0482276 0.998816 5.26101 0.00437401 0.999218 0.0392924 -568.986 -0.999807 0.00361804 0.01929 111.306 0.0191328 -0.0393692 0.999042 5.24082 0.00651425 0.999276 0.0374718 -568.976 -0.997304 0.00375305 0.0732912 112.542 0.0730975 -0.0378482 0.996606 5.19738 0.00408605 0.999155 0.040896 -568.977 -0.998853 0.00212734 0.0478243 113.654 0.0476969 -0.0410445 0.998018 5.28649 0.0014018 0.99948 0.032227 -569.004 -0.999986 0.00123612 0.00516015 114.812 0.00511763 -0.0322338 0.999467 5.2565 0.000690922 0.9995 0.0316142 -568.992 -0.99989 0.000221382 0.0148533 116.042 0.0148388 -0.031621 0.99939 5.20702 -0.000985068 0.999348 0.0360859 -568.976 -0.999622 -0.0019756 0.027424 117.234 0.0274775 -0.0360452 0.998972 5.23176 -0.00317156 0.999217 0.0394273 -568.963 -0.999815 -0.00391645 0.0188298 118.408 0.0189695 -0.0393603 0.999045 5.24174 -0.00496715 0.999049 0.0433143 -568.932 -0.999844 -0.0056957 0.0167128 119.612 0.0169436 -0.0432245 0.998922 5.22761 -0.00621275 0.998538 0.0536919 -568.922 -0.999701 -0.00747098 0.0232655 120.816 0.0236326 -0.0535313 0.998286 5.22129 -0.00713424 0.998099 0.0612194 -568.901 -0.999633 -0.00871844 0.0256493 122.002 0.0261343 -0.0610139 0.997795 5.23299 -0.00797008 0.997879 0.0646108 -568.88 -0.999675 -0.0095158 0.0236513 123.193 0.024216 -0.0644013 0.99763 5.23288 -0.00738505 0.997634 0.0683455 -568.855 -0.999653 -0.0090934 0.0247186 124.384 0.0252816 -0.0681392 0.997355 5.23246 -0.00711259 0.997662 0.067972 -568.828 -0.999614 -0.00891993 0.0263231 126.179 0.0268679 -0.0677585 0.99734 5.24245 -0.00777992 0.997769 0.0663083 -568.822 -0.999818 -0.00891735 0.016875 127.358 0.0174286 -0.0661649 0.997656 5.24408 -0.0071822 0.997606 0.0687844 -568.809 -0.999927 -0.00783251 0.00918931 128.536 0.00970607 -0.0687134 0.997589 5.26448 -0.00641852 0.997944 0.0637671 -568.801 -0.9992 -0.00891688 0.0389726 129.776 0.0394611 -0.063466 0.997204 5.22063 -0.00485727 0.99754 0.0699357 -568.77 -0.998198 -0.00901974 0.0593264 130.996 0.0598113 -0.0695216 0.995786 5.20499 -0.00729837 0.997164 0.0749069 -568.759 -0.999357 -0.00990221 0.0344487 132.162 0.0350927 -0.0746073 0.996595 5.20393 -0.0092719 0.996918 0.077899 -568.741 -0.999494 -0.0116093 0.0296067 133.368 0.0304198 -0.0775851 0.996522 5.16716 -0.0104157 0.996408 0.0840375 -568.701 -0.999118 -0.0137898 0.0396703 134.597 0.0406867 -0.0835502 0.995673 5.14456 -0.0131663 0.995769 0.0909402 -568.666 -0.999232 -0.0164586 0.0355489 135.807 0.0368953 -0.0904023 0.995222 5.14402 -0.0165825 0.995384 0.0945296 -568.633 -0.999354 -0.0195133 0.0301642 137.003 0.0318696 -0.0939684 0.995065 5.13225 -0.019485 0.994848 0.0994901 -568.598 -0.999256 -0.0226906 0.0311912 138.227 0.033288 -0.0988083 0.99455 5.10781 -0.0234398 0.994344 0.103587 -568.547 -0.999169 -0.026757 0.0307501 139.423 0.0333479 -0.102781 0.994145 5.11016 -0.0300545 0.994336 0.101948 -568.508 -0.999292 -0.0321991 0.0194558 140.587 0.0226282 -0.101291 0.994599 5.13682 -0.0404843 0.993844 0.103124 -568.451 -0.998973 -0.0423631 0.0160944 141.765 0.020364 -0.102366 0.994538 5.11126 -0.051764 0.992855 0.107514 -568.36 -0.998056 -0.0551739 0.0289847 142.943 0.0347096 -0.105804 0.993781 5.11171 -0.0674448 0.992268 0.104191 -568.263 -0.996982 -0.071049 0.0312727 144.092 0.0384336 -0.101768 0.994065 5.11239 -0.0909386 0.99056 0.102572 -568.144 -0.995339 -0.0937267 0.0226879 145.223 0.0320875 -0.100031 0.994467 5.09514 -0.121109 0.987256 0.103235 -567.98 -0.992141 -0.123687 0.0189244 146.366 0.031452 -0.100132 0.994477 5.08176 -0.156412 0.982514 0.101001 -567.774 -0.987173 -0.158825 0.0162618 147.479 0.0320189 -0.0971616 0.994753 5.07561 -0.1924 0.976199 0.100093 -567.526 -0.980743 -0.194772 0.0144 148.599 0.0335526 -0.0953951 0.994874 5.05168 -0.228566 0.96843 0.0995017 -567.236 -0.973013 -0.230575 0.00902845 149.696 0.031686 -0.0947528 0.994996 5.03765 -0.266483 0.959382 0.0925954 -566.913 -0.963494 -0.267727 0.00106003 150.775 0.0258073 -0.0889326 0.995703 5.07527 -0.306433 0.948872 0.0757709 -566.55 -0.951685 -0.307054 -0.00360847 151.852 0.0198417 -0.0732158 0.997119 5.05565 -0.345965 0.934825 0.0800663 -566.118 -0.93791 -0.346867 -0.00279569 152.913 0.0251589 -0.0760622 0.996786 5.00976 -0.38714 0.918158 0.0843079 -565.623 -0.921575 -0.388176 -0.00440442 153.92 0.0286824 -0.0794012 0.99643 5.0351 -0.450565 0.889474 0.0763413 -564.823 -0.892192 -0.451643 -0.00349011 155.445 0.0313746 -0.0696836 0.997076 4.98297 -0.492324 0.866078 0.0867516 -564.209 -0.869559 -0.493803 -0.00498515 156.403 0.0385207 -0.07789 0.996217 4.96946 -0.53557 0.839869 0.0882329 -563.564 -0.843629 -0.536814 -0.0109799 157.33 0.038143 -0.0803163 0.996039 4.96491 -0.578078 0.812053 0.0799703 -562.872 -0.815226 -0.578978 -0.0137949 158.245 0.0350989 -0.0731684 0.996702 4.97061 -0.618499 0.782211 0.0748691 -562.118 -0.784751 -0.619762 -0.00779292 159.141 0.0403053 -0.0635735 0.997163 4.9147 -0.657191 0.749205 0.0824186 -561.306 -0.752127 -0.65898 -0.00703705 159.977 0.04904 -0.0666139 0.996573 4.8751 -0.695212 0.714432 0.0791715 -560.453 -0.717235 -0.696748 -0.0107495 160.792 0.0474828 -0.0642577 0.996803 4.87188 -0.731291 0.678015 0.0742294 -559.554 -0.680523 -0.732618 -0.0125907 161.568 0.0458451 -0.0597223 0.997162 4.82663 -0.762855 0.642018 0.0765818 -558.597 -0.644704 -0.76429 -0.0147187 162.299 0.049081 -0.0606009 0.996955 4.78486 -0.790435 0.60782 0.0759418 -557.586 -0.610544 -0.791789 -0.0175043 163.01 0.0494904 -0.0602018 0.996959 4.76278 -0.815473 0.574006 0.0743052 -556.574 -0.5764 -0.817046 -0.0141116 163.693 0.0526106 -0.0543371 0.997136 4.72573 -0.838131 0.540306 0.0748695 -555.514 -0.542559 -0.839928 -0.012252 164.337 0.0562652 -0.0508898 0.997118 4.67154 -0.870244 0.487183 0.0730014 -553.851 -0.489325 -0.871991 -0.0138772 165.215 0.0568958 -0.047798 0.997235 4.60923 -0.889388 0.450877 0.075494 -552.698 -0.453096 -0.891344 -0.014453 165.756 0.0607746 -0.0470603 0.997042 4.54212 -0.906147 0.415162 0.0808559 -551.51 -0.41769 -0.908439 -0.0165672 166.251 0.0665745 -0.048785 0.996588 4.49278 -0.926543 0.366994 0.0826688 -549.681 -0.369696 -0.928946 -0.0196119 166.936 0.0695974 -0.0487335 0.996384 4.42479 -0.937973 0.337426 0.0796905 -548.452 -0.339667 -0.940409 -0.0160553 167.364 0.0695242 -0.0421277 0.99669 4.35271 -0.947389 0.309808 0.0804535 -547.201 -0.312411 -0.949698 -0.0217606 167.739 0.0696649 -0.0457504 0.996521 4.26988 -0.954017 0.288666 0.0807685 -545.944 -0.291418 -0.956285 -0.0243933 168.086 0.0701962 -0.046809 0.996434 4.24696 -0.959277 0.271646 0.0774291 -544.705 -0.273714 -0.961656 -0.0172692 168.434 0.0697691 -0.0377594 0.996848 4.1813 -0.96341 0.256482 0.0778327 -543.443 -0.258536 -0.965845 -0.0173892 168.752 0.0707143 -0.0368754 0.996815 4.1083 -0.96628 0.245671 0.0771283 -542.179 -0.24747 -0.968786 -0.0145545 169.058 0.0711452 -0.0331507 0.996915 4.06929 -0.968488 0.239332 0.0689311 -540.925 -0.240342 -0.970666 -0.00663687 169.387 0.0653206 -0.0229948 0.997599 4.01557 -0.970206 0.237685 0.0469737 -539.686 -0.238715 -0.97093 -0.0176026 169.669 0.0414243 -0.0282915 0.998741 3.98639 -0.969819 0.236856 0.0578796 -538.403 -0.23786 -0.971237 -0.0110214 169.955 0.0536043 -0.024456 0.998263 3.95182 -0.965682 0.238134 0.103685 -537.072 -0.238625 -0.97108 0.00782485 170.307 0.10255 -0.0171856 0.994579 3.8472 -0.968194 0.243536 0.0573576 -535.197 -0.244322 -0.969669 -0.00699503 170.785 0.0539144 -0.0207863 0.998329 3.77655 -0.967074 0.248389 0.0554124 -533.9 -0.248942 -0.968513 -0.00321301 171.102 0.0528695 -0.0169017 0.998458 3.72162 -0.964971 0.252491 0.0712703 -532.565 -0.252987 -0.967467 0.00212266 171.468 0.0694877 -0.0159822 0.997455 3.64966 -0.964377 0.256546 0.0645088 -531.245 -0.25742 -0.966284 -0.00548379 171.813 0.060927 -0.0218943 0.997902 3.5978 -0.963702 0.260238 0.0596245 -529.914 -0.26099 -0.965328 -0.00506504 172.168 0.0562391 -0.0204426 0.998208 3.56382 -0.962728 0.263208 0.0622668 -528.557 -0.263938 -0.964533 -0.00365578 172.541 0.0590962 -0.0199541 0.998053 3.50129 -0.961874 0.265904 0.063978 -527.184 -0.266718 -0.963764 -0.00438979 172.914 0.0604925 -0.0212865 0.997942 3.44662 -0.961193 0.268385 0.0638576 -525.82 -0.269114 -0.963104 -0.00294155 173.31 0.0607121 -0.0200124 0.997955 3.40017 -0.960338 0.271343 0.0642114 -524.424 -0.272019 -0.96229 -0.00186947 173.701 0.0612827 -0.0192621 0.997935 3.34491 -0.959988 0.273176 0.0616294 -523.027 -0.273799 -0.961785 -0.00173227 174.098 0.058801 -0.018537 0.998098 3.29541 -0.959879 0.274084 0.0592427 -521.612 -0.274651 -0.961543 -0.00149549 174.501 0.0565545 -0.0177066 0.998242 3.24621 -0.959771 0.274416 0.0594626 -520.169 -0.275044 -0.961428 -0.00249483 174.91 0.0564844 -0.0187493 0.998227 3.19313 -0.959475 0.274953 0.061709 -518.708 -0.27556 -0.961283 -0.00138187 175.326 0.0589398 -0.0183304 0.998093 3.14204 -0.959146 0.275741 0.0632892 -517.237 -0.276293 -0.961073 2.47303e-05 175.751 0.0608324 -0.0174627 0.997995 3.07157 -0.958982 0.275898 0.0650659 -515.764 -0.276517 -0.961009 -0.000521979 176.169 0.0623849 -0.0184924 0.997881 3.02157 -0.959109 0.275066 0.0667052 -514.258 -0.275868 -0.961191 -0.0029489 176.593 0.0633053 -0.0212302 0.997768 2.94601 -0.959236 0.274265 0.0681488 -512.748 -0.275186 -0.961381 -0.00434176 177.015 0.0643262 -0.0229184 0.997666 2.89658 -0.959531 0.273832 0.0657026 -511.222 -0.274693 -0.961523 -0.00426289 177.455 0.0620072 -0.0221384 0.99783 2.83205 -0.959678 0.273434 0.0652094 -509.677 -0.274153 -0.961684 -0.00217994 177.892 0.0621147 -0.0199694 0.997869 2.7703 -0.959942 0.272809 0.063925 -508.126 -0.273428 -0.961892 -0.000978137 178.336 0.0612221 -0.0184178 0.997954 2.71324 -0.960189 0.271987 0.0637146 -506.555 -0.27266 -0.962108 -0.00195717 178.781 0.060768 -0.0192517 0.997966 2.63753 -0.96029 0.271338 0.0649472 -504.961 -0.272155 -0.962245 -0.00391587 179.217 0.0614326 -0.0214361 0.997881 2.57616 -0.960231 0.270987 0.067244 -503.37 -0.271835 -0.962337 -0.00362418 179.66 0.0637293 -0.0217593 0.99773 2.51847 -0.959962 0.271869 0.067528 -501.782 -0.272755 -0.962075 -0.00409562 180.101 0.0638535 -0.0223503 0.997709 2.45615 -0.959561 0.27369 0.0658532 -500.208 -0.274525 -0.961572 -0.0038135 180.561 0.0622789 -0.0217377 0.997822 2.4009 -0.959064 0.275353 0.0661663 -498.614 -0.276257 -0.961072 -0.00475182 181.012 0.0622821 -0.0228362 0.997797 2.32499 -0.958502 0.277144 0.0668204 -497.021 -0.278339 -0.96044 -0.00911277 181.458 0.0616514 -0.0273333 0.997723 2.26475 -0.957484 0.280201 0.0686447 -495.412 -0.281347 -0.959577 -0.0074495 181.918 0.0637825 -0.0264458 0.997613 2.20167 -0.956092 0.285343 0.0668456 -493.801 -0.286554 -0.958021 -0.00909417 182.401 0.0614445 -0.0278498 0.997722 2.12714 -0.954419 0.292306 0.0603412 -492.226 -0.293499 -0.955887 -0.0117444 182.894 0.0542464 -0.0289192 0.998109 2.09645 -0.952103 0.299671 0.0607991 -490.6 -0.300968 -0.953542 -0.0132323 183.398 0.0540092 -0.0308971 0.998062 2.02781 -0.949659 0.306658 0.0641003 -488.981 -0.308318 -0.95112 -0.0176073 183.905 0.0555677 -0.0364842 0.997788 1.97883 -0.947339 0.314219 0.0617627 -487.389 -0.315952 -0.948556 -0.0203893 184.429 0.0521786 -0.0388296 0.997883 1.93553 -0.944938 0.322057 0.0580673 -485.795 -0.323687 -0.945931 -0.0210077 184.976 0.048162 -0.0386466 0.998092 1.89798 -0.941877 0.330655 0.0594606 -484.188 -0.332182 -0.94305 -0.0176575 185.546 0.0502357 -0.0363829 0.998074 1.85657 -0.938485 0.339678 0.0621637 -482.581 -0.341155 -0.939894 -0.0145895 186.133 0.0534715 -0.0348995 0.997959 1.81126 -0.935289 0.348506 0.0614703 -480.988 -0.34978 -0.936767 -0.0109976 186.732 0.0537506 -0.031787 0.998048 1.76996 -0.932108 0.357529 0.0578652 -479.4 -0.358643 -0.933423 -0.00980873 187.345 0.0505058 -0.0298957 0.998276 1.71934 -0.92876 0.366094 0.0581354 -477.809 -0.367133 -0.930135 -0.00794518 187.974 0.0511651 -0.0287226 0.998277 1.6717 -0.925172 0.374957 0.0588523 -476.229 -0.376037 -0.926569 -0.00808966 188.623 0.0514975 -0.029615 0.998234 1.63021 -0.919346 0.389064 0.0585923 -473.854 -0.390145 -0.92072 -0.00783002 189.634 0.0509007 -0.030058 0.998251 1.5648 -0.91564 0.398114 0.0557509 -472.283 -0.399011 -0.91693 -0.00551812 190.318 0.0489228 -0.0272978 0.998429 1.51917 -0.912104 0.406194 0.0554409 -470.704 -0.407161 -0.91333 -0.00693627 191.027 0.0478183 -0.0289 0.998438 1.48737 -0.908646 0.413689 0.0567762 -469.116 -0.414549 -0.910019 -0.00376374 191.755 0.0501104 -0.0269565 0.99838 1.42746 -0.905813 0.420237 0.053892 -467.552 -0.420964 -0.907074 -0.00237457 192.484 0.0478862 -0.0248375 0.998544 1.39333 -0.902932 0.426562 0.0525273 -465.99 -0.427328 -0.904089 -0.00376891 193.222 0.0458817 -0.0258494 0.998612 1.36009 -0.899891 0.432566 0.0555269 -464.42 -0.433312 -0.901243 -0.00155616 193.981 0.0493701 -0.0254608 0.998456 1.3178 -0.896761 0.439212 0.0539729 -462.85 -0.439963 -0.898013 -0.00227418 194.75 0.0474695 -0.0257854 0.99854 1.27025 -0.894198 0.444762 0.0509559 -461.304 -0.44561 -0.895207 -0.006073 195.517 0.0429151 -0.0281369 0.998682 1.24583 -0.890962 0.451275 0.0503642 -459.74 -0.452105 -0.891946 -0.00586389 196.31 0.0422759 -0.0279944 0.998714 1.21012 -0.887074 0.458616 0.0526417 -458.168 -0.459464 -0.888184 -0.00461906 197.125 0.0446372 -0.0282844 0.998603 1.1717 -0.882978 0.46639 0.0532034 -456.623 -0.467268 -0.884103 -0.00471606 197.938 0.0448378 -0.0290244 0.998573 1.14428 -0.87912 0.47414 0.0483661 -455.076 -0.47483 -0.880072 -0.00321014 198.782 0.0410436 -0.0257877 0.998825 1.11889 -0.874429 0.482563 0.0500688 -453.524 -0.483274 -0.875466 -0.00244345 199.645 0.0426544 -0.0263336 0.998743 1.08 -0.869936 0.49033 0.0527939 -451.982 -0.491053 -0.871129 -0.000822691 200.521 0.0455869 -0.0266403 0.998605 1.05348 -0.865136 0.498608 0.054123 -450.442 -0.499618 -0.866224 -0.00612213 201.4 0.0438301 -0.0323373 0.998516 1.00183 -0.860741 0.50619 0.0538174 -448.901 -0.507332 -0.861701 -0.00924374 202.302 0.0416954 -0.0352597 0.998508 0.979193 -0.856023 0.514271 0.052436 -447.374 -0.515619 -0.856676 -0.0156047 203.216 0.0368956 -0.040395 0.998502 0.951712 -0.851103 0.522162 0.0545043 -445.851 -0.523622 -0.851798 -0.0161291 204.138 0.0380046 -0.0422671 0.998383 0.930199 -0.843534 0.534261 0.054916 -443.59 -0.535732 -0.844243 -0.0156857 205.566 0.0379822 -0.0426516 0.998368 0.901186 -0.838659 0.542211 0.0515589 -442.091 -0.543555 -0.839222 -0.0159363 206.541 0.0346286 -0.0413902 0.998543 0.876201 -0.833493 0.550187 0.0508283 -440.597 -0.551482 -0.834048 -0.0152214 207.522 0.0340186 -0.0407178 0.998591 0.85951 -0.828413 0.557959 0.049121 -439.105 -0.55909 -0.829017 -0.0121984 208.536 0.0339159 -0.0375683 0.998718 0.842993 -0.822868 0.566033 0.0499514 -437.64 -0.567011 -0.823682 -0.00689361 209.55 0.037242 -0.0339955 0.998728 0.827322 -0.81744 0.573856 0.0498179 -436.188 -0.574591 -0.818441 -0.000537892 210.581 0.0404643 -0.0290646 0.998758 0.802041 -0.812069 0.581557 0.0483346 -434.752 -0.582128 -0.813093 0.00273082 211.614 0.0408886 -0.0259193 0.998827 0.763382 -0.806933 0.588873 0.0456955 -433.343 -0.589274 -0.807914 0.0055629 212.655 0.0401939 -0.0224383 0.99894 0.74398 -0.801806 0.595921 0.0445538 -431.94 -0.596307 -0.802738 0.00553511 213.701 0.0390635 -0.0221297 0.998992 0.708552 -0.796895 0.602414 0.0453356 -430.561 -0.602786 -0.797876 0.00651312 214.749 0.0400958 -0.0221374 0.998951 0.68981 -0.791775 0.609249 0.0436812 -429.203 -0.609682 -0.792635 0.00413495 215.791 0.0371424 -0.0233577 0.999037 0.663551 -0.78672 0.615884 0.0419406 -427.869 -0.616342 -0.787475 0.00248602 216.834 0.0345582 -0.0238939 0.999117 0.660407 -0.781685 0.622266 0.0418718 -426.534 -0.622737 -0.782428 0.00223549 217.898 0.0341527 -0.0243277 0.99912 0.642417 -0.776762 0.628277 0.043686 -425.218 -0.628637 -0.77767 0.00665781 218.973 0.0381563 -0.0222911 0.999023 0.610567 -0.771503 0.634327 0.0491048 -423.948 -0.634741 -0.772676 0.00864778 220.014 0.0434276 -0.024497 0.998756 0.615942 -0.768128 0.638205 0.0517112 -422.735 -0.638326 -0.769594 0.0162958 221.029 0.0501967 -0.0204913 0.998529 0.588081 -0.767088 0.639448 0.0517824 -421.558 -0.639727 -0.76849 0.0131755 222.005 0.0482193 -0.0230198 0.998571 0.562717 -0.765258 0.641468 0.0538465 -420.433 -0.641987 -0.766659 0.00931176 222.938 0.0472551 -0.0274428 0.998506 0.531918 -0.763371 0.643044 0.061314 -419.349 -0.643762 -0.765162 0.00983653 223.849 0.0532404 -0.0319627 0.99807 0.519765 -0.762475 0.644043 0.0619687 -418.325 -0.645037 -0.764135 0.00501085 224.714 0.0505796 -0.0361514 0.998066 0.50788 -0.761151 0.646487 0.0520004 -416.852 -0.647279 -0.762251 0.00208275 225.963 0.0409838 -0.0320735 0.998645 0.499674 -0.75952 0.648642 0.0489232 -415.897 -0.649355 -0.760484 0.00171292 226.787 0.0383163 -0.0304675 0.998801 0.45543 -0.758345 0.64974 0.0524524 -414.97 -0.650477 -0.759517 0.00385916 227.579 0.0423459 -0.0311925 0.998616 0.450119 -0.758741 0.649445 0.0503326 -414.083 -0.649826 -0.760009 0.0106045 228.357 0.0451402 -0.0246614 0.998676 0.448529 -0.758838 0.649462 0.0486306 -413.225 -0.649939 -0.75995 0.00740536 229.094 0.0417663 -0.0259874 0.998789 0.424421 -0.758918 0.649452 0.0474921 -412.392 -0.649897 -0.759985 0.00748153 229.811 0.0409522 -0.0251871 0.998844 0.423903 -0.758968 0.649826 0.0411517 -411.574 -0.650161 -0.759769 0.00646414 230.514 0.0354664 -0.0218492 0.999132 0.413703 -0.755286 0.654037 0.0421673 -410.757 -0.654472 -0.756073 0.00440525 231.227 0.0347628 -0.0242701 0.999101 0.388923 -0.748668 0.661382 0.0455105 -409.964 -0.661813 -0.749636 0.00698318 231.953 0.0387349 -0.0248914 0.998939 0.386432 -0.739596 0.671484 0.0458998 -409.196 -0.671762 -0.74068 0.0113755 232.698 0.0416355 -0.0224205 0.998881 0.36757 -0.728025 0.683797 0.049006 -408.418 -0.684243 -0.72919 0.00962915 233.43 0.0423191 -0.0265217 0.998752 0.345682 -0.714154 0.698169 0.0504367 -407.673 -0.69869 -0.715363 0.00936231 234.175 0.042617 -0.0285535 0.998683 0.330487 -0.699954 0.712517 0.0488327 -406.95 -0.712957 -0.701125 0.0107803 234.942 0.0419189 -0.0272699 0.998749 0.316048 -0.687108 0.724851 0.0497253 -406.227 -0.725352 -0.688299 0.0104312 235.732 0.0417869 -0.028901 0.998708 0.289133 -0.674536 0.736331 0.0530808 -405.502 -0.737035 -0.6758 0.00858987 236.529 0.042197 -0.0333282 0.998553 0.274603 -0.663125 0.746736 0.051488 -404.799 -0.747371 -0.66434 0.00944267 237.35 0.0412567 -0.032219 0.998629 0.268196 -0.653986 0.75499 0.0478818 -404.091 -0.755606 -0.654985 0.00735152 238.19 0.0369122 -0.0313719 0.998826 0.249362 -0.645611 0.762152 0.0480588 -403.368 -0.762728 -0.646658 0.00887095 239.052 0.0378386 -0.0309286 0.998805 0.241283 -0.638764 0.768842 0.0293819 -402.672 -0.76888 -0.639272 0.0124713 239.936 0.0283714 -0.0146249 0.99949 0.271555 -0.631762 0.774954 0.0179685 -401.958 -0.775149 -0.631719 -0.00869483 240.837 0.00461292 -0.0194213 0.999801 0.255363 -0.625875 0.779522 0.0250143 -401.202 -0.77975 -0.626091 0.00101586 241.787 0.0164531 -0.0188691 0.999687 0.252137 -0.620082 0.782735 0.0531398 -400.409 -0.781033 -0.62229 0.0523728 242.812 0.0740624 -0.00902849 0.997213 0.167671 -0.615389 0.78431 0.0784433 -399.607 -0.782382 -0.61989 0.0601288 243.769 0.0957859 -0.02437 0.995104 0.0923176 -0.615522 0.785891 0.0592262 -398.85 -0.78576 -0.617755 0.0310015 244.734 0.0609511 -0.0274555 0.997763 0.121055 -0.614967 0.787466 0.0413726 -398.059 -0.787948 -0.615704 0.00686968 245.765 0.0308829 -0.0283748 0.99912 0.0368558 -0.613281 0.787743 0.0578578 -396.773 -0.788751 -0.61466 0.0080793 247.365 0.0419272 -0.0406805 0.998292 0.0195522 -0.612104 0.788673 0.0576483 -395.937 -0.789687 -0.61346 0.00778087 248.459 0.0415015 -0.0407614 0.998307 -0.00399816 -0.610935 0.789595 0.0574262 -395.081 -0.790619 -0.612264 0.00738485 249.579 0.0409911 -0.0408906 0.998322 -0.0289074 -0.610342 0.79015 0.0560851 -394.198 -0.791114 -0.611622 0.00754242 250.715 0.0402626 -0.0397663 0.998398 -0.046508 -0.609784 0.79131 0.04463 -393.325 -0.791982 -0.610531 0.0040416 251.873 0.0304461 -0.0328816 0.998995 -0.0446038 -0.608706 0.792554 0.036541 -392.422 -0.79298 -0.609232 0.00431901 253.05 0.025685 -0.0263473 0.999323 -0.058629 -0.607413 0.793561 0.0362112 -391.504 -0.793649 -0.608182 0.0153567 254.259 0.0342094 -0.0194112 0.999226 -0.0617951 -0.606489 0.794331 0.0347837 -390.583 -0.794303 -0.607256 0.0179926 255.487 0.0354147 -0.0167165 0.999233 -0.0945668 -0.605735 0.79494 0.0339946 -389.641 -0.794987 -0.60643 0.0154085 256.717 0.0328642 -0.0176919 0.999303 -0.115924 -0.604541 0.795752 0.0361762 -388.683 -0.79582 -0.605321 0.0160414 257.959 0.0346632 -0.0190921 0.999217 -0.128601 -0.603366 0.796713 0.0346071 -387.75 -0.796768 -0.604084 0.0155792 259.215 0.0333178 -0.0181739 0.99928 -0.146941 -0.601479 0.797953 0.0386465 -386.787 -0.7981 -0.602331 0.0152893 260.479 0.0354781 -0.0216476 0.999136 -0.169391 -0.59969 0.799021 0.0440001 -385.809 -0.799142 -0.600836 0.0191635 261.757 0.0417489 -0.0236701 0.998848 -0.202965 -0.597755 0.800349 0.0461624 -384.846 -0.800617 -0.598935 0.0169857 263.056 0.0412428 -0.0268051 0.99879 -0.232323 -0.595648 0.801953 0.0455553 -383.87 -0.802273 -0.596759 0.0153625 264.361 0.0395056 -0.0273972 0.998844 -0.250016 -0.591433 0.805062 0.0456367 -382.876 -0.805325 -0.59259 0.0169838 265.702 0.0407168 -0.0267076 0.998814 -0.284183 -0.585181 0.8096 0.0459524 -381.9 -0.809679 -0.586471 0.0217117 267.072 0.0445275 -0.0245014 0.998708 -0.305523 -0.578539 0.814555 0.0423531 -380.932 -0.814556 -0.579674 0.0218207 268.467 0.0423251 -0.0218749 0.998864 -0.349063 -0.567965 0.821724 0.0467347 -379.45 -0.821672 -0.569385 0.0255998 270.588 0.047646 -0.0238608 0.998579 -0.419259 -0.560677 0.826533 0.0498497 -378.474 -0.826713 -0.562167 0.0226696 272.023 0.046761 -0.0285011 0.998499 -0.458059 -0.553308 0.831411 0.0510542 -377.489 -0.831956 -0.554624 0.0155217 273.476 0.0412207 -0.0338866 0.998575 -0.495777 -0.544571 0.836992 0.0537376 -376.515 -0.837815 -0.545835 0.0113529 274.963 0.0388342 -0.0388397 0.998491 -0.527125 -0.53558 0.842338 0.0601675 -375.547 -0.843451 -0.537089 0.0112328 276.479 0.0417772 -0.0447323 0.998125 -0.554445 -0.526705 0.848013 0.0587872 -374.592 -0.84914 -0.528079 0.00972043 278.015 0.0392873 -0.0447988 0.998223 -0.570697 -0.516607 0.854268 0.0578254 -373.642 -0.855468 -0.517808 0.00701942 279.579 0.0359389 -0.0458415 0.998302 -0.592555 -0.506291 0.860296 0.0596748 -372.707 -0.861293 -0.507896 0.0146768 281.179 0.042935 -0.0439668 0.99811 -0.62216 -0.49617 0.866337 0.0572348 -371.776 -0.867038 -0.49786 0.0195008 282.807 0.0453892 -0.039949 0.99817 -0.64604 -0.486433 0.872385 0.0482377 -370.9 -0.872691 -0.487796 0.0215635 284.444 0.0423418 -0.0316074 0.998603 -0.669482 -0.47687 0.878213 0.0365684 -370.022 -0.878169 -0.477801 0.0229292 286.097 0.0376092 -0.021179 0.999068 -0.697328 -0.468525 0.882899 0.031205 -369.152 -0.88276 -0.469263 0.022966 287.747 0.03492 -0.0167864 0.999249 -0.732924 -0.460194 0.887212 0.0328235 -368.289 -0.887095 -0.460997 0.0233373 289.395 0.0358367 -0.0183779 0.999189 -0.760591 -0.451816 0.891417 0.0351759 -367.44 -0.891326 -0.452721 0.0241105 291.043 0.0374174 -0.0204597 0.99909 -0.776225 -0.441572 0.896537 0.0351388 -366.206 -0.896494 -0.442455 0.0230598 293.538 0.0362213 -0.0213192 0.999116 -0.813287 -0.434754 0.899638 0.0404956 -365.392 -0.899689 -0.435865 0.0241122 295.209 0.0393429 -0.0259505 0.998889 -0.84067 -0.428633 0.90221 0.0478592 -364.578 -0.902503 -0.43003 0.0237147 296.882 0.0419766 -0.0330282 0.998573 -0.870144 -0.422948 0.904628 0.0525757 -363.772 -0.905256 -0.424399 0.0199142 298.567 0.040328 -0.0391718 0.998418 -0.882033 -0.41445 0.908682 0.050286 -362.599 -0.909338 -0.415702 0.0172308 301.132 0.0365613 -0.0385857 0.998586 -0.897095 -0.40849 0.911843 0.0409562 -361.845 -0.912224 -0.409379 0.0160063 302.849 0.0313619 -0.0308228 0.999033 -0.912032 -0.402417 0.914746 0.036054 -361.108 -0.914827 -0.403289 0.0212021 304.573 0.0339347 -0.0244511 0.999125 -0.920053 -0.396029 0.917642 0.0330652 -360.356 -0.917812 -0.396685 0.0161789 306.293 0.027963 -0.0239403 0.999322 -0.92887 -0.388535 0.920781 0.0346887 -359.635 -0.921097 -0.389136 0.0123992 308.017 0.0249156 -0.0271341 0.999321 -0.931067 -0.37579 0.925341 0.0502552 -358.533 -0.925914 -0.377157 0.0208707 310.634 0.0382666 -0.038689 0.998518 -0.955472 -0.368345 0.92853 0.0464249 -357.863 -0.929018 -0.369516 0.0195581 312.367 0.035315 -0.0359254 0.99873 -0.937355 -0.359153 0.932293 0.0428771 -357.21 -0.932776 -0.360088 0.0163012 314.094 0.030637 -0.0341401 0.998947 -0.958341 -0.349548 0.935768 0.046418 -356.55 -0.93597 -0.350994 0.0276325 315.852 0.04215 -0.0337869 0.99854 -0.987545 -0.339905 0.939508 0.0422987 -355.928 -0.939416 -0.341303 0.0317861 317.586 0.0443 -0.0289318 0.998599 -0.992567 -0.329435 0.943425 0.0376987 -355.33 -0.943913 -0.330026 0.0105235 319.295 0.0223696 -0.0321175 0.999234 -0.988485 -0.317248 0.947577 0.038107 -354.757 -0.948132 -0.317768 0.00829994 321.037 0.019974 -0.0334974 0.999239 -1.01215 -0.303609 0.95181 0.0433475 -354.19 -0.95235 -0.304545 0.0167799 322.783 0.0291725 -0.0361875 0.998919 -1.00493 -0.2894 0.956126 0.0454992 -353.657 -0.95691 -0.290169 0.011179 324.509 0.023891 -0.0403034 0.998902 -1.00803 -0.273643 0.960612 0.0484202 -353.175 -0.961627 -0.274275 0.00681386 326.24 0.0198259 -0.0446976 0.998804 -0.992537 -0.25495 0.965758 0.0480858 -352.721 -0.966767 -0.255565 0.00698918 327.984 0.0190389 -0.0447059 0.998819 -0.972247 -0.23583 0.970653 0.0470904 -352.307 -0.971622 -0.236423 0.00736921 329.733 0.0182862 -0.0440162 0.998863 -0.954089 -0.216619 0.975175 0.0459285 -351.922 -0.976028 -0.217346 0.0114215 331.485 0.0211203 -0.0423534 0.998879 -0.948657 -0.197775 0.979354 0.0418513 -351.583 -0.980126 -0.198242 0.00727678 333.23 0.0154232 -0.0395804 0.999097 -0.906153 -0.178882 0.98309 0.0391733 -351.265 -0.983819 -0.179138 0.00307375 334.988 0.0100392 -0.0379896 0.999228 -0.904351 -0.151067 0.987971 0.0330459 -350.859 -0.988322 -0.151626 0.0151206 337.654 0.0199493 -0.0303758 0.999339 -0.873637 -0.132466 0.990674 0.0319007 -350.628 -0.990966 -0.133048 0.0168749 339.435 0.0209618 -0.0293772 0.999349 -0.861268 -0.1155 0.992918 0.0278248 -350.434 -0.993078 -0.11603 0.0182454 341.212 0.0213447 -0.0255249 0.999446 -0.836825 -0.0930162 0.9954 0.0229528 -350.177 -0.995568 -0.093303 0.0117576 343.87 0.0138451 -0.0217575 0.999667 -0.835718 -0.0805756 0.996601 0.0171604 -350.047 -0.996597 -0.0808512 0.0160171 345.652 0.0173501 -0.0158115 0.999724 -0.798283 -0.0700092 0.997454 0.0135607 -349.927 -0.99736 -0.0702526 0.0183848 347.442 0.0192907 -0.0122378 0.999739 -0.808376 -0.0615046 0.99797 0.0165156 -349.81 -0.997787 -0.0618952 0.0242793 349.208 0.0252523 -0.0149858 0.999569 -0.812667 -0.0521975 0.998473 0.0180996 -349.669 -0.998534 -0.0524441 0.0134277 351.835 0.0143564 -0.0173722 0.999746 -0.783267 -0.0463562 0.998792 0.0162801 -349.576 -0.998888 -0.0464887 0.00785702 353.609 0.00860438 -0.0158977 0.999837 -0.754757 -0.0424931 0.999029 0.011644 -349.505 -0.99897 -0.0426707 0.0154589 355.403 0.0159408 -0.0109751 0.999813 -0.719976 -0.0428406 0.999057 0.0070566 -349.452 -0.998986 -0.0429334 0.0135592 357.155 0.0138494 -0.00646856 0.999883 -0.679541 -0.0442934 0.999001 0.00587795 -349.368 -0.998984 -0.0442424 -0.00853503 358.93 -0.00826645 -0.00625002 0.999946 -0.662682 -0.0459543 0.998937 0.00349906 -349.268 -0.998902 -0.0459842 0.00901044 360.735 0.00916176 -0.00308115 0.999953 -0.646041 -0.0488111 0.998784 0.00695201 -349.18 -0.998525 -0.0489616 0.0234492 362.508 0.0237611 -0.00579718 0.999701 -0.627383 -0.0524131 0.998587 0.00876308 -349.074 -0.99855 -0.0525149 0.0118112 364.271 0.0122547 -0.00813131 0.999892 -0.580001 -0.055241 0.998432 0.00905325 -348.974 -0.998439 -0.0553122 0.00781478 366.06 0.00830328 -0.00860742 0.999928 -0.587383 -0.0581307 0.998263 0.00957986 -348.855 -0.998141 -0.0582942 0.0177761 367.86 0.0183036 -0.00852872 0.999796 -0.559615 -0.0612418 0.998054 0.0117189 -348.738 -0.997994 -0.0614184 0.0153529 369.638 0.0160427 -0.0107552 0.999813 -0.539383 -0.0638461 0.997829 0.0161592 -348.601 -0.997888 -0.0640274 0.0109673 371.416 0.0119781 -0.0154248 0.999809 -0.537217 -0.0670001 0.997513 0.0218882 -348.471 -0.997687 -0.0672319 0.0100313 373.194 0.011478 -0.0211655 0.99971 -0.506347 -0.0710637 0.997162 0.0248712 -348.34 -0.997435 -0.071254 0.00684666 374.972 0.0085994 -0.0243208 0.999667 -0.476802 -0.074526 0.996757 0.0303462 -348.189 -0.997179 -0.0747626 0.00673768 376.75 0.00898459 -0.0297584 0.999517 -0.441768 -0.0784334 0.996365 0.0332484 -348.038 -0.996844 -0.0787945 0.0096931 378.521 0.0122776 -0.0323832 0.9994 -0.403939 -0.0844133 0.99598 0.0299827 -347.886 -0.996385 -0.0846599 0.00705041 380.27 0.0095604 -0.0292791 0.999526 -0.358141 -0.0897861 0.995587 0.0273103 -347.739 -0.995958 -0.089817 -9.3554e-05 381.992 0.00235979 -0.0272083 0.999627 -0.312998 -0.0948267 0.995173 0.0252598 -347.567 -0.995469 -0.0949722 0.00462249 383.735 0.00699916 -0.024707 0.99967 -0.296894 -0.100077 0.994707 0.0232932 -347.397 -0.994829 -0.100441 0.0150506 385.462 0.0173106 -0.0216666 0.999615 -0.257571 -0.105773 0.994121 0.0231518 -347.211 -0.994259 -0.106109 0.0137976 387.179 0.0161731 -0.0215594 0.999637 -0.239488 -0.111334 0.993533 0.0222912 -347.02 -0.993689 -0.111604 0.0112563 388.871 0.0136713 -0.0208974 0.999688 -0.225331 -0.116555 0.993036 0.017166 -346.817 -0.993093 -0.116761 0.0115671 390.557 0.0134909 -0.0156992 0.999786 -0.195224 -0.122169 0.992436 0.0120853 -346.616 -0.992501 -0.122209 0.00255832 392.217 0.00401589 -0.0116821 0.999924 -0.144419 -0.129523 0.991557 0.0062472 -346.403 -0.991576 -0.129528 0.000355834 393.884 0.00116202 -0.00614849 0.99998 -0.116123 -0.138456 0.990337 0.007875 -346.168 -0.990342 -0.138507 0.00626522 395.535 0.00729542 -0.00693149 0.999949 -0.0753762 -0.150744 0.988543 0.00765698 -345.91 -0.988458 -0.15084 0.0140528 397.164 0.0150468 -0.00545023 0.999872 -0.0410686 -0.164908 0.986283 0.00715092 -345.63 -0.986079 -0.165022 0.0204149 398.784 0.0213149 -0.00368479 0.999766 -0.0505591 -0.180668 0.98346 0.0128496 -345.304 -0.983381 -0.180861 0.015845 400.366 0.017907 -0.00977339 0.999792 -0.0083421 -0.199897 0.979764 0.0101481 -344.977 -0.979799 -0.199946 0.00403973 401.926 0.00598706 -0.00913557 0.99994 0.0188796 -0.219519 0.97546 0.0170062 -344.607 -0.975493 -0.219727 0.0115141 403.471 0.0149683 -0.0140619 0.999789 0.0360259 -0.240719 0.970435 0.0176185 -344.206 -0.970302 -0.241052 0.0201891 404.994 0.0238392 -0.0122353 0.999641 0.0636261 -0.262779 0.964667 0.0191172 -343.781 -0.96466 -0.263073 0.0149626 406.456 0.0194631 -0.0145097 0.999705 0.0666647 -0.28627 0.957753 0.0275279 -343.331 -0.957991 -0.286626 0.0098973 407.865 0.0173694 -0.0235382 0.999572 0.0950709 -0.312831 0.949304 0.0309573 -342.855 -0.949529 -0.313364 0.0140563 409.241 0.0230446 -0.0249976 0.999422 0.112315 -0.341198 0.939341 0.0349811 -342.346 -0.939649 -0.341842 0.0142875 410.583 0.0253788 -0.0279951 0.999286 0.120448 -0.372355 0.927428 0.0350607 -341.787 -0.927719 -0.373009 0.0142166 411.891 0.0262629 -0.0272328 0.999284 0.124275 -0.406676 0.913007 0.0321269 -341.193 -0.913277 -0.407187 0.0111328 413.161 0.023246 -0.0248134 0.999422 0.134025 -0.44118 0.896981 0.0280309 -340.555 -0.897254 -0.441482 0.00534777 414.395 0.017172 -0.0227915 0.999593 0.142282 -0.473346 0.880058 0.0379659 -339.858 -0.880453 -0.474016 0.0106003 415.606 0.0273253 -0.0284095 0.999223 0.109593 -0.502983 0.863463 0.0379472 -339.156 -0.863945 -0.503546 0.00641097 416.773 0.0246438 -0.0295597 0.999259 0.172094 -0.533144 0.84574 0.02192 -338.44 -0.845979 -0.533206 -0.00341887 417.897 0.00879637 -0.0203666 0.999754 0.159843 -0.558772 0.828424 0.0385834 -337.648 -0.828922 -0.559342 0.00502204 418.961 0.0257417 -0.0291764 0.999243 0.149898 -0.586677 0.808517 0.0459398 -336.863 -0.808763 -0.587866 0.0177792 420.006 0.0413812 -0.0267237 0.998786 0.160053 -0.612689 0.788967 0.0462995 -336.064 -0.789366 -0.613779 0.0132887 420.975 0.038902 -0.0284054 0.998839 0.134641 -0.63791 0.768607 0.0481037 -335.262 -0.769298 -0.638861 0.00603272 421.875 0.0353683 -0.0331577 0.998824 0.145366 -0.662972 0.747375 0.0435629 -334.484 -0.747964 -0.663728 0.00400838 422.719 0.0319097 -0.029926 0.999043 0.155122 -0.687857 0.724828 0.0384213 -333.684 -0.725315 -0.688415 0.00181009 423.507 0.0277618 -0.0266224 0.99926 0.14541 -0.711432 0.701586 0.0405106 -332.879 -0.702178 -0.712001 -0.000541689 424.244 0.0284636 -0.028831 0.999179 0.137707 -0.734564 0.677338 0.0403585 -332.097 -0.677888 -0.735166 8.89217e-05 424.934 0.0297304 -0.0272932 0.999185 0.150548 -0.757304 0.652077 0.0358662 -331.3 -0.652381 -0.75788 0.00406079 425.587 0.0298303 -0.0203232 0.999348 0.159251 -0.779048 0.62661 0.0210951 -330.496 -0.626642 -0.779285 0.00585394 426.221 0.0201073 -0.00865861 0.99976 0.158773 -0.809938 0.586172 0.0200698 -329.239 -0.586498 -0.809699 -0.0201731 427.016 0.00442559 -0.0281099 0.999595 0.142595 -0.830036 0.556357 0.0388153 -328.382 -0.556701 -0.830709 0.00228733 427.56 0.0335168 -0.01971 0.999244 0.126505 -0.845829 0.529576 0.0642101 -327.487 -0.528697 -0.84823 0.0313817 428.098 0.0710839 -0.00740418 0.997443 0.0907812 -0.875608 0.480826 0.0460174 -326.209 -0.481399 -0.8765 -0.00160118 428.736 0.0395643 -0.0235547 0.998939 0.126249 -0.90444 0.425912 0.0242256 -324.862 -0.426076 -0.904686 -0.00178445 429.32 0.0211565 -0.0119359 0.999705 0.0789445 -0.920412 0.388946 0.0395235 -323.913 -0.389343 -0.921089 -0.0025847 429.67 0.0353993 -0.0177672 0.999215 0.0698541 -0.936147 0.34957 0.037812 -322.96 -0.34997 -0.936751 -0.00432135 429.977 0.0339098 -0.0172785 0.999276 0.071688 -0.950207 0.309802 0.0336079 -321.993 -0.310048 -0.950718 -0.00222164 430.26 0.0312633 -0.0125311 0.999433 0.0567386 -0.962047 0.270439 0.0364497 -320.994 -0.270726 -0.962652 -0.00307726 430.493 0.0342561 -0.0128283 0.999331 0.0420548 -0.976895 0.210498 0.036967 -319.462 -0.210724 -0.977543 -0.00227099 430.773 0.0356588 -0.0100083 0.999314 0.0237979 -0.984742 0.17036 0.0355121 -318.426 -0.170614 -0.985329 -0.00420825 430.911 0.0342742 -0.0102029 0.99936 0.0046861 -0.990648 0.131476 0.0364833 -317.353 -0.131848 -0.991238 -0.00796762 430.999 0.0351161 -0.0127034 0.999302 -0.0146898 -0.994488 0.0975815 0.0383485 -316.256 -0.0980373 -0.995131 -0.0101875 431.056 0.0371676 -0.0138909 0.999212 -0.0302254 -0.996728 0.0710279 0.0385823 -315.147 -0.0715872 -0.997346 -0.0133118 431.103 0.0375344 -0.0160302 0.999167 -0.0454692 -0.997945 0.0507262 0.0391507 -314.019 -0.0513283 -0.998576 -0.0145285 431.139 0.038358 -0.0165082 0.999128 -0.0608638 -0.998555 0.0348222 0.0409338 -312.88 -0.0353317 -0.999306 -0.0117901 431.166 0.0404949 -0.0132193 0.999092 -0.0807247 -0.998907 0.0212359 0.0416467 -311.716 -0.0217137 -0.999703 -0.0110535 431.161 0.0413996 -0.0119457 0.999071 -0.102687 -0.999071 0.0111145 0.0416427 -310.541 -0.0115606 -0.999878 -0.0104868 431.157 0.041521 -0.0109585 0.999078 -0.11728 -0.999146 0.00244649 0.0412478 -309.35 -0.00275946 -0.999968 -0.00753221 431.156 0.0412281 -0.0076396 0.999121 -0.14047 -0.999105 -0.0049882 0.0420038 -308.132 0.00470842 -0.999966 -0.00675733 431.144 0.0420361 -0.00655351 0.999095 -0.171242 -0.999046 -0.0108513 0.0423078 -306.899 0.0104308 -0.999894 -0.010147 431.115 0.0424135 -0.00969597 0.999053 -0.200525 -0.999087 -0.0128055 0.0407598 -305.647 0.0122494 -0.999829 -0.0138637 431.077 0.0409303 -0.0133518 0.999073 -0.222958 -0.999005 -0.0124202 0.0428299 -304.379 0.0117537 -0.999806 -0.0157775 431.044 0.0430175 -0.0152584 0.998958 -0.24983 -0.999021 -0.0113674 0.0427614 -303.087 0.0105607 -0.999763 -0.0190429 431.025 0.0429677 -0.0185727 0.998904 -0.275778 -0.99912 -0.010292 0.0406641 -301.787 0.00939436 -0.999709 -0.0222031 431.002 0.0408808 -0.0218016 0.998926 -0.297794 -0.999202 -0.00902526 0.0388983 -300.474 0.00816095 -0.999718 -0.0223215 430.984 0.0390888 -0.0219863 0.998994 -0.324869 -0.999286 -0.00775134 0.0369886 -299.145 0.00688998 -0.999703 -0.0233581 430.966 0.0371587 -0.0230866 0.999043 -0.331148 -0.999372 -0.00654804 0.0348178 -297.805 0.00574233 -0.999715 -0.0231905 430.943 0.0349597 -0.0229761 0.999125 -0.353847 -0.999367 -0.00517746 0.0351906 -296.444 0.00427529 -0.999661 -0.0256636 430.932 0.0353115 -0.0254969 0.999051 -0.379715 -0.999365 -0.0042669 0.0353759 -295.069 0.00326804 -0.999596 -0.0282456 430.916 0.0354821 -0.028112 0.998975 -0.394735 -0.999331 -0.00372996 0.0363711 -293.682 0.00302983 -0.999809 -0.0192856 430.92 0.0364361 -0.0191625 0.999152 -0.406463 -0.999224 -0.0034038 0.0392502 -292.274 0.00210888 -0.999454 -0.0329857 430.891 0.039341 -0.0328773 0.998685 -0.44904 -0.999075 -0.00350863 0.0428511 -290.853 0.00258418 -0.999763 -0.0216098 430.911 0.0429168 -0.0214791 0.998848 -0.416859 -0.999226 -0.00225592 0.0392823 -289.427 0.00154617 -0.999835 -0.018089 430.911 0.0393167 -0.0180143 0.999064 -0.487909 -0.999394 -0.00211479 0.0347494 -287.968 0.00131897 -0.999737 -0.0229084 430.909 0.0347887 -0.0228487 0.999133 -0.493599 -0.99947 -0.00042876 0.0325601 -286.498 -0.000181173 -0.999825 -0.0187273 430.916 0.0325624 -0.0187232 0.999294 -0.507028 -0.99935 0.00112731 0.0360399 -285.004 -0.0018199 -0.999814 -0.0191902 430.913 0.0360115 -0.0192433 0.999166 -0.541689 -0.999277 0.00171911 0.0379709 -283.512 -0.002403 -0.999836 -0.0179725 430.924 0.0379338 -0.0180507 0.999117 -0.539905 -0.999382 0.00266823 0.0350529 -282.004 -0.00316074 -0.999897 -0.0140027 430.928 0.0350119 -0.0141049 0.999287 -0.561295 -0.999398 0.00307071 0.034571 -280.484 -0.00351617 -0.999911 -0.0128318 430.927 0.0345285 -0.0129457 0.99932 -0.584433 -0.999386 0.00326409 0.0348931 -278.949 -0.00379458 -0.999878 -0.0151479 430.914 0.0348394 -0.015271 0.999276 -0.604216 -0.999395 0.00278964 0.0346604 -277.398 -0.00337562 -0.999852 -0.0168594 430.909 0.0346082 -0.0169662 0.999257 -0.621408 -0.999356 0.00131763 0.0358671 -275.841 -0.00194189 -0.999847 -0.0173757 430.909 0.0358387 -0.0174342 0.999206 -0.647272 -0.999284 -0.00205375 0.0377716 -274.26 0.00135508 -0.999828 -0.0185135 430.894 0.0378031 -0.0184491 0.999115 -0.668364 -0.999317 -0.00520668 0.0365776 -272.666 0.00447704 -0.99979 -0.0200014 430.879 0.0366741 -0.0198239 0.999131 -0.679403 -0.999271 -0.00800821 0.037316 -271.046 0.00729443 -0.999789 -0.0192251 430.859 0.0374621 -0.0189388 0.999119 -0.698381 -0.999242 -0.0103811 0.0375154 -269.413 0.00969885 -0.999785 -0.0183235 430.837 0.0376976 -0.0179458 0.999128 -0.726739 -0.999229 -0.0123339 0.0372649 -267.75 0.0117167 -0.999791 -0.0167358 430.827 0.0374635 -0.0162863 0.999165 -0.755608 -0.999199 -0.0134724 0.0376791 -266.076 0.012844 -0.999775 -0.0168684 430.797 0.0378979 -0.016371 0.999148 -0.784262 -0.999134 -0.0141357 0.0391431 -264.4 0.0135145 -0.999779 -0.0160893 430.765 0.0393619 -0.0155464 0.999104 -0.807342 -0.999173 -0.0144565 0.0379923 -262.698 0.0138817 -0.999786 -0.015351 430.744 0.0382061 -0.0148109 0.99916 -0.830716 -0.999145 -0.0139514 0.0389144 -261.001 0.0133544 -0.99979 -0.0155589 430.714 0.0391232 -0.0150259 0.999121 -0.859128 -0.999043 -0.0137537 0.0415144 -259.288 0.0131225 -0.999795 -0.0154395 430.692 0.0417182 -0.01488 0.999019 -0.889086 -0.999063 -0.0131961 0.0412264 -257.577 0.0126119 -0.999817 -0.0143988 430.66 0.0414088 -0.0138653 0.999046 -0.914857 -0.999084 -0.0126144 0.040895 -255.869 0.0120381 -0.999825 -0.0143078 430.634 0.0410683 -0.0138024 0.999061 -0.929993 -0.99909 -0.0127524 0.0406961 -254.143 0.0122583 -0.999848 -0.0123693 430.629 0.0408476 -0.0118592 0.999095 -0.956383 -0.999101 -0.013217 0.0402736 -252.455 0.012798 -0.999861 -0.0106458 430.614 0.0404087 -0.0101208 0.999132 -0.972722 -0.999172 -0.0134919 0.0383866 -250.766 0.0131155 -0.999864 -0.0100383 430.602 0.0385168 -0.00952653 0.999213 -0.98212 -0.9991 -0.0134791 0.0402202 -249.109 0.013007 -0.999844 -0.0119771 430.565 0.0403754 -0.0114431 0.999119 -1.00942 -0.998983 -0.0135921 0.0429998 -247.458 0.0128518 -0.999765 -0.0174441 430.53 0.0432268 -0.0168737 0.998923 -1.02073 -0.999102 -0.0104609 0.0410626 -245.819 0.00975033 -0.9998 -0.0174674 430.501 0.0412371 -0.0170514 0.999004 -1.04537 -0.999245 -0.00670456 0.0382664 -244.177 0.00593422 -0.999778 -0.0202093 430.487 0.0383934 -0.0199669 0.999063 -1.06093 -0.99934 -0.00462712 0.036023 -242.56 0.00388515 -0.999779 -0.0206401 430.485 0.0361106 -0.0204866 0.999138 -1.07239 -0.999288 -0.00219843 0.0376562 -240.948 0.00136499 -0.999754 -0.0221444 430.473 0.0376956 -0.0220772 0.999045 -1.08809 -0.999292 8.83893e-05 0.0376143 -239.326 -0.000982523 -0.999717 -0.0237533 430.468 0.0376015 -0.0237735 0.99901 -1.10237 -0.999298 0.00148195 0.0374396 -237.72 -0.00231694 -0.999749 -0.0222689 430.477 0.0373972 -0.02234 0.999051 -1.11612 -0.999262 0.00288861 0.0383016 -236.096 -0.00361088 -0.999817 -0.0188018 430.483 0.0382403 -0.0189262 0.999089 -1.12991 -0.999271 0.00331631 0.0380302 -234.477 -0.00408659 -0.999788 -0.0201946 430.477 0.0379552 -0.0203352 0.999073 -1.16173 -0.998792 0.00236644 0.0490878 -232.847 -0.00325966 -0.99983 -0.0181243 430.477 0.0490365 -0.0182625 0.99863 -1.20535 -0.999022 0.00114326 0.0441947 -231.236 -0.00197844 -0.99982 -0.0188587 430.479 0.0441652 -0.0189277 0.998845 -1.21487 -0.999315 0.000877912 0.0370059 -229.607 -0.00159821 -0.99981 -0.0194392 430.474 0.0369818 -0.019485 0.999126 -1.25282 -0.999221 -0.000246634 0.0394525 -227.15 -0.000514568 -0.999814 -0.0192828 430.474 0.0394499 -0.0192881 0.999035 -1.28351 -0.999263 -0.00110355 0.0383654 -225.512 0.000345826 -0.999805 -0.0197513 430.461 0.0383797 -0.0197235 0.999069 -1.30714 -0.999315 -0.00212632 0.0369534 -223.875 0.0013777 -0.999794 -0.0202723 430.448 0.0369889 -0.0202075 0.999111 -1.33025 -0.999246 -0.00293043 0.0387051 -222.25 0.0021009 -0.999768 -0.0214555 430.437 0.038759 -0.0213581 0.99902 -1.35334 -0.999206 -0.00378307 0.0396564 -220.641 0.00286703 -0.999728 -0.0231311 430.432 0.0397331 -0.022999 0.998946 -1.37846 -0.999178 -0.00473944 0.0402689 -219.043 0.00381072 -0.999726 -0.0231087 430.415 0.0403673 -0.0229363 0.998922 -1.3914 -0.999173 -0.00561397 0.0402735 -217.456 0.00478661 -0.999776 -0.0206106 430.408 0.0403802 -0.0204008 0.998976 -1.40614 -0.999175 -0.00585849 0.0401915 -215.889 0.00516726 -0.999837 -0.0172806 430.391 0.0402862 -0.0170586 0.999043 -1.42372 -0.999136 -0.00662346 0.0410217 -214.331 0.0061069 -0.999901 -0.0127048 430.386 0.0411018 -0.0124433 0.999077 -1.45083 -0.999076 -0.00755708 0.0423028 -212.813 0.00713604 -0.999924 -0.0100952 430.384 0.0423759 -0.00978399 0.999054 -1.47585 -0.999038 -0.00916108 0.0428792 -211.336 0.00883216 -0.99993 -0.00785408 430.37 0.0429482 -0.00746782 0.999049 -1.49512 -0.999023 -0.011927 0.0425528 -209.899 0.0116524 -0.99991 -0.00669576 430.351 0.0426288 -0.00619338 0.999072 -1.50974 -0.999181 -0.0131841 0.0382657 -208.472 0.0129586 -0.999897 -0.00613312 430.321 0.0383426 -0.00563222 0.999249 -1.52714 -0.999188 -0.0150053 0.0373817 -207.11 0.0146582 -0.999847 -0.00954457 430.295 0.0375192 -0.00898888 0.999255 -1.5436 -0.999112 -0.0170781 0.0385162 -205.756 0.0166273 -0.99979 -0.011994 430.267 0.0387129 -0.0113429 0.999186 -1.55914 -0.999031 -0.0182715 0.040036 -204.409 0.0176189 -0.999707 -0.0165933 430.226 0.0403275 -0.0158718 0.99906 -1.57553 -0.99946 -0.0179477 0.0275164 -203.097 0.0174433 -0.999677 -0.0184634 430.192 0.0278389 -0.0179735 0.999451 -1.54973 -0.998988 -0.0155635 0.0421994 -201.746 0.0148769 -0.999753 -0.0165359 430.178 0.0424463 -0.0158913 0.998972 -1.61794 -0.997901 -0.0130741 0.0634222 -200.432 0.0122809 -0.999842 -0.0128814 430.16 0.0635805 -0.0120755 0.997904 -1.62057 -0.999437 -0.00966285 0.0321296 -199.155 0.00933356 -0.999903 -0.010383 430.155 0.0322268 -0.0100773 0.99943 -1.62041 -0.999749 -0.00512523 0.0218275 -197.845 0.00501375 -0.999974 -0.00515911 430.157 0.0218534 -0.00504838 0.999748 -1.65345 -0.999356 -0.00145063 0.0358534 -196.53 0.00148332 -0.999999 0.000885194 430.158 0.035852 0.000937806 0.999357 -1.67273 -0.999256 0.00228143 0.0384881 -195.216 -0.0021303 -0.99999 0.00396716 430.158 0.0384968 0.00388222 0.999251 -1.67359 -0.999297 0.00560428 0.0370664 -193.92 -0.00541729 -0.999972 0.00514315 430.172 0.0370941 0.00493873 0.9993 -1.69223 -0.999229 0.00874424 0.0382789 -192.633 -0.00848234 -0.99994 0.00699903 430.193 0.0383378 0.00666894 0.999243 -1.71109 -0.999259 0.0119193 0.0365859 -191.369 -0.0117824 -0.999923 0.00395621 430.198 0.0366302 0.00352221 0.999323 -1.72407 -0.999271 0.0142677 0.0354121 -190.114 -0.014333 -0.999896 -0.00159234 430.212 0.0353857 -0.00209875 0.999372 -1.74332 -0.999192 0.0158718 0.0369142 -188.872 -0.0160219 -0.999865 -0.00377457 430.227 0.0368493 -0.00436296 0.999311 -1.75467 -0.999131 0.0157669 0.0385795 -187.643 -0.0160593 -0.999845 -0.00728171 430.237 0.0384587 -0.00789494 0.999229 -1.76888 -0.999076 0.0154509 0.0401059 -186.428 -0.015979 -0.999789 -0.0128812 430.247 0.0398984 -0.0135101 0.999112 -1.78375 -0.999028 0.0152475 0.0413664 -185.216 -0.0157825 -0.999796 -0.0126366 430.257 0.0411653 -0.0132772 0.999064 -1.78733 -0.999032 0.0153613 0.0412247 -184.039 -0.0159189 -0.999786 -0.0132328 430.264 0.0410126 -0.0138763 0.999062 -1.8081 -0.999008 0.0151173 0.0418919 -182.902 -0.0156489 -0.999801 -0.0123893 430.289 0.0416963 -0.0130326 0.999045 -1.81612 -0.999008 0.0149472 0.0419465 -181.802 -0.0154477 -0.999813 -0.0116329 430.31 0.0417648 -0.0122694 0.999052 -1.8306 -0.999051 0.0168012 0.0401746 -180.744 -0.0172994 -0.999777 -0.0120836 430.326 0.0399626 -0.0127672 0.99912 -1.84868 -0.9992 0.022371 0.0331611 -179.741 -0.0230409 -0.999535 -0.0199575 430.343 0.0326992 -0.0207056 0.999251 -1.84337 -0.998451 0.0383847 0.0402685 -178.312 -0.038932 -0.999159 -0.0128948 430.435 0.0397397 -0.0144425 0.999106 -1.84461 -0.996541 0.0722944 0.0409844 -176.989 -0.0730158 -0.997196 -0.0163852 430.531 0.0396849 -0.019321 0.999025 -1.87233 -0.991829 0.122358 0.0361125 -175.706 -0.122737 -0.992403 -0.00846607 430.764 0.0348023 -0.0128292 0.999312 -1.86075 -0.982351 0.183358 0.0369591 -174.459 -0.183625 -0.982988 -0.00394147 431.049 0.0356077 -0.0106585 0.999309 -1.89426 -0.966948 0.252445 0.035809 -173.263 -0.252593 -0.967573 0.000411611 431.452 0.0347517 -0.00864709 0.999359 -1.88862 -0.942972 0.329771 0.0453354 -172.084 -0.330197 -0.94391 -0.00204647 431.921 0.0421177 -0.0168994 0.99897 -1.88682 -0.91064 0.41217 0.0291802 -171.006 -0.411688 -0.911078 0.0212208 432.565 0.035332 0.00731133 0.999349 -1.9155 -0.867042 0.496757 0.0383531 -169.962 -0.496952 -0.867764 0.00494561 433.201 0.0357382 -0.0147716 0.999252 -1.96037 -0.816581 0.576214 0.034237 -169.037 -0.576033 -0.817271 0.0159291 433.993 0.0371595 -0.00671419 0.999287 -1.92368 -0.755858 0.654298 0.0239258 -168.197 -0.654304 -0.756182 0.00864673 434.852 0.0237498 -0.00911903 0.999676 -1.94945 -0.685858 0.727115 0.0300283 -167.418 -0.727365 -0.686241 0.0035592 435.788 0.0231946 -0.0194004 0.999543 -1.97568 -0.6102 0.791941 0.0220227 -166.773 -0.792026 -0.610451 0.00666025 436.813 0.0187183 -0.0133784 0.999735 -1.92695 -0.526895 0.849707 0.0194722 -166.204 -0.849846 -0.527027 0.00196156 437.894 0.0119291 -0.0155148 0.999808 -1.95495 -0.441304 0.897279 0.0119024 -165.75 -0.897269 -0.441408 0.00816746 439.044 0.0125823 -0.00707534 0.999896 -1.89828 -0.353159 0.935437 0.0153525 -165.393 -0.935541 -0.353216 0.00111156 440.22 0.00646256 -0.0139704 0.999882 -1.92454 -0.273326 0.961831 0.0131928 -165.153 -0.961875 -0.273422 0.00608558 441.436 0.0094605 -0.0110265 0.999894 -1.86521 -0.204542 0.978785 0.0119488 -164.967 -0.978776 -0.204668 0.0104578 442.672 0.0126815 -0.00955618 0.999874 -1.88666 -0.146565 0.989041 0.0178033 -164.823 -0.989101 -0.146783 0.0115992 443.936 0.0140853 -0.0159092 0.999774 -1.84579 -0.09944 0.99497 0.0121394 -164.711 -0.994922 -0.099611 0.0144006 445.249 0.0155373 -0.0106458 0.999823 -1.85044 -0.0657515 0.997771 0.0113746 -164.633 -0.997696 -0.0659287 0.015981 446.582 0.0166953 -0.0102976 0.999808 -1.83105 -0.0452102 0.998926 0.0101574 -164.586 -0.998848 -0.0453656 0.0156281 447.956 0.0160721 -0.00943916 0.999826 -1.83692 -0.0278983 0.999576 0.00839618 -164.566 -0.999456 -0.0280409 0.0173736 449.396 0.0176017 -0.00790692 0.999814 -1.82163 -0.0151482 0.999861 0.00701936 -164.562 -0.999745 -0.0152634 0.0166598 450.868 0.0167646 -0.0067652 0.999837 -1.81987 -0.00758712 0.999947 0.00701057 -164.547 -0.999845 -0.00769738 0.0158372 451.884 0.0158903 -0.00688932 0.99985 -1.8094 -0.00215685 0.99999 0.00382845 -164.551 -0.999879 -0.00221543 0.0153656 452.901 0.0153739 -0.00379484 0.999875 -1.79401 -0.00136052 0.999992 0.00364144 -164.548 -0.999903 -0.00141094 0.0138819 453.941 0.0138869 -0.0036222 0.999897 -1.79922 -0.000102577 0.999983 0.00577111 -164.539 -0.999914 -0.000178095 0.0130866 454.991 0.0130874 -0.00576927 0.999898 -1.79253 -6.95625e-05 0.99997 0.0077942 -164.539 -0.999928 -0.000163207 0.0120146 456.067 0.0120155 -0.0077928 0.999897 -1.77795 0.00058304 0.999972 0.00744463 -164.537 -0.999942 0.000502995 0.0107495 457.15 0.0107455 -0.00745047 0.999915 -1.77172 -0.00202685 0.999953 0.00946149 -164.529 -0.999949 -0.00212031 0.00987802 458.247 0.00989762 -0.00944098 0.999906 -1.75566 -0.00601665 0.999932 0.00995075 -164.512 -0.999934 -0.00611349 0.00973081 459.341 0.00979098 -0.00989154 0.999903 -1.73992 -0.0111003 0.999894 0.00945953 -164.493 -0.999896 -0.0111862 0.00907626 460.477 0.00918111 -0.0093578 0.999914 -1.72133 -0.0161413 0.99983 0.00891889 -164.471 -0.999817 -0.0162313 0.01011 461.629 0.0102531 -0.00875407 0.999909 -1.71669 -0.0208243 0.999747 0.00848119 -164.444 -0.999717 -0.0209195 0.0113045 462.787 0.011479 -0.00824339 0.9999 -1.7 -0.028147 0.999568 0.00841039 -164.39 -0.999547 -0.0282338 0.0103919 464.553 0.0106249 -0.00811408 0.999911 -1.69154 -0.0307454 0.999431 0.0138551 -164.331 -0.999451 -0.0309111 0.011904 465.754 0.0123255 -0.0134815 0.999833 -1.66764 -0.0337862 0.999333 0.0138769 -164.282 -0.999346 -0.0339596 0.0124567 466.987 0.0129196 -0.0134469 0.999826 -1.64905 -0.0343704 0.999237 0.0185595 -164.226 -0.99933 -0.0345951 0.0119224 468.204 0.0125553 -0.0181373 0.999757 -1.64251 -0.034352 0.999145 0.0229907 -164.18 -0.999314 -0.0346582 0.0130555 469.434 0.0138411 -0.0225264 0.99965 -1.63046 -0.0337918 0.999102 0.0255583 -164.113 -0.999347 -0.0341044 0.0118985 470.699 0.0127595 -0.0251396 0.999603 -1.62707 -0.0311145 0.999032 0.0310919 -164.068 -0.999438 -0.0314853 0.0115066 471.96 0.0124744 -0.0307164 0.99945 -1.61113 -0.02943 0.999107 0.0303117 -164.031 -0.999466 -0.0298437 0.0132844 473.245 0.0141771 -0.0299046 0.999452 -1.60167 -0.0263805 0.999105 0.0330791 -163.975 -0.999499 -0.0269409 0.0166094 475.193 0.0174857 -0.0326243 0.999315 -1.58132 -0.024151 0.999145 0.0335568 -163.95 -0.999548 -0.0247347 0.0170905 476.49 0.0179059 -0.0331289 0.999291 -1.5786 -0.0217338 0.999098 0.036474 -163.913 -0.999626 -0.0223221 0.0158018 477.823 0.0166017 -0.0361169 0.99921 -1.57082 -0.0194755 0.999013 0.039918 -163.876 -0.999684 -0.0200932 0.015131 479.164 0.0159181 -0.0396107 0.999088 -1.55911 -0.0178224 0.998978 0.0415424 -163.843 -0.999728 -0.0184291 0.0142687 480.523 0.0150197 -0.0412768 0.999035 -1.54484 -0.015078 0.998944 0.043398 -163.792 -0.999787 -0.015675 0.013448 481.896 0.014114 -0.0431859 0.998967 -1.53141 -0.0128589 0.999037 0.0419432 -163.789 -0.999806 -0.0134732 0.0143951 483.292 0.0149464 -0.0417499 0.999016 -1.51105 -0.0104578 0.99918 0.0391216 -163.746 -0.999828 -0.0110491 0.0149285 485.387 0.0153485 -0.0389588 0.999123 -1.48696 -0.0103144 0.999365 0.0340928 -163.737 -0.999867 -0.0107385 0.0122775 486.789 0.0126359 -0.0339617 0.999343 -1.47918 -0.0099145 0.999387 0.0335746 -163.724 -0.99988 -0.0103085 0.0115814 488.198 0.0119204 -0.0334557 0.999369 -1.46397 -0.0103382 0.999461 0.0311431 -163.705 -0.999864 -0.0107316 0.0124926 489.646 0.0128201 -0.0310097 0.999437 -1.45353 -0.0102563 0.999418 0.0325429 -163.683 -0.999853 -0.0106974 0.0134115 491.079 0.0137518 -0.0324006 0.99938 -1.43862 -0.0118119 0.999456 0.0307945 -163.663 -0.999865 -0.0121577 0.011068 492.517 0.0114364 -0.0306596 0.999464 -1.42073 -0.0130865 0.999382 0.0326373 -163.619 -0.999821 -0.0135236 0.013206 494.699 0.0136392 -0.0324586 0.99938 -1.4082 -0.0147667 0.999304 0.0342581 -163.59 -0.999822 -0.0151594 0.0112312 496.163 0.0117427 -0.0340861 0.99935 -1.38063 -0.0162095 0.999289 0.0340401 -163.537 -0.999812 -0.0165602 0.0100475 498.37 0.010604 -0.0338708 0.99937 -1.35953 -0.0175478 0.999223 0.0352774 -163.507 -0.99978 -0.0179419 0.0108861 499.855 0.0115106 -0.0350786 0.999318 -1.34464 -0.0185229 0.999161 0.0365399 -163.459 -0.999768 -0.0189109 0.0103014 501.338 0.0109838 -0.0363406 0.999279 -1.32819 -0.0208667 0.998986 0.0399037 -163.391 -0.999692 -0.0213854 0.0126152 503.562 0.0134557 -0.0396282 0.999124 -1.29839 -0.0223043 0.998871 0.0419348 -163.351 -0.999641 -0.0229042 0.0138807 505.022 0.0148255 -0.0416101 0.999024 -1.27352 -0.023753 0.998783 0.0432326 -163.299 -0.999611 -0.0243591 0.0135463 506.477 0.0145829 -0.0428941 0.998973 -1.25512 -0.0251474 0.998628 0.0459262 -163.252 -0.999599 -0.0257183 0.0118831 507.904 0.0130479 -0.0456089 0.998874 -1.24414 -0.0249116 0.998453 0.0496994 -163.2 -0.999621 -0.0254623 0.0104786 509.336 0.0117279 -0.0494195 0.998709 -1.21924 -0.0243103 0.998407 0.0509129 -163.164 -0.999632 -0.0248912 0.0108054 510.758 0.0120555 -0.0506315 0.998645 -1.19673 -0.0227028 0.99842 0.051405 -163.117 -0.999649 -0.0233726 0.0124665 512.193 0.0136483 -0.0511039 0.9986 -1.17901 -0.02156 0.998568 0.048961 -163.05 -0.99965 -0.0222817 0.0142425 514.352 0.015313 -0.0486368 0.998699 -1.14808 -0.0195718 0.998725 0.0465321 -163.023 -0.999711 -0.020199 0.0130457 515.787 0.013969 -0.0462633 0.998832 -1.14014 -0.0176031 0.998748 0.046817 -162.988 -0.999783 -0.0181045 0.0103055 517.229 0.0111402 -0.0466255 0.99885 -1.12004 -0.0138764 0.998965 0.0433173 -162.966 -0.999838 -0.0143584 0.0108364 518.667 0.0114472 -0.0431599 0.999003 -1.09223 -0.00809382 0.999071 0.0423212 -162.962 -0.999886 -0.00862516 0.0123874 520.114 0.0127409 -0.0422161 0.999027 -1.09692 -0.000846654 0.999141 0.0414253 -162.954 -0.999922 -0.00136129 0.0123967 521.568 0.0124424 -0.0414116 0.999065 -1.07576 0.00577562 0.9992 0.0395677 -162.967 -0.999939 0.00539626 0.00968774 522.997 0.00946647 -0.0396212 0.99917 -1.05557 0.0128067 0.999002 0.0427988 -162.984 -0.999892 0.0124872 0.00772307 524.441 0.00718092 -0.0428931 0.999054 -1.03449 0.0195367 0.998977 0.040782 -163.019 -0.999683 0.0188713 0.0166387 525.899 0.0158521 -0.0410942 0.99903 -1.01716 0.0270678 0.998877 0.0388816 -163.056 -0.999512 0.0264374 0.0166369 527.361 0.0155903 -0.039313 0.999105 -0.991882 0.0330384 0.998749 0.0375407 -163.108 -0.999451 0.0329283 0.00354944 528.807 0.00230885 -0.0376373 0.999289 -0.954153 0.0391078 0.998662 0.0338413 -163.162 -0.99923 0.038982 0.00437094 530.269 0.00304589 -0.0339862 0.999418 -0.940763 0.0423357 0.998633 0.0306469 -163.217 -0.999069 0.0420602 0.00957859 531.751 0.00827649 -0.0310239 0.999484 -0.907375 0.044625 0.998492 0.0319585 -163.282 -0.99899 0.0444334 0.00668118 533.205 0.00525108 -0.0322244 0.999467 -0.889632 0.0457762 0.998499 0.0300825 -163.375 -0.998945 0.0456439 0.0050719 535.409 0.00369121 -0.030283 0.999535 -0.845481 0.0435312 0.998606 0.0298522 -163.433 -0.999039 0.0433577 0.00643251 536.877 0.00512922 -0.0301035 0.999534 -0.822243 0.0400851 0.998672 0.0323625 -163.499 -0.999184 0.0399056 0.00617607 539.116 0.00487642 -0.0325837 0.999457 -0.776642 0.0375074 0.99877 0.0324318 -163.557 -0.999285 0.0373319 0.00600015 540.591 0.00478203 -0.0326337 0.999456 -0.743114 0.0340766 0.998932 0.0312135 -163.627 -0.999402 0.0338737 0.00700765 542.817 0.00594285 -0.0314336 0.999488 -0.705475 0.0302953 0.999096 0.0298233 -163.683 -0.999511 0.0300486 0.00868589 545.058 0.00778189 -0.0300719 0.999517 -0.66842 0.0277758 0.999128 0.0311775 -163.713 -0.999564 0.0274495 0.0108448 546.554 0.00997949 -0.0314651 0.999455 -0.649963 0.0255329 0.999134 0.0328665 -163.743 -0.999598 0.0251118 0.0131594 548.03 0.0123227 -0.0331893 0.999373 -0.617393 0.0217672 0.999217 0.0330415 -163.785 -0.999719 0.0214456 0.0100565 550.227 0.00934 -0.0332511 0.999403 -0.579994 0.0196377 0.99929 0.0321539 -163.813 -0.999771 0.0193551 0.00907602 551.698 0.00844724 -0.0323248 0.999442 -0.573331 0.0182908 0.999256 0.0339455 -163.828 -0.999801 0.0180078 0.00862577 553.161 0.00800807 -0.0340965 0.999386 -0.546985 0.0173645 0.999291 0.0334161 -163.851 -0.999826 0.0171254 0.00742736 554.629 0.00684983 -0.0335393 0.999414 -0.522264 0.0162681 0.999292 0.0339109 -163.871 -0.999836 0.0159867 0.0085509 556.094 0.00800272 -0.0340444 0.999388 -0.496746 0.0148403 0.999134 0.0388643 -163.884 -0.999864 0.0145499 0.00774345 558.302 0.00717127 -0.038974 0.999214 -0.460045 0.0134577 0.999119 0.0397623 -163.895 -0.999897 0.0132464 0.00557324 559.776 0.00504162 -0.0398332 0.999194 -0.429789 0.0132343 0.999082 0.040743 -163.905 -0.999903 0.0130451 0.00490564 561.253 0.00436964 -0.0408039 0.999158 -0.403 0.0138689 0.999123 0.0395042 -163.925 -0.999892 0.0136663 0.0053931 562.727 0.00484849 -0.0395747 0.999205 -0.365448 0.0146433 0.999403 0.0313078 -163.955 -0.999871 0.0144284 0.00708057 564.939 0.00662462 -0.0314075 0.999485 -0.319221 0.0150563 0.99954 0.0263214 -163.985 -0.99985 0.0148249 0.00896284 566.411 0.00856851 -0.0264524 0.999613 -0.294087 0.01596 0.999656 0.0208322 -164.015 -0.999823 0.015749 0.0102528 568.62 0.00992118 -0.0209921 0.99973 -0.248733 0.016373 0.999735 0.0161834 -164.053 -0.999826 0.0162257 0.00919146 570.099 0.00892644 -0.016331 0.999827 -0.242307 0.0184257 0.999664 0.0182204 -164.076 -0.999788 0.0182536 0.0095652 571.575 0.0092294 -0.0183928 0.999788 -0.2149 0.0201005 0.999653 0.0170478 -164.106 -0.999747 0.0199236 0.0104826 573.04 0.0101393 -0.0172542 0.9998 -0.199233 0.022586 0.999612 0.0163268 -164.133 -0.999687 0.0224062 0.0111085 574.516 0.0107384 -0.0165726 0.999805 -0.174215 0.0249785 0.999557 0.0161676 -164.17 -0.999642 0.0248181 0.0100453 575.984 0.00963959 -0.0164127 0.999819 -0.149534 0.0265234 0.999543 0.0144934 -164.213 -0.999599 0.0263758 0.0102878 577.434 0.00990079 -0.0147604 0.999842 -0.123136 0.0288666 0.999514 0.0117643 -164.275 -0.999546 0.0287624 0.0089386 579.578 0.00859589 -0.012017 0.999891 -0.0806227 0.0299004 0.999512 0.00905644 -164.312 -0.999511 0.0298146 0.00945911 580.992 0.00918448 -0.00933484 0.999914 -0.0500044 0.031443 0.999495 0.0044861 -164.364 -0.999485 0.0314132 0.00655708 582.396 0.00641285 -0.00468996 0.999968 -0.0166383 0.0333585 0.99944 0.0027981 -164.416 -0.999426 0.0333411 0.00606831 583.783 0.00597162 -0.00299892 0.999978 0.00941505 0.0344382 0.999404 -0.0022357 -164.461 -0.999357 0.0344588 0.00994611 585.155 0.0100172 0.00189174 0.999948 0.0403365 0.036171 0.999329 -0.00578785 -164.518 -0.99929 0.0362292 0.0102989 586.515 0.0105017 0.00541122 0.99993 0.0633299 0.0377715 0.999249 -0.00861135 -164.572 -0.999249 0.0378434 0.00835003 587.848 0.00866964 0.00828949 0.999928 0.0904724 0.0391012 0.99914 -0.0137814 -164.634 -0.999199 0.0392131 0.00794773 589.172 0.00848131 0.0134596 0.999873 0.112113 0.0399143 0.999029 -0.0186539 -164.698 -0.999134 0.0401235 0.0109804 590.484 0.0117182 0.0181994 0.999766 0.136638 0.0383116 0.99902 -0.0221545 -164.749 -0.999162 0.0386175 0.0135444 591.768 0.0143867 0.0216171 0.999663 0.156641 0.0346043 0.999127 -0.023409 -164.798 -0.999315 0.0348986 0.0122819 593.057 0.0130882 0.022968 0.999651 0.177445 0.0310421 0.999209 -0.0248473 -164.842 -0.999447 0.0313265 0.0111404 594.322 0.01191 0.0244877 0.999629 0.20332 0.0267842 0.999211 -0.0293359 -164.86 -0.999554 0.0271581 0.0124215 595.57 0.0132084 0.0289901 0.999492 0.231375 0.0158884 0.999324 -0.0331384 -164.886 -0.99978 0.0163314 0.013142 596.803 0.0136743 0.0329223 0.999364 0.241953 0.000862956 0.999444 -0.0333221 -164.87 -0.999893 0.00134925 0.0145739 597.996 0.0146108 0.033306 0.999338 0.261228 -0.017546 0.999186 -0.0363214 -164.837 -0.999718 -0.0169516 0.0166081 599.162 0.0159788 0.0366025 0.999202 0.275507 -0.0429506 0.998298 -0.0394543 -164.769 -0.998947 -0.0422742 0.0178208 600.275 0.0161226 0.0401782 0.999062 0.286057 -0.0785813 0.996263 -0.0358498 -164.633 -0.996836 -0.0780934 0.0148141 601.36 0.0119591 0.0369005 0.999247 0.298588 -0.124663 0.991767 -0.0292764 -164.452 -0.992129 -0.12425 0.0155239 602.43 0.0117585 0.0309813 0.999451 0.312911 -0.178627 0.983661 -0.0224266 -164.209 -0.983825 -0.178252 0.0177537 603.469 0.0134661 0.0252351 0.999591 0.318788 -0.239809 0.97079 -0.00767748 -163.884 -0.970697 -0.239645 0.0178374 604.479 0.0154765 0.0117301 0.999811 0.337795 -0.302887 0.952984 0.00902981 -163.473 -0.952901 -0.302987 0.0133448 605.448 0.0154533 -0.00456254 0.99987 0.34853 -0.36631 0.930322 0.0178249 -163.015 -0.930318 -0.366545 0.012383 606.373 0.0180538 -0.0120468 0.999764 0.377151 -0.427698 0.903537 0.0263827 -162.532 -0.903542 -0.428182 0.0164942 607.285 0.0261997 -0.0167834 0.999516 0.356572 -0.485884 0.873311 0.0352861 -161.952 -0.873365 -0.486689 0.0191737 608.139 0.033918 -0.0215015 0.999193 0.382652 -0.545098 0.838129 0.0202062 -161.372 -0.837929 -0.545435 0.0193669 608.971 0.0272531 -0.00637451 0.999608 0.397828 -0.600712 0.79941 0.00946209 -160.724 -0.799368 -0.600781 0.00855321 609.738 0.0125222 -0.00242567 0.999919 0.393905 -0.68004 0.733169 0.00304159 -159.645 -0.733175 -0.680032 -0.00330937 610.788 -0.000357948 -0.00448053 0.99999 0.42769 -0.726976 0.686661 -0.0013455 -158.881 -0.686634 -0.726926 0.0106377 611.463 0.0063264 0.0086572 0.999943 0.410286 -0.765018 0.643931 0.0100467 -158.045 -0.64386 -0.765083 0.00958838 612.071 0.0138608 0.000866586 0.999904 0.424264 -0.795778 0.60555 0.00685574 -157.209 -0.605507 -0.795805 0.00747575 612.666 0.00998277 0.00179784 0.999949 0.475232 -0.823448 0.567391 -0.000881315 -156.346 -0.567343 -0.823357 0.0142972 613.231 0.00738644 0.012273 0.999897 0.472256 -0.848762 0.528774 -0.00084567 -155.44 -0.528741 -0.84869 0.0125287 613.751 0.00590714 0.011081 0.999921 0.476203 -0.871712 0.490016 -0.00161822 -154.507 -0.490012 -0.87168 0.00791066 614.211 0.00246578 0.00768877 0.999967 0.520861 -0.891293 0.453371 -0.0071212 -153.543 -0.453415 -0.891273 0.00682354 614.663 -0.00325334 0.00931064 0.999951 0.531458 -0.912766 0.408448 -0.00533533 -152.037 -0.408471 -0.91276 0.00445788 615.298 -0.00304907 0.00624833 0.999976 0.575984 -0.922714 0.385398 -0.00813311 -151.02 -0.385435 -0.922728 0.0035784 615.699 -0.00612554 0.00643663 0.999961 0.618439 -0.929191 0.369514 -0.0079049 -149.947 -0.369539 -0.929213 0.00184189 616.098 -0.00666474 0.00463264 0.999967 0.640298 -0.932804 0.360371 -0.00297913 -148.904 -0.36038 -0.9328 0.0031994 616.496 -0.00162596 0.00405803 0.99999 0.678208 -0.935704 0.352786 0.00018288 -147.84 -0.352766 -0.935659 0.00994019 616.899 0.00367787 0.00923657 0.999951 0.698996 -0.938726 0.344643 0.00391029 -146.758 -0.34462 -0.938726 0.00549908 617.289 0.00556591 0.00381456 0.999977 0.700431 -0.941022 0.338347 0.000112117 -145.678 -0.338346 -0.94102 0.00169797 617.663 0.000680008 0.0015599 0.999999 0.752103 -0.943092 0.332496 -0.00485006 -144.58 -0.332514 -0.943092 0.00342408 618.05 -0.00343556 0.00484193 0.999982 0.768907 -0.944638 0.328115 -0.000537893 -143.446 -0.328115 -0.944637 0.00106992 618.425 -0.000157058 0.00118718 0.999999 0.794858 -0.945957 0.324283 0.00254956 -142.329 -0.324278 -0.945959 0.00213234 618.798 0.00310326 0.00119033 0.999994 0.843278 -0.946847 0.321678 0.00208417 -141.198 -0.321664 -0.946839 0.00536053 619.198 0.00369774 0.00440519 0.999983 0.857673 -0.947639 0.319314 0.00444098 -140.045 -0.319301 -0.947647 0.00340868 619.567 0.00529692 0.00181219 0.999984 0.87222 -0.947965 0.318338 0.00476023 -138.885 -0.318332 -0.947977 0.00208906 619.944 0.00517762 0.000465021 0.999986 0.904651 -0.948273 0.317426 0.00432167 -137.715 -0.317426 -0.948283 0.000682279 620.327 0.00431474 -0.000724823 0.99999 0.916837 -0.948506 0.3167 0.00615986 -136.524 -0.316703 -0.948525 0.00056133 620.715 0.00602055 -0.00141842 0.999981 0.947529 -0.948707 0.316103 0.00581582 -135.329 -0.316096 -0.948725 0.00214956 621.117 0.0061971 0.000200949 0.999981 0.971337 -0.948672 0.316118 0.00955481 -134.108 -0.316128 -0.948716 0.00043871 621.517 0.00920349 -0.00260435 0.999954 0.973757 -0.948593 0.316195 0.0138852 -132.889 -0.316151 -0.948694 0.00530336 621.92 0.0148497 0.000640905 0.99989 1.00669 -0.948479 0.316647 0.0110908 -131.66 -0.316612 -0.948543 0.00481583 622.345 0.012045 0.00105623 0.999927 1.01284 -0.948368 0.31696 0.0116054 -130.407 -0.316961 -0.948437 0.00179366 622.738 0.0115755 -0.00197743 0.999931 1.0194 -0.948409 0.316979 0.00669861 -129.135 -0.316982 -0.948431 0.000650205 623.178 0.00655927 -0.00150668 0.999977 1.04266 -0.948133 0.317863 -0.00268063 -127.847 -0.317842 -0.948123 -0.00622509 623.594 -0.0045203 -0.0050502 0.999977 1.0741 -0.948101 0.317965 0.00158778 -126.528 -0.317968 -0.948069 -0.00792237 624.025 -0.00101372 -0.00801607 0.999967 1.09554 -0.948002 0.318175 0.00754223 -125.197 -0.318225 -0.94799 -0.00689366 624.455 0.00495657 -0.00893534 0.999948 1.14727 -0.947981 0.318277 0.00561837 -123.873 -0.318306 -0.947973 -0.00533229 624.911 0.00362892 -0.00684327 0.99997 1.15852 -0.947905 0.318394 0.0100379 -122.547 -0.318476 -0.947898 -0.00792241 625.345 0.00699249 -0.0107065 0.999918 1.1853 -0.947852 0.318474 0.0123027 -121.193 -0.318564 -0.947882 -0.00608639 625.798 0.00972314 -0.00968818 0.999906 1.20663 -0.947767 0.31872 0.0124734 -119.807 -0.318847 -0.947754 -0.00994624 626.253 0.00865161 -0.0134038 0.999873 1.22057 -0.947645 0.319073 0.0126948 -118.406 -0.319232 -0.947581 -0.0134384 626.716 0.00774149 -0.0167874 0.999829 1.24773 -0.947248 0.320286 0.0117407 -117.016 -0.320431 -0.947171 -0.0138479 627.18 0.00668518 -0.0168795 0.999835 1.26562 -0.947025 0.320887 0.0132222 -115.589 -0.321077 -0.946912 -0.016361 627.656 0.00727026 -0.0197396 0.999779 1.28881 -0.946903 0.321145 0.015492 -114.166 -0.321365 -0.946842 -0.0146586 628.142 0.0099609 -0.0188588 0.999773 1.32806 -0.946483 0.322607 0.00976029 -112.729 -0.322696 -0.946453 -0.00967534 628.644 0.00611633 -0.0123072 0.999906 1.33865 -0.946292 0.323122 0.0111513 -111.273 -0.323234 -0.946262 -0.0103555 629.135 0.00720594 -0.0134038 0.999884 1.35824 -0.94602 0.32367 0.0168594 -109.779 -0.323801 -0.946108 -0.00564084 629.645 0.014125 -0.0107954 0.999842 1.36918 -0.945647 0.324774 0.0165689 -108.294 -0.32488 -0.945746 -0.00412698 630.157 0.0143296 -0.00928557 0.999854 1.38644 -0.945352 0.32577 0.0135298 -106.787 -0.32586 -0.945405 -0.00498732 630.67 0.0111664 -0.00912361 0.999896 1.409 -0.945069 0.326694 0.0107625 -104.483 -0.326692 -0.945129 0.00200997 631.469 0.0108286 -0.00161647 0.99994 1.44716 -0.944792 0.327478 0.0112574 -102.931 -0.327407 -0.944852 0.00769367 632.015 0.0131561 0.00358316 0.999907 1.47052 -0.94431 0.329016 0.00519233 -101.374 -0.329033 -0.944314 -0.00295002 632.542 0.00393259 -0.00449418 0.999982 1.49272 -0.944249 0.329226 0.00195056 -99.8023 -0.329229 -0.944249 -0.00118609 633.09 0.00145133 -0.00176214 0.999997 1.54355 -0.943795 0.329971 0.0192457 -98.1943 -0.329994 -0.943981 0.00208151 633.653 0.0188544 -0.00438644 0.999813 1.49037 -0.943931 0.329678 0.0175155 -96.5807 -0.329794 -0.944044 -0.00410408 634.206 0.0151824 -0.00965047 0.999838 1.53324 -0.944028 0.329763 0.0082083 -94.9812 -0.329835 -0.943986 -0.00995903 634.752 0.00446441 -0.012109 0.999917 1.55728 -0.944526 0.328209 0.0122077 -93.3256 -0.328311 -0.94454 -0.00748484 635.327 0.00907412 -0.0110776 0.999897 1.58362 -0.945617 0.325161 0.008905 -90.8135 -0.325234 -0.945595 -0.00854723 636.17 0.00564129 -0.0109786 0.999924 1.63026 -0.946727 0.321948 0.00763758 -89.1431 -0.322004 -0.946706 -0.00780739 636.736 0.00471697 -0.00985079 0.99994 1.66049 -0.947718 0.318724 0.0156554 -86.5584 -0.31888 -0.947755 -0.00871687 637.584 0.0120592 -0.0132533 0.999839 1.67613 -0.948527 0.316134 0.0188857 -84.8215 -0.316339 -0.948603 -0.00903916 638.155 0.0150575 -0.0145482 0.999781 1.71423 -0.949395 0.313828 0.0126913 -83.0869 -0.313998 -0.949308 -0.0148108 638.721 0.00739987 -0.0180463 0.99981 1.72027 -0.950518 0.310288 0.0153956 -81.3169 -0.310524 -0.950423 -0.016479 639.303 0.00951905 -0.0204443 0.999746 1.755 -0.951124 0.308048 0.0216604 -79.5301 -0.308419 -0.951107 -0.0165244 639.864 0.015511 -0.0223972 0.999629 1.77309 -0.951943 0.305359 0.0236965 -77.748 -0.305779 -0.951955 -0.0167479 640.427 0.0174439 -0.023189 0.999579 1.79201 -0.952971 0.302514 0.0181938 -75.9957 -0.302814 -0.952901 -0.0168437 640.979 0.0122415 -0.0215608 0.999693 1.82922 -0.953937 0.299534 0.0168388 -74.2281 -0.299829 -0.9538 -0.0191757 641.525 0.010317 -0.0233412 0.999674 1.84142 -0.954716 0.29683 0.0202465 -72.4885 -0.297176 -0.954672 -0.0169436 642.066 0.0142994 -0.0221931 0.999651 1.88164 -0.955668 0.29405 0.0152856 -70.7514 -0.294287 -0.955571 -0.0167078 642.601 0.00969357 -0.0204654 0.999744 1.89772 -0.956822 0.289951 0.0205005 -68.1412 -0.29028 -0.956818 -0.0153829 643.379 0.015155 -0.0206696 0.999671 1.92847 -0.957651 0.286767 0.0258807 -66.3815 -0.287251 -0.957697 -0.0174057 643.897 0.0197945 -0.0241029 0.999513 1.93241 -0.959312 0.281296 0.0243488 -63.7688 -0.281842 -0.959187 -0.0229371 644.642 0.0169029 -0.0288664 0.99944 1.98307 -0.960364 0.277486 0.0264962 -61.1416 -0.278128 -0.960225 -0.0247578 645.379 0.0185723 -0.0311458 0.999342 2.01815 -0.961008 0.275176 0.0272389 -59.4091 -0.275724 -0.96105 -0.018922 645.895 0.0209711 -0.0256946 0.99945 2.01665 -0.962046 0.271965 0.0224126 -57.7185 -0.272311 -0.962105 -0.014135 646.367 0.0177191 -0.0197017 0.999649 2.04107 -0.96348 0.266864 0.0221194 -55.167 -0.267142 -0.963598 -0.0106991 647.08 0.018459 -0.0162174 0.999698 2.04046 -0.965107 0.261058 0.0204286 -53.4817 -0.261292 -0.965211 -0.00971933 647.528 0.0171806 -0.014718 0.999744 2.07446 -0.968295 0.249184 0.0176482 -51.7996 -0.249373 -0.968361 -0.00947062 647.955 0.0147299 -0.0135713 0.999799 2.08309 -0.974713 0.223062 0.0133496 -49.3029 -0.223119 -0.974787 -0.00293659 648.524 0.012358 -0.00584088 0.999907 2.13868 -0.979261 0.202556 0.00434703 -47.6187 -0.20256 -0.97927 -0.00043545 648.862 0.00416871 -0.00130695 0.99999 2.16112 -0.983831 0.178983 0.00648723 -45.9149 -0.178957 -0.983847 0.00438084 649.169 0.00716653 0.00314907 0.999969 2.20192 -0.987495 0.157143 0.0126262 -44.2017 -0.157021 -0.987543 0.0101688 649.44 0.0140668 0.00805903 0.999869 2.2152 -0.990894 0.133826 0.0148441 -42.4682 -0.133628 -0.990938 0.0136134 649.668 0.0165314 0.0115058 0.999797 2.22961 -0.99367 0.111469 0.0139784 -40.7342 -0.111272 -0.993689 0.0141653 649.851 0.0154692 0.0125202 0.999802 2.23702 -0.996019 0.0883932 0.0115407 -38.9664 -0.0882519 -0.996023 0.0122236 649.984 0.0125753 0.0111565 0.999859 2.24811 -0.99777 0.0652058 0.0142446 -37.1944 -0.0650225 -0.9978 0.0129687 650.086 0.0150589 0.0120136 0.999814 2.2756 -0.998836 0.0450347 0.0172955 -35.4071 -0.0448296 -0.998922 0.0120654 650.162 0.0178202 0.011276 0.999778 2.27817 -0.999335 0.0305509 0.0198917 -33.5985 -0.0303552 -0.999488 0.0100663 650.209 0.0201891 0.00945577 0.999751 2.29797 -0.99961 0.0180829 0.0212724 -31.7775 -0.0178459 -0.999777 0.0112777 650.235 0.0214716 0.0108937 0.99971 2.30389 -0.999679 0.00696704 0.0243464 -29.9757 -0.0067464 -0.999936 0.00913271 650.224 0.0244084 0.00896553 0.999662 2.30224 -0.99971 -0.00171802 0.0240261 -28.1869 0.00186595 -0.999979 0.00613615 650.211 0.0240151 0.00617921 0.999692 2.31002 -0.999664 -0.00757524 0.0247775 -26.408 0.0076633 -0.999965 0.00346106 650.196 0.0247505 0.00364978 0.999687 2.3123 -0.999614 -0.012508 0.0248112 -24.6368 0.0126042 -0.999914 0.00372646 650.176 0.0247624 0.00403775 0.999685 2.32375 -0.999552 -0.0156315 0.0255238 -22.8835 0.0158287 -0.999846 0.00754403 650.149 0.0254019 0.00794466 0.999646 2.32759 -0.999478 -0.0182472 0.0266426 -21.1482 0.0185359 -0.999772 0.0106307 650.12 0.0264425 0.011119 0.999588 2.33484 -0.999462 -0.0209019 0.0252741 -19.4097 0.021257 -0.999678 0.0138618 650.089 0.0249763 0.0143916 0.999584 2.33216 -0.999461 -0.0221681 0.0242066 -17.6948 0.0225114 -0.999649 0.0140003 650.044 0.0238877 0.0145377 0.999609 2.32264 -0.999351 -0.0222249 0.0283451 -15.9904 0.0226486 -0.999635 0.0147169 650.005 0.0280077 0.0153493 0.99949 2.33231 -0.99922 -0.0220132 0.0327969 -14.2947 0.0225717 -0.999605 0.0167576 649.974 0.032415 0.0174848 0.999322 2.31515 -0.999683 -0.0214226 0.0132228 -12.6361 0.0215917 -0.999685 0.0127797 649.931 0.0129449 0.0130612 0.999831 2.3305 -0.999747 -0.0203547 0.00960113 -10.9675 0.0204354 -0.999756 0.00837972 649.89 0.00942822 0.0085738 0.999919 2.34046 -0.999478 -0.0190105 0.0261219 -9.28623 0.019132 -0.999807 0.00440937 649.854 0.0260331 0.00490683 0.999649 2.3783 -0.999563 -0.0173613 0.023929 -7.63161 0.0174859 -0.999835 0.00500784 649.809 0.0238381 0.00542407 0.999701 2.39466 -0.999695 -0.0160312 0.0187898 -5.99667 0.0161735 -0.999841 0.00744346 649.785 0.0186675 0.00774508 0.999796 2.39396 -0.999538 -0.0150175 0.0264208 -4.35101 0.0152048 -0.999861 0.00690451 649.75 0.0263134 0.00730304 0.999627 2.40062 -0.999689 -0.0147105 0.0201299 -2.71438 0.0147618 -0.999888 0.00239865 649.711 0.0200924 0.00269505 0.999794 2.40604 -0.99983 -0.0147021 0.0110951 -1.0961 0.0146909 -0.999891 -0.00108617 649.67 0.0111099 -0.000922988 0.999938 2.42793 -0.999694 -0.0142317 0.0202537 0.552063 0.0141642 -0.999894 -0.00347426 649.623 0.020301 -0.00318631 0.999789 2.43583 -0.999716 -0.0147459 0.0187299 2.16816 0.0146604 -0.999881 -0.00469749 649.588 0.0187969 -0.00442157 0.999814 2.4627 -0.999763 -0.0165288 0.0141894 3.77605 0.0164856 -0.999859 -0.00316113 649.561 0.0142397 -0.00292646 0.999894 2.48464 -0.999684 -0.0218549 0.0124115 5.37634 0.0217725 -0.99974 -0.00674092 649.511 0.0125556 -0.00646856 0.9999 2.498 -0.999501 -0.0287565 0.0130606 6.96733 0.0286486 -0.999554 -0.0083748 649.449 0.0132956 -0.00799645 0.99988 2.52819 -0.999015 -0.0378915 0.0231014 8.58036 0.0376993 -0.999251 -0.00870127 649.378 0.0234138 -0.00782179 0.999695 2.52931 -0.998424 -0.0514836 0.0223241 10.1781 0.0511996 -0.998602 -0.0131136 649.282 0.0229681 -0.01195 0.999665 2.54384 -0.997773 -0.0653922 0.0131631 11.7425 0.0651585 -0.997722 -0.01746 649.169 0.0142749 -0.0165634 0.999761 2.55775 -0.996475 -0.0822226 0.0166368 13.3243 0.081919 -0.996473 -0.018176 649.027 0.0180726 -0.016749 0.999696 2.57927 -0.9943 -0.103909 0.0238789 14.8963 0.103528 -0.994487 -0.0166677 648.854 0.0254792 -0.0141005 0.999576 2.5963 -0.991425 -0.128917 0.0213749 16.4551 0.128631 -0.99159 -0.0142662 648.64 0.0230343 -0.0113944 0.99967 2.60752 -0.987832 -0.155098 0.0115349 18.0155 0.154905 -0.987799 -0.0160511 648.381 0.0138837 -0.014069 0.999805 2.58507 -0.98285 -0.184304 0.00615443 19.5466 0.184123 -0.982642 -0.0226432 648.072 0.0102208 -0.0211217 0.999725 2.62262 -0.976668 -0.214182 0.0156465 21.0936 0.21368 -0.97648 -0.0287536 647.711 0.021437 -0.0247394 0.999464 2.63178 -0.969894 -0.242547 0.0218186 22.6167 0.241802 -0.969797 -0.032037 647.308 0.0289301 -0.0257968 0.999249 2.63988 -0.962414 -0.27127 0.0130864 24.1341 0.270792 -0.962168 -0.0300747 646.867 0.0207496 -0.0254007 0.999462 2.64709 -0.954198 -0.298976 0.0108945 25.6278 0.298514 -0.953879 -0.031683 646.377 0.0198645 -0.0269798 0.999439 2.6434 -0.945876 -0.324477 0.00570428 27.099 0.324054 -0.945299 -0.037405 645.828 0.0175293 -0.033532 0.999284 2.65483 -0.937447 -0.348048 0.00740284 28.5555 0.347498 -0.93682 -0.0401726 645.249 0.0209171 -0.0350872 0.999165 2.67185 -0.929665 -0.3681 0.0149815 30.0015 0.367149 -0.929085 -0.0447634 644.652 0.0303965 -0.0361146 0.998885 2.67171 -0.921955 -0.386935 0.016722 31.4266 0.385898 -0.921436 -0.0451418 644.035 0.0328752 -0.0351657 0.998841 2.6599 -0.915251 -0.402637 0.0140817 32.8338 0.401682 -0.914662 -0.0452294 643.392 0.031091 -0.0357399 0.998877 2.64343 -0.908515 -0.417765 0.00851409 34.2146 0.417146 -0.907976 -0.0396001 642.732 0.0242741 -0.0324256 0.999179 2.65485 -0.900079 -0.435645 0.00846391 35.59 0.43514 -0.899704 -0.0344473 642.053 0.0226218 -0.0273223 0.999371 2.65861 -0.888927 -0.457748 0.0166241 36.948 0.457115 -0.888846 -0.0316043 641.366 0.0292431 -0.0204948 0.999362 2.65845 -0.87578 -0.482413 0.0169528 38.2939 0.481855 -0.875778 -0.0287866 640.612 0.0287339 -0.0170419 0.999442 2.6438 -0.86231 -0.506303 0.00887711 39.6083 0.505834 -0.86206 -0.0313905 639.807 0.0235457 -0.022578 0.999468 2.63711 -0.847335 -0.531017 0.00660742 40.9099 0.530654 -0.847109 -0.028509 638.967 0.020736 -0.0206504 0.999572 2.65254 -0.832291 -0.554305 0.00605771 42.1954 0.55397 -0.832087 -0.0273598 638.08 0.0202062 -0.0194155 0.999607 2.6422 -0.815877 -0.578184 0.00692141 43.455 0.577771 -0.815651 -0.0299065 637.162 0.0229369 -0.0204011 0.999529 2.66194 -0.79877 -0.601587 0.00769391 44.7035 0.601186 -0.798602 -0.0284488 636.194 0.0232588 -0.0180986 0.999566 2.6519 -0.781456 -0.623948 0.00393051 45.9175 0.623524 -0.781129 -0.0324896 635.186 0.023342 -0.0229384 0.999464 2.64302 -0.76376 -0.645417 0.0103547 47.1108 0.644622 -0.763459 -0.0399172 634.139 0.0336687 -0.0238123 0.999149 2.61899 -0.746897 -0.664586 0.0216734 48.2899 0.663215 -0.74691 -0.0476532 633.048 0.0478577 -0.0212179 0.998629 2.64545 -0.730555 -0.682853 0.00148286 49.4422 0.682586 -0.730327 -0.0264405 631.986 0.019138 -0.0183041 0.999649 2.62425 -0.715838 -0.698192 -0.0101685 50.5328 0.698211 -0.715521 -0.0230644 630.875 0.00882759 -0.0236102 0.999682 2.60689 -0.701719 -0.712329 0.013307 51.6784 0.711379 -0.701564 -0.0418077 629.683 0.0391166 -0.0198709 0.999037 2.59123 -0.686275 -0.727191 0.0148347 52.7606 0.726288 -0.686235 -0.0398389 628.509 0.0391505 -0.0165662 0.999096 2.60906 -0.670736 -0.741577 0.0133035 53.8238 0.741041 -0.67079 -0.0299905 627.32 0.0311642 -0.0102573 0.999462 2.5821 -0.654505 -0.756027 0.00686215 54.8358 0.755649 -0.654422 -0.026945 626.131 0.0248619 -0.0124503 0.999613 2.58137 -0.637111 -0.77075 0.00592225 55.8453 0.770444 -0.637045 -0.0242974 624.909 0.0224999 -0.0109174 0.999687 2.57981 -0.620656 -0.784083 0.000395088 56.7974 0.783719 -0.620384 -0.0301377 623.649 0.0238756 -0.0183955 0.999546 2.567 -0.602546 -0.798074 -0.00401013 57.7281 0.797412 -0.601824 -0.0440631 622.36 0.0327523 -0.0297478 0.999021 2.57971 -0.584173 -0.811629 0.000887071 58.6488 0.810678 -0.583541 -0.047756 621.05 0.0392778 -0.0271786 0.998859 2.55153 -0.566553 -0.824021 -0.00256761 59.5101 0.823206 -0.565848 -0.0463495 619.707 0.0367401 -0.0283731 0.998922 2.52446 -0.547018 -0.837112 -0.00398594 60.3626 0.836586 -0.546492 -0.0383443 618.386 0.0299202 -0.0243096 0.999257 2.5203 -0.525954 -0.850513 -0.000729579 61.2202 0.849845 -0.525506 -0.0400903 617.062 0.0337139 -0.0217057 0.999196 2.50766 -0.508036 -0.861324 -0.00453778 62.0092 0.860817 -0.50754 -0.0373769 615.659 0.0298905 -0.022895 0.999291 2.49465 -0.489134 -0.872195 -0.00488642 62.7756 0.871776 -0.488707 -0.0342215 614.276 0.0274598 -0.0209988 0.999402 2.4836 -0.472627 -0.881263 -0.000393624 63.5156 0.880539 -0.47222 -0.0407345 612.836 0.0357119 -0.0195988 0.99917 2.46908 -0.457146 -0.889371 -0.00610114 64.2478 0.888766 -0.456556 -0.0406436 611.374 0.0333617 -0.0240025 0.999155 2.45304 -0.444589 -0.895543 -0.0185164 64.9515 0.895311 -0.443647 -0.0399477 609.93 0.0275602 -0.0343383 0.99903 2.42815 -0.433112 -0.901167 -0.0176587 65.6416 0.900684 -0.431966 -0.0466271 608.435 0.0343909 -0.0360997 0.998756 2.44074 -0.4228 -0.906013 -0.0195111 66.3085 0.90555 -0.421558 -0.0476141 606.937 0.034914 -0.0377995 0.998675 2.40099 -0.411997 -0.910846 -0.0248676 66.9784 0.910754 -0.410809 -0.0419928 605.432 0.0280331 -0.0399491 0.998808 2.40479 -0.401309 -0.915853 -0.0128549 67.6454 0.915076 -0.400279 -0.0491104 603.889 0.0398323 -0.0314716 0.998711 2.39798 -0.390788 -0.920447 -0.00783527 68.3064 0.919302 -0.389842 -0.0539209 602.317 0.0465768 -0.0282746 0.998514 2.34588 -0.380124 -0.924896 -0.00856536 68.9218 0.924321 -0.379518 -0.039968 600.768 0.0337155 -0.0231099 0.999164 2.32289 -0.368965 -0.929388 -0.010154 69.5271 0.928966 -0.368404 -0.0360491 599.22 0.0297628 -0.0227336 0.999298 2.32109 -0.355324 -0.934687 -0.0102076 70.1345 0.934365 -0.354848 -0.0323347 597.628 0.0266007 -0.0210269 0.999425 2.31777 -0.340895 -0.940051 -0.00975275 70.6862 0.939731 -0.340452 -0.0315858 596.05 0.0263719 -0.0199324 0.999453 2.32366 -0.326774 -0.945054 -0.0095822 71.2335 0.944649 -0.326286 -0.0342956 594.439 0.0292846 -0.0202587 0.999366 2.3274 -0.30997 -0.950734 -0.00479283 71.758 0.950223 -0.309628 -0.0347257 592.865 0.031531 -0.0153182 0.999385 2.32469 -0.283362 -0.959013 -0.000144445 72.4698 0.958359 -0.283163 -0.0369758 590.487 0.0354194 -0.010616 0.999316 2.3129 -0.261978 -0.96506 0.00512397 72.9031 0.96426 -0.261971 -0.0396612 588.896 0.0396178 -0.00544951 0.9992 2.28927 -0.240044 -0.970738 0.00681015 73.2713 0.969923 -0.240122 -0.039889 587.287 0.040357 -0.0029698 0.999181 2.25444 -0.217759 -0.975979 0.00672277 73.6091 0.975332 -0.217859 -0.0355531 585.695 0.0361637 -0.00118505 0.999345 2.24089 -0.194766 -0.980826 0.00675413 73.9157 0.98035 -0.194882 -0.0305819 584.098 0.0313118 0.000665082 0.999509 2.22551 -0.172375 -0.984996 0.00838896 74.1785 0.984479 -0.172558 -0.032022 582.485 0.0329891 0.00273894 0.999452 2.223 -0.148923 -0.988792 0.0106277 74.415 0.988248 -0.149199 -0.0332562 580.862 0.0344691 0.00555014 0.99939 2.20045 -0.125361 -0.992069 0.00911219 74.608 0.991636 -0.12558 -0.0298078 579.253 0.0307157 0.00529925 0.999514 2.1845 -0.102364 -0.99473 0.00578799 74.736 0.994219 -0.102497 -0.031994 577.619 0.0324186 0.00247951 0.999471 2.16195 -0.0787664 -0.996893 6.02843e-07 74.8372 0.996248 -0.0787154 -0.0359788 575.98 0.0358671 -0.00283332 0.999353 2.14731 -0.0549943 -0.998484 -0.00221572 74.9039 0.997945 -0.0548913 -0.0330593 574.353 0.0328876 -0.00402924 0.999451 2.14236 -0.0320104 -0.999463 -0.0069537 74.9492 0.999048 -0.0317893 -0.0298678 572.715 0.0296307 -0.00790316 0.99953 2.13266 -0.00887584 -0.999926 -0.00829114 74.941 0.99946 -0.00860886 -0.0316989 571.085 0.0316252 -0.00856802 0.999463 2.12761 0.0118416 -0.999901 -0.00754492 74.9257 0.999348 0.0120918 -0.0340259 569.452 0.0341137 -0.00713708 0.999392 2.11769 0.0295122 -0.999512 -0.010245 74.8502 0.999015 0.0298342 -0.0328374 567.815 0.0331271 -0.00926584 0.999408 2.09619 0.0417284 -0.999066 -0.0112129 74.7597 0.998473 0.0421051 -0.0357668 566.201 0.0362055 -0.00970328 0.999297 2.09392 0.0526874 -0.998543 -0.0116768 74.6731 0.997855 0.0530988 -0.0382859 564.59 0.0388501 -0.00963455 0.999199 2.06829 0.0623122 -0.997946 -0.0148469 74.5479 0.99714 0.0628855 -0.0419135 562.99 0.042761 -0.0121927 0.999011 2.05016 0.0721066 -0.997271 -0.0158586 74.4355 0.996384 0.0727406 -0.0438988 561.392 0.0449325 -0.0126359 0.99891 2.01872 0.0814426 -0.996501 -0.018781 74.2823 0.99581 0.0821436 -0.0401885 559.811 0.0415906 -0.0154292 0.999016 1.99005 0.0897623 -0.995781 -0.019033 74.1125 0.995189 0.0904296 -0.037708 558.253 0.0392701 -0.0155566 0.999108 1.98003 0.0984643 -0.994987 -0.0174869 73.9559 0.994136 0.0991391 -0.0431845 556.688 0.0447016 -0.0131322 0.998914 1.96266 0.10708 -0.994106 -0.016933 73.776 0.992984 0.107788 -0.0486226 555.14 0.0501612 -0.0116077 0.998674 1.92768 0.116028 -0.993145 -0.0141951 73.5993 0.992025 0.116582 -0.0478992 553.602 0.0492257 -0.00852427 0.998751 1.89109 0.124582 -0.992119 -0.0133601 73.3931 0.991192 0.125053 -0.0435996 552.098 0.0449268 -0.00781062 0.99896 1.8487 0.133507 -0.990936 -0.0149157 73.1794 0.989921 0.134057 -0.0456569 550.582 0.0472426 -0.00866984 0.998846 1.82475 0.144476 -0.989419 -0.0133299 72.9584 0.988344 0.144946 -0.0465588 549.109 0.0479983 -0.00644786 0.998827 1.81218 0.159969 -0.987087 -0.00837978 72.7152 0.986429 0.160168 -0.0361046 547.678 0.0369805 -0.00249046 0.999313 1.78157 0.17842 -0.98384 -0.0150028 72.4356 0.983291 0.178839 -0.0339886 546.294 0.0361225 -0.00868782 0.99931 1.74862 0.198813 -0.979821 -0.020578 72.1388 0.979095 0.199499 -0.0396589 544.956 0.0429639 -0.0122631 0.999001 1.74925 0.222065 -0.974794 -0.0215407 71.8215 0.974085 0.222768 -0.0391446 543.66 0.0429565 -0.0122898 0.999001 1.74722 0.249627 -0.968187 -0.0173425 71.4885 0.967183 0.250164 -0.0444387 542.383 0.0473635 -0.0056803 0.998862 1.73042 0.279265 -0.960046 -0.0179578 71.0821 0.958561 0.279833 -0.0534345 541.132 0.0563247 -0.00229123 0.99841 1.66318 0.315146 -0.948682 -0.0261779 70.6365 0.946891 0.316168 -0.0586023 539.911 0.0638716 -0.00631931 0.997938 1.6274 0.351172 -0.935745 -0.0325537 70.1343 0.932244 0.352672 -0.0808946 538.683 0.0871775 -0.00194009 0.996191 1.55788 0.385891 -0.921366 -0.0466226 69.5765 0.914629 0.388697 -0.111214 537.471 0.120591 0.000274231 0.992702 1.42612 0.423305 -0.904236 -0.0563009 68.9803 0.89578 0.427029 -0.123391 536.273 0.135616 0.00179862 0.99076 1.30684 0.45963 -0.886564 -0.052392 68.3276 0.876611 0.462351 -0.133359 535.092 0.142455 0.0153685 0.989682 1.19934 0.492871 -0.868847 -0.04672 67.6444 0.857234 0.494082 -0.145029 533.904 0.149091 0.0314305 0.988324 1.00812 0.524058 -0.850634 -0.042252 66.8902 0.839765 0.524358 -0.140867 532.738 0.141982 0.0383408 0.989126 0.838732 0.555565 -0.830903 -0.0307867 66.0954 0.822678 0.554683 -0.124609 531.59 0.120615 0.0439007 0.991728 0.702211 0.580625 -0.813559 -0.0315688 65.2504 0.806407 0.579998 -0.115374 530.421 0.112173 0.0415317 0.99282 0.542313 0.59523 -0.802418 -0.0427512 64.3469 0.795345 0.595899 -0.111044 529.279 0.114579 0.0320946 0.992896 0.41565 0.606591 -0.794097 -0.0381599 63.4302 0.787974 0.606901 -0.103772 528.13 0.105564 0.032878 0.993869 0.323818 0.61815 -0.785295 -0.0346729 62.5219 0.779753 0.61817 -0.0992559 526.985 0.0993788 0.0343188 0.994458 0.210301 0.627202 -0.778019 -0.0361064 61.5599 0.772547 0.627341 -0.0980523 525.842 0.0989376 0.0336047 0.994526 0.104201 0.635827 -0.771103 -0.0335348 60.5851 0.765803 0.635684 -0.0972156 524.693 0.0962808 0.0361313 0.994698 0.00783555 0.644753 -0.763939 -0.0262776 59.6061 0.759187 0.64399 -0.0943997 523.569 0.0890381 0.0409149 0.995188 -0.0872329 0.653138 -0.757042 -0.0172861 58.6296 0.755073 0.652825 -0.060695 522.457 0.0572335 0.02659 0.998007 -0.154712 0.660754 -0.750479 -0.0135911 57.6309 0.749495 0.660655 -0.0423486 521.356 0.0407608 0.0177956 0.99901 -0.178374 0.667037 -0.744403 -0.0304349 56.6028 0.742859 0.667655 -0.0489532 520.248 0.056761 0.0100447 0.998337 -0.218824 0.673218 -0.738553 -0.0362967 55.5681 0.736939 0.674165 -0.0492108 519.156 0.0608148 0.00638116 0.998129 -0.259945 0.679743 -0.73275 -0.0320476 54.56 0.731502 0.680473 -0.0431529 518.094 0.0534278 0.00589004 0.998554 -0.304649 0.686714 -0.726083 -0.035033 53.5442 0.725056 0.687607 -0.0386063 517.042 0.0521203 0.00111057 0.99864 -0.331525 0.693784 -0.719407 -0.0334356 52.5214 0.718643 0.694588 -0.0331635 516.002 0.0470821 -0.00101994 0.998891 -0.359759 0.701157 -0.711948 -0.0388559 51.481 0.711399 0.702194 -0.0289033 514.973 0.047862 -0.00737626 0.998827 -0.395733 0.708739 -0.704093 -0.0440672 50.4272 0.703574 0.710031 -0.0289771 513.97 0.0516917 -0.0104674 0.998608 -0.419748 0.717466 -0.695 -0.0470877 49.3801 0.694674 0.718867 -0.0256609 512.986 0.0516841 -0.0142998 0.998561 -0.463816 0.725255 -0.686522 -0.0518875 48.3266 0.686248 0.726912 -0.0257524 512.012 0.0553972 -0.0169306 0.998321 -0.496731 0.733555 -0.677602 -0.05246 47.274 0.677079 0.735303 -0.0299022 511.04 0.0588358 -0.0135846 0.998175 -0.535746 0.742623 -0.667541 -0.0538485 46.2429 0.666888 0.744473 -0.0319486 510.093 0.0614158 -0.0121851 0.998038 -0.575964 0.752727 -0.65626 -0.0522022 45.2023 0.655516 0.754473 -0.0326931 509.215 0.0608403 -0.0096104 0.998101 -0.613893 0.762832 -0.643469 -0.0635221 44.1601 0.643132 0.76523 -0.0283472 508.373 0.0668496 -0.019229 0.997578 -0.663466 0.773343 -0.631449 -0.0566875 43.142 0.630658 0.775352 -0.0331688 507.549 0.0648971 -0.0100995 0.997841 -0.688825 0.783526 -0.618659 -0.0578577 42.1339 0.618448 0.78547 -0.0236408 506.78 0.0600711 -0.0172588 0.998045 -0.751954 0.792807 -0.604815 -0.0752096 41.1155 0.605496 0.795688 -0.0159818 506.062 0.0695094 -0.0328687 0.99704 -0.788106 0.802346 -0.592963 -0.0680837 40.1389 0.59137 0.805213 -0.0437421 505.301 0.0807594 -0.0051663 0.99672 -0.820877 0.813129 -0.579261 -0.0572524 39.1803 0.577201 0.815111 -0.0493266 504.625 0.0752401 0.00706271 0.99714 -0.913858 0.826046 -0.561387 -0.0499227 38.21 0.560589 0.827548 -0.0300895 504.018 0.0582052 -0.00313077 0.9983 -0.912673 0.842001 -0.537716 -0.0435452 37.2272 0.536301 0.843058 -0.0404265 503.374 0.0584491 0.0106858 0.998233 -0.98241 0.856656 -0.515127 -0.0279972 36.2392 0.514312 0.85702 -0.03163 502.808 0.0402877 0.0126967 0.999107 -0.984586 0.875129 -0.483881 -0.00276103 35.237 0.483738 0.874985 -0.0199713 502.295 0.0120796 0.0161419 0.999797 -0.989062 0.892005 -0.451752 -0.0157075 34.2031 0.451534 0.892118 -0.0155967 501.822 0.0210588 0.00681989 0.999755 -1.0122 0.909079 -0.415373 -0.0322578 33.1353 0.414428 0.909512 -0.0322059 501.362 0.0427163 0.0159091 0.998961 -0.99559 0.926233 -0.376196 -0.0238408 32.0687 0.375152 0.926139 -0.0390922 500.94 0.0367862 0.0272646 0.998951 -1.01832 0.941775 -0.335778 -0.0177191 30.9817 0.335113 0.94162 -0.0324224 500.61 0.0275714 0.0245967 0.999317 -1.04152 0.955871 -0.293138 -0.0195227 29.8596 0.292437 0.955738 -0.0323324 500.308 0.0281364 0.0251965 0.999286 -1.02881 0.967855 -0.250772 -0.0192255 28.7173 0.249921 0.967509 -0.0382985 500.037 0.028205 0.0322626 0.999081 -1.04568 0.977899 -0.208225 -0.0188883 27.5528 0.20754 0.977672 -0.0329478 499.845 0.0253271 0.0282996 0.999279 -1.06402 0.98597 -0.165521 -0.0215866 26.3653 0.164814 0.98583 -0.0312347 499.692 0.0264507 0.0272387 0.999279 -1.06257 0.99126 -0.129252 -0.02641 25.1547 0.128381 0.991197 -0.032359 499.57 0.03036 0.0286857 0.999127 -1.07591 0.994567 -0.0999836 -0.0289847 23.916 0.0990822 0.994595 -0.0310285 499.471 0.0319304 0.027988 0.999098 -1.09377 0.996768 -0.0751305 -0.0284577 22.6641 0.0742446 0.996758 -0.0310059 499.398 0.0306949 0.0287928 0.999114 -1.10559 0.998253 -0.0511678 -0.0295471 21.4145 0.0501809 0.998187 -0.0332289 499.345 0.0311938 0.0316882 0.999011 -1.1215 0.999146 -0.0299636 -0.0284543 20.1584 0.0290657 0.999082 -0.0314609 499.335 0.0293709 0.030607 0.9991 -1.14259 0.999557 -0.0117993 -0.0273403 18.8731 0.0109391 0.999447 -0.031402 499.321 0.0276957 0.031089 0.999133 -1.14569 0.999573 1.56447e-05 -0.0292174 17.5626 -0.000840857 0.999601 -0.0282318 499.341 0.0292053 0.0282443 0.999174 -1.17337 0.999418 0.00737013 -0.0333216 16.2492 -0.0082 0.999658 -0.0248372 499.362 0.0331271 0.025096 0.999136 -1.15908 0.999484 0.0142769 -0.0287733 14.9351 -0.0150881 0.999489 -0.0281778 499.373 0.0283563 0.0285974 0.999189 -1.17024 0.999449 0.0195687 -0.0267994 13.627 -0.0202476 0.999475 -0.0253021 499.413 0.0262902 0.0258307 0.999321 -1.19128 0.999267 0.0244168 -0.0294951 12.3066 -0.0251511 0.999376 -0.0247866 499.456 0.0288715 0.0255102 0.999258 -1.18342 0.999185 0.0295021 -0.0275514 10.9984 -0.0302624 0.999161 -0.0275991 499.486 0.026714 0.0284104 0.999239 -1.19389 0.999092 0.0315011 -0.0286709 9.66835 -0.0322525 0.999138 -0.0261329 499.543 0.027823 0.0270339 0.999247 -1.19796 0.998976 0.0326619 -0.0313085 8.36179 -0.033588 0.999 -0.0295264 499.581 0.0303128 0.0305478 0.999074 -1.18999 0.999058 0.0317918 -0.0295392 7.07421 -0.0327577 0.998925 -0.03281 499.613 0.0284644 0.0337468 0.999025 -1.20187 0.999179 0.0281692 -0.0291079 5.80292 -0.0289997 0.999173 -0.0285149 499.65 0.0282806 0.0293357 0.999169 -1.21653 0.999262 0.023896 -0.030088 4.52824 -0.0248131 0.999227 -0.0304883 499.679 0.0293362 0.0312124 0.999082 -1.20494 0.999471 0.0187397 -0.0265902 3.27104 -0.0195178 0.99938 -0.0293108 499.688 0.0260245 0.0298143 0.999217 -1.21703 0.999602 0.0135588 -0.0247208 2.02716 -0.0141976 0.999565 -0.0258499 499.714 0.0243596 0.0261906 0.99936 -1.21864 0.999625 0.00859951 -0.0259988 0.785089 -0.009288 0.999606 -0.0264781 499.713 0.0257608 0.0267097 0.999311 -1.21223 0.99963 0.0015987 -0.0271512 -0.426117 -0.00225773 0.999703 -0.024259 499.72 0.0271044 0.0243113 0.999337 -1.21684 0.999628 -0.00713255 -0.0263319 -1.63018 0.00654095 0.999726 -0.0224851 499.701 0.0264851 0.0223045 0.9994 -1.20838 0.999558 -0.0165897 -0.0246811 -2.83176 0.0160167 0.999602 -0.0232332 499.671 0.0250567 0.0228276 0.999425 -1.21598 0.999311 -0.0269506 -0.0255263 -4.00742 0.0264761 0.999474 -0.0187492 499.641 0.0260182 0.0180604 0.999498 -1.21686 0.99882 -0.0413837 -0.0254018 -5.18537 0.0409281 0.998996 -0.0181992 499.574 0.0261294 0.0171381 0.999512 -1.20345 0.997665 -0.0636522 -0.0247519 -6.35197 0.0633426 0.997906 -0.0131015 499.47 0.025534 0.0115031 0.999608 -1.21874 0.995368 -0.0917287 -0.0287704 -7.49982 0.0913821 0.995729 -0.0131414 499.346 0.0298529 0.0104515 0.9995 -1.21733 0.991978 -0.122402 -0.0315907 -8.64119 0.122098 0.992453 -0.0113927 499.169 0.0327468 0.00744416 0.999436 -1.23089 0.986956 -0.157965 -0.0310662 -9.76287 0.157828 0.987443 -0.00685153 498.963 0.0317584 0.00185906 0.999494 -1.23002 0.979561 -0.19891 -0.0299248 -10.8414 0.198785 0.980017 -0.00713828 498.699 0.0307467 0.00104379 0.999527 -1.22486 0.968944 -0.245489 -0.0297247 -11.9119 0.245249 0.969392 -0.0115004 498.352 0.0316381 0.00385332 0.999492 -1.23986 0.953639 -0.299565 -0.0288604 -12.963 0.299366 0.954074 -0.0110769 497.953 0.0308532 0.00192353 0.999522 -1.25621 0.932614 -0.359172 -0.0350075 -13.9581 0.358932 0.933271 -0.0131292 497.498 0.0373872 -0.000320853 0.999301 -1.26605 0.905502 -0.422636 -0.0380277 -14.9388 0.422371 0.906297 -0.0151487 496.974 0.0408668 -0.00234461 0.999162 -1.29353 0.871622 -0.487993 -0.046234 -15.8539 0.487826 0.872802 -0.0155977 496.389 0.0479646 -0.0089588 0.998809 -1.28693 0.810382 -0.58507 -0.0312315 -17.1118 0.584391 0.810968 -0.0285977 495.338 0.0420594 0.00492366 0.999103 -1.35227 0.75974 -0.649153 -0.0373601 -17.9016 0.649019 0.760576 -0.0172479 494.625 0.0396117 -0.0111435 0.999153 -1.32951 0.677832 -0.734823 -0.0240348 -18.9463 0.734162 0.678249 -0.0313799 493.307 0.0393602 0.00362485 0.999219 -1.38517 0.615894 -0.787367 -0.0269591 -19.5749 0.786928 0.616466 -0.0267205 492.416 0.0376582 -0.00475784 0.999279 -1.37463 0.519289 -0.854199 -0.0261456 -20.3758 0.853665 0.519908 -0.0308448 490.937 0.0399409 -0.00630219 0.999182 -1.42395 0.459064 -0.88821 -0.0185257 -20.8085 0.88751 0.459437 -0.0352528 489.937 0.0398233 -0.000258418 0.999207 -1.40022 0.409772 -0.912107 -0.0121515 -21.1886 0.911436 0.409939 -0.035145 488.903 0.0370373 0.00332613 0.999308 -1.46644 0.366364 -0.930285 -0.0186217 -21.587 0.929623 0.366811 -0.03536 487.827 0.0397255 -0.0043565 0.999201 -1.49078 0.324567 -0.945674 -0.0188999 -21.9337 0.945011 0.325058 -0.0359345 486.728 0.0401259 -0.00619742 0.999175 -1.47769 0.264406 -0.964252 -0.0175471 -22.3524 0.963512 0.264902 -0.0383571 485.012 0.0416342 -0.00676503 0.99911 -1.55067 0.224699 -0.974217 -0.0202715 -22.5962 0.973194 0.225412 -0.0456299 483.84 0.0490229 -0.00947512 0.998753 -1.5698 0.187422 -0.981992 -0.02377 -22.7971 0.981481 0.188191 -0.0357802 482.644 0.0396091 -0.0166238 0.999077 -1.59634 0.130345 -0.991215 -0.0224214 -23.0144 0.990969 0.130964 -0.0288057 480.821 0.031489 -0.0184643 0.999334 -1.59099 0.0992675 -0.994806 -0.0225274 -23.1191 0.994074 0.100152 -0.0422616 479.564 0.0442982 -0.0181987 0.998853 -1.63593 0.0750571 -0.996871 -0.0248083 -23.1911 0.996151 0.0760858 -0.0435103 478.315 0.0452617 -0.0214471 0.998745 -1.65248 0.0456118 -0.998724 -0.0216952 -23.2647 0.998177 0.0464248 -0.0385769 476.402 0.0395349 -0.0198961 0.99902 -1.67609 0.0206813 -0.999559 -0.0213086 -23.2973 0.998842 0.0215831 -0.0430006 474.433 0.0434416 -0.0203946 0.998848 -1.72009 0.00755424 -0.999765 -0.0203088 -23.2942 0.99885 0.008506 -0.0471945 473.101 0.0473561 -0.0199289 0.998679 -1.74533 -0.00178469 -0.999793 -0.0202774 -23.2855 0.998765 -0.000775151 -0.0496856 471.758 0.0496596 -0.0203411 0.998559 -1.78609 -0.00702993 -0.999796 -0.0189415 -23.2687 0.99873 -0.00607482 -0.0500184 470.421 0.0498931 -0.0192691 0.998569 -1.81774 -0.00952215 -0.9998 -0.0176019 -23.2689 0.998784 -0.0086582 -0.0485239 468.417 0.0483617 -0.0180425 0.998667 -1.85277 -0.0101065 -0.999792 -0.0177423 -23.2557 0.998858 -0.00926525 -0.0468708 467.071 0.0466966 -0.0181957 0.998743 -1.8825 -0.0109412 -0.999756 -0.0191833 -23.2451 0.998794 -0.0100082 -0.0480748 465.737 0.0478711 -0.0196862 0.99866 -1.90958 -0.00874213 -0.999775 -0.019325 -23.234 0.998828 -0.00781069 -0.0477592 463.765 0.0475975 -0.0197199 0.998672 -1.94393 -0.00537923 -0.999747 -0.0218462 -23.2414 0.99878 -0.00429912 -0.049191 462.448 0.0490847 -0.0220841 0.99855 -1.97553 -0.00190913 -0.999719 -0.0236363 -23.2536 0.998736 -0.000719128 -0.0502527 461.145 0.0502216 -0.0237024 0.998457 -1.98942 0.00172388 -0.999686 -0.0250179 -23.2601 0.998764 0.00296418 -0.0496243 459.843 0.0496829 -0.0249014 0.998455 -2.02774 0.00584129 -0.999671 -0.0249811 -23.2865 0.998344 0.00725944 -0.0570608 457.96 0.0572234 -0.0246064 0.998058 -2.06868 0.00845203 -0.999647 -0.0251862 -23.302 0.998298 0.00988858 -0.0574701 456.795 0.0576988 -0.0246576 0.998029 -2.08494 0.0110362 -0.999627 -0.0249792 -23.307 0.998383 0.0124086 -0.0554683 455.677 0.0557575 -0.0243266 0.998148 -2.12115 0.0126544 -0.999644 -0.0234851 -23.3205 0.998286 0.0139723 -0.056831 454.649 0.0571389 -0.0227256 0.998108 -2.14966 0.0129264 -0.999633 -0.0238238 -23.3475 0.998149 0.014316 -0.0591128 453.227 0.0594321 -0.0230156 0.997967 -2.17715 0.0127191 -0.999736 -0.0191296 -23.3622 0.998168 0.0138263 -0.058906 451.94 0.0591549 -0.0183453 0.99808 -2.21573 0.011647 -0.999727 -0.0202509 -23.3803 0.998527 0.0127014 -0.0527401 450.736 0.0529829 -0.0196069 0.998403 -2.24213 0.0105561 -0.999811 -0.0163153 -23.376 0.998476 0.011423 -0.0539877 449.528 0.0541638 -0.0157205 0.998408 -2.28004 0.00842234 -0.999888 -0.0123402 -23.3819 0.998518 0.00907295 -0.0536517 448.336 0.0537577 -0.0118701 0.998483 -2.31229 0.00467598 -0.999902 -0.0132 -23.3984 0.998459 0.00539835 -0.0552307 447.129 0.0552966 -0.0129214 0.998386 -2.33519 -0.000535566 -0.999948 -0.0102004 -23.3749 0.998438 3.51719e-05 -0.0558702 445.945 0.0558677 -0.0102144 0.998386 -2.38228 -0.00618767 -0.999932 -0.00987915 -23.3647 0.998426 -0.00562703 -0.0558026 444.761 0.0557432 -0.0102089 0.998393 -2.41259 -0.0122362 -0.999897 -0.00752122 -23.3431 0.998346 -0.011794 -0.05626 443.577 0.0561655 -0.00819719 0.998388 -2.44436 -0.0198243 -0.999781 -0.00664335 -23.3102 0.998066 -0.0193979 -0.0590571 442.43 0.0589153 -0.00780127 0.998232 -2.48788 -0.0258572 -0.999661 -0.0030059 -23.2748 0.997832 -0.0256276 -0.0606126 441.422 0.0605151 -0.00456666 0.998157 -2.49931 -0.0326385 -0.999467 -0.00101593 -23.2269 0.997496 -0.0325104 -0.0628022 440.262 0.0627357 -0.00306316 0.998025 -2.53942 -0.0404543 -0.999177 0.00286654 -23.1708 0.997653 -0.0405508 -0.0551762 439.171 0.055247 0.000627704 0.998473 -2.57014 -0.0578387 -0.998325 -0.00120453 -23.083 0.996668 -0.0576731 -0.0576758 437.851 0.0575097 -0.00453641 0.998335 -2.6099 -0.0976305 -0.995222 -0.00133708 -22.9047 0.993961 -0.0974391 -0.0504636 436.64 0.0500922 -0.0062558 0.998725 -2.63368 -0.178875 -0.983872 -0.000456519 -22.6105 0.983379 -0.178771 -0.0317185 435.615 0.0311253 -0.00612257 0.999497 -2.66327 -0.3188 -0.947794 -0.00729284 -22.0423 0.947723 -0.318646 -0.0168786 434.319 0.0136736 -0.0122925 0.999831 -2.6456 -0.449987 -0.893001 0.007816 -21.3408 0.892951 -0.450048 -0.00980162 433.312 0.0122704 0.0025687 0.999921 -2.64529 -0.587622 -0.809131 0.00273149 -20.497 0.808937 -0.58755 -0.0201447 432.353 0.0179046 -0.00962788 0.999793 -2.64513 -0.714343 -0.699787 0.00345885 -19.4164 0.699524 -0.714194 -0.0243303 431.505 0.0194964 -0.0149607 0.999698 -2.65464 -0.795659 -0.60561 0.0128072 -18.4707 0.605104 -0.795611 -0.0291906 430.964 0.0278677 -0.0154761 0.999492 -2.63465 -0.880167 -0.474379 0.0164741 -17.0644 0.473374 -0.879802 -0.0431855 430.326 0.0349802 -0.030212 0.998931 -2.70491 -0.938202 -0.345976 0.00882784 -15.5292 0.345425 -0.937678 -0.0379628 429.907 0.0214119 -0.0325675 0.99924 -2.63504 -0.965378 -0.260767 0.00681087 -14.2528 0.260293 -0.964682 -0.0404416 429.637 0.0171161 -0.0372686 0.999159 -2.66427 -0.980374 -0.195894 0.0221738 -12.8966 0.195 -0.980101 -0.0371108 429.444 0.0290024 -0.0320586 0.999065 -2.65662 -0.989192 -0.146126 0.0121412 -11.5036 0.145537 -0.98854 -0.0400959 429.271 0.0178611 -0.0378955 0.999122 -2.62693 -0.99442 -0.103973 0.0178504 -9.50503 0.103334 -0.994079 -0.0336041 429.113 0.0212386 -0.031572 0.999276 -2.64246 -0.995963 -0.0897633 0.000396875 -8.51011 0.0896736 -0.995146 -0.0405251 429.025 0.00403262 -0.0403259 0.999178 -2.57744 -0.996724 -0.0800502 -0.0115314 -7.46337 0.0803846 -0.996242 -0.0322526 428.954 -0.00890627 -0.0330739 0.999413 -2.56614 -0.996813 -0.0787387 0.0128471 -6.34036 0.0784017 -0.996611 -0.0249046 428.881 0.0147645 -0.023818 0.999607 -2.60492 -0.995433 -0.0784189 0.0544305 -5.20606 0.0762164 -0.99623 -0.0414277 428.76 0.057474 -0.03709 0.997658 -2.60193 -0.996145 -0.0800884 0.0357769 -4.10942 0.0788365 -0.996268 -0.0351323 428.688 0.0384571 -0.0321764 0.998742 -2.55401 -0.996638 -0.0819018 0.0021053 -2.95801 0.081802 -0.996196 -0.0300483 428.597 0.0045583 -0.0297751 0.999546 -2.58523 -0.996147 -0.0865807 0.0139608 -1.17872 0.0860891 -0.995753 -0.0326341 428.422 0.016727 -0.0313065 0.99937 -2.58129 -0.995782 -0.0901711 0.0169605 0.0259342 0.0895953 -0.995461 -0.0320976 428.313 0.0197778 -0.0304426 0.999341 -2.57399 -0.99533 -0.0954743 0.0142483 1.25816 0.09495 -0.994908 -0.0337994 428.176 0.0174027 -0.0322887 0.999327 -2.56764 -0.994586 -0.102537 0.016898 2.51414 0.101901 -0.994178 -0.0350061 428.039 0.0203891 -0.0330947 0.999244 -2.56454 -0.993582 -0.111501 0.019014 3.78871 0.110758 -0.993178 -0.0364786 427.88 0.0229517 -0.0341385 0.999154 -2.55654 -0.992121 -0.124087 0.0172813 5.09294 0.123252 -0.99144 -0.0430794 427.704 0.022479 -0.04061 0.998922 -2.56555 -0.990419 -0.137023 0.0171984 6.38614 0.136195 -0.98977 -0.04249 427.503 0.0228446 -0.0397405 0.998949 -2.55047 -0.988304 -0.151619 0.0163572 7.74828 0.150752 -0.987534 -0.0452932 427.275 0.0230206 -0.0422975 0.99884 -2.55433 -0.985572 -0.168407 0.0169346 9.11451 0.167299 -0.984459 -0.0533989 427.003 0.0256641 -0.0497953 0.99843 -2.54113 -0.982602 -0.185301 0.0125508 10.5033 0.184513 -0.981664 -0.0478595 426.749 0.0211891 -0.044711 0.998775 -2.54765 -0.979053 -0.203356 0.0101039 11.8936 0.202684 -0.978135 -0.0465965 426.434 0.0193586 -0.0435726 0.998863 -2.56998 -0.974564 -0.223738 0.0129067 13.3138 0.222874 -0.973621 -0.0488857 426.104 0.0235039 -0.0447656 0.998721 -2.56679 -0.969473 -0.244911 0.0118433 14.7197 0.244297 -0.968922 -0.0388421 425.716 0.0209881 -0.0347631 0.999175 -2.53286 -0.964231 -0.264897 0.00936171 16.1666 0.264277 -0.963492 -0.0428957 425.305 0.0203829 -0.0388873 0.999036 -2.54318 -0.958661 -0.284234 0.0134297 17.6078 0.283414 -0.957982 -0.0441255 424.847 0.0254074 -0.0384952 0.998936 -2.53891 -0.952875 -0.30305 0.0137886 19.0424 0.302207 -0.952226 -0.0440143 424.373 0.0264685 -0.0377731 0.998936 -2.5379 -0.94726 -0.320316 0.00979799 20.4409 0.319614 -0.946529 -0.0439358 423.867 0.0233474 -0.0384871 0.998986 -2.53065 -0.941895 -0.335845 0.00642312 21.835 0.335325 -0.941218 -0.0408237 423.339 0.019756 -0.0362978 0.999146 -2.51151 -0.936509 -0.350608 0.0050535 23.2136 0.350168 -0.93589 -0.0386283 422.799 0.0182729 -0.0344062 0.999241 -2.50758 -0.930806 -0.365504 0.00254188 24.5946 0.365164 -0.930199 -0.0372181 422.244 0.0159678 -0.0337146 0.999304 -2.49057 -0.924933 -0.380126 0.00196856 25.9748 0.379861 -0.924459 -0.0328911 421.663 0.0143226 -0.0296743 0.999457 -2.47894 -0.918801 -0.394684 0.00537004 27.3553 0.394345 -0.918435 -0.0311337 421.056 0.01722 -0.026488 0.999501 -2.47708 -0.91201 -0.410121 0.00618051 28.7308 0.409781 -0.911701 -0.0296758 420.421 0.0178055 -0.024532 0.99954 -2.46044 -0.904733 -0.425941 0.00571963 30.1082 0.425586 -0.904394 -0.0307856 419.747 0.0182856 -0.0254186 0.99951 -2.45224 -0.896697 -0.442599 0.00644864 31.4922 0.442205 -0.896358 -0.0315689 419.053 0.0197526 -0.0254561 0.999481 -2.44391 -0.887182 -0.46137 0.00680524 32.8695 0.460935 -0.886829 -0.0327556 418.328 0.0211475 -0.0259234 0.99944 -2.44626 -0.87741 -0.47969 0.00706869 34.2459 0.4792 -0.877024 -0.0345926 417.546 0.0227931 -0.0269645 0.999376 -2.43622 -0.866889 -0.498479 0.0046687 35.6129 0.498068 -0.866489 -0.033527 416.735 0.0207579 -0.0267389 0.999427 -2.43107 -0.856047 -0.516877 0.00473498 36.9812 0.516464 -0.855667 -0.0331416 415.894 0.0211817 -0.0259253 0.999439 -2.43309 -0.844601 -0.535374 0.0048384 38.3395 0.53496 -0.844244 -0.0327206 415.015 0.0216026 -0.0250475 0.999453 -2.4261 -0.833425 -0.552617 0.00422953 39.702 0.552213 -0.833066 -0.0325875 414.102 0.0215319 -0.0248236 0.99946 -2.4212 -0.817441 -0.576011 0.0015247 41.7171 0.575628 -0.816989 -0.034375 412.668 0.021046 -0.0272218 0.999408 -2.42016 -0.805949 -0.591977 0.00319811 43.0457 0.591509 -0.805505 -0.0357665 411.679 0.023749 -0.0269342 0.999355 -2.42404 -0.793516 -0.608532 0.00468294 44.3748 0.607903 -0.793004 -0.0399745 410.632 0.0280394 -0.0288736 0.99919 -2.42741 -0.780172 -0.625559 0.00276005 45.6867 0.624758 -0.779383 -0.0473279 409.556 0.0317575 -0.0351995 0.998876 -2.45061 -0.762761 -0.646667 0.0042294 47.6403 0.645895 -0.76214 -0.0443058 407.886 0.0318745 -0.031063 0.999009 -2.4526 -0.75144 -0.659796 0.00247704 48.9435 0.659515 -0.75122 -0.0266036 406.747 0.0194137 -0.0183573 0.999643 -2.42577 -0.732535 -0.680617 0.0123869 50.8857 0.680661 -0.732598 -0.000868394 404.997 0.00966565 0.00779515 0.999923 -2.43182 -0.716718 -0.697236 0.0133099 52.1347 0.697172 -0.716838 -0.00970006 403.76 0.0163043 0.00232709 0.999864 -2.40959 -0.694989 -0.718756 0.0195148 53.9864 0.718215 -0.695239 -0.0284689 401.818 0.0340296 -0.00576974 0.999404 -2.41461 -0.678921 -0.734041 0.0158078 55.1865 0.733505 -0.679056 -0.0292134 400.509 0.0321782 -0.00823847 0.999448 -2.42528 -0.663037 -0.748472 0.0131387 56.368 0.747918 -0.663084 -0.0306311 399.15 0.0316385 -0.0104829 0.999444 -2.45188 -0.64649 -0.762877 0.00829194 57.5263 0.762416 -0.646419 -0.0294092 397.769 0.0277957 -0.0126908 0.999533 -2.4552 -0.629622 -0.776862 0.00787152 58.6686 0.776434 -0.629563 -0.0283072 396.34 0.0269464 -0.0117111 0.999568 -2.46119 -0.612509 -0.790411 0.00914697 59.7821 0.789969 -0.612493 -0.0282973 394.881 0.0279689 -0.0101065 0.999558 -2.47002 -0.587496 -0.809205 0.00590769 61.4184 0.808799 -0.587408 -0.0282146 392.638 0.0263016 -0.0117978 0.999584 -2.48215 -0.571042 -0.820917 0.00268943 62.4815 0.820536 -0.570871 -0.0287448 391.102 0.0251324 -0.0142077 0.999583 -2.48783 -0.548287 -0.836289 0.00136918 64.0243 0.835788 -0.548014 -0.0337529 388.738 0.0289776 -0.0173619 0.999429 -2.48619 -0.533092 -0.846053 0.00252842 65.0247 0.845573 -0.532886 -0.0322331 387.135 0.0286183 -0.0150453 0.999477 -2.49352 -0.517968 -0.855393 0.00347025 66.0005 0.854968 -0.517832 -0.0296719 385.517 0.0271781 -0.0124022 0.999554 -2.50657 -0.502478 -0.864553 0.00798642 66.9554 0.864019 -0.50246 -0.0317033 383.87 0.0314221 -0.00902978 0.999465 -2.52529 -0.486561 -0.873555 0.0126246 67.8711 0.873006 -0.486706 -0.0312761 382.213 0.0334659 -0.00419633 0.999431 -2.5258 -0.468169 -0.883519 0.0145706 68.7688 0.883045 -0.468395 -0.0289258 380.518 0.0323813 -0.000675692 0.999475 -2.54604 -0.449238 -0.893326 0.0124063 69.6137 0.892951 -0.449409 -0.0258896 378.816 0.0287033 -0.00055242 0.999588 -2.55902 -0.429418 -0.903066 0.00841505 70.4352 0.902796 -0.429497 -0.0222031 377.073 0.0236651 -0.00193737 0.999718 -2.56238 -0.407091 -0.913376 0.00456897 71.2093 0.913141 -0.407092 -0.0212127 375.314 0.0212352 -0.00446339 0.999765 -2.56962 -0.384393 -0.92317 0.000228239 71.9297 0.922851 -0.384267 -0.0261581 373.518 0.024236 -0.00984434 0.999658 -2.56782 -0.361642 -0.932312 -0.00310156 72.6195 0.932004 -0.361432 -0.0271041 371.706 0.0241485 -0.0126927 0.999628 -2.56251 -0.338923 -0.940803 -0.00457534 73.2685 0.940547 -0.338707 -0.0254608 369.863 0.0224039 -0.0129326 0.999665 -2.55433 -0.315774 -0.948765 -0.0115084 73.8552 0.948646 -0.315446 -0.0237689 368.032 0.0189208 -0.018423 0.999651 -2.56101 -0.292199 -0.956188 -0.0179848 74.4075 0.956148 -0.291689 -0.0264418 366.158 0.0200374 -0.0249224 0.999489 -2.54948 -0.272108 -0.962057 -0.0200737 74.9288 0.962049 -0.271543 -0.0269619 364.265 0.020488 -0.0266484 0.999435 -2.53238 -0.251036 -0.967843 -0.0161576 75.4208 0.967692 -0.250521 -0.0284913 362.374 0.0235273 -0.0227879 0.999463 -2.52835 -0.227874 -0.973607 -0.0127227 75.8713 0.97337 -0.227444 -0.028643 360.467 0.0249934 -0.0189109 0.999509 -2.52537 -0.204444 -0.9788 -0.0123732 76.2613 0.978581 -0.204054 -0.0272116 358.537 0.0241099 -0.0176715 0.999553 -2.52837 -0.180988 -0.983428 -0.0106119 76.6092 0.983198 -0.180664 -0.026134 356.598 0.0237837 -0.0151635 0.999602 -2.52478 -0.1571 -0.987549 -0.00811064 76.9234 0.987297 -0.156852 -0.025329 354.642 0.0237415 -0.0119868 0.999646 -2.52577 -0.132931 -0.991097 -0.00745504 77.1793 0.990863 -0.132719 -0.0239991 352.679 0.022796 -0.0105771 0.999684 -2.52852 -0.121172 -0.992602 -0.00766914 77.2836 0.992392 -0.120969 -0.0229047 351.683 0.0218076 -0.0103862 0.999708 -2.5248 -0.109484 -0.993965 -0.00681863 77.3863 0.99374 -0.1093 -0.0231267 350.681 0.0222419 -0.00930793 0.999709 -2.5254 -0.0801542 -0.996749 -0.00815366 77.6208 0.996449 -0.0799134 -0.026501 347.694 0.0257633 -0.0102489 0.999616 -2.53207 -0.071563 -0.997397 -0.00881629 77.6835 0.997089 -0.0713023 -0.0270007 346.694 0.0263018 -0.0107229 0.999597 -2.5318 -0.0637212 -0.997921 -0.00962069 77.7356 0.997646 -0.0634528 -0.0260138 345.683 0.0253493 -0.0112557 0.999615 -2.53315 -0.0478702 -0.998769 -0.0129838 77.8205 0.998565 -0.0475399 -0.0246557 343.685 0.0240081 -0.0141454 0.999612 -2.53759 -0.0405807 -0.999057 -0.0154239 77.8637 0.998871 -0.0401821 -0.0253296 342.676 0.0246859 -0.0164344 0.99956 -2.54184 -0.0340712 -0.999272 -0.0171392 77.8761 0.999072 -0.0336025 -0.0269293 341.662 0.0263337 -0.0180409 0.99949 -2.54269 -0.0268474 -0.999447 -0.0196206 77.8848 0.999264 -0.0262942 -0.0279252 340.654 0.0273939 -0.0203559 0.999417 -2.54688 -0.0206387 -0.999544 -0.0220614 77.8869 0.999431 -0.0200377 -0.0271255 339.652 0.026671 -0.0226087 0.999389 -2.5506 -0.0164182 -0.999523 -0.0261512 77.8942 0.999548 -0.0157482 -0.0256233 338.619 0.0251992 -0.02656 0.99933 -2.55862 -0.0154717 -0.999465 -0.0288287 77.9024 0.99961 -0.0147909 -0.0236838 337.593 0.0232447 -0.0291839 0.999304 -2.54716 -0.0160315 -0.999442 -0.0293184 77.9099 0.999636 -0.0153846 -0.022157 336.575 0.0216936 -0.0296629 0.999325 -2.54889 -0.015007 -0.999478 -0.0286037 77.9237 0.999634 -0.0143531 -0.0229282 335.557 0.0225056 -0.0289373 0.999328 -2.54075 -0.0137843 -0.99957 -0.0258692 77.9321 0.999629 -0.0131683 -0.0238342 334.534 0.0234833 -0.0261882 0.999381 -2.54097 -0.0124689 -0.999641 -0.0237056 77.9413 0.999663 -0.0119226 -0.0230483 333.511 0.0227574 -0.023985 0.999453 -2.53671 -0.0107725 -0.999685 -0.0226591 77.9564 0.999725 -0.0102957 -0.0210554 332.49 0.0208155 -0.0228797 0.999522 -2.53389 -0.00983103 -0.9997 -0.0224498 77.9548 0.999753 -0.00937896 -0.0201542 331.463 0.0199376 -0.0226424 0.999545 -2.5311 -0.00852838 -0.999732 -0.021506 77.9673 0.999741 -0.008071 -0.0212653 330.432 0.0210861 -0.0216818 0.999543 -2.52875 -0.007195 -0.999768 -0.0202926 77.9654 0.999704 -0.0067202 -0.0233697 329.399 0.0232279 -0.0204547 0.999521 -2.5234 -0.00565489 -0.99979 -0.019677 77.9664 0.999696 -0.00517997 -0.0241037 328.364 0.0239967 -0.0198074 0.999516 -2.52937 -0.00423743 -0.999824 -0.0182669 77.9691 0.999692 -0.00378902 -0.024513 327.334 0.0244395 -0.0183652 0.999533 -2.51819 -0.00308647 -0.999877 -0.0153849 77.9722 0.99971 -0.00271747 -0.0239479 326.304 0.0239032 -0.0154543 0.999595 -2.51948 -0.00177117 -0.999895 -0.0143536 77.9688 0.999757 -0.0014552 -0.0219941 325.26 0.0219709 -0.0143891 0.999655 -2.51989 -0.000770487 -0.999915 -0.012992 77.9715 0.999785 -0.000501159 -0.0207208 324.225 0.0207125 -0.0130052 0.999701 -2.51611 0.000551288 -0.999938 -0.0110945 77.9679 0.999768 0.000789916 -0.0215158 323.19 0.0215233 -0.0110801 0.999707 -2.51298 0.00189445 -0.999936 -0.0111472 77.9555 0.999746 0.00214415 -0.0224314 322.149 0.0224539 -0.0111019 0.999686 -2.52332 0.00330443 -0.999924 -0.011855 77.9518 0.999726 0.0035779 -0.0231213 321.103 0.023162 -0.0117753 0.999662 -2.52295 0.00491267 -0.999923 -0.0113579 77.9428 0.999718 0.00517505 -0.0231877 320.058 0.0232447 -0.0112408 0.999667 -2.51317 0.00597444 -0.99993 -0.0102345 77.9353 0.999714 0.00620954 -0.0230959 319.018 0.0231578 -0.0100936 0.999681 -2.51218 0.00685998 -0.999936 -0.00903342 77.9237 0.99966 0.00708491 -0.0251069 317.975 0.0251692 -0.00885811 0.999644 -2.52139 0.00819379 -0.99994 -0.00728232 77.9109 0.999535 0.00840388 -0.029302 316.924 0.0293615 -0.00703884 0.999544 -2.51864 0.00936092 -0.999935 -0.00647068 77.8974 0.999448 0.00956219 -0.0318067 315.873 0.0318666 -0.00616937 0.999473 -2.53255 0.0108795 -0.999918 -0.006737 77.8817 0.999435 0.0110881 -0.0317396 314.818 0.0318117 -0.00638788 0.999473 -2.55392 0.0121971 -0.999887 -0.008819 77.8668 0.999477 0.0124554 -0.0298524 313.768 0.0299589 -0.00845027 0.999515 -2.55871 0.0130645 -0.999862 -0.0102371 77.8397 0.999482 0.0133593 -0.0292774 312.714 0.0294102 -0.00984926 0.999519 -2.56884 0.0141428 -0.999828 -0.0119848 77.8177 0.999374 0.0145231 -0.0322675 311.655 0.032436 -0.011521 0.999407 -2.57359 0.0150343 -0.999805 -0.0127651 77.7851 0.999363 0.0154385 -0.0321749 310.59 0.0323657 -0.0122732 0.999401 -2.57813 0.014588 -0.999787 -0.0146131 77.7691 0.999528 0.0149762 -0.026819 309.532 0.0270321 -0.0142149 0.999533 -2.57513 0.0128707 -0.99981 -0.0146336 77.7548 0.99967 0.0131914 -0.0220325 308.477 0.0222213 -0.0143452 0.99965 -2.57055 0.0114738 -0.999844 -0.0134509 77.7424 0.999729 0.0117429 -0.020108 307.41 0.0202628 -0.0132166 0.999707 -2.56682 0.00859172 -0.999899 -0.0113323 77.7271 0.999694 0.0088519 -0.0231128 305.284 0.0232108 -0.0111302 0.999669 -2.56198 0.00726482 -0.999918 -0.0105422 77.7067 0.999682 0.00751679 -0.024061 304.223 0.0241383 -0.0103641 0.999655 -2.56233 0.00607466 -0.999925 -0.0105941 77.7028 0.999707 0.0063208 -0.0233572 303.157 0.0234224 -0.0104491 0.999671 -2.5582 0.00440251 -0.999932 -0.0107926 77.6905 0.999747 0.0046394 -0.0220233 302.087 0.0220719 -0.0106929 0.999699 -2.56195 0.00330843 -0.999944 -0.0100453 77.6917 0.999775 0.00351787 -0.0209038 301.025 0.020938 -0.00997385 0.999731 -2.55605 0.00199909 -0.999947 -0.0100833 77.6881 0.999794 0.00220238 -0.0201911 299.954 0.0202122 -0.0100408 0.999745 -2.55211 0.00094548 -0.999954 -0.00950133 77.6902 0.999782 0.00114362 -0.0208707 298.887 0.0208806 -0.00947952 0.999737 -2.55191 -0.000180305 -0.999959 -0.00904377 77.6817 0.999752 2.10154e-05 -0.0222557 297.825 0.022255 -0.00904554 0.999711 -2.54584 -0.0010863 -0.99996 -0.008851 77.69 0.999762 -0.000893301 -0.02178 296.771 0.0217713 -0.00887256 0.999724 -2.54419 -0.00214233 -0.999935 -0.0111656 77.6824 0.999762 -0.00189916 -0.0217441 295.716 0.0217214 -0.0112095 0.999701 -2.5474 -0.00304246 -0.999931 -0.0113522 77.6831 0.999742 -0.00278606 -0.0225336 294.667 0.0225004 -0.0114179 0.999682 -2.54206 -0.00333048 -0.999918 -0.0123516 77.6897 0.999722 -0.00304098 -0.0233839 293.619 0.0233444 -0.0124261 0.99965 -2.54572 -0.00374452 -0.999895 -0.0139771 77.687 0.999725 -0.00341963 -0.0231966 292.57 0.0231463 -0.0140601 0.999633 -2.53511 -0.00418725 -0.999881 -0.014821 77.6855 0.999727 -0.0038453 -0.023026 291.547 0.0229663 -0.0149134 0.999625 -2.53457 -0.00391913 -0.999878 -0.0150952 77.6841 0.99974 -0.00357847 -0.0225286 290.509 0.0224719 -0.0151796 0.999632 -2.53166 -0.00408805 -0.999861 -0.0161882 77.6873 0.999756 -0.00373517 -0.0217692 289.453 0.0217057 -0.0162732 0.999632 -2.52449 -0.00434718 -0.999841 -0.0172995 77.6837 0.999775 -0.00398648 -0.0208304 288.412 0.0207581 -0.0173862 0.999633 -2.52223 -0.00462051 -0.99982 -0.0183885 77.6833 0.99976 -0.0042251 -0.0214845 287.376 0.021403 -0.0184833 0.9996 -2.52203 -0.00483576 -0.999776 -0.0206152 77.6914 0.999729 -0.00436384 -0.0228761 286.34 0.022781 -0.0207203 0.999526 -2.52099 -0.00514836 -0.99974 -0.0221936 77.6921 0.999692 -0.00460654 -0.0243956 285.308 0.024287 -0.0223123 0.999456 -2.51142 -0.00524535 -0.999712 -0.0234345 77.693 0.999688 -0.00466974 -0.0245499 284.283 0.0244334 -0.023556 0.999424 -2.50737 -0.00506662 -0.999721 -0.0230816 77.6961 0.999706 -0.0045168 -0.0238108 283.262 0.0236999 -0.0231954 0.99945 -2.50166 -0.00443966 -0.999734 -0.0226367 77.6996 0.999756 -0.00394723 -0.0217523 282.242 0.0216571 -0.0227278 0.999507 -2.49872 -0.00328106 -0.99978 -0.0207114 77.6898 0.999788 -0.00285839 -0.0204044 281.223 0.0203408 -0.020774 0.999577 -2.49248 -0.00180788 -0.999806 -0.0196311 77.6909 0.999809 -0.00142519 -0.0194905 280.21 0.0194588 -0.0196626 0.999617 -2.49224 -0.000391134 -0.999826 -0.0186541 77.7049 0.999808 -2.55761e-05 -0.0195929 279.192 0.019589 -0.0186582 0.999634 -2.48467 0.00112993 -0.999824 -0.0187242 77.6954 0.999799 0.00150415 -0.0199838 278.165 0.0200085 -0.0186979 0.999625 -2.47933 0.00246135 -0.999818 -0.0189421 77.6875 0.999792 0.00284407 -0.0202041 277.151 0.0202543 -0.0188884 0.999616 -2.47499 0.00393358 -0.999788 -0.0202274 77.6778 0.999772 0.00435604 -0.0208839 276.124 0.0209676 -0.0201407 0.999577 -2.47111 0.00537425 -0.999768 -0.0208737 77.6669 0.999752 0.0058231 -0.0215021 275.1 0.0216187 -0.0207529 0.999551 -2.46339 0.00668459 -0.999729 -0.0222945 77.6481 0.999742 0.00716508 -0.0215419 274.086 0.0216958 -0.0221448 0.999519 -2.46401 0.00789368 -0.99966 -0.0248643 77.6341 0.999734 0.00842822 -0.0214674 273.06 0.0216697 -0.0246882 0.99946 -2.46375 0.00846454 -0.999609 -0.0266323 77.6191 0.999723 0.00904396 -0.0217119 272.049 0.0219443 -0.0264411 0.999409 -2.456 0.00928856 -0.999585 -0.0272631 77.6126 0.999715 0.00988273 -0.0217406 271.031 0.022001 -0.0270534 0.999392 -2.45476 0.00952639 -0.999567 -0.0278431 77.5918 0.999708 0.010139 -0.0219448 270.015 0.0222176 -0.0276259 0.999371 -2.45076 0.010134 -0.999561 -0.0278342 77.5779 0.999702 0.0107454 -0.0219042 269.013 0.0221936 -0.0276039 0.999373 -2.44886 0.0104319 -0.999579 -0.0270845 77.5673 0.99969 0.0110378 -0.0223205 268.005 0.02261 -0.0268433 0.999384 -2.43994 0.0104301 -0.999633 -0.0250156 77.5641 0.999694 0.010985 -0.0221501 266.994 0.0224167 -0.0247769 0.999442 -2.43594 0.0102613 -0.999687 -0.0228044 77.5453 0.999707 0.0107567 -0.0217049 265.992 0.0219434 -0.022575 0.999504 -2.42816 0.0101114 -0.999738 -0.0205209 77.5389 0.999727 0.0105396 -0.0208663 264.987 0.0210772 -0.0203043 0.999572 -2.42293 0.00935528 -0.999863 -0.0136437 77.5109 0.999761 0.0096221 -0.0196236 261.995 0.0197522 -0.0134569 0.999714 -2.40584 0.00848855 -0.999865 -0.0140973 77.4862 0.9997 0.0088092 -0.0228408 260.023 0.0229618 -0.0138992 0.99964 -2.4047 0.00816359 -0.999819 -0.0171633 77.4651 0.999666 0.00858114 -0.0243966 258.058 0.0245395 -0.0169584 0.999555 -2.3992 0.00744734 -0.99977 -0.020102 77.4289 0.999719 0.00789668 -0.0223667 255.134 0.0225203 -0.0199298 0.999548 -2.39203 0.00748775 -0.999732 -0.0218867 77.4081 0.999707 0.0079878 -0.02285 253.203 0.0230187 -0.0217092 0.999499 -2.37969 0.00765939 -0.999712 -0.0227327 77.3854 0.999758 0.0081244 -0.0204339 251.29 0.0206127 -0.0225707 0.999533 -2.37363 0.00807064 -0.999677 -0.0240829 77.3529 0.99976 0.00855718 -0.0201685 248.431 0.0203681 -0.0239143 0.999507 -2.35166 0.00801365 -0.9997 -0.0231553 77.3396 0.999793 0.00844363 -0.0185317 246.558 0.0187216 -0.023002 0.99956 -2.33998 0.00687925 -0.99974 -0.0217402 77.3313 0.9998 0.0072847 -0.0186259 244.68 0.0187794 -0.0216077 0.99959 -2.31825 0.00606643 -0.999795 -0.0193033 77.321 0.999813 0.00641839 -0.0182237 242.818 0.0183439 -0.0191892 0.999648 -2.29757 0.00458385 -0.999852 -0.0165963 77.3107 0.999794 0.00491079 -0.019713 240.993 0.0197916 -0.0165026 0.999668 -2.28307 0.00354496 -0.99988 -0.0150635 77.3049 0.99975 0.00387614 -0.0220135 239.182 0.0220693 -0.0149817 0.999644 -2.27374 0.00272259 -0.999906 -0.0134039 77.2909 0.999683 0.00305707 -0.0249973 237.414 0.0250359 -0.0133316 0.999598 -2.26431 0.00215047 -0.999914 -0.0129649 77.292 0.999729 0.00245016 -0.0231437 235.704 0.0231735 -0.0129116 0.999648 -2.24571 0.00157132 -0.999904 -0.0137835 77.29 0.99977 0.00186569 -0.0213698 234.038 0.0213934 -0.0137468 0.999677 -2.23732 0.00027616 -0.999878 -0.0156115 77.2797 0.99968 0.000670819 -0.0252805 232.412 0.0252878 -0.0155995 0.999558 -2.21974 -0.000188023 -0.999886 -0.0151228 77.2725 0.999701 0.000181722 -0.0244445 230.847 0.0244444 -0.0151229 0.999587 -2.21229 -0.00121107 -0.999867 -0.0162705 77.2736 0.999712 -0.00082056 -0.0239862 229.321 0.0239697 -0.0162949 0.99958 -2.20594 -0.00133846 -0.999851 -0.0172051 77.2722 0.999707 -0.000922051 -0.0241878 227.856 0.0241684 -0.0172325 0.999559 -2.18813 -0.00196232 -0.999844 -0.0175695 77.273 0.999785 -0.00159924 -0.0206557 226.443 0.0206244 -0.0176062 0.999632 -2.18417 -0.00323535 -0.999809 -0.0192948 77.2793 0.999767 -0.00282233 -0.0213948 224.425 0.0213363 -0.0193595 0.999585 -2.16528 -0.0035245 -0.999801 -0.0196123 77.2846 0.99986 -0.00320239 -0.0164312 222.491 0.0163651 -0.0196675 0.999673 -2.14777 -0.00395744 -0.999777 -0.0207659 77.2781 0.999868 -0.00362904 -0.0158281 221.213 0.0157492 -0.0208258 0.999659 -2.13098 -0.0039645 -0.999818 -0.0186831 77.2845 0.999915 -0.00373182 -0.0124727 219.954 0.0124007 -0.018731 0.999748 -2.09597 -0.00338336 -0.999751 -0.0220557 77.2783 0.999845 -0.00376257 0.0171748 218.75 -0.0172535 -0.0219942 0.999609 -2.05208 -0.00308621 -0.999755 -0.0219032 77.2808 0.999924 -0.00334661 0.011862 217.506 -0.0119324 -0.0218649 0.99969 -2.02178 -0.00213508 -0.999902 -0.0138526 77.2777 0.997636 -0.00117825 -0.0687161 215.551 0.068693 -0.0139666 0.99754 -2.07096 0.00101863 -0.999876 -0.0157114 77.2644 0.999456 0.00153604 -0.0329548 214.35 0.0329748 -0.0156693 0.999333 -2.0219 0.00301775 -0.999911 -0.0130051 77.2688 0.999992 0.00305275 -0.00267216 212.497 0.00271162 -0.0129969 0.999912 -2.04819 0.00398228 -0.99991 -0.012803 77.2649 0.999707 0.00428657 -0.0238282 211.203 0.023881 -0.0127044 0.999634 -2.04674 0.00559183 -0.999854 -0.0161407 77.2378 0.999874 0.0058306 -0.014784 209.307 0.014876 -0.016056 0.99976 -2.01464 0.00630108 -0.999827 -0.017522 77.2254 0.999877 0.00655076 -0.014229 207.374 0.0143413 -0.0174302 0.999745 -1.99572 0.00687067 -0.999855 -0.0155777 77.2117 0.999838 0.00712824 -0.0165396 206.069 0.0166483 -0.0154615 0.999742 -1.99018 0.00717094 -0.999836 -0.0166515 77.1939 0.999863 0.00741771 -0.0148054 204.776 0.0149265 -0.0165431 0.999752 -1.96916 0.00800318 -0.999889 -0.0125903 77.1984 0.999856 0.00818995 -0.0148535 202.823 0.014955 -0.0124696 0.99981 -1.95542 0.00839418 -0.999864 -0.0141771 77.1736 0.999849 0.00860789 -0.0150806 201.533 0.0152005 -0.0140483 0.999786 -1.94254 0.00923233 -0.99979 -0.0183161 77.1409 0.999837 0.00951429 -0.0153674 200.237 0.0155384 -0.0181712 0.999714 -1.93343 0.0100737 -0.99975 -0.0199685 77.1143 0.999809 0.0104047 -0.0165378 198.945 0.0167414 -0.019798 0.999664 -1.91722 0.0106988 -0.999576 -0.0270807 77.0865 0.999848 0.0110661 -0.0134512 197.008 0.0137452 -0.0269327 0.999543 -1.91456 0.0116676 -0.999499 -0.029414 77.0564 0.999849 0.0120396 -0.0125018 195.712 0.0128497 -0.0292637 0.999489 -1.88405 0.0123595 -0.999393 -0.0325708 77.0418 0.999857 0.0127275 -0.0111169 194.428 0.0115247 -0.0324287 0.999408 -1.87379 0.0125653 -0.999265 -0.0362099 77.0198 0.99986 0.0129579 -0.0106279 193.15 0.0110893 -0.0360712 0.999288 -1.85042 0.0104152 -0.999248 -0.037336 76.9951 0.999884 0.0108223 -0.0107208 191.866 0.0111168 -0.03722 0.999245 -1.82556 0.0078466 -0.999284 -0.0370019 76.9979 0.999892 0.00829977 -0.0121096 190.597 0.012408 -0.0369029 0.999242 -1.80566 0.00540088 -0.999345 -0.0357705 76.9818 0.999899 0.00586737 -0.0129493 189.347 0.0131507 -0.0356969 0.999276 -1.79238 0.00250684 -0.999368 -0.0354472 76.9789 0.999895 0.00301132 -0.0141856 188.095 0.0142834 -0.035408 0.999271 -1.77014 0.00050963 -0.999465 -0.0326998 76.9824 0.999862 0.00105234 -0.0165817 186.858 0.0166073 -0.0326869 0.999328 -1.75371 -0.00321276 -0.999241 -0.0388317 76.9618 0.99972 -0.00229971 -0.0235348 185.031 0.0234276 -0.0388964 0.998969 -1.74105 -0.00241841 -0.999706 -0.0241363 76.9851 0.999973 -0.00224954 -0.00702154 183.848 0.00696518 -0.0241526 0.999684 -1.6631 -0.00441883 -0.999736 -0.0225697 77.0081 0.999623 -0.00502734 0.0269761 182.665 -0.0270824 -0.022442 0.999381 -1.67819 -0.00610494 -0.999721 -0.0228271 76.9978 0.999979 -0.0061498 0.00189552 181.464 -0.00203537 -0.022815 0.999738 -1.67164 -0.00692608 -0.999825 -0.0173725 77.0227 0.997925 -0.00579877 -0.0641214 180.204 0.0640095 -0.0177806 0.997791 -1.70553 -0.00762631 -0.999835 -0.0164917 77.0259 0.998771 -0.00680852 -0.0490883 179.037 0.0489679 -0.0168458 0.998658 -1.6277 -0.00887686 -0.99973 -0.0214817 77.04 0.99996 -0.00889223 0.000620069 177.877 -0.000810921 -0.0214754 0.999769 -1.66133 -0.00881866 -0.999803 -0.0177663 77.0523 0.999959 -0.00878388 -0.00203466 176.704 0.0018782 -0.0177835 0.99984 -1.66129 -0.00982114 -0.999881 -0.0119087 77.0645 0.999775 -0.00959491 -0.0189077 175.499 0.0187912 -0.0120917 0.99975 -1.6381 -0.00950457 -0.999854 -0.0142335 77.0805 0.999925 -0.00939373 -0.00783388 173.722 0.00769903 -0.0143069 0.999868 -1.60948 -0.00775376 -0.999897 -0.0121038 77.0965 0.999921 -0.00763324 -0.00997165 172.504 0.00987823 -0.0121802 0.999877 -1.59614 -0.00608816 -0.999918 -0.0112355 77.0934 0.999892 -0.00593703 -0.0134366 171.289 0.0133688 -0.011316 0.999847 -1.58848 -0.00427731 -0.999918 -0.0120503 77.0961 0.999911 -0.00412474 -0.0126577 170.073 0.012607 -0.0121034 0.999847 -1.57266 -0.00217378 -0.999938 -0.0109343 77.0915 0.99993 -0.00204666 -0.0116236 168.851 0.0116005 -0.0109588 0.999873 -1.55685 4.44453e-05 -0.999952 -0.00982362 77.0913 0.999933 0.000158526 -0.0116124 167.614 0.0116134 -0.00982245 0.999884 -1.53446 0.00298951 -0.999968 -0.00737329 77.0777 0.999939 0.00306764 -0.0106079 165.735 0.0106302 -0.00734113 0.999917 -1.51258 0.00534003 -0.999964 -0.00657882 77.0685 0.999925 0.00541239 -0.0110296 164.466 0.0110648 -0.00651943 0.999918 -1.50273 0.00665537 -0.999961 -0.00583244 77.0521 0.999903 0.00672631 -0.0122284 162.542 0.0122671 -0.00575049 0.999908 -1.47711 0.00756373 -0.999958 -0.00516356 77.0449 0.999916 0.00761756 -0.0104868 161.27 0.0105257 -0.00508381 0.999932 -1.45601 0.00810198 -0.999963 -0.00293174 77.0361 0.999923 0.00812913 -0.0093701 159.961 0.00939359 -0.00285559 0.999952 -1.44569 0.00809083 -0.999952 -0.00553815 77.0082 0.999934 0.00813565 -0.0081205 158.651 0.00816516 -0.00547208 0.999952 -1.4333 0.00930213 -0.999938 -0.00618911 76.9949 0.999927 0.00934907 -0.00759769 157.328 0.00765508 -0.00611798 0.999952 -1.41271 0.00891361 -0.999916 -0.00936526 76.9739 0.999924 0.00899274 -0.0084417 155.983 0.00852522 -0.0092893 0.999921 -1.39682 0.0102411 -0.999862 -0.013086 76.9458 0.999909 0.0103548 -0.00864999 154.645 0.0087843 -0.0129963 0.999877 -1.38078 0.0105552 -0.99982 -0.0157472 76.9215 0.999905 0.010694 -0.00875213 153.287 0.00891895 -0.0156533 0.999838 -1.356 0.0103343 -0.999773 -0.0186551 76.9059 0.999907 0.0104991 -0.00875502 151.929 0.00894889 -0.0185628 0.999788 -1.3297 0.00856756 -0.999754 -0.0204626 76.8799 0.99992 0.00875652 -0.00916236 150.547 0.00933928 -0.0203825 0.999749 -1.30854 0.00791173 -0.999757 -0.0205833 76.877 0.999932 0.00808524 -0.00836026 149.154 0.00852465 -0.0205158 0.999753 -1.28849 0.00714166 -0.999742 -0.0215723 76.8662 0.999938 0.00732411 -0.00839022 147.768 0.00854605 -0.0215111 0.999732 -1.26564 0.00584244 -0.99977 -0.0206437 76.8481 0.999932 0.00604999 -0.0100059 146.396 0.0101285 -0.0205839 0.999737 -1.23597 0.00509275 -0.999765 -0.0210543 76.8432 0.999927 0.00532142 -0.0108193 145.012 0.0109288 -0.0209977 0.99972 -1.22428 0.00467827 -0.999749 -0.0219005 76.8293 0.99993 0.00491397 -0.0107209 143.675 0.0108258 -0.0218489 0.999703 -1.18572 0.00348473 -0.999767 -0.0213129 76.8246 0.999949 0.00368557 -0.00939161 142.315 0.00946797 -0.0212791 0.999729 -1.17372 0.00362105 -0.99979 -0.0201895 76.8288 0.999925 0.00338399 0.0117636 140.995 -0.0116928 -0.0202306 0.999727 -1.10212 0.00310431 -0.999856 -0.0166735 76.8305 0.999202 0.00243765 0.0398554 139.687 -0.039809 -0.016784 0.999066 -1.05493 0.00111884 -0.999755 -0.0221053 76.8017 0.9999 0.00142994 -0.0140626 138.26 0.0140908 -0.0220874 0.999657 -1.11893 0.00211985 -0.999866 -0.0162399 76.8026 0.997568 0.00324586 -0.0696266 136.878 0.06967 -0.0160528 0.997441 -1.0723 0.00342259 -0.999795 -0.0199346 76.7985 0.999988 0.0033539 0.00347811 135.596 -0.00341054 -0.0199462 0.999795 -1.00954 0.00342297 -0.999699 -0.0243045 76.7838 0.999803 0.00294654 0.0196113 134.233 -0.0195338 -0.0243668 0.999512 -1.05754 0.00326913 -0.999806 -0.019429 76.7802 0.999505 0.00387468 -0.0312118 132.782 0.0312811 -0.0193173 0.999324 -1.02096 0.00480846 -0.999861 -0.0159625 76.7831 0.999886 0.00503574 -0.0142285 131.416 0.014307 -0.0158922 0.999771 -0.958972 0.00500543 -0.999916 -0.0119499 76.7795 0.999964 0.00492389 0.00684345 130.016 -0.00678404 -0.0119838 0.999905 -0.947008 0.00567963 -0.999965 -0.00611092 76.768 0.999979 0.00569823 -0.00303156 128.581 0.00306627 -0.00609357 0.999977 -0.925478 0.00676407 -0.999968 -0.00416883 76.7502 0.999947 0.00679602 -0.00769915 126.424 0.00772724 -0.00411653 0.999962 -0.893155 0.00743789 -0.999958 -0.00530489 76.7405 0.999959 0.00746494 -0.00509902 124.971 0.0051384 -0.00526675 0.999973 -0.871771 0.00851406 -0.999931 -0.00813882 76.7228 0.999945 0.00856392 -0.00611105 123.518 0.00618032 -0.00808634 0.999948 -0.853574 0.00874952 -0.999873 -0.0133538 76.6997 0.99992 0.00887052 -0.00902854 122.046 0.00914585 -0.0132737 0.99987 -0.823478 0.01226 -0.999776 -0.017263 76.6747 0.999874 0.0124315 -0.00986452 120.566 0.0100769 -0.0171399 0.999802 -0.800444 0.0157842 -0.999663 -0.0206075 76.6406 0.999836 0.0159623 -0.00850672 119.078 0.0088328 -0.0204699 0.999751 -0.777816 0.020011 -0.999563 -0.0217734 76.5893 0.999756 0.0202083 -0.00887937 117.591 0.00931549 -0.0215904 0.999724 -0.754268 0.0273239 -0.999428 -0.0199447 76.5343 0.999579 0.0275124 -0.00923826 116.087 0.0097817 -0.0196839 0.999758 -0.709358 0.0361206 -0.999187 -0.0178926 76.4846 0.9993 0.0362874 -0.0090857 114.55 0.00972759 -0.0175519 0.999799 -0.708153 0.0463911 -0.99878 -0.0168969 76.3986 0.99886 0.0465721 -0.0104793 113.021 0.0112534 -0.0163915 0.999802 -0.675391 0.0614742 -0.998011 -0.0139466 76.2998 0.998074 0.0615821 -0.00744405 111.474 0.0082881 -0.0134622 0.999875 -0.652695 0.0786596 -0.996798 -0.0143611 76.1493 0.996889 0.0787233 -0.00392391 109.91 0.0050419 -0.0140078 0.999889 -0.636415 0.097282 -0.99512 -0.01648 75.9967 0.995243 0.0973542 -0.00363372 108.339 0.00522038 -0.0160481 0.999858 -0.604647 0.115527 -0.993138 -0.0181944 75.7897 0.993278 0.115638 -0.00514208 106.77 0.00721076 -0.0174781 0.999821 -0.581943 0.135002 -0.990666 -0.0188392 75.5556 0.990811 0.135131 -0.00573671 105.191 0.00822892 -0.0178916 0.999806 -0.561349 0.164944 -0.98612 -0.0189731 75.1489 0.986268 0.16507 -0.00525172 102.812 0.00831071 -0.0178463 0.999806 -0.516322 0.185547 -0.982489 -0.016942 74.848 0.982596 0.185667 -0.00577122 101.202 0.00881574 -0.0155764 0.99984 -0.485731 0.207823 -0.978081 -0.0129232 74.4978 0.978141 0.207894 -0.00441144 99.6098 0.0070014 -0.0117239 0.999907 -0.460205 0.242103 -0.97019 -0.0108164 73.8905 0.970239 0.242139 -0.00207422 97.1983 0.00463145 -0.00999229 0.999939 -0.42327 0.265029 -0.964177 -0.0110147 73.4266 0.964223 0.265076 -0.0030075 95.5887 0.0058195 -0.00982355 0.999935 -0.389805 0.288111 -0.957523 -0.0119224 72.9251 0.957582 0.288153 -0.00195357 93.9901 0.00530606 -0.0108538 0.999927 -0.367862 0.324857 -0.945644 -0.0150238 72.1015 0.945742 0.324916 -0.00159461 91.5928 0.0063894 -0.0136906 0.999886 -0.323403 0.349882 -0.93664 -0.0169914 71.4782 0.936759 0.349968 -0.00225339 90.0093 0.00805705 -0.0151284 0.999853 -0.291088 0.373958 -0.927279 -0.0175649 70.8153 0.927413 0.374036 -0.00128144 88.4258 0.00775818 -0.0158108 0.999845 -0.264737 0.397941 -0.917228 -0.0183195 70.124 0.91737 0.398032 -0.00145428 86.8486 0.00862565 -0.016227 0.999831 -0.239185 0.416889 -0.908705 -0.0214107 69.3561 0.908907 0.416998 -0.000708236 85.2716 0.00957179 -0.019165 0.999771 -0.222278 0.432533 -0.901309 -0.0235981 68.5863 0.901564 0.432645 0.000385218 83.6924 0.00986238 -0.0214418 0.999721 -0.185196 0.448017 -0.89376 -0.0217534 67.7852 0.89398 0.448106 0.000858263 82.1176 0.00898077 -0.0198317 0.999763 -0.155278 0.468122 -0.883386 -0.0221516 66.5324 0.883596 0.468249 -0.000627634 79.7609 0.0109269 -0.0192792 0.999754 -0.121272 0.478356 -0.877927 -0.0204904 65.6684 0.878092 0.478488 -0.00179128 78.1917 0.011377 -0.0171356 0.999788 -0.0897987 0.493519 -0.869554 -0.0177662 64.324 0.869662 0.49364 -0.00289981 75.8599 0.0112917 -0.0140195 0.999838 -0.0673291 0.500881 -0.865235 -0.0220387 63.4057 0.865397 0.501072 -0.00380475 74.3053 0.014335 -0.0171665 0.99975 -0.0484548 0.50561 -0.862446 -0.0233595 62.4833 0.862643 0.505805 -0.00293088 72.768 0.0143431 -0.018669 0.999723 -0.020998 0.509166 -0.860384 -0.0221238 61.5823 0.860565 0.509335 -0.00243099 71.2586 0.01336 -0.0178012 0.999752 0.00165494 0.513597 -0.857746 -0.0221317 60.2176 0.857957 0.513722 5.06427e-05 68.9963 0.0113261 -0.019014 0.999755 0.0275294 0.515076 -0.85677 -0.0253233 58.8324 0.857092 0.515146 0.00419697 66.7247 0.00944938 -0.0238662 0.999671 0.0716462 0.5139 -0.857422 -0.0271025 57.9184 0.857791 0.51398 0.00446287 65.2178 0.0101036 -0.0255417 0.999623 0.100878 0.508519 -0.860648 -0.0263457 56.5697 0.861002 0.508578 0.00488786 62.9378 0.00919213 -0.0251693 0.999641 0.146803 0.503592 -0.863468 -0.028595 55.6709 0.863881 0.503674 0.00479937 61.418 0.0102584 -0.0271196 0.99958 0.169365 0.497845 -0.866746 -0.0300309 54.7935 0.867191 0.497958 0.00410653 59.8935 0.0113948 -0.028087 0.999541 0.190018 0.490921 -0.870686 -0.0300474 53.9414 0.871118 0.491066 0.00283729 58.3675 0.0122849 -0.0275677 0.999544 0.224363 0.483789 -0.874644 -0.0307686 53.0782 0.875073 0.48399 0.00102184 56.8215 0.013998 -0.0274191 0.999526 0.244076 0.476666 -0.878557 -0.0304693 52.2505 0.878996 0.476823 0.00233145 55.3138 0.0124802 -0.0278937 0.999533 0.277053 0.468845 -0.882809 -0.0288515 51.4356 0.883234 0.468906 0.00504158 53.8008 0.00907789 -0.0278463 0.999571 0.299324 0.460651 -0.887101 -0.0291975 50.6501 0.887541 0.460693 0.00568708 52.2915 0.00840605 -0.0285337 0.999557 0.331645 0.452199 -0.891396 -0.0304811 49.8834 0.89187 0.452264 0.0051216 50.7905 0.00922011 -0.0295011 0.999522 0.361217 0.44254 -0.896202 -0.0313275 49.1408 0.896697 0.442619 0.0047356 49.2772 0.00962208 -0.0301869 0.999498 0.392611 0.432104 -0.901261 -0.0318496 48.4021 0.901777 0.43217 0.0051248 47.7812 0.00914566 -0.0309357 0.99948 0.418435 0.419857 -0.906827 -0.0372202 47.7151 0.907521 0.419978 0.00487205 46.2648 0.0112136 -0.0358236 0.999295 0.432287 0.405798 -0.913283 -0.0352537 47.0518 0.913916 0.405865 0.00554468 44.7691 0.00924438 -0.0344689 0.999363 0.478146 0.389849 -0.92021 -0.0350869 46.4222 0.920852 0.389844 0.00727948 43.2735 0.00697974 -0.0351477 0.999358 0.496709 0.374438 -0.926521 -0.0368239 45.8192 0.927157 0.374673 0.000555308 41.7727 0.0132824 -0.0343495 0.999322 0.536541 0.357282 -0.933377 -0.0340069 45.2564 0.933924 0.357471 0.000551027 40.2672 0.0116422 -0.0319567 0.999421 0.567374 0.329622 -0.943664 -0.0291061 44.4739 0.944087 0.329683 0.00281222 38.011 0.00694199 -0.0284056 0.999572 0.61181 0.309878 -0.95037 -0.0278069 44.0013 0.950739 0.309994 0.000145059 36.5142 0.00848212 -0.0264821 0.999613 0.638011 0.289977 -0.956707 -0.0250132 43.563 0.956979 0.29014 -0.00306542 35.0211 0.0101901 -0.0230482 0.999682 0.676079 0.271052 -0.962306 -0.0223289 43.1779 0.962509 0.271213 -0.00446006 33.5612 0.0103478 -0.0202829 0.999741 0.709732 0.252716 -0.967303 -0.0214456 42.8091 0.967486 0.252875 -0.00501783 32.1318 0.0102768 -0.0194802 0.999757 0.732235 0.223298 -0.974507 -0.0217868 42.3357 0.974718 0.223416 -0.00312043 30.0319 0.00790839 -0.0205392 0.999758 0.776278 0.202469 -0.979066 -0.0208689 42.0737 0.979269 0.202554 -0.00201644 28.6751 0.00620129 -0.020028 0.99978 0.808764 0.182783 -0.982977 -0.0185887 41.8416 0.983139 0.182849 -0.00187413 27.3224 0.00524115 -0.0179327 0.999825 0.835432 0.164776 -0.986153 -0.0187373 41.63 0.986301 0.164889 -0.00459621 25.9853 0.00762213 -0.0177233 0.999814 0.861815 0.151324 -0.988321 -0.0179762 41.4406 0.988452 0.151442 -0.00533926 24.6682 0.00799924 -0.0169607 0.999824 0.883728 0.137837 -0.990125 -0.0255654 41.1685 0.990287 0.137292 0.0219911 22.7522 -0.018264 -0.0283483 0.999431 0.986493 0.130435 -0.99124 -0.0207562 40.9349 0.991427 0.130564 -0.0049721 20.8642 0.00763855 -0.0199298 0.999772 0.987179 0.128538 -0.991001 -0.0373492 40.7533 0.991273 0.129502 -0.0246504 19.6631 0.0292654 -0.0338548 0.998998 1.02352 0.128773 -0.991272 -0.0282518 40.6091 0.99155 0.129155 -0.0121577 18.5076 0.0157005 -0.0264475 0.999527 1.08425 0.12804 -0.991316 -0.029967 40.387 0.991768 0.127933 0.0054677 16.7709 -0.00158647 -0.0304203 0.999536 1.07384 0.129748 -0.991052 -0.0313301 40.23 0.991546 0.129716 0.00307091 15.6329 0.00102059 -0.0314637 0.999504 1.13415 0.13527 -0.990478 -0.0255981 40.0928 0.990796 0.135356 -0.00162176 14.5308 0.00507116 -0.0251431 0.999671 1.15863 0.157913 -0.987065 -0.0276761 39.7495 0.987435 0.158018 -0.00161523 12.4967 0.00596766 -0.0270733 0.999616 1.21733 0.186459 -0.982197 -0.0228594 39.4731 0.98246 0.18646 0.00211143 11.1122 0.00218853 -0.0228522 0.999736 1.2524 0.221892 -0.974747 -0.0251385 39.1581 0.975029 0.222047 -0.00354769 9.86262 0.00904004 -0.0237236 0.999678 1.27867 0.262037 -0.964756 -0.0241379 38.8266 0.965001 0.262212 -0.00431865 8.7627 0.0104957 -0.0221614 0.999699 1.31048 0.320119 -0.947112 -0.0224388 38.3538 0.947329 0.320252 -0.00248511 7.51508 0.00953973 -0.0204614 0.999745 1.33455 0.379867 -0.924755 -0.0230074 37.8526 0.925028 0.379877 0.00410622 6.45328 0.00494273 -0.0228423 0.999727 1.36452 0.444482 -0.895522 -0.0218159 37.2909 0.895772 0.444485 0.00499342 5.46713 0.00522513 -0.0217616 0.99975 1.38479 0.516001 -0.856305 -0.0220347 36.6286 0.856587 0.51587 0.0116739 4.5206 0.0013706 -0.0248984 0.999689 1.40531 0.609001 -0.792857 -0.0222432 35.631 0.793098 0.609083 0.00367646 3.3574 0.010633 -0.01988 0.999746 1.43016 0.692634 -0.720795 -0.0267057 34.6501 0.72114 0.692769 0.00529913 2.49369 0.0146813 -0.0229289 0.999629 1.43272 0.755444 -0.654617 -0.0279553 33.7932 0.654983 0.755622 0.00569877 1.88139 0.0173931 -0.0226153 0.999593 1.42286 0.815183 -0.578279 -0.0327081 32.8258 0.578798 0.815426 0.00863192 1.31942 0.0216793 -0.0259679 0.999428 1.43524 0.870115 -0.49204 -0.028229 31.7665 0.492488 0.870242 0.0115884 0.812309 0.0188641 -0.0239857 0.999534 1.41142 0.929033 -0.368643 -0.0316094 30.1415 0.369303 0.929129 0.0182706 0.282744 0.0226339 -0.0286474 0.999333 1.4208 0.970354 -0.240048 -0.0281174 28.3324 0.24079 0.970206 0.0268339 -0.0808593 0.0208382 -0.0328088 0.999244 1.38706 0.98976 -0.139569 -0.0299184 26.3059 0.140599 0.989425 0.0356424 -0.294155 0.0246275 -0.0394839 0.998917 1.38204 0.993811 -0.105724 -0.0340968 25.2237 0.106971 0.99357 0.0370843 -0.378433 0.0299569 -0.0405021 0.99873 1.35984 0.996554 -0.0757561 -0.0337883 24.131 0.0771373 0.99615 0.0416422 -0.419705 0.0305035 -0.044105 0.998561 1.36246 0.998672 -0.0455504 -0.0240846 22.4247 0.0465672 0.997968 0.0434935 -0.473812 0.0220545 -0.0445573 0.998763 1.35578 0.999065 -0.0328085 -0.0281722 21.2333 0.0340424 0.998429 0.0444988 -0.497782 0.026668 -0.0454162 0.998612 1.33661 0.999213 -0.0217368 -0.0331692 20.0302 0.023092 0.998891 0.0410368 -0.52197 0.0322404 -0.0417705 0.998607 1.351 0.999649 -0.00704273 -0.025546 18.158 0.00803381 0.999211 0.038903 -0.518597 0.0252519 -0.0390946 0.998916 1.32868 0.999665 0.00201893 -0.0257875 16.1871 -0.000942563 0.99913 0.0416842 -0.502004 0.0258492 -0.0416459 0.998798 1.31278 0.99962 0.00396251 -0.027263 14.1744 -0.00280707 0.999101 0.0422898 -0.48391 0.027406 -0.0421973 0.998733 1.29947 0.99955 0.00197139 -0.0299438 12.1004 -0.000621763 0.998986 0.0450146 -0.470716 0.0300022 -0.0449758 0.998537 1.28795 0.999648 0.000254519 -0.0265348 10.6803 0.000897076 0.999058 0.0433785 -0.465597 0.0265208 -0.0433871 0.998706 1.28363 0.999646 -0.0015048 -0.0265717 9.23925 0.00263968 0.999083 0.0427271 -0.46407 0.0264831 -0.0427821 0.998733 1.26615 0.999536 -0.00271179 -0.030355 7.77586 0.00399769 0.999093 0.0423817 -0.47213 0.0302125 -0.0424834 0.99864 1.26783 0.999643 -0.0034889 -0.0264887 6.30045 0.00454262 0.999196 0.039825 -0.467842 0.0263285 -0.0399311 0.998856 1.26672 0.999686 -0.00420077 -0.0247045 4.04938 0.00516984 0.999214 0.0392943 -0.472864 0.0245201 -0.0394097 0.998922 1.25279 0.999633 -0.00521463 -0.0265927 1.7529 0.0062425 0.999231 0.0387168 -0.483443 0.0263704 -0.0388686 0.998896 1.24513 0.999531 -0.00569562 -0.0300751 0.209662 0.00687771 0.999202 0.0393486 -0.483577 0.029827 -0.039537 0.998773 1.22631 0.999456 -0.00619097 -0.0323787 -1.34424 0.00746815 0.999193 0.0394741 -0.494936 0.0321082 -0.0396944 0.998696 1.22259 0.999642 -0.00686729 -0.025869 -2.90328 0.0078661 0.99922 0.0387085 -0.497204 0.025583 -0.0388981 0.998916 1.21676 0.999602 -0.00785292 -0.0270807 -5.30729 0.00888916 0.999224 0.0383592 -0.510671 0.0267585 -0.0385846 0.998897 1.2019 0.999508 -0.00609719 -0.0307522 -7.74889 0.00727903 0.999233 0.0384669 -0.515667 0.0304941 -0.0386718 0.998787 1.18708 0.999649 -0.00310343 -0.0263013 -10.2206 0.00410009 0.999272 0.037925 -0.523826 0.0261645 -0.0380195 0.998934 1.18692 0.999628 -0.00128348 -0.0272429 -11.8757 0.00229537 0.999307 0.0371447 -0.521009 0.0271763 -0.0371934 0.998938 1.17973 0.999613 0.00141518 -0.0277856 -14.4018 -0.000424718 0.999365 0.0356203 -0.51781 0.0278184 -0.0355947 0.998979 1.16667 0.99956 0.00438194 -0.0293282 -17.8088 -0.0032905 0.999304 0.0371601 -0.495284 0.0294706 -0.0370473 0.998879 1.14519 0.99966 0.00351757 -0.0258541 -19.5282 -0.00254247 0.999288 0.0376521 -0.473576 0.0259681 -0.0375735 0.998956 1.13515 0.999672 0.00264501 -0.0254914 -22.1487 -0.00169066 0.999299 0.0373872 -0.468674 0.0255724 -0.0373318 0.998976 1.1317 0.999592 0.000531092 -0.0285586 -24.8228 0.000566195 0.999262 0.0384006 -0.455755 0.0285579 -0.0384011 0.998854 1.11682 0.999609 -0.000184137 -0.0279624 -26.6107 0.00129784 0.999206 0.0398157 -0.44677 0.0279329 -0.0398364 0.998816 1.10897 0.999696 -0.00161761 -0.0246193 -29.3027 0.00258061 0.999231 0.0391342 -0.449584 0.0245371 -0.0391858 0.998931 1.11414 0.999589 -0.00287929 -0.0285083 -31.1219 0.00395352 0.999281 0.0376968 -0.448907 0.0283793 -0.0377941 0.998882 1.10921 0.999607 -0.00382066 -0.0277688 -33.8468 0.00477353 0.999399 0.0343295 -0.459853 0.027621 -0.0344485 0.999025 1.09959 0.999626 -0.00426285 -0.0270275 -36.5801 0.00522582 0.99935 0.0356591 -0.455796 0.0268579 -0.035787 0.998998 1.07968 0.999449 -0.0043974 -0.032886 -38.4225 0.00559374 0.999323 0.0363756 -0.463441 0.0327037 -0.0365395 0.998797 1.07341 0.999638 -0.00339495 -0.0266859 -41.168 0.00438708 0.999298 0.0372079 -0.470114 0.0265408 -0.0373115 0.998951 1.05785 0.999655 -0.0028095 -0.0261182 -43.0193 0.00382085 0.999242 0.0387533 -0.46582 0.0259895 -0.0388397 0.998907 1.05734 0.999377 -0.00187832 -0.0352413 -45.8031 0.00324757 0.99924 0.0388367 -0.462931 0.0351416 -0.038927 0.998624 1.03941 0.999319 -0.000631342 -0.0368838 -47.6382 0.00203873 0.999271 0.0381323 -0.466039 0.0368328 -0.0381816 0.998592 1.03553 0.999697 0.00152421 -0.0245571 -49.4424 -0.000648761 0.999365 0.0356183 -0.454262 0.0245958 -0.0355916 0.999064 1.02427 0.999617 0.00426441 -0.027357 -51.2672 -0.00323906 0.999295 0.0374156 -0.439536 0.0274972 -0.0373126 0.998925 1.00584 0.999399 0.00864196 -0.0335689 -53.9889 -0.0073064 0.999185 0.0397065 -0.410226 0.0338847 -0.0394374 0.998647 1.00486 0.999545 0.0113684 -0.0279264 -55.7837 -0.0102593 0.999165 0.0395418 -0.386031 0.0283526 -0.0392373 0.998828 0.998285 0.999524 0.0143723 -0.0272926 -58.447 -0.0133033 0.999152 0.0389558 -0.343773 0.0278294 -0.0385742 0.998868 0.979257 0.999342 0.0152077 -0.0329249 -60.2278 -0.0138866 0.999104 0.0399888 -0.309483 0.0335035 -0.0395053 0.998658 0.978981 0.999512 0.0165483 -0.0264764 -62.8571 -0.0155109 0.999122 0.0389185 -0.267842 0.0270972 -0.0384888 0.998892 0.976213 0.999538 0.0154084 -0.0261811 -65.4785 -0.0144481 0.99923 0.0364809 -0.227769 0.026723 -0.0360858 0.998991 0.968438 0.999463 0.0150239 -0.0291231 -67.2312 -0.0139706 0.999253 0.0360402 -0.203537 0.0296428 -0.035614 0.998926 0.965374 0.999468 0.0134165 -0.0297384 -69.7977 -0.0124107 0.999353 0.0337525 -0.167284 0.030172 -0.0333655 0.998988 0.968633 0.999415 0.0129333 -0.031672 -71.4931 -0.0119131 0.999411 0.0321895 -0.150115 0.0320696 -0.0317933 0.99898 0.952562 0.999484 0.0122734 -0.0296953 -73.1785 -0.011376 0.999479 0.0302023 -0.134729 0.0300505 -0.0298489 0.999103 0.953741 0.999454 0.0112418 -0.0310829 -74.8479 -0.0103219 0.999509 0.0295974 -0.109816 0.0314003 -0.0292604 0.999078 0.929135 0.999132 0.00998819 -0.0404389 -78.1646 -0.00863053 0.999398 0.0336097 -0.065438 0.0407502 -0.0332315 0.998617 0.904128 0.999711 0.00888233 -0.0223301 -79.8008 -0.00818163 0.999477 0.0312769 -0.0587126 0.0225963 -0.0310852 0.999261 0.906533 0.999724 0.00855604 -0.0218954 -81.4192 -0.00794093 0.999576 0.0280274 -0.0377311 0.0221259 -0.0278458 0.999367 0.906386 0.999501 0.0110469 -0.0295904 -83.0391 -0.0103378 0.999658 0.0240102 -0.0296125 0.0298455 -0.0236923 0.999274 0.903023 0.999872 0.0148931 0.00587919 -85.3936 -0.0151144 0.999102 0.039576 0.0352204 -0.0052845 -0.0396598 0.999199 0.931102 0.999513 0.0173847 -0.0259095 -87.0001 -0.0163939 0.999144 0.0379758 0.080214 0.0265475 -0.0375326 0.998943 0.928838 0.998142 0.0199412 -0.0575694 -89.4321 -0.0183317 0.99943 0.0283518 0.108539 0.0581019 -0.0272438 0.997939 0.887514 0.999656 0.022994 -0.0126548 -91.7699 -0.0226487 0.999385 0.0267866 0.187782 0.0132629 -0.0264907 0.999561 0.872577 0.999351 0.0258499 -0.0250846 -93.4075 -0.0251546 0.999301 0.0276482 0.235679 0.0257818 -0.0269993 0.999303 0.865368 0.999211 0.0294852 -0.0266268 -95.0413 -0.0287094 0.999165 0.0290611 0.293394 0.0274615 -0.0282737 0.999223 0.8646 0.999072 0.0348694 -0.0252901 -96.6617 -0.0340603 0.998915 0.031746 0.358444 0.0263696 -0.0308551 0.999176 0.85904 0.998822 0.0406813 -0.0264463 -98.331 -0.0398864 0.998756 0.0299204 0.430081 0.0276306 -0.0288303 0.999202 0.857022 0.998486 0.048357 -0.0262289 -100.853 -0.0474733 0.998316 0.0333284 0.576766 0.0277964 -0.0320327 0.9991 0.835065 0.99826 0.0543766 -0.0227968 -102.564 -0.0536403 0.998055 0.0317527 0.662132 0.0244791 -0.0304746 0.999236 0.847714 0.998002 0.0597516 -0.0205432 -104.277 -0.059153 0.997839 0.028605 0.770089 0.022208 -0.0273327 0.99938 0.841828 0.997563 0.0642761 -0.0271254 -105.98 -0.0635055 0.997578 0.0283729 0.896175 0.0288834 -0.0265812 0.999229 0.829707 0.996868 0.0731765 -0.0299843 -108.62 -0.072408 0.997037 0.0259623 1.08019 0.0317952 -0.0237099 0.999213 0.824015 0.996773 0.078425 -0.0171342 -110.35 -0.0780194 0.996682 0.0231818 1.22325 0.0188954 -0.0217701 0.999584 0.81559 0.996275 0.0836684 -0.0208725 -112.112 -0.0832073 0.996288 0.0220617 1.3849 0.0226409 -0.0202428 0.999539 0.812425 0.9955 0.0897982 -0.0302684 -113.909 -0.0890531 0.99571 0.0251287 1.56019 0.0323951 -0.0223201 0.999226 0.79744 0.99503 0.0955948 -0.0278893 -115.672 -0.0948937 0.995162 0.0254652 1.73909 0.0301887 -0.0226921 0.999287 0.799701 0.994591 0.102899 -0.0141796 -118.367 -0.102524 0.994415 0.0250351 2.03475 0.0166765 -0.0234459 0.999586 0.796714 0.993919 0.108716 -0.0174779 -120.198 -0.108226 0.993762 0.0269105 2.23679 0.0202945 -0.0248553 0.999485 0.809274 0.993857 0.109913 -0.0129268 -122.014 -0.109569 0.993667 0.0249026 2.44353 0.015582 -0.0233332 0.999606 0.828444 0.993577 0.111012 -0.0219338 -123.845 -0.11037 0.99348 0.0285803 2.66288 0.0249635 -0.0259759 0.999351 0.833345 0.992895 0.11444 -0.0326138 -126.596 -0.113483 0.993092 0.029828 2.97104 0.035802 -0.0259149 0.999023 0.795703 0.992924 0.116317 -0.0239066 -128.443 -0.115593 0.992852 0.0297349 3.19386 0.0271943 -0.0267611 0.999272 0.788608 0.992685 0.117881 -0.026092 -130.324 -0.117137 0.992709 0.0284126 3.42677 0.0292511 -0.0251484 0.999256 0.777565 0.992418 0.118852 -0.0313025 -132.204 -0.11787 0.992528 0.0315456 3.6534 0.0348178 -0.0276168 0.999012 0.756072 0.992463 0.119643 -0.0265075 -134.075 -0.118754 0.992378 0.0329078 3.90154 0.0302427 -0.0295119 0.999107 0.748988 0.992304 0.120381 -0.0290052 -136.927 -0.119397 0.992279 0.0335801 4.26282 0.0328236 -0.0298585 0.999015 0.72274 0.992158 0.121015 -0.0312551 -138.839 -0.120014 0.992251 0.0321512 4.4895 0.0349037 -0.028148 0.998994 0.702758 0.992149 0.121485 -0.0296984 -140.744 -0.120554 0.992211 0.0313562 4.72601 0.0332764 -0.0275298 0.999067 0.689568 0.992288 0.121359 -0.0252318 -143.618 -0.120563 0.992221 0.0310087 5.08172 0.0287987 -0.0277276 0.999201 0.659813 0.992294 0.121225 -0.0256337 -145.56 -0.120444 0.992265 0.0300727 5.33713 0.029081 -0.0267535 0.999219 0.650932 0.992186 0.121282 -0.0292943 -147.464 -0.12043 0.992289 0.0292979 5.56237 0.0326218 -0.0255411 0.999141 0.644657 0.992253 0.121447 -0.0261604 -149.394 -0.120702 0.992282 0.0284165 5.79904 0.0294097 -0.0250388 0.999254 0.627615 0.9923 0.121322 -0.0249175 -151.319 -0.120603 0.992289 0.0286029 6.03211 0.0281955 -0.0253776 0.99928 0.619051 0.992422 0.120635 -0.0233568 -153.253 -0.119882 0.992293 0.0313383 6.28454 0.0269573 -0.0283007 0.999236 0.60821 0.992656 0.118037 -0.0264913 -156.167 -0.117095 0.992515 0.0346703 6.64245 0.0303854 -0.0313137 0.999048 0.593303 0.993002 0.115227 -0.0258764 -159.076 -0.114293 0.992828 0.0350605 6.99238 0.0297307 -0.0318576 0.99905 0.590447 0.993201 0.11333 -0.0266012 -161.014 -0.112389 0.993064 0.0345375 7.21505 0.0303308 -0.031313 0.999049 0.579633 0.993299 0.111916 -0.0288563 -162.908 -0.110918 0.993243 0.0341253 7.42945 0.0324805 -0.0306959 0.999001 0.562055 0.993434 0.110328 -0.0302893 -165.764 -0.109324 0.993457 0.033033 7.75693 0.0337356 -0.0295048 0.998995 0.546645 0.993373 0.11135 -0.028465 -167.656 -0.110413 0.99335 0.0326313 7.96791 0.0319092 -0.0292721 0.999062 0.527984 0.993279 0.112657 -0.0265641 -169.541 -0.11175 0.993173 0.033449 8.18091 0.030151 -0.0302556 0.999087 0.512313 0.993056 0.115081 -0.0244271 -172.328 -0.114225 0.992874 0.0339576 8.51239 0.028161 -0.0309316 0.999125 0.518824 0.992582 0.117968 -0.0293899 -175.125 -0.117094 0.992671 0.0298858 8.84238 0.0327001 -0.0262227 0.999121 0.509 0.992332 0.120231 -0.0286615 -176.969 -0.119426 0.992439 0.0283287 9.05899 0.0318507 -0.0246886 0.999188 0.501349 0.992173 0.12216 -0.0258876 -178.811 -0.121488 0.992249 0.0261306 9.29834 0.028879 -0.022781 0.999323 0.495108 0.991945 0.124363 -0.0240451 -180.622 -0.123826 0.992045 0.0226873 9.51659 0.0266753 -0.0195272 0.999453 0.484417 0.991799 0.125853 -0.0222741 -182.449 -0.125489 0.991948 0.0170511 9.74406 0.0242407 -0.0141161 0.999606 0.491559 0.991972 0.126416 -0.00334636 -185.134 -0.126278 0.991615 0.0274438 10.1243 0.00678764 -0.0268009 0.999618 0.518202 0.989615 0.126524 -0.0682233 -187.892 -0.124309 0.991597 0.0358015 10.4862 0.0721798 -0.026949 0.997027 0.469983 0.991816 0.127674 0.00101147 -190.513 -0.127659 0.991501 0.0250806 10.8294 0.00219927 -0.0250045 0.999685 0.437699 0.991661 0.127267 -0.020312 -192.323 -0.126596 0.991455 0.0314665 11.0848 0.024143 -0.0286326 0.999298 0.449511 0.991085 0.128819 -0.034003 -195.022 -0.127822 0.991342 0.030038 11.4249 0.037578 -0.0254239 0.99897 0.42551 0.991589 0.126837 -0.025747 -197.703 -0.125838 0.99135 0.0372772 11.7931 0.0302524 -0.0337237 0.998973 0.407423 0.991965 0.125266 -0.0176965 -200.365 -0.124544 0.991514 0.0372674 12.1341 0.0222147 -0.034764 0.999149 0.404061 0.991987 0.124349 -0.022356 -202.163 -0.123428 0.991598 0.0387145 12.3733 0.0269823 -0.035645 0.999 0.401753 0.991945 0.123667 -0.0274017 -204.828 -0.122678 0.99182 0.0352639 12.686 0.0315386 -0.0316183 0.999002 0.391023 0.991921 0.123846 -0.0274718 -207.464 -0.122908 0.991851 0.0335354 13.0118 0.0314012 -0.029888 0.99906 0.370988 0.991905 0.124591 -0.0245118 -210.064 -0.123731 0.991735 0.0339575 13.3414 0.02854 -0.0306498 0.999123 0.364018 0.991915 0.124426 -0.0249638 -211.789 -0.123533 0.991738 0.0345763 13.5652 0.0290597 -0.0312128 0.99909 0.354946 0.991812 0.12457 -0.0281431 -213.504 -0.123555 0.99171 0.0353004 13.7851 0.0323071 -0.0315341 0.99898 0.353175 0.991969 0.124566 -0.0219253 -216.044 -0.123803 0.991748 0.0332984 14.1068 0.0258923 -0.0303166 0.999205 0.344831 0.991745 0.124799 -0.0294414 -218.561 -0.123726 0.99167 0.0358269 14.4351 0.0336673 -0.0318885 0.998924 0.332787 0.992237 0.124362 0.000129851 -221.01 -0.124291 0.991634 0.0348349 14.7375 0.00420339 -0.0345806 0.999393 0.340759 0.992024 0.122915 -0.0279153 -223.5 -0.121798 0.991799 0.0387208 15.0704 0.0324458 -0.035012 0.99886 0.355846 0.990041 0.122048 -0.0701703 -225.182 -0.119485 0.992044 0.0396508 15.2772 0.0744513 -0.0308716 0.996747 0.311446 0.992519 0.122081 -0.00162687 -227.6 -0.121958 0.99197 0.033492 15.5676 0.00570254 -0.033043 0.999438 0.301989 0.992163 0.12076 -0.032075 -230.144 -0.119509 0.992089 0.0384212 15.8924 0.0364611 -0.0342869 0.998747 0.278165 0.992304 0.120273 -0.0294431 -231.819 -0.119048 0.992069 0.0403296 16.098 0.0340602 -0.0365141 0.998753 0.277097 0.99272 0.119529 -0.0147985 -234.348 -0.118968 0.992305 0.0343047 16.3997 0.0187851 -0.0322944 0.999302 0.284305 0.992681 0.119069 -0.0201865 -236.063 -0.118409 0.992473 0.0312547 16.5958 0.023756 -0.0286357 0.999308 0.278947 0.992541 0.118418 -0.0289688 -237.784 -0.117534 0.9926 0.0305045 16.8071 0.0323667 -0.0268722 0.999115 0.273603 0.992747 0.117756 -0.0242192 -240.355 -0.117036 0.992697 0.0292326 17.1155 0.0274847 -0.0261861 0.999279 0.259399 0.992627 0.118011 -0.0276604 -242.966 -0.117162 0.992643 0.030532 17.4271 0.03106 -0.0270661 0.999151 0.249615 0.992544 0.118404 -0.0289386 -244.685 -0.117435 0.992527 0.03316 17.6438 0.0326486 -0.0295144 0.999031 0.234829 0.992566 0.119852 -0.0211653 -247.253 -0.119124 0.992336 0.0328518 17.9572 0.0249405 -0.0300863 0.999236 0.237097 0.992541 0.119609 -0.0235996 -249.773 -0.118834 0.992402 0.0319018 18.2697 0.027236 -0.0288594 0.999212 0.235228 0.992342 0.120822 -0.0256908 -252.281 -0.119964 0.992241 0.0326544 18.5829 0.0294368 -0.0293223 0.999136 0.233239 0.992165 0.122098 -0.0264752 -254.744 -0.121163 0.992034 0.0344646 18.8914 0.0304723 -0.0309867 0.999055 0.224806 0.992028 0.122982 -0.0275089 -256.365 -0.121962 0.991878 0.0360901 19.092 0.0317239 -0.0324473 0.99897 0.218986 0.992052 0.124009 -0.0213446 -258.786 -0.123192 0.991727 0.0360588 19.3983 0.0256396 -0.0331427 0.999122 0.213384 0.991878 0.124751 -0.0248139 -260.38 -0.123775 0.991597 0.0376171 19.6101 0.0292982 -0.0342402 0.998984 0.21057 0.992045 0.12588 0.000684721 -263.547 -0.125824 0.991405 0.0358283 20.0089 0.00383123 -0.0356294 0.999358 0.245561 0.992221 0.12306 0.0188001 -265.84 -0.123794 0.991294 0.0448452 20.3476 -0.0131178 -0.0468237 0.998817 0.305634 0.991201 0.121108 -0.0534128 -267.497 -0.118627 0.991807 0.0473997 20.5455 0.0587156 -0.0406464 0.997447 0.284753 0.99034 0.118732 -0.071623 -269.137 -0.11542 0.99212 0.0487403 20.7346 0.0768456 -0.0400028 0.99624 0.234735 0.993228 0.115922 0.00774006 -271.517 -0.116122 0.99264 0.0343848 21.0146 -0.00369713 -0.0350507 0.999379 0.234089 0.99342 0.11419 -0.00881251 -273.175 -0.113791 0.992811 0.0371093 21.2177 0.0129867 -0.0358623 0.999272 0.231236 0.993553 0.111509 -0.0204597 -275.673 -0.110631 0.993059 0.0399451 21.4956 0.0247719 -0.0374241 0.998992 0.244823 0.994031 0.108428 -0.012084 -277.33 -0.107877 0.99338 0.039477 21.69 0.0162844 -0.0379377 0.999147 0.25025 0.994348 0.105468 -0.0121615 -278.996 -0.104935 0.99374 0.0383266 21.8612 0.0161276 -0.0368338 0.999191 0.272883 0.994866 0.100606 -0.0109239 -281.526 -0.100164 0.994341 0.0353874 22.1163 0.0144223 -0.0341116 0.999314 0.287783 0.995106 0.0975719 -0.0155829 -283.232 -0.0969959 0.994696 0.0342153 22.2754 0.0188387 -0.0325363 0.999293 0.300158 0.995419 0.0940766 -0.0170714 -284.932 -0.0934527 0.995034 0.0342597 22.4337 0.0202097 -0.0325073 0.999267 0.303748 0.995938 0.0885485 -0.0163241 -287.51 -0.0879591 0.995549 0.0338508 22.6722 0.0192489 -0.0322775 0.999294 0.312661 0.996257 0.0849594 -0.0159382 -289.223 -0.0843801 0.995852 0.0340502 22.8187 0.018765 -0.0325779 0.999293 0.323755 0.996833 0.0774974 -0.0178629 -292.599 -0.0768798 0.996494 0.0329951 23.0851 0.0203573 -0.0315173 0.999296 0.344898 0.997073 0.0741571 -0.018588 -294.261 -0.0734985 0.996719 0.0339127 23.2136 0.0210419 -0.0324473 0.999252 0.350303 0.997507 0.0684482 -0.0171513 -296.764 -0.067888 0.9972 0.0313517 23.3864 0.0192493 -0.0301092 0.999361 0.374438 0.997894 0.0642245 -0.00912071 -298.4 -0.0639138 0.997473 0.0310244 23.5051 0.0110902 -0.0303761 0.999477 0.39096 0.998009 0.0606393 -0.0173725 -300.033 -0.0600782 0.997707 0.031177 23.6013 0.0192232 -0.0300712 0.999363 0.41149 0.998404 0.0547973 -0.0136492 -302.434 -0.0543642 0.998061 0.0303058 23.7339 0.0152834 -0.0295154 0.999447 0.438439 0.99861 0.0515507 -0.0110154 -304.002 -0.0512018 0.998241 0.0299045 23.8142 0.0125376 -0.0292989 0.999492 0.454368 0.998844 0.0459559 -0.0141168 -306.345 -0.0455276 0.998534 0.0292895 23.9249 0.0154421 -0.0286129 0.999471 0.485415 0.999003 0.0423068 -0.0142638 -307.904 -0.0418787 0.998699 0.0290842 23.9938 0.0154757 -0.0284579 0.999475 0.498161 0.9992 0.0371357 -0.0148707 -309.468 -0.0366328 0.998791 0.0327726 24.058 0.0160698 -0.0322016 0.999352 0.499812 0.999498 0.0299146 -0.0104387 -311.762 -0.0295971 0.999131 0.029345 24.1249 0.0113075 -0.0290213 0.999515 0.557501 0.999657 0.0261528 -0.00109554 -313.298 -0.026114 0.999299 0.0268189 24.1581 0.00179616 -0.0267811 0.99964 0.568232 0.999334 0.0243943 0.0271327 -315.579 -0.025089 0.999358 0.0255628 24.2128 -0.0264917 -0.0262265 0.999305 0.661358 0.99955 0.023504 0.018649 -317.132 -0.0239629 0.999406 0.024774 24.2522 -0.0180556 -0.0252097 0.999519 0.706026 0.997595 0.029134 -0.0628854 -320.305 -0.0273988 0.999224 0.0282817 24.3469 0.0636606 -0.0264907 0.99762 0.670666 0.999369 0.0305266 0.0181698 -322.611 -0.0309558 0.999237 0.0238262 24.4203 -0.0174286 -0.0243736 0.999551 0.691945 0.999504 0.0303183 -0.00852577 -325.024 -0.0301687 0.999398 0.0171565 24.4766 0.00904079 -0.0168907 0.999816 0.740148 0.999565 0.0294543 0.00160165 -327.434 -0.0294776 0.999416 0.0172965 24.5571 -0.00109125 -0.0173362 0.999849 0.770994 0.999496 0.028717 -0.0135297 -329.895 -0.0284482 0.999402 0.0196554 24.6351 0.0140861 -0.0192606 0.999715 0.806037 0.99954 0.0282116 -0.0111178 -331.539 -0.0279839 0.999406 0.0201282 24.6886 0.0116791 -0.0198078 0.999736 0.825449 0.999593 0.0267237 -0.00996595 -334.082 -0.0264576 0.999313 0.0259404 24.7767 0.0106523 -0.0256662 0.999614 0.844923 0.999661 0.0255248 -0.00515552 -336.651 -0.0253679 0.999272 0.0285031 24.8603 0.0058793 -0.0283627 0.99958 0.88025 0.999737 0.0228756 0.00139897 -338.385 -0.0229055 0.999359 0.0275261 24.8973 -0.000768393 -0.027551 0.99962 0.930565 0.999797 0.019977 -0.00268281 -341.022 -0.0199172 0.999587 0.0207058 24.9532 0.00309534 -0.0206481 0.999782 0.978167 0.999855 0.0157109 -0.00663881 -343.693 -0.0155796 0.999691 0.0193853 24.9913 0.00694132 -0.019279 0.99979 1.01343 0.999895 0.0143633 -0.00205703 -345.46 -0.0143207 0.99971 0.0193815 25.0193 0.00233482 -0.01935 0.99981 1.04275 0.999928 0.0119012 -0.00149879 -347.264 -0.0118723 0.999767 0.0180211 25.05 0.00171292 -0.018002 0.999836 1.08007 0.999949 0.00949292 -0.00337679 -349.065 -0.00943409 0.99981 0.0170312 25.0689 0.00353783 -0.0169985 0.999849 1.11607 0.999955 0.0058448 -0.00753622 -351.841 -0.0056981 0.999797 0.0193422 25.0882 0.00764774 -0.0192984 0.999785 1.15805 0.999973 0.00304673 -0.00665815 -353.698 -0.00291616 0.999805 0.019532 25.1063 0.00671636 -0.0195121 0.999787 1.18987 0.999996 8.7427e-05 -0.0028363 -356.49 -3.31167e-05 0.999817 0.0191427 25.1049 0.00283746 -0.0191425 0.999813 1.24878 0.999959 -0.00163332 -0.00887036 -358.352 0.00180562 0.999809 0.0194504 25.0924 0.0088369 -0.0194656 0.999771 1.27635 0.999957 -0.00490232 -0.00790947 -361.113 0.00505922 0.999788 0.01994 25.0876 0.00781005 -0.0199792 0.99977 1.32503 0.999983 -0.00566245 -0.00109184 -363.825 0.00568162 0.999814 0.0184349 25.0604 0.000987246 -0.0184408 0.999829 1.38784 0.999955 -0.00618292 -0.00715033 -366.552 0.0063065 0.999829 0.0173917 25.0749 0.00704158 -0.017436 0.999823 1.43543 0.999964 -0.00564585 -0.00634923 -369.264 0.00576863 0.999793 0.019488 25.0612 0.00623789 -0.019524 0.99979 1.48164 0.999983 -0.00575017 0.000853569 -371.045 0.00573208 0.999786 0.0198697 25.0484 -0.000967641 -0.0198645 0.999802 1.52729 0.999131 -0.00705057 0.0410898 -373.703 0.0060836 0.999703 0.0236108 25.0421 -0.041244 -0.0233403 0.998876 1.64374 0.998278 -0.00995584 -0.0578082 -377.416 0.0113323 0.999659 0.0235322 25.0082 0.0575542 -0.0241468 0.99805 1.68177 0.999032 -0.00983989 -0.0428701 -379.166 0.010785 0.999703 0.0218701 25.0127 0.0426421 -0.0223113 0.998841 1.72402 0.999778 -0.013301 0.0163483 -381.869 0.0129274 0.999658 0.0227499 24.966 -0.0166453 -0.0225335 0.999608 1.73717 0.999782 -0.0139745 -0.0155314 -384.678 0.0143492 0.999602 0.0242789 24.9442 0.0151859 -0.0244965 0.999585 1.7994 0.999877 -0.0152573 0.00373039 -386.506 0.0151606 0.99958 0.0247067 24.926 -0.00410578 -0.0246471 0.999688 1.83835 0.999841 -0.0173512 -0.00402572 -389.306 0.0174415 0.999569 0.0235957 24.8812 0.00361457 -0.0236622 0.999713 1.89057 0.999814 -0.0185302 -0.00527802 -392.103 0.0186522 0.999535 0.0241027 24.837 0.00482894 -0.0241967 0.999696 1.95473 0.999796 -0.0201831 0.00057899 -393.915 0.0201636 0.999515 0.0237341 24.7987 -0.00105774 -0.0237176 0.999718 1.99927 0.999739 -0.0222626 -0.00521892 -396.687 0.0223878 0.999429 0.0252981 24.7466 0.00465274 -0.0254083 0.999666 2.05466 0.999684 -0.0245464 -0.00546775 -398.499 0.024697 0.999262 0.0294202 24.7067 0.00474156 -0.0295459 0.999552 2.10405 0.99956 -0.0294245 0.00366955 -401.173 0.0293092 0.99917 0.0282728 24.6325 -0.00449842 -0.0281529 0.999594 2.18162 0.999485 -0.0320842 -0.000116935 -402.946 0.032075 0.999094 0.0279865 24.5787 -0.000781097 -0.0279759 0.999608 2.22155 0.999346 -0.0360622 -0.00283087 -405.561 0.0361284 0.998938 0.0285845 24.4924 0.00179705 -0.0286681 0.999587 2.29262 0.999147 -0.0412857 4.14073e-05 -408.164 0.041266 0.998701 0.0298904 24.3937 -0.0012754 -0.0298632 0.999553 2.35737 0.998882 -0.0469268 0.00575776 -410.76 0.0467419 0.998489 0.0288785 24.2841 -0.00710424 -0.0285771 0.999566 2.41511 0.99845 -0.0516623 0.0207126 -413.319 0.0508469 0.997978 0.0381302 24.1663 -0.0226407 -0.0370179 0.999058 2.55147 0.997981 -0.0574325 -0.0271178 -415.969 0.0581762 0.997928 0.0274818 24.0148 0.0254833 -0.0290039 0.999254 2.57617 0.998188 -0.0601172 -0.00242976 -418.524 0.0601613 0.997813 0.027388 23.8612 0.000777954 -0.0274845 0.999622 2.62522 0.997865 -0.0650063 0.00631239 -421.145 0.064804 0.997499 0.028213 23.7 -0.00813063 -0.0277437 0.999582 2.67893 0.99769 -0.0671383 -0.0103945 -422.909 0.0674016 0.997347 0.02749 23.592 0.00852134 -0.0281271 0.999568 2.71808 0.997437 -0.0711674 -0.00740176 -425.522 0.0713607 0.996989 0.0303461 23.405 0.00521981 -0.0307965 0.999512 2.75957 0.997141 -0.0747929 -0.0107704 -428.137 0.0750983 0.996679 0.0314737 23.2287 0.00838061 -0.0321926 0.999447 2.81222 0.996974 -0.0775109 -0.00586713 -429.859 0.0776619 0.996445 0.032657 23.0893 0.003315 -0.0330138 0.999449 2.86069 0.996598 -0.0816133 0.0114513 -432.445 0.0812069 0.996173 0.0323378 22.8882 -0.0140466 -0.0312978 0.999411 2.94063 0.996436 -0.0843032 -0.0030391 -434.18 0.0843543 0.99608 0.0266449 22.7428 0.000780933 -0.0268063 0.99964 2.99036 0.996172 -0.0870504 -0.00802828 -435.889 0.0872307 0.995854 0.0258132 22.5963 0.00574794 -0.0264147 0.999635 3.02332 0.995883 -0.0905818 -0.00332594 -437.602 0.0906376 0.995549 0.0258191 22.4451 0.000972397 -0.0260143 0.999661 3.05477 0.995159 -0.0982066 -0.00362444 -440.155 0.0982689 0.99479 0.0271251 22.2007 0.000941686 -0.02735 0.999625 3.11755 0.994631 -0.103447 0.0027378 -441.857 0.103342 0.994307 0.025981 22.0205 -0.00540988 -0.0255586 0.999659 3.16754 0.993836 -0.110847 -0.00177387 -444.411 0.110853 0.993439 0.0281195 21.745 -0.00135473 -0.0281428 0.999603 3.23396 0.992769 -0.117799 0.0230724 -446.905 0.116976 0.992544 0.0342429 21.447 -0.0269342 -0.0312963 0.999147 3.32362 0.992016 -0.125437 0.0130368 -449.44 0.124928 0.991569 0.034401 21.1706 -0.017242 -0.0324976 0.999323 3.44843 0.989299 -0.133161 -0.0596349 -452.053 0.134673 0.990644 0.0220872 20.8211 0.0561358 -0.029882 0.997976 3.45349 0.989892 -0.140528 0.0191221 -454.532 0.139927 0.989714 0.0297864 20.4547 -0.0231112 -0.0268097 0.999373 3.50286 0.9893 -0.145163 -0.0146324 -457.187 0.145512 0.989 0.0265551 20.0962 0.0106166 -0.0284002 0.99954 3.56133 0.988686 -0.149858 -0.00650181 -459.774 0.149984 0.988274 0.0286308 19.7002 0.002135 -0.029282 0.999569 3.59908 0.988185 -0.15306 -0.00793637 -461.491 0.153226 0.987789 0.0282058 19.4538 0.00352226 -0.0290886 0.999571 3.63819 0.987715 -0.15577 -0.0124748 -463.192 0.156054 0.987395 0.0264258 19.1787 0.00820122 -0.0280479 0.999573 3.68215 0.987295 -0.158841 -0.00414347 -464.873 0.158895 0.986906 0.0277164 18.9001 -0.000313294 -0.0280226 0.999607 3.71295 0.986884 -0.161298 -0.00660495 -466.527 0.161416 0.986545 0.0259536 18.625 0.00232982 -0.0266794 0.999641 3.75459 0.986101 -0.165941 -0.00832262 -469.022 0.166099 0.985796 0.0248325 18.2153 0.00408369 -0.0258697 0.999657 3.80843 0.985397 -0.169968 -0.0101606 -471.472 0.170171 0.985111 0.0244448 17.8014 0.00585447 -0.0258169 0.99965 3.85973 0.984948 -0.172681 -0.00771752 -473.096 0.17282 0.984657 0.0241501 17.5142 0.00342883 -0.0251204 0.999679 3.90182 0.984522 -0.175002 -0.00947717 -475.514 0.175187 0.984226 0.0246914 17.0886 0.00500661 -0.0259696 0.99965 3.94995 0.984006 -0.177959 0.00793513 -477.848 0.177745 0.983821 0.0224158 16.6479 -0.0117958 -0.0206468 0.999717 4.0088 0.982997 -0.18038 0.0343461 -479.381 0.179202 0.983197 0.0347811 16.381 -0.0400428 -0.0280348 0.998805 4.10431 0.982579 -0.1823 -0.0361205 -481.794 0.183314 0.982682 0.0270722 15.9562 0.0305597 -0.033222 0.998981 4.14648 0.980832 -0.184732 -0.0619922 -483.392 0.186184 0.982341 0.0184842 15.6384 0.0574829 -0.0296718 0.997905 4.12267 0.98212 -0.188196 0.00482605 -485.689 0.187946 0.981644 0.0324288 15.2046 -0.0108404 -0.0309419 0.999462 4.16434 0.981704 -0.190216 -0.00867023 -487.295 0.190383 0.981339 0.0269665 14.9052 0.00337898 -0.0281238 0.999599 4.18878 0.981102 -0.192818 -0.0160984 -488.871 0.193194 0.980803 0.0264813 14.589 0.0106833 -0.029091 0.99952 4.22536 0.980597 -0.196013 -0.00287081 -491.174 0.19602 0.98025 0.0261913 14.1281 -0.00231972 -0.0262459 0.999653 4.28774 0.979575 -0.201059 -0.00290335 -493.494 0.201073 0.979309 0.0228807 13.6537 -0.00175711 -0.0229972 0.999734 4.34931 0.978951 -0.203992 -0.00652203 -495.017 0.204089 0.978679 0.0231235 13.3342 0.00166597 -0.0239678 0.999711 4.38809 0.977935 -0.208892 -0.00255512 -497.29 0.208895 0.977667 0.0230138 12.8497 -0.00230934 -0.0230398 0.999732 4.44865 0.977195 -0.212324 -0.00288227 -498.794 0.212335 0.976948 0.0220456 12.5298 -0.00186498 -0.0221549 0.999753 4.48834 0.976285 -0.216303 -0.00896461 -501.041 0.216444 0.976085 0.020235 12.0303 0.00437334 -0.0216955 0.999755 4.54269 0.975731 -0.218794 -0.00885645 -502.519 0.218937 0.975504 0.0213982 11.7008 0.00395771 -0.0228179 0.999732 4.58038 0.974982 -0.222119 -0.00857852 -504.721 0.222254 0.974758 0.0211915 11.2002 0.00365496 -0.0225679 0.999739 4.61996 0.974711 -0.223227 -0.010402 -506.189 0.223413 0.974457 0.0228229 10.8668 0.00504158 -0.0245696 0.999685 4.65429 0.974342 -0.224903 -0.00878071 -508.365 0.22505 0.974065 0.0234304 10.3626 0.00328344 -0.0248053 0.999687 4.70445 0.974006 -0.226364 -0.00842699 -510.55 0.226503 0.973726 0.0235374 9.86273 0.00287756 -0.0248343 0.999687 4.74482 0.973667 -0.227779 -0.0094546 -512.712 0.227947 0.973357 0.0248285 9.36888 0.00354729 -0.0263298 0.999647 4.7995 0.973123 -0.23014 -0.00821582 -515.56 0.230278 0.972779 0.0259557 8.70132 0.00201873 -0.02715 0.999629 4.86491 0.972813 -0.231251 -0.0125416 -516.985 0.231506 0.972498 0.0255373 8.37043 0.00629119 -0.0277464 0.999595 4.8903 0.972266 -0.233444 -0.0142354 -518.4 0.233752 0.971938 0.0263872 8.03121 0.00767598 -0.028983 0.99955 4.92187 0.971148 -0.238265 -0.0100618 -520.472 0.238453 0.970786 0.0267248 7.51318 0.00340027 -0.028353 0.999592 4.96143 0.969958 -0.242588 -0.0182311 -522.553 0.243054 0.969538 0.0303371 7.00865 0.0103163 -0.0338568 0.999373 5.00231 0.969303 -0.245402 -0.0151188 -523.926 0.24577 0.968818 0.031453 6.65923 0.00692874 -0.0342033 0.999391 5.03115 0.968671 -0.248172 -0.00933906 -525.286 0.248346 0.968115 0.032817 6.3185 0.000897026 -0.0341082 0.999418 5.06405 0.968052 -0.250749 0.00107882 -527.315 0.250656 0.967793 0.0234028 5.77552 -0.0069123 -0.0223847 0.999726 5.14803 0.967589 -0.252404 -0.00793092 -529.328 0.252521 0.967335 0.0222802 5.25403 0.00204824 -0.0235608 0.99972 5.18019 0.967247 -0.253715 -0.00788434 -531.313 0.253815 0.967112 0.0164952 4.71509 0.00343996 -0.0179561 0.999833 5.23154 0.965959 -0.258511 -0.00975336 -533.287 0.258671 0.965692 0.0229841 4.19529 0.0034771 -0.0247246 0.999688 5.26071 0.964361 -0.264591 1.77277e-05 -535.878 0.264478 0.963949 0.029229 3.48094 -0.00775084 -0.0281826 0.999573 5.33391 0.961745 -0.272349 0.0295366 -537.779 0.270795 0.961447 0.0478558 2.96858 -0.0414314 -0.0380267 0.998417 5.46505 0.961067 -0.276314 0.00106827 -539.081 0.276131 0.960559 0.0328195 2.59443 -0.0100946 -0.0312467 0.999461 5.49416 0.958678 -0.280905 -0.0450363 -540.428 0.281957 0.959241 0.0188862 2.19998 0.0378955 -0.0308041 0.998807 5.49052 0.957985 -0.284056 -0.0397118 -541.708 0.284902 0.958398 0.0174556 1.81186 0.0331013 -0.0280362 0.999059 5.52533 0.957045 -0.289926 0.00281715 -543.558 0.289709 0.956627 0.03054 1.24978 -0.0115493 -0.028412 0.99953 5.54742 0.956236 -0.29253 -0.0062757 -544.812 0.292592 0.955865 0.0266685 0.873219 -0.00180263 -0.0273376 0.999625 5.58018 0.955445 -0.294995 -0.0101159 -546.675 0.295167 0.955007 0.0289674 0.307383 0.00111548 -0.0306626 0.999529 5.63096 0.954969 -0.296676 -0.00424691 -548.478 0.296666 0.954503 0.0302126 -0.249534 -0.00490967 -0.030112 0.999534 5.69379 0.95483 -0.297145 -0.00212314 -550.257 0.297081 0.954419 0.0287696 -0.821451 -0.00652237 -0.0281008 0.999584 5.74884 0.955275 -0.295708 -0.00266752 -551.425 0.295668 0.954899 0.0273557 -1.17149 -0.00554207 -0.0269209 0.999622 5.79722 0.957655 -0.287913 -0.00162944 -553.138 0.287882 0.957431 0.0212168 -1.68271 -0.00454852 -0.0207874 0.999774 5.85474 0.964984 -0.262264 -0.00492988 -555.293 0.26231 0.964845 0.0163473 -2.26499 0.000469252 -0.0170681 0.999854 5.93423 0.971277 -0.237898 -0.00505828 -556.313 0.237945 0.971192 0.01297 -2.49445 0.00182703 -0.013801 0.999903 5.95001 0.983214 -0.182392 -0.00486583 -557.776 0.182442 0.983124 0.0134663 -2.72394 0.00232757 -0.014128 0.999897 5.99324 0.994587 -0.103296 -0.0112754 -559.261 0.103454 0.99453 0.0144268 -2.80855 0.00972352 -0.0155152 0.999832 5.99769 0.999971 -0.00471001 -0.00599295 -560.696 0.00488331 0.999561 0.0292392 -2.72253 0.0058526 -0.0292676 0.999554 6.02173 0.993989 0.10883 -0.0118797 -562.096 -0.108307 0.993383 0.0382018 -2.45868 0.0159586 -0.0366855 0.999199 6.03157 0.971352 0.237529 -0.00746513 -563.429 -0.23692 0.970359 0.0476638 -2.01597 0.0185654 -0.0445297 0.998836 6.04912 0.928686 0.370798 0.00716294 -564.667 -0.370746 0.927716 0.0434795 -1.40426 0.00947695 -0.0430344 0.999029 6.06765 0.861374 0.507965 -0.0024877 -565.822 -0.507392 0.860616 0.0435038 -0.626334 0.0242394 -0.0362108 0.99905 6.05808 0.768888 0.639318 -0.00917871 -566.816 -0.638378 0.768404 0.0450416 0.337394 0.0358489 -0.0287725 0.998943 6.04308 0.656681 0.754118 -0.00867261 -567.635 -0.753116 0.656329 0.0452734 1.45563 0.0398336 -0.0231987 0.998937 5.99045 0.479676 0.877435 -0.00439832 -568.435 -0.876323 0.479309 0.0481786 3.18259 0.0443817 -0.0192558 0.998829 5.94198 0.344415 0.938691 0.015445 -568.834 -0.938147 0.3435 0.0434553 4.63099 0.0354857 -0.0294563 0.998936 5.8996 0.239136 0.97088 0.0143429 -569.144 -0.970697 0.238678 0.0279422 6.11682 0.0237052 -0.0206046 0.999507 5.91995 0.185094 0.98251 0.020351 -569.292 -0.982688 0.184879 0.0120184 7.16684 0.00804574 -0.0222232 0.999721 5.90536 0.137762 0.99004 0.0290431 -569.378 -0.990465 0.137678 0.00486129 8.20873 0.000814272 -0.0294359 0.999566 5.90777 0.0774608 0.996651 0.0262201 -569.453 -0.996306 0.0764028 0.0391974 9.86849 0.0370628 -0.0291595 0.998887 5.87854 0.0378791 0.99875 0.0326271 -569.496 -0.997331 0.0357455 0.0636657 11.6091 0.0624198 -0.0349516 0.997438 5.87768 0.0190418 0.999393 0.029176 -569.518 -0.998911 0.0177729 0.0431483 12.7502 0.0426035 -0.0299658 0.998643 5.85198 0.00916548 0.999399 0.0334382 -569.495 -0.999703 0.00840267 0.0228821 13.902 0.0225874 -0.0336379 0.999179 5.82486 -0.000250877 0.999627 0.0273238 -569.507 -0.999472 -0.00113818 0.0324631 15.7514 0.0324821 -0.0273012 0.999099 5.80554 -0.00435624 0.999578 0.0287165 -569.471 -0.999502 -0.00524993 0.0311194 17.6125 0.0312571 -0.0285666 0.999103 5.8017 -0.00583657 0.999654 0.0256456 -569.45 -0.999507 -0.00662308 0.0306912 19.524 0.0308504 -0.0254538 0.9992 5.7673 -0.00796209 0.999631 0.0259834 -569.416 -0.999401 -0.00882982 0.0334535 21.4733 0.0336706 -0.0257015 0.999102 5.75569 -0.00937688 0.999538 0.028927 -569.386 -0.999524 -0.0102196 0.0291224 23.4789 0.0294046 -0.0286401 0.999157 5.71595 -0.0093671 0.999256 0.037418 -569.341 -0.999399 -0.0106045 0.0330101 25.5281 0.0333824 -0.0370863 0.998754 5.70438 -0.00878897 0.999197 0.0391031 -569.314 -0.999374 -0.0101169 0.0338928 26.9525 0.0342612 -0.0387808 0.99866 5.68576 -0.00809979 0.999025 0.0434098 -569.259 -0.999464 -0.00946451 0.0313255 29.1094 0.0317058 -0.0431328 0.998566 5.66575 -0.00859079 0.999049 0.0427429 -569.252 -0.999346 -0.0100795 0.0347372 30.5719 0.035135 -0.0424165 0.998482 5.65398 -0.00723438 0.998999 0.0441324 -569.238 -0.999547 -0.00851396 0.0288755 32.8219 0.0292223 -0.0439035 0.998608 5.61998 -0.00680394 0.99897 0.0448582 -569.198 -0.999641 -0.00795695 0.0255751 34.3662 0.0259056 -0.0446681 0.998666 5.61378 -0.00590974 0.998884 0.0468684 -569.191 -0.999656 -0.00709869 0.025242 35.9119 0.0255466 -0.0467031 0.998582 5.60155 -0.00481732 0.998694 0.0508579 -569.16 -0.999705 -0.00601999 0.0235211 38.2796 0.0237966 -0.0507296 0.998429 5.58967 -0.00427881 0.998493 0.0547074 -569.146 -0.999672 -0.00565149 0.0249612 39.8988 0.0252328 -0.0545827 0.99819 5.59022 -0.00387743 0.998398 0.056456 -569.122 -0.999689 -0.00526061 0.0243721 41.5044 0.0246301 -0.0563439 0.998108 5.5818 -0.00313651 0.998227 0.0594335 -569.1 -0.999681 -0.00461966 0.0248338 43.1587 0.0250643 -0.0593366 0.997923 5.57473 -0.00305477 0.997471 0.0710069 -569.061 -0.999493 -0.00529675 0.0314073 45.6333 0.031704 -0.0708749 0.996981 5.55352 -0.00219363 0.997099 0.0760821 -569.034 -0.999541 -0.00448469 0.0299552 47.3096 0.0302095 -0.0759815 0.996651 5.54753 -0.000861037 0.996834 0.0795058 -569.015 -0.999559 -0.00321906 0.0295351 48.9993 0.0296975 -0.0794453 0.996397 5.54404 0.00131952 0.996933 0.078245 -569.023 -0.999543 -0.001048 0.030209 50.691 0.0301984 -0.0782491 0.996476 5.5348 0.00190664 0.997747 0.067055 -569.027 -0.999645 0.000119681 0.0266431 53.2084 0.0265751 -0.067082 0.997393 5.54219 0.00205102 0.998122 0.0612182 -569.031 -0.999432 -1.39478e-05 0.0337118 54.8971 0.0336494 -0.0612525 0.997555 5.53657 0.00162857 0.998427 0.0560408 -569.039 -0.999354 -0.000387349 0.0359427 57.4472 0.0359079 -0.0560631 0.997781 5.50253 0.000778352 0.998513 0.0545138 -569.032 -0.999317 -0.00123721 0.0369299 59.1216 0.0369424 -0.0545053 0.99783 5.47277 0.000532481 0.998549 0.0538454 -569.019 -0.999343 -0.0014193 0.036203 60.7649 0.0362269 -0.0538294 0.997893 5.46938 0.000613485 0.998688 0.0512103 -569.014 -0.99938 -0.00118994 0.0351781 62.4265 0.0351928 -0.0512001 0.998068 5.45086 0.000590679 0.998489 0.0549415 -569.001 -0.999244 -0.00154612 0.0388417 64.0634 0.0388679 -0.0549229 0.997734 5.42115 -0.000653438 0.998184 0.0602367 -569.004 -0.999405 -0.00272922 0.0343846 66.4566 0.0344866 -0.0601784 0.997592 5.40434 0.000141602 0.998069 0.062108 -568.982 -0.999497 -0.00182788 0.0316526 68.0592 0.0317051 -0.0620813 0.997567 5.39173 0.00121243 0.998034 0.0626608 -568.965 -0.999168 -0.0013447 0.0407508 69.6266 0.0407549 -0.0626581 0.997203 5.36614 0.000703197 0.998468 0.0553287 -568.979 -0.998981 -0.00179578 0.0451033 71.1205 0.0451335 -0.055304 0.997449 5.40359 0.00072545 0.99884 0.0481566 -568.99 -0.999712 -0.000429763 0.023974 72.5792 0.0239669 -0.0481601 0.998552 5.38315 -3.69282e-05 0.998795 0.0490828 -568.974 -0.999653 -0.00132953 0.0263028 73.9898 0.0263364 -0.0490648 0.998448 5.36192 0.000874616 0.99925 0.0387205 -568.992 -0.999458 -0.000401045 0.0329253 76.046 0.0329161 -0.0387283 0.998707 5.36294 0.000386348 0.999137 0.0415384 -568.967 -0.999682 -0.000662265 0.0252277 78.0452 0.0252334 -0.0415349 0.998818 5.36022 0.00113285 0.999167 0.0407919 -568.974 -0.999647 4.89315e-05 0.0265631 79.3788 0.026539 -0.0408076 0.998815 5.35357 0.000202282 0.998877 0.04737 -568.973 -0.999999 0.000123279 0.00167071 80.6667 0.00166299 -0.0473703 0.998876 5.38485 0.00227565 0.998753 0.0498646 -568.935 -0.999645 0.000948046 0.0266316 82.6861 0.0265511 -0.0499075 0.998401 5.32998 0.00387565 0.998604 0.0526848 -568.927 -0.997095 -0.000148677 0.0761674 84.0392 0.0760689 -0.0528269 0.995702 5.31881 0.00138086 0.998409 0.0563699 -568.925 -0.999046 -0.00108263 0.0436483 85.3217 0.0436399 -0.0563764 0.997455 5.37409 -0.000926067 0.998518 0.0544159 -568.919 -0.999961 -0.00140265 0.00872057 86.6467 0.00878397 -0.0544057 0.99848 5.31879 -0.000600425 0.9985 0.0547536 -568.91 -0.999527 -0.00228223 0.0306586 88.0502 0.0307376 -0.0547093 0.998029 5.29375 -0.00100997 0.998557 0.0536988 -568.903 -0.999524 -0.00266439 0.0307467 90.103 0.0308454 -0.0536421 0.998084 5.29672 -0.00121846 0.998489 0.0549315 -568.902 -0.999709 -0.00253865 0.02397 91.488 0.0240733 -0.0548864 0.998202 5.28172 -0.000689193 0.998696 0.0510435 -568.907 -0.999587 -0.00215483 0.0286639 92.9035 0.0287366 -0.0510027 0.998285 5.28115 -0.000230422 0.998819 0.0485793 -568.908 -0.999626 -0.00155813 0.0272947 94.2952 0.0273381 -0.0485548 0.998446 5.28027 -0.000171665 0.998963 0.0455312 -568.903 -0.999657 -0.00136385 0.0261541 95.6943 0.0261891 -0.0455111 0.99862 5.27016 0.000234362 0.998925 0.0463491 -568.892 -0.99957 -0.00112466 0.0292933 97.0958 0.0293139 -0.046336 0.998496 5.24581 0.000461175 0.998866 0.0476084 -568.89 -0.999467 -0.00109355 0.0326254 98.4676 0.0326404 -0.047598 0.998333 5.25558 0.000845986 0.998747 0.0500414 -568.878 -0.999535 -0.000680031 0.0304702 99.8397 0.0304661 -0.0500439 0.998282 5.23394 0.000316358 0.998632 0.0522912 -568.872 -0.99955 -0.00125201 0.0299575 101.869 0.0299819 -0.0522771 0.998182 5.2402 -0.000386352 0.998691 0.0511515 -568.867 -0.999536 -0.00194327 0.0303912 103.248 0.0304508 -0.051116 0.998228 5.22684 -0.00161372 0.99883 0.0483387 -568.869 -0.999538 -0.003079 0.0302537 104.612 0.0303671 -0.0482676 0.998373 5.23986 -0.00234222 0.999015 0.0443011 -568.859 -0.999512 -0.00371881 0.0310167 105.983 0.0311509 -0.0442068 0.998537 5.22603 -0.0027358 0.999037 0.0437928 -568.86 -0.999529 -0.0040698 0.0304016 107.364 0.0305505 -0.043689 0.998578 5.20975 -0.00354773 0.998953 0.0456027 -568.851 -0.999897 -0.00417755 0.013723 108.71 0.0138991 -0.0455493 0.998865 5.19832 -0.00407342 0.998483 0.054908 -568.825 -0.999936 -0.00348566 -0.010796 110.063 -0.0105882 -0.0549484 0.998433 5.25919 -0.00171532 0.999196 0.0400488 -568.845 -0.999715 -0.0026669 0.0237192 111.503 0.0238069 -0.0399967 0.998916 5.2224 0.00174254 0.999244 0.0388364 -568.831 -0.997185 -0.00117469 0.0749666 112.934 0.0749556 -0.0388577 0.996429 5.19537 -0.000376636 0.999323 0.0367943 -568.835 -0.999486 -0.00155577 0.0320233 114.219 0.0320588 -0.0367633 0.99881 5.2718 0.000101892 0.999499 0.031659 -568.846 -0.999997 0.000181114 -0.00249952 115.617 -0.002504 -0.0316586 0.999496 5.20019 0.000289135 0.999419 0.0340905 -568.834 -0.999717 -0.000522741 0.023804 117.063 0.023808 -0.0340877 0.999135 5.18427 -0.000365855 0.999202 0.0399283 -568.817 -0.999655 -0.0014143 0.0262332 118.437 0.0262687 -0.0399049 0.998858 5.21425 -0.00112576 0.998949 0.045818 -568.803 -0.999845 -0.00192866 0.0174831 119.845 0.0175531 -0.0457912 0.998797 5.20218 -0.00147551 0.997933 0.0642524 -568.772 -0.999619 -0.00324341 0.0274193 122.613 0.027571 -0.0641875 0.997557 5.20014 -0.00229993 0.997705 0.0676764 -568.749 -0.999699 -0.00394697 0.0242133 123.978 0.0244249 -0.0676003 0.997413 5.20817 -0.00253771 0.997677 0.0680728 -568.725 -0.999654 -0.00431316 0.0259474 126.032 0.0261808 -0.0679834 0.997343 5.21525 -0.00327657 0.997734 0.0672014 -568.727 -0.999819 -0.00452852 0.0184859 127.373 0.0187483 -0.0671287 0.997568 5.21248 -0.000950308 0.997766 0.0667919 -568.717 -0.999697 -0.00259037 0.0244725 129.423 0.0245908 -0.0667484 0.997467 5.22158 0.00289788 0.997724 0.0673654 -568.709 -0.998205 -0.00114395 0.0598827 130.817 0.0598235 -0.067418 0.99593 5.17521 0.00259492 0.996941 0.0781088 -568.68 -0.99953 0.000198626 0.0306711 132.803 0.0305618 -0.0781516 0.996473 5.16182 0.00352975 0.99661 0.0821973 -568.676 -0.99943 0.00075621 0.0337492 134.154 0.0335727 -0.0822695 0.996044 5.13371 0.00325845 0.996124 0.087904 -568.656 -0.999207 -0.000243972 0.0398036 135.485 0.0396708 -0.087964 0.995333 5.13119 -0.00159921 0.995352 0.0962943 -568.635 -0.999576 -0.0043923 0.0288008 137.473 0.0290899 -0.0962074 0.994936 5.11703 -0.00507597 0.994863 0.101106 -568.616 -0.999539 -0.00807531 0.0292782 138.809 0.0299442 -0.100911 0.994445 5.10528 -0.0114637 0.994672 0.102448 -568.599 -0.999575 -0.0141437 0.025472 140.095 0.0267853 -0.102112 0.994412 5.13158 -0.0209828 0.994505 0.102568 -568.546 -0.999621 -0.022695 0.0155552 141.406 0.0177975 -0.102203 0.994604 5.10613 -0.0370943 0.993665 0.106081 -568.435 -0.998709 -0.0405502 0.030608 143.358 0.0347157 -0.104808 0.993886 5.11341 -0.0577576 0.99322 0.100889 -568.351 -0.997713 -0.0609802 0.0291529 144.616 0.0351075 -0.098975 0.99447 5.10382 -0.0873696 0.990927 0.102126 -568.22 -0.995732 -0.0899304 0.020737 145.858 0.0297331 -0.0998785 0.994555 5.08467 -0.125437 0.986915 0.101312 -568.031 -0.991645 -0.127821 0.0173674 147.077 0.03009 -0.0982867 0.994703 5.09234 -0.167757 0.981196 0.0954577 -567.814 -0.985345 -0.169918 0.0149123 148.305 0.0308518 -0.0915571 0.995322 5.07554 -0.21235 0.972386 0.0968132 -567.515 -0.976636 -0.214532 0.0125971 149.506 0.0330187 -0.0918762 0.995223 5.05119 -0.283036 0.955444 0.0837672 -566.979 -0.958802 -0.284076 0.000516312 151.257 0.0242895 -0.08017 0.996485 5.07124 -0.376144 0.923214 0.0786828 -566.021 -0.926131 -0.3772 -0.00155496 153.548 0.0282436 -0.0734554 0.996898 5.01218 -0.424506 0.902119 0.0773008 -565.46 -0.904908 -0.425599 -0.00256828 154.644 0.0305822 -0.0710404 0.997005 5.04475 -0.471205 0.878911 0.0740356 -564.844 -0.881419 -0.472329 -0.00262433 155.729 0.0326626 -0.066493 0.997252 4.98811 -0.518082 0.851199 0.083973 -564.149 -0.854406 -0.519585 -0.00455407 156.766 0.0397547 -0.0741065 0.996458 4.95396 -0.565206 0.820417 0.0863638 -563.394 -0.824068 -0.566339 -0.0131232 157.759 0.0381447 -0.078587 0.996177 4.96652 -0.61064 0.788342 0.0750701 -562.608 -0.790934 -0.611843 -0.00844547 158.737 0.0392732 -0.0645326 0.997142 4.95538 -0.652371 0.754045 0.0763438 -561.738 -0.756447 -0.654043 -0.0039989 159.697 0.0469168 -0.0603588 0.997074 4.87556 -0.693989 0.715409 0.0810494 -560.808 -0.718235 -0.695747 -0.00868359 160.57 0.0501775 -0.0642388 0.996672 4.87103 -0.734623 0.674561 0.0727745 -559.825 -0.676902 -0.735992 -0.0109354 161.416 0.0461849 -0.0572946 0.997288 4.83455 -0.788824 0.610084 0.074523 -558.238 -0.612468 -0.790399 -0.0123442 162.586 0.0513719 -0.0553804 0.997143 4.76372 -0.821103 0.565916 0.0743512 -557.122 -0.56833 -0.822669 -0.0147442 163.303 0.0528225 -0.0543625 0.997123 4.73086 -0.848316 0.524307 0.0738985 -555.951 -0.526469 -0.850109 -0.012093 163.976 0.0564813 -0.0491639 0.997192 4.68351 -0.870806 0.485763 0.075701 -554.732 -0.488042 -0.872708 -0.0140158 164.603 0.0592565 -0.0491503 0.997032 4.62918 -0.891141 0.44767 0.0738878 -553.494 -0.449768 -0.893038 -0.0138129 165.186 0.059801 -0.0455416 0.997171 4.57394 -0.907463 0.412761 0.0783518 -552.197 -0.415111 -0.909634 -0.015778 165.732 0.0647589 -0.0468427 0.996801 4.50062 -0.920813 0.380665 0.0848372 -550.894 -0.383391 -0.923412 -0.0179204 166.224 0.071518 -0.0490271 0.996234 4.44367 -0.93269 0.350687 0.08431 -549.575 -0.353417 -0.935263 -0.0195015 166.697 0.072013 -0.0479855 0.996249 4.38422 -0.942425 0.324758 0.0797996 -548.259 -0.327351 -0.944658 -0.0215248 167.128 0.0683929 -0.0464079 0.996579 4.32236 -0.949301 0.303696 0.0812231 -546.904 -0.306537 -0.951534 -0.0248496 167.523 0.0697398 -0.0484876 0.996386 4.24293 -0.956726 0.279899 0.0795769 -544.917 -0.282387 -0.959054 -0.0217251 168.094 0.0702378 -0.0432565 0.996592 4.18212 -0.961138 0.265707 0.0749212 -543.57 -0.267733 -0.963321 -0.0182433 168.461 0.0673258 -0.0375932 0.997023 4.10127 -0.96427 0.254233 0.0744916 -542.199 -0.256109 -0.966505 -0.0166547 168.802 0.0677623 -0.0351376 0.997083 4.04141 -0.966757 0.249181 0.05735 -540.148 -0.250222 -0.968119 -0.0116336 169.348 0.0526227 -0.0255971 0.998286 3.97513 -0.968046 0.24669 0.0450689 -538.777 -0.247632 -0.968712 -0.0165967 169.673 0.0395645 -0.0272268 0.998846 3.945 -0.965748 0.245549 0.0838806 -537.3 -0.246266 -0.9692 0.00185281 170.031 0.0817521 -0.0188676 0.996474 3.81768 -0.965159 0.244473 0.0932794 -535.923 -0.244926 -0.969518 0.00673065 170.411 0.0920815 -0.0163504 0.995617 3.77936 -0.968591 0.24425 0.0466207 -534.556 -0.244722 -0.969582 -0.00461839 170.753 0.0440746 -0.0158825 0.998902 3.72577 -0.967719 0.244544 0.0609725 -533.096 -0.244964 -0.969532 0.000593179 171.099 0.0592599 -0.0143621 0.998139 3.6748 -0.966536 0.246704 0.0703151 -531.613 -0.247373 -0.96892 -0.000830988 171.482 0.0679247 -0.0181973 0.997524 3.60051 -0.966136 0.250734 0.0609428 -530.145 -0.251599 -0.967807 -0.00683728 171.852 0.0572666 -0.0219389 0.998118 3.55524 -0.964136 0.257798 0.0631012 -527.885 -0.258693 -0.965939 -0.00630089 172.451 0.0593276 -0.0223988 0.997987 3.47561 -0.962818 0.262495 0.0638652 -526.356 -0.263333 -0.964693 -0.00492662 172.87 0.060317 -0.0215612 0.997946 3.41415 -0.961597 0.267013 0.0635189 -524.825 -0.267819 -0.963459 -0.00436655 173.298 0.060032 -0.0212104 0.997971 3.35885 -0.960797 0.270184 0.0622034 -523.293 -0.270844 -0.962621 -0.00227811 173.733 0.0592628 -0.0190362 0.998061 3.30954 -0.960566 0.271556 0.0597474 -521.738 -0.272191 -0.96224 -0.00260379 174.173 0.0567842 -0.0187638 0.99821 3.25247 -0.960556 0.271564 0.0598717 -520.159 -0.272235 -0.962226 -0.00318447 174.612 0.0567453 -0.019358 0.998201 3.19629 -0.960722 0.270586 0.0616165 -516.132 -0.271172 -0.96253 -0.00119613 175.743 0.0589841 -0.0178578 0.998099 3.02616 -0.960802 0.269554 0.0648147 -514.491 -0.270266 -0.962783 -0.00231154 176.198 0.0617794 -0.0197381 0.997895 2.96062 -0.960875 0.26838 0.0684888 -512.859 -0.269346 -0.96303 -0.00510614 176.652 0.0645864 -0.0233535 0.997639 2.89699 -0.961714 0.266444 0.0641446 -510.379 -0.267365 -0.963576 -0.00606322 177.339 0.0601927 -0.0229811 0.997922 2.81036 -0.96207 0.265111 0.0643144 -508.693 -0.265923 -0.963985 -0.00425302 177.799 0.0608706 -0.0211944 0.997921 2.73769 -0.962417 0.263961 0.0638672 -506.995 -0.264725 -0.964317 -0.00365583 178.26 0.0606232 -0.0204257 0.997952 2.68455 -0.962169 0.264407 0.0657193 -505.273 -0.265394 -0.964117 -0.00662349 178.733 0.0616098 -0.0238144 0.997816 2.61061 -0.961108 0.268178 0.0659609 -503.535 -0.269199 -0.963059 -0.00694448 179.207 0.0616619 -0.024431 0.997798 2.54178 -0.959943 0.272829 0.0638228 -501.79 -0.273703 -0.9618 -0.00521262 179.704 0.0599626 -0.0224723 0.997948 2.4765 -0.958518 0.277545 0.0648971 -500.044 -0.278494 -0.96042 -0.00587732 180.212 0.0606972 -0.0237069 0.997875 2.41188 -0.956564 0.283386 0.0683953 -498.266 -0.284552 -0.958629 -0.00773884 180.733 0.0633727 -0.0268647 0.997628 2.33862 -0.954855 0.289304 0.0674964 -496.486 -0.290664 -0.956761 -0.0110764 181.259 0.0613735 -0.0301951 0.997658 2.26713 -0.952893 0.295728 0.067379 -494.702 -0.297041 -0.954811 -0.0101638 181.811 0.0613285 -0.0296994 0.997676 2.19273 -0.949876 0.30695 0.0593087 -492.002 -0.308144 -0.951265 -0.011929 182.684 0.0527567 -0.0296067 0.998168 2.09245 -0.947342 0.314006 0.0627882 -490.192 -0.315405 -0.948861 -0.0135136 183.278 0.0553339 -0.0326058 0.997935 2.04252 -0.945132 0.319685 0.0672837 -488.416 -0.321433 -0.946785 -0.0167101 183.873 0.0583613 -0.0374204 0.997594 1.99674 -0.942389 0.329149 0.059708 -485.766 -0.330841 -0.943457 -0.0208233 184.792 0.049478 -0.0393775 0.997999 1.92392 -0.93861 0.339181 0.0629951 -483.103 -0.340749 -0.940022 -0.0157598 185.753 0.0538714 -0.0362578 0.997889 1.87254 -0.93591 0.346185 0.0650291 -481.333 -0.347663 -0.937535 -0.0126124 186.411 0.0566008 -0.0344123 0.997804 1.8138 -0.933467 0.353847 0.0585864 -479.566 -0.355038 -0.934787 -0.0109977 187.087 0.0508743 -0.0310664 0.998222 1.7443 -0.928558 0.366535 0.0585785 -476.931 -0.367625 -0.929933 -0.00867641 188.129 0.0512939 -0.0295915 0.998245 1.67382 -0.925066 0.374841 0.0612222 -475.179 -0.376024 -0.92657 -0.00866353 188.842 0.0534792 -0.0310353 0.998087 1.63104 -0.920015 0.38764 0.0575111 -472.539 -0.3887 -0.921329 -0.0080955 189.951 0.0498485 -0.0298025 0.998312 1.55218 -0.916553 0.395795 0.0572389 -470.784 -0.396915 -0.917809 -0.00925544 190.712 0.0488711 -0.0312021 0.998318 1.50485 -0.912693 0.404225 0.0599575 -469.023 -0.405383 -0.914111 -0.00807317 191.492 0.0515445 -0.0316741 0.998168 1.44543 -0.907427 0.416655 0.0545355 -466.399 -0.417661 -0.908567 -0.00802632 192.692 0.0462049 -0.0300607 0.99848 1.3789 -0.90318 0.42545 0.0570774 -464.647 -0.426575 -0.904411 -0.00862649 193.52 0.0479513 -0.032139 0.998332 1.33915 -0.898431 0.43509 0.0593119 -462.896 -0.436342 -0.899732 -0.00941732 194.365 0.0492674 -0.0343411 0.998195 1.28941 -0.893892 0.444918 0.0548234 -461.166 -0.44617 -0.89486 -0.0125584 195.227 0.0434718 -0.0356864 0.998417 1.25422 -0.88887 0.455014 0.0535981 -459.412 -0.456178 -0.889818 -0.0112541 196.129 0.0425718 -0.0344537 0.998499 1.22159 -0.883276 0.465444 0.0564356 -457.66 -0.466601 -0.884426 -0.00863299 197.058 0.0458949 -0.0339582 0.998369 1.17511 -0.877808 0.475838 0.0550567 -455.93 -0.476935 -0.878902 -0.00803683 197.997 0.0445652 -0.0333133 0.998451 1.14899 -0.872114 0.486741 0.0500079 -454.191 -0.487618 -0.873034 -0.00632421 198.973 0.0405804 -0.0299002 0.998729 1.10837 -0.865939 0.497151 0.0546865 -452.445 -0.498073 -0.867127 -0.00379978 199.967 0.0455311 -0.0305283 0.998496 1.06901 -0.856706 0.512431 0.058893 -449.839 -0.513754 -0.857891 -0.00894584 201.511 0.0459396 -0.0379205 0.998224 0.993044 -0.850952 0.521954 0.0586886 -448.123 -0.523436 -0.851975 -0.0124006 202.56 0.0435287 -0.0412721 0.998199 0.948108 -0.843283 0.534298 0.0583017 -445.587 -0.535915 -0.844127 -0.0156671 204.145 0.0408431 -0.0444566 0.998176 0.910519 -0.838735 0.541343 0.0589167 -443.903 -0.542987 -0.839599 -0.0154515 205.223 0.0411018 -0.0449506 0.998143 0.880228 -0.831902 0.552293 0.0539627 -441.445 -0.553739 -0.832541 -0.0157541 206.854 0.0362253 -0.0429872 0.998419 0.84226 -0.82479 0.563246 0.0497409 -438.98 -0.564385 -0.82543 -0.0116287 208.521 0.0345078 -0.0376642 0.998694 0.812051 -0.819671 0.570741 0.0489356 -437.365 -0.571687 -0.820443 -0.00683607 209.655 0.0362473 -0.0335792 0.998779 0.786303 -0.81239 0.581148 0.0478565 -434.953 -0.581781 -0.813345 0.000863356 211.391 0.0394256 -0.0271406 0.998854 0.730965 -0.80531 0.591174 0.0445893 -432.553 -0.591599 -0.806221 0.00440072 213.145 0.0385504 -0.022835 0.998996 0.689668 -0.800199 0.598154 0.0435177 -430.963 -0.598561 -0.801065 0.00440553 214.334 0.0374957 -0.0225227 0.999043 0.653325 -0.792535 0.608354 0.0423475 -428.598 -0.608844 -0.793288 0.00164556 216.138 0.0345949 -0.0244789 0.999102 0.611172 -0.783496 0.619975 0.0420165 -426.297 -0.620491 -0.784213 0.000967611 217.953 0.0335498 -0.0253127 0.999116 0.595619 -0.771624 0.63442 0.0459058 -423.268 -0.634821 -0.772626 0.00710671 220.401 0.0399767 -0.0236583 0.99892 0.566456 -0.762728 0.644689 0.0512057 -421.071 -0.64517 -0.763989 0.0087009 222.246 0.04473 -0.0264 0.99865 0.515913 -0.754056 0.653959 0.0611306 -418.981 -0.654866 -0.755716 0.00656411 224.027 0.05049 -0.0350826 0.998108 0.470869 -0.746416 0.662948 0.0579928 -417.009 -0.663957 -0.747766 0.0024502 225.775 0.0449894 -0.0366759 0.998314 0.481316 -0.736754 0.674344 0.0495249 -415.152 -0.674962 -0.737832 0.00549989 227.512 0.0402499 -0.0293753 0.998758 0.43022 -0.729952 0.681484 0.0524387 -413.952 -0.681999 -0.731283 0.010139 228.636 0.0452571 -0.0283622 0.998573 0.421308 -0.723871 0.688172 0.0492908 -412.821 -0.688572 -0.725084 0.0110555 229.727 0.0433481 -0.0259375 0.998723 0.413176 -0.71421 0.698553 0.0439091 -411.217 -0.698774 -0.715231 0.012639 231.306 0.0402342 -0.0216557 0.998956 0.394309 -0.704821 0.708055 0.043414 -409.703 -0.708188 -0.705867 0.0148969 232.833 0.0411924 -0.0202456 0.998946 0.373061 -0.695084 0.717498 0.0453333 -408.222 -0.717725 -0.696187 0.013969 234.358 0.0415832 -0.0228272 0.998874 0.333774 -0.686535 0.72557 0.0470925 -406.782 -0.725762 -0.687759 0.0160353 235.884 0.044023 -0.0231691 0.998762 0.300918 -0.678985 0.732553 0.0484279 -405.363 -0.732792 -0.680264 0.0159966 237.416 0.0446621 -0.0246261 0.998699 0.270761 -0.671429 0.739506 0.0480905 -403.98 -0.739956 -0.672563 0.0111469 238.95 0.0405871 -0.0281006 0.998781 0.240412 -0.665084 0.746354 0.0248691 -402.645 -0.746293 -0.66548 0.0135226 240.497 0.0266425 -0.00956596 0.999599 0.256532 -0.658354 0.752501 0.0176615 -401.754 -0.752683 -0.658342 -0.00731372 241.521 0.0061237 -0.0181085 0.999817 0.254201 -0.648722 0.758485 0.062132 -400.329 -0.756403 -0.651614 0.0570407 243.183 0.0837506 -0.00999323 0.996437 0.138948 -0.642371 0.762051 0.0814746 -399.42 -0.760207 -0.647055 0.0583507 244.212 0.0971847 -0.0244548 0.994966 0.0930214 -0.638995 0.768051 0.0422214 -398.092 -0.768739 -0.639563 -8.30059e-05 245.827 0.0269395 -0.0325102 0.999108 0.0352678 -0.632028 0.772699 0.0589647 -396.665 -0.773743 -0.633455 0.00750604 247.542 0.0431514 -0.0408795 0.998232 0.0017772 -0.628038 0.775809 0.0607286 -395.714 -0.776983 -0.629489 0.00638874 248.705 0.0431844 -0.0431728 0.998134 -0.0225678 -0.624033 0.779194 0.0586423 -394.76 -0.780299 -0.625377 0.00609439 249.909 0.0414222 -0.0419554 0.99826 -0.0493731 -0.616534 0.785894 0.0474935 -393.336 -0.78666 -0.617374 0.00396136 251.744 0.0324345 -0.0349189 0.998864 -0.0525101 -0.611822 0.790152 0.0365241 -392.376 -0.790592 -0.612333 0.00367123 253.011 0.0252657 -0.0266295 0.999326 -0.064127 -0.604624 0.795704 0.0358406 -390.926 -0.79567 -0.605441 0.0187112 254.945 0.036588 -0.017204 0.999182 -0.0927086 -0.600549 0.798882 0.0335861 -389.446 -0.798952 -0.601217 0.0146307 256.913 0.0318807 -0.0180472 0.999329 -0.135961 -0.597264 0.801311 0.0343009 -388.447 -0.801405 -0.59795 0.0143728 258.24 0.0320273 -0.0189045 0.999308 -0.133784 -0.592624 0.80462 0.0371821 -386.935 -0.804742 -0.593425 0.015374 260.278 0.034435 -0.0208109 0.99919 -0.172714 -0.590786 0.805703 0.0425894 -385.91 -0.805853 -0.591844 0.0179313 261.679 0.0396536 -0.0237272 0.998932 -0.202273 -0.586736 0.808489 0.0456745 -384.364 -0.808799 -0.587867 0.0160304 263.774 0.0398109 -0.0275358 0.998828 -0.244652 -0.583322 0.81103 0.044337 -383.329 -0.811302 -0.584403 0.0161902 265.224 0.0390415 -0.0265266 0.998885 -0.271296 -0.576808 0.815724 0.0434444 -381.776 -0.815665 -0.578034 0.0238139 267.426 0.0445379 -0.0217001 0.998772 -0.316715 -0.572288 0.81904 0.040737 -380.727 -0.819023 -0.573352 0.021641 268.928 0.0410815 -0.0209797 0.998936 -0.362125 -0.563947 0.824407 0.0481306 -379.142 -0.824426 -0.565418 0.024974 271.218 0.0478026 -0.0255961 0.998529 -0.440054 -0.55704 0.828941 0.0506269 -378.082 -0.829352 -0.558429 0.0182244 272.784 0.0433785 -0.0318359 0.998551 -0.482308 -0.546452 0.835855 0.0523072 -376.492 -0.83668 -0.547605 0.00980815 275.176 0.0368419 -0.0384047 0.998583 -0.533248 -0.537664 0.840943 0.0610934 -375.432 -0.842114 -0.539193 0.0107366 276.815 0.04197 -0.0456749 0.998074 -0.554304 -0.523963 0.849803 0.057428 -373.882 -0.851007 -0.525119 0.00611671 279.315 0.0353546 -0.0456667 0.998331 -0.586626 -0.51326 0.85621 0.0588933 -372.854 -0.857258 -0.514738 0.0123479 281.037 0.040887 -0.0441491 0.998188 -0.611618 -0.496372 0.866455 0.0535667 -371.345 -0.866991 -0.49792 0.0200657 283.666 0.044058 -0.0364817 0.998363 -0.65316 -0.48568 0.873244 0.0394987 -370.385 -0.873389 -0.486638 0.0193928 285.45 0.0361562 -0.0250791 0.999031 -0.682743 -0.480309 0.876454 0.0336392 -369.906 -0.876484 -0.481055 0.0190233 286.359 0.0328554 -0.0203472 0.999253 -0.705173 -0.469237 0.882547 0.0304525 -368.931 -0.882509 -0.46989 0.0195254 288.187 0.0315414 -0.0177125 0.999345 -0.731836 -0.463942 0.885277 0.0322865 -368.455 -0.885231 -0.464681 0.0209346 289.114 0.0335358 -0.0188686 0.999259 -0.752314 -0.453266 0.890604 0.0370749 -367.498 -0.890545 -0.454247 0.0242801 290.964 0.0384651 -0.0220115 0.999017 -0.772249 -0.443637 0.895415 0.0376712 -366.59 -0.895399 -0.444629 0.0237753 292.807 0.0380384 -0.0231831 0.999007 -0.787851 -0.439654 0.89738 0.0375999 -366.141 -0.897364 -0.440645 0.023856 293.727 0.0379761 -0.0232524 0.999008 -0.806204 -0.432788 0.900472 0.0429491 -365.243 -0.900513 -0.43405 0.0260324 295.572 0.0420835 -0.0274097 0.998738 -0.850337 -0.425675 0.903325 0.0529603 -364.347 -0.903765 -0.427323 0.0245714 297.414 0.0448271 -0.0374043 0.998294 -0.867326 -0.422291 0.90479 0.0550038 -363.905 -0.905419 -0.423936 0.0222423 298.336 0.0434427 -0.0404087 0.998238 -0.874 -0.415192 0.907987 0.0563519 -363.053 -0.90882 -0.416754 0.0190317 300.19 0.0407654 -0.0433119 0.99823 -0.894195 -0.407718 0.91196 0.0457713 -362.241 -0.912561 -0.408696 0.0141431 302.046 0.0316045 -0.0360027 0.998852 -0.886051 -0.403514 0.914035 0.0414282 -361.834 -0.914486 -0.404363 0.0143557 302.997 0.0298737 -0.0320928 0.999038 -0.903059 -0.395143 0.917919 0.035876 -361.025 -0.918054 -0.395969 0.0196425 304.897 0.0322361 -0.0251745 0.999163 -0.921957 -0.386868 0.921508 0.0339924 -360.224 -0.921743 -0.387515 0.014864 306.786 0.0268698 -0.0255819 0.999312 -0.924449 -0.378412 0.924953 0.0355937 -359.428 -0.925304 -0.379028 0.0122636 308.695 0.0248343 -0.0282943 0.999291 -0.94013 -0.374529 0.926355 0.0399276 -359.042 -0.926739 -0.37537 0.0159059 309.658 0.0297221 -0.0310453 0.999076 -0.963692 -0.365672 0.929335 0.0511846 -358.252 -0.929917 -0.367111 0.0219823 311.598 0.0392194 -0.0395591 0.998447 -0.936328 -0.358117 0.932603 0.0447564 -357.527 -0.933186 -0.359071 0.0152069 313.516 0.0302527 -0.0363202 0.998882 -0.94747 -0.344388 0.937744 0.0450848 -356.43 -0.937915 -0.345773 0.0275045 316.465 0.0413813 -0.0328135 0.998604 -0.989291 -0.335965 0.941004 0.0404759 -355.744 -0.941185 -0.337053 0.0238008 318.405 0.0360392 -0.0300991 0.998897 -1.00158 -0.33174 0.9426 0.038137 -355.407 -0.94306 -0.332397 0.0122368 319.374 0.024211 -0.031906 0.999198 -0.985361 -0.320675 0.946396 0.0387421 -354.723 -0.947034 -0.321094 0.00495333 321.365 0.0171277 -0.0351016 0.999237 -1.01224 -0.30771 0.95044 0.0444886 -354.068 -0.951105 -0.308565 0.0136643 323.359 0.0267147 -0.0381087 0.998916 -0.993969 -0.301767 0.952277 0.0458722 -353.749 -0.953022 -0.302625 0.0129186 324.344 0.0261841 -0.0398188 0.998864 -0.997092 -0.289995 0.955815 0.0481648 -353.133 -0.956814 -0.290628 0.00656286 326.339 0.0202709 -0.0441816 0.998818 -0.991141 -0.278435 0.959279 0.0475056 -352.557 -0.960301 -0.278935 0.00409587 328.343 0.01718 -0.0444793 0.998863 -0.973051 -0.266204 0.962725 0.0479165 -351.987 -0.963704 -0.26686 0.00774385 330.374 0.0202422 -0.0441159 0.998821 -0.961042 -0.260059 0.964366 0.048658 -351.706 -0.965312 -0.260869 0.011 331.39 0.0233014 -0.0441095 0.998755 -0.969456 -0.248778 0.967475 0.0458456 -351.199 -0.968351 -0.24943 0.00900107 333.378 0.0201436 -0.0421553 0.998908 -0.914849 -0.237227 0.970458 0.043985 -350.71 -0.971364 -0.237579 0.00288659 335.38 0.0132512 -0.0420407 0.999028 -0.912954 -0.225222 0.972728 0.0554522 -350.217 -0.974045 -0.226116 0.0103425 337.399 0.0225991 -0.0516836 0.998408 -0.922845 -0.213934 0.97577 0.0458822 -349.798 -0.976452 -0.214948 0.0184002 339.392 0.0278166 -0.0408654 0.998777 -0.874937 -0.201571 0.978619 0.0409128 -349.399 -0.97935 -0.202035 0.00749873 341.377 0.0156042 -0.0385564 0.999135 -0.896563 -0.187514 0.981433 0.0403416 -349.019 -0.982118 -0.188031 0.00941382 343.38 0.0168245 -0.037855 0.999142 -0.835827 -0.180371 0.982885 0.0374686 -348.845 -0.983444 -0.180887 0.0108437 344.391 0.0174357 -0.0348923 0.999239 -0.845921 -0.163426 0.985808 0.0384119 -348.502 -0.986426 -0.163912 0.00986467 346.399 0.0160209 -0.0362784 0.999213 -0.820838 -0.145404 0.988726 0.0357628 -348.208 -0.989032 -0.146208 0.020962 348.436 0.0259544 -0.0323226 0.99914 -0.811433 -0.126572 0.991312 0.0357666 -347.943 -0.991596 -0.127417 0.0224243 350.45 0.0267868 -0.0326277 0.999109 -0.836122 -0.0996678 0.994102 0.0427607 -347.589 -0.994833 -0.100392 0.0151436 353.482 0.0193471 -0.0410304 0.998971 -0.790071 -0.092645 0.994923 0.0393103 -347.521 -0.995576 -0.0931831 0.0120802 354.503 0.0156819 -0.0380172 0.999154 -0.772196 -0.0772523 0.996557 0.0300962 -347.374 -0.99701 -0.0772635 -0.000790452 356.508 0.00153761 -0.0300673 0.999547 -0.74105 -0.0705495 0.996997 0.0319181 -347.298 -0.997505 -0.0704311 -0.00481935 357.526 -0.00255685 -0.0321785 0.999479 -0.720295 -0.0569275 0.997856 0.0322933 -347.174 -0.998372 -0.05701 0.00164013 359.589 0.00347766 -0.0321474 0.999477 -0.675168 -0.0500197 0.998237 0.0319582 -347.126 -0.998692 -0.0503316 0.00903149 360.624 0.0106241 -0.0314646 0.999448 -0.668925 -0.0368865 0.998688 0.0355219 -347.036 -0.999107 -0.0375882 0.0192937 362.663 0.0206036 -0.0347785 0.999183 -0.636198 -0.032314 0.999003 0.030815 -347.018 -0.999295 -0.0328817 0.0180951 363.682 0.0190903 -0.0302085 0.999361 -0.605864 -0.023903 0.99947 0.0220764 -346.979 -0.999679 -0.0240807 0.0078191 365.726 0.00834658 -0.0218825 0.999726 -0.598268 -0.0202976 0.999604 0.0194869 -346.956 -0.999766 -0.0204402 0.00714457 366.749 0.00754006 -0.0193373 0.999785 -0.592589 -0.0129908 0.999753 0.0180492 -346.923 -0.999824 -0.0132313 0.0132689 368.805 0.0135045 -0.0178736 0.999749 -0.563252 -0.0108416 0.999808 0.0163304 -346.914 -0.999819 -0.0110946 0.0154863 369.821 0.0156645 -0.0161595 0.999747 -0.562435 -0.00789124 0.999875 0.0137349 -346.891 -0.999853 -0.00809881 0.0151234 371.85 0.0152327 -0.0136135 0.999791 -0.52786 -0.00698128 0.99988 0.0138516 -346.885 -0.999888 -0.00716385 0.0131748 372.862 0.0132724 -0.0137581 0.999817 -0.534871 -0.00742854 0.999888 0.0130162 -346.866 -0.999902 -0.00758165 0.0117539 374.856 0.0118513 -0.0129276 0.999846 -0.501295 -0.0123636 0.999842 0.0128081 -346.839 -0.999848 -0.0125193 0.0121496 376.837 0.0123081 -0.0126559 0.999844 -0.480527 -0.0178436 0.999716 0.0157737 -346.805 -0.999761 -0.0180397 0.0123762 378.822 0.0126572 -0.0155491 0.999799 -0.430987 -0.0233914 0.999486 0.0219412 -346.739 -0.999716 -0.0234841 0.00397409 380.786 0.00448732 -0.021842 0.999751 -0.381342 -0.0323074 0.999209 0.0231889 -346.646 -0.999478 -0.0323042 -0.000515511 383.703 0.000233996 -0.0231935 0.999731 -0.311528 -0.0423902 0.998853 0.0222575 -346.536 -0.99896 -0.0427476 0.0158342 386.62 0.0167674 -0.0215632 0.999627 -0.251043 -0.0512261 0.998479 0.0203782 -346.387 -0.998566 -0.0515275 0.014548 389.424 0.0155759 -0.0196037 0.999686 -0.208392 -0.0681803 0.997628 0.00951416 -346.223 -0.997672 -0.0681658 -0.00183211 392.131 -0.00117922 -0.00961693 0.999953 -0.129763 -0.0857747 0.996312 0.00213568 -346.069 -0.996312 -0.0857691 -0.0025698 393.922 -0.00237714 -0.00234822 0.999994 -0.098648 -0.106834 0.994256 0.00647136 -345.863 -0.994253 -0.106874 0.00617184 395.701 0.00682801 -0.00577481 0.99996 -0.0567023 -0.129407 0.991559 0.00807856 -345.625 -0.991492 -0.129505 0.0130973 397.459 0.0140329 -0.00631495 0.999882 -0.025314 -0.151528 0.988377 0.0122909 -345.342 -0.988293 -0.151715 0.0160958 399.197 0.0177734 -0.00970805 0.999795 -0.0202483 -0.186481 0.982361 0.0138344 -344.851 -0.982437 -0.186553 0.00404043 401.734 0.00655 -0.0128379 0.999896 0.0386244 -0.21186 0.977165 0.0162204 -344.471 -0.977241 -0.212 0.00745913 403.421 0.0107275 -0.014271 0.999841 0.0598835 -0.240575 0.970482 0.0170083 -344.034 -0.970452 -0.24083 0.014967 405.082 0.0186213 -0.0129051 0.999743 0.0772916 -0.270575 0.962443 0.0221781 -343.564 -0.962635 -0.270751 0.0053114 406.686 0.0111167 -0.0199123 0.99974 0.106447 -0.312872 0.949205 0.0334853 -342.764 -0.949599 -0.313329 0.0092663 409.064 0.0192875 -0.0288985 0.999396 0.149432 -0.359626 0.932391 0.0362869 -341.859 -0.93277 -0.360257 0.0124733 411.387 0.0247026 -0.0293616 0.999264 0.164953 -0.406734 0.912904 0.0342674 -340.857 -0.913341 -0.407152 0.00592827 413.608 0.019364 -0.0288866 0.999395 0.19592 -0.43754 0.898596 0.0329252 -340.156 -0.898957 -0.437978 0.00716729 415.014 0.020861 -0.0264624 0.999432 0.198828 -0.467152 0.883012 0.0453677 -339.401 -0.883603 -0.468082 0.0120079 416.344 0.031839 -0.0344775 0.998898 0.210749 -0.499771 0.865688 0.0285206 -338.669 -0.866 -0.500037 0.0025744 417.581 0.01649 -0.0234123 0.99959 0.242324 -0.528766 0.848008 0.0358933 -337.908 -0.848521 -0.529159 0.0017232 418.739 0.0204546 -0.0295451 0.999354 0.220186 -0.558873 0.827813 0.0488423 -337.14 -0.828235 -0.560136 0.0165675 419.812 0.0410731 -0.0311937 0.998669 0.239649 -0.604598 0.795069 0.0482329 -335.987 -0.795751 -0.605576 0.00756912 421.256 0.0352267 -0.0338051 0.998807 0.222373 -0.653254 0.755998 0.0415342 -334.827 -0.756693 -0.653769 -0.00155623 422.563 0.0259773 -0.0324453 0.999136 0.227054 -0.701185 0.712076 0.0358812 -333.587 -0.712682 -0.701458 -0.00642298 423.752 0.0205955 -0.0300756 0.999335 0.221437 -0.745619 0.66518 0.0398488 -332.289 -0.665841 -0.746079 -0.00469795 424.856 0.0266054 -0.0300359 0.999195 0.231651 -0.786899 0.616242 0.0321755 -330.954 -0.616492 -0.787357 0.0026636 425.882 0.026975 -0.01774 0.999479 0.246881 -0.823956 0.566389 0.0173284 -329.581 -0.566654 -0.823548 -0.0259292 426.757 -0.00041523 -0.0311837 0.999514 0.229789 -0.857399 0.511799 0.0541134 -328.117 -0.512173 -0.858847 0.00776525 427.572 0.0504494 -0.0210575 0.998505 0.23106 -0.885861 0.459672 0.0628649 -326.63 -0.460306 -0.887747 0.00486467 428.301 0.0580443 -0.0246277 0.99801 0.184774 -0.907834 0.418271 0.0297679 -325.679 -0.418786 -0.907983 -0.013614 428.687 0.0213344 -0.0248256 0.999464 0.194892 -0.925241 0.378507 0.0257394 -324.624 -0.378875 -0.92538 -0.011185 429.066 0.0195851 -0.0201008 0.999606 0.135691 -0.93887 0.341403 0.0443632 -323.561 -0.342204 -0.939553 -0.0116864 429.405 0.0376919 -0.0261533 0.998947 0.138445 -0.958251 0.283756 0.0351713 -321.948 -0.284266 -0.958689 -0.0103585 429.851 0.0307791 -0.019924 0.999328 0.126702 -0.973247 0.22667 0.0375756 -320.272 -0.227273 -0.97375 -0.0125826 430.193 0.0337371 -0.0207859 0.999215 0.0935861 -0.98469 0.170333 0.0370511 -318.541 -0.170936 -0.985187 -0.0137168 430.454 0.0341658 -0.0198401 0.999219 0.0720024 -0.992039 0.119627 0.0393409 -316.75 -0.12036 -0.992588 -0.0168089 430.609 0.0370385 -0.0214101 0.999084 0.0382554 -0.997011 0.0662259 0.0397823 -314.301 -0.0671061 -0.99752 -0.0212131 430.747 0.0382788 -0.0238193 0.998983 0.00980817 -0.998447 0.0375179 0.0411717 -312.388 -0.0383463 -0.999074 -0.01952 430.793 0.0404013 -0.0210685 0.998961 -0.0290521 -0.99891 0.0214534 0.0414598 -310.442 -0.022197 -0.999599 -0.0175593 430.826 0.0410665 -0.0184604 0.998986 -0.0535458 -0.999052 0.00867405 0.0426655 -308.452 -0.00930429 -0.99985 -0.0145953 430.831 0.0425325 -0.0149785 0.998983 -0.105808 -0.999144 -0.000754034 0.0413653 -306.409 -9.13078e-05 -0.999791 -0.0204303 430.814 0.0413721 -0.0204166 0.998935 -0.152989 -0.99904 -0.00599227 0.0434022 -304.308 0.00482989 -0.999628 -0.0268373 430.785 0.0435468 -0.0266019 0.998697 -0.203029 -0.999074 -0.00913861 0.0420369 -302.16 0.00783437 -0.999486 -0.0310869 430.736 0.0422994 -0.0307287 0.998632 -0.231218 -0.999168 -0.0103859 0.0394362 -300.699 0.00914868 -0.999464 -0.0314247 430.707 0.0397414 -0.0310378 0.998728 -0.26124 -0.999288 -0.00795551 0.036879 -299.208 0.00663638 -0.999339 -0.0357546 430.687 0.0371391 -0.0354844 0.99868 -0.27965 -0.999394 -0.00483385 0.034467 -297.685 0.0036509 -0.999405 -0.0343018 430.674 0.0346123 -0.0341552 0.998817 -0.304836 -0.999347 -0.000454754 0.0361418 -295.369 -0.000900404 -0.999297 -0.0374705 430.658 0.0361334 -0.0374785 0.998644 -0.342243 -0.99929 0.00210573 0.0376249 -293.793 -0.00344112 -0.999365 -0.0354629 430.657 0.0375263 -0.0355672 0.998662 -0.376759 -0.999041 0.00597537 0.0433849 -291.389 -0.00739045 -0.999443 -0.0325301 430.69 0.0431664 -0.0328195 0.998529 -0.360281 -0.999257 0.00585616 0.0381047 -288.936 -0.00697288 -0.999548 -0.0292399 430.727 0.0379162 -0.0294839 0.998846 -0.419335 -0.999428 0.00502039 0.0334336 -287.264 -0.00596382 -0.999585 -0.0281784 430.736 0.0332783 -0.0283617 0.999044 -0.445932 -0.999301 0.00520134 0.0370298 -284.723 -0.00623287 -0.999594 -0.0277959 430.743 0.0368702 -0.0280072 0.998928 -0.474948 -0.999346 0.00505458 0.035812 -282.137 -0.00586975 -0.999725 -0.0226942 430.764 0.0356875 -0.0228896 0.999101 -0.504726 -0.999389 0.0051447 0.0345829 -279.523 -0.00597446 -0.999696 -0.0239332 430.748 0.0344493 -0.0241252 0.999115 -0.552249 -0.999355 0.00440124 0.0356307 -276.861 -0.00540942 -0.999586 -0.0282484 430.742 0.0354917 -0.0284229 0.998966 -0.583409 -0.999263 0.00350727 0.038234 -275.057 -0.00465944 -0.999536 -0.0300875 430.747 0.0381107 -0.0302435 0.998816 -0.605687 -0.99933 0.000331842 0.0366023 -272.323 -0.00143895 -0.999542 -0.0302246 430.751 0.0365755 -0.030257 0.998873 -0.640452 -0.999311 -0.00215364 0.0370422 -270.468 0.00104458 -0.999551 -0.029934 430.743 0.03709 -0.0298747 0.998865 -0.666402 -0.999274 -0.0058178 0.037644 -267.634 0.00481321 -0.999631 -0.0267224 430.733 0.0377856 -0.0265218 0.998934 -0.708838 -0.999221 -0.00827948 0.0385972 -265.714 0.00729068 -0.999643 -0.0256891 430.701 0.0387962 -0.0253877 0.998925 -0.747116 -0.999109 -0.0112999 0.0406663 -263.782 0.0102825 -0.999631 -0.0251409 430.671 0.0409353 -0.0247004 0.998856 -0.77531 -0.999101 -0.0143364 0.0399057 -260.884 0.0133295 -0.999589 -0.0253853 430.622 0.0402533 -0.0248305 0.998881 -0.829333 -0.999036 -0.0142023 0.0415469 -258.933 0.0131262 -0.999574 -0.0260611 430.6 0.0418993 -0.0254906 0.998797 -0.863759 -0.999151 -0.0137541 0.0388466 -256 0.0127906 -0.999607 -0.0249411 430.553 0.0391744 -0.0244231 0.998934 -0.89997 -0.999013 -0.0118364 0.0428037 -253.032 0.0108075 -0.999649 -0.0241893 430.53 0.043075 -0.0237028 0.998791 -0.948568 -0.999176 -0.00863944 0.039669 -250.122 0.00772692 -0.999703 -0.0230995 430.518 0.0398568 -0.022774 0.998946 -0.974487 -0.998987 -0.00559102 0.0446585 -247.254 0.00427751 -0.999557 -0.029454 430.484 0.0448034 -0.0292331 0.998568 -1.01771 -0.999169 -0.00215864 0.0407084 -244.402 0.000861025 -0.999492 -0.0318665 430.469 0.0407565 -0.0318049 0.998663 -1.05466 -0.999343 0.00155007 0.0362041 -241.567 -0.00279366 -0.999407 -0.0343243 430.446 0.0361294 -0.0344029 0.998755 -1.08865 -0.999203 0.00396813 0.0397211 -238.747 -0.00539844 -0.999338 -0.0359665 430.453 0.0395521 -0.0361523 0.998563 -1.11247 -0.999182 0.00540628 0.0400792 -236.852 -0.00677622 -0.999395 -0.0341242 430.467 0.0398705 -0.0343679 0.998614 -1.13207 -0.999129 0.00471816 0.0414667 -234.053 -0.00606373 -0.999457 -0.0323836 430.484 0.0412914 -0.0326068 0.998615 -1.18524 -0.999127 0.0025856 0.0416855 -230.319 -0.00389795 -0.999498 -0.0314319 430.481 0.0415833 -0.0315669 0.998636 -1.24526 -0.999231 0.00192689 0.0391596 -228.467 -0.00318945 -0.999476 -0.0322046 430.473 0.039077 -0.0323048 0.998714 -1.27939 -0.999096 0.00118574 0.0424857 -225.691 -0.00258299 -0.999457 -0.0328478 430.469 0.0424237 -0.0329279 0.998557 -1.3049 -0.999205 0.00127668 0.0398515 -223.875 -0.00261463 -0.999434 -0.0335393 430.471 0.0397861 -0.0336168 0.998643 -1.33444 -0.99901 0.00139709 0.0444736 -221.183 -0.00299886 -0.999348 -0.03597 430.463 0.0443943 -0.0360678 0.998363 -1.37737 -0.9989 0.00114476 0.0468801 -219.437 -0.0028616 -0.999327 -0.0365711 430.461 0.0468067 -0.036665 0.998231 -1.39895 -0.998991 0.00111378 0.044905 -216.93 -0.00258672 -0.99946 -0.0327565 430.462 0.0448443 -0.0328395 0.998454 -1.4214 -0.998971 0.00145832 0.0453405 -215.312 -0.00274421 -0.999595 -0.0283113 430.472 0.0452809 -0.0284066 0.99857 -1.4456 -0.998919 0.00159422 0.0464506 -212.952 -0.00268092 -0.999724 -0.0233418 430.475 0.0464006 -0.0234411 0.998648 -1.48199 -0.999089 0.0011795 0.0426506 -210.722 -0.00209088 -0.99977 -0.0213303 430.481 0.0426157 -0.0214 0.998862 -1.51281 -0.999298 0.000435836 0.0374519 -209.266 -0.0011481 -0.999819 -0.0189988 430.473 0.0374369 -0.0190284 0.999118 -1.52396 -0.999327 0.0011435 0.0366636 -207.054 -0.00204281 -0.999698 -0.0245007 430.471 0.0366245 -0.0245591 0.999027 -1.56022 -0.999234 0.000682233 0.0391165 -205.587 -0.00175539 -0.999623 -0.0274072 430.465 0.039083 -0.0274549 0.998859 -1.58389 -0.999601 0.000518554 0.0282275 -202.732 -0.00140438 -0.999507 -0.0313708 430.461 0.0281973 -0.0313979 0.999109 -1.60941 -0.998366 0.00109616 0.0571325 -201.27 -0.00272218 -0.999593 -0.0283904 430.465 0.0570782 -0.0284996 0.997963 -1.64934 -0.998391 -0.000566238 0.0567043 -199.884 -0.000667495 -0.999764 -0.021736 430.458 0.0567032 -0.0217389 0.998154 -1.62007 -0.999695 0.000467345 0.0246901 -197.752 -0.000870483 -0.999866 -0.0163197 430.458 0.0246792 -0.0163362 0.999562 -1.67272 -0.999236 0.000422933 0.03908 -196.301 -0.000818493 -0.999949 -0.0101064 430.469 0.0390737 -0.0101306 0.999185 -1.69741 -0.999167 0.000435924 0.0407998 -194.863 -0.000752617 -0.99997 -0.00774708 430.476 0.0407952 -0.00777133 0.999137 -1.70257 -0.999252 9.86483e-05 0.0386583 -193.435 -0.000332032 -0.999982 -0.00603073 430.474 0.038657 -0.00603905 0.999234 -1.7137 -0.999352 -0.00161746 0.0359616 -191.28 0.00125535 -0.999948 -0.0100899 430.457 0.035976 -0.0100382 0.999302 -1.75828 -0.999329 -0.00586557 0.0361508 -189.109 0.00519678 -0.999814 -0.0185665 430.435 0.036253 -0.0183662 0.999174 -1.79769 -0.999105 -0.0103092 0.0410338 -186.928 0.00926983 -0.999633 -0.0254397 430.383 0.041281 -0.0250365 0.998834 -1.81697 -0.999104 -0.0145645 0.0397403 -184.724 0.0134222 -0.999493 -0.0288613 430.345 0.0401405 -0.0283021 0.998793 -1.85533 -0.999101 -0.0185138 0.0381383 -182.512 0.0174987 -0.999488 -0.0267803 430.309 0.0386146 -0.0260888 0.998914 -1.88658 -0.999332 -0.0202433 0.0304174 -179.555 0.0191838 -0.999213 -0.0347274 430.23 0.0310964 -0.0341207 0.998934 -1.91867 -0.999206 -0.0194832 0.0347411 -178.04 0.0184339 -0.999372 -0.0302743 430.224 0.0353092 -0.0296098 0.998938 -1.92994 -0.999075 -0.0171711 0.0394273 -175.822 0.0162194 -0.999572 -0.0243323 430.196 0.0398282 -0.0236703 0.998926 -1.95217 -0.999175 -0.0176424 0.0365822 -173.609 0.0169267 -0.999661 -0.0197826 430.16 0.0369188 -0.0191471 0.999135 -1.98425 -0.998998 -0.0183987 0.0408095 -172.138 0.0176212 -0.999658 -0.0193297 430.124 0.0411511 -0.0185912 0.99898 -2.0361 -0.999042 -0.0202859 0.0387647 -169.97 0.0192295 -0.999439 -0.0274319 430.069 0.0392994 -0.0266602 0.998872 -2.02633 -0.998935 -0.0192427 0.0419415 -168.527 0.0181521 -0.999491 -0.0262308 430.037 0.0424249 -0.0254415 0.998776 -2.06353 -0.999008 -0.0188689 0.0403382 -166.396 0.0176805 -0.999405 -0.029619 429.993 0.040873 -0.0288764 0.998747 -2.09047 -0.999028 -0.0150552 0.0414272 -164.288 0.0139035 -0.999513 -0.0279493 429.961 0.0418278 -0.0273461 0.998751 -2.11428 -0.999207 -0.0102076 0.0384776 -162.214 0.00918307 -0.999601 -0.0267097 429.933 0.0387349 -0.0263352 0.998902 -2.14044 -0.999344 -0.00742991 0.0354571 -160.851 0.00650443 -0.999637 -0.0261458 429.93 0.0356384 -0.025898 0.999029 -2.15796 -0.999408 -0.00531427 0.033994 -158.799 0.00445718 -0.999671 -0.0252395 429.915 0.034117 -0.025073 0.999103 -2.18543 -0.999988 -0.0030821 -0.00391367 -156.087 0.00322788 -0.99928 -0.0378044 429.902 -0.00379434 -0.0378166 0.999277 -2.14792 -0.998643 -0.00336944 0.0519728 -154.645 0.00161741 -0.99943 -0.0337158 429.902 0.0520568 -0.033586 0.998079 -2.16471 -0.996418 -0.00282019 0.0845168 -153.267 -0.000528078 -0.999217 -0.0395681 429.868 0.0845622 -0.039471 0.995636 -2.2344 -0.999541 -0.0027865 0.0301573 -151.229 0.001456 -0.999028 -0.0440511 429.86 0.0302507 -0.043987 0.998574 -2.24778 -0.99967 -0.00215641 0.0256038 -149.041 0.00117723 -0.99927 -0.0381972 429.863 0.0256675 -0.0381544 0.998942 -2.28566 -0.99922 -0.000608003 0.0394915 -146.847 -0.000809832 -0.999356 -0.0358764 429.859 0.0394879 -0.0358804 0.998576 -2.28637 -0.999557 -0.00104484 0.0297394 -145.362 0.000110286 -0.999507 -0.031409 429.863 0.0297576 -0.0313918 0.999064 -2.29959 -0.999514 -0.000470205 0.0311759 -143.85 -0.000454833 -0.99956 -0.0296578 429.855 0.0311762 -0.0296576 0.999074 -2.33014 -0.999381 2.91279e-05 0.0351735 -142.318 -0.00108414 -0.99955 -0.0299757 429.855 0.0351568 -0.0299953 0.998932 -2.33348 -0.999499 0.00105011 0.0316415 -140.04 -0.00196553 -0.99958 -0.0289135 429.851 0.0315979 -0.0289612 0.999081 -2.35972 -0.999442 0.000515658 0.0333996 -138.475 -0.00144939 -0.999609 -0.0279382 429.84 0.0333721 -0.027971 0.999052 -2.38131 -0.999537 -0.00221398 0.0303322 -136.142 0.00129596 -0.999541 -0.030252 429.828 0.0303852 -0.0301987 0.999082 -2.39437 -0.999591 -0.00434429 0.0282729 -134.563 0.00354917 -0.999598 -0.028113 429.806 0.0283837 -0.0280012 0.999205 -2.39811 -0.999387 -0.00757164 0.034182 -132.136 0.00678342 -0.99971 -0.0231166 429.795 0.0343471 -0.0228706 0.999148 -2.41285 -0.999341 -0.00972647 0.0349577 -130.509 0.00890604 -0.999683 -0.0235488 429.77 0.0351757 -0.0232219 0.999111 -2.42932 -0.999385 -0.0134527 0.0323805 -128.067 0.0126643 -0.999621 -0.0244299 429.726 0.0326969 -0.0240048 0.999177 -2.44241 -0.999375 -0.0147859 0.0321163 -126.41 0.0138974 -0.999519 -0.0277147 429.683 0.0325106 -0.027251 0.9991 -2.47095 -0.999336 -0.0143737 0.0334795 -123.887 0.0134356 -0.999515 -0.0280779 429.638 0.0338669 -0.0276095 0.999045 -2.49126 -0.999441 -0.0130281 0.0307884 -122.189 0.0121522 -0.999521 -0.0284658 429.609 0.0311445 -0.0280757 0.999121 -2.51522 -0.999493 -0.0124129 0.0293194 -120.471 0.0116005 -0.999548 -0.027719 429.59 0.0296502 -0.0273648 0.999186 -2.52223 -0.999518 -0.0105437 0.0291876 -117.86 0.00977819 -0.999608 -0.0262461 429.556 0.0294529 -0.025948 0.999229 -2.54297 -0.999536 -0.00945629 0.0289458 -116.11 0.00870632 -0.999626 -0.0259267 429.534 0.0291802 -0.0256627 0.999245 -2.55707 -0.99944 -0.00603561 0.0328988 -112.595 0.00516956 -0.99964 -0.0263466 429.498 0.033046 -0.0261618 0.999111 -2.56814 -0.99944 -0.00426368 0.0331789 -110.805 0.00345983 -0.9997 -0.0242475 429.478 0.0332723 -0.0241192 0.999155 -2.58966 -0.99953 -0.00276282 0.0305312 -109.013 0.0020934 -0.999757 -0.0219359 429.48 0.0305844 -0.0218617 0.999293 -2.60724 -0.99953 -0.00129836 0.0306377 -107.197 0.000685039 -0.999799 -0.0200204 429.468 0.0306575 -0.01999 0.99933 -2.6247 -0.999455 0.000250105 0.0330069 -105.411 -0.000907356 -0.999802 -0.019899 429.464 0.0329954 -0.0199181 0.999257 -2.63429 -0.999511 0.000597418 0.0312524 -103.595 -0.0012448 -0.999785 -0.0206994 429.472 0.0312333 -0.0207282 0.999297 -2.6382 -0.999547 0.00176253 0.0300332 -101.776 -0.00242794 -0.999752 -0.0221337 429.463 0.0299868 -0.0221966 0.999304 -2.64886 -0.999498 0.00232348 0.0315843 -99.9646 -0.00315542 -0.999649 -0.0263161 429.466 0.031512 -0.0264025 0.999155 -2.65628 -0.999451 0.00236168 0.0330474 -98.1151 -0.0033866 -0.999514 -0.0309921 429.459 0.0329582 -0.031087 0.998973 -2.67517 -0.999533 0.00376728 0.0303199 -96.2903 -0.00470669 -0.999509 -0.0309717 429.449 0.0301883 -0.0310999 0.99906 -2.67765 -0.999635 0.00448365 0.0266583 -94.4617 -0.005273 -0.999548 -0.0296138 429.452 0.0265134 -0.0297435 0.999206 -2.67585 -0.999543 0.00630047 0.0295668 -91.6979 -0.00704422 -0.99966 -0.0251186 429.462 0.0293985 -0.0253154 0.999247 -2.68727 -0.999569 0.00819468 0.0281834 -88.9421 -0.00886555 -0.999678 -0.0237616 429.478 0.0279796 -0.0240012 0.99932 -2.68767 -0.999568 0.00825989 0.0282183 -87.0968 -0.00889656 -0.999707 -0.0225119 429.494 0.0280241 -0.0227532 0.999348 -2.6907 -0.999502 0.00931498 0.0301561 -85.2744 -0.0100318 -0.999669 -0.0237061 429.498 0.0299253 -0.0239968 0.999264 -2.7017 -0.999461 0.0100136 0.0312648 -83.4677 -0.0107464 -0.999669 -0.0233571 429.507 0.0310206 -0.0236805 0.999238 -2.69794 -0.999263 0.0097595 0.0371234 -81.6622 -0.0104673 -0.999766 -0.0189199 429.518 0.03693 -0.0192945 0.999132 -2.72877 -0.999627 0.00853195 0.0259325 -79.0033 -0.00903342 -0.999773 -0.0192822 429.543 0.0257621 -0.0195093 0.999478 -2.73394 -0.999723 0.00816971 0.0220641 -77.2498 -0.00879701 -0.999556 -0.0284849 429.526 0.0218216 -0.0286711 0.999351 -2.73357 -0.999399 0.00701901 0.0339381 -74.6281 -0.00811647 -0.999445 -0.0323082 429.541 0.0336925 -0.0325643 0.998902 -2.72993 -0.9995 0.00458161 0.0312772 -72.898 -0.0054944 -0.99956 -0.0291609 429.553 0.0311298 -0.0293182 0.999085 -2.72475 -0.999471 0.00341807 0.0323392 -71.1955 -0.00411655 -0.999759 -0.0215566 429.553 0.0322577 -0.0216783 0.999244 -2.73296 -0.999435 0.000946839 0.0335961 -69.4824 -0.00159742 -0.999812 -0.0193433 429.562 0.0335715 -0.0193861 0.999248 -2.73885 -0.999608 -0.0010144 0.0279703 -66.0939 0.000504122 -0.999833 -0.0182444 429.549 0.0279842 -0.0182232 0.999442 -2.75274 -0.999667 -0.000770131 0.0258059 -64.4135 0.000356247 -0.999871 -0.0160391 429.542 0.0258149 -0.0160246 0.999538 -2.75869 -0.999661 -0.000550228 0.0260198 -62.7479 0.000100431 -0.999851 -0.0172849 429.538 0.0260254 -0.0172764 0.999512 -2.75786 -0.999622 -6.8728e-05 0.0274836 -61.0864 -0.000443189 -0.999827 -0.0186197 429.533 0.0274801 -0.0186249 0.999449 -2.75865 -0.9996 0.000341283 0.0282811 -59.45 -0.000878132 -0.99982 -0.0189724 429.523 0.0282695 -0.0189896 0.99942 -2.76867 -0.999532 -0.000498998 0.030572 -57.8268 -0.000145846 -0.999778 -0.0210868 429.517 0.0305757 -0.0210814 0.99931 -2.76225 -0.999481 -0.000754569 0.0322131 -56.2269 -4.58404e-05 -0.999691 -0.0248394 429.513 0.0322219 -0.024828 0.999172 -2.75877 -0.999512 -0.00103515 0.0312159 -54.6513 0.000295312 -0.999719 -0.023696 429.508 0.0312317 -0.0236752 0.999232 -2.76129 -0.999547 -0.00112583 0.0300664 -53.1252 0.000457543 -0.999753 -0.0222247 429.499 0.0300839 -0.0222009 0.999301 -2.76065 -0.999598 -0.00130508 0.0283126 -51.6311 0.00073193 -0.999795 -0.0202444 429.499 0.0283332 -0.0202156 0.999394 -2.75336 -0.999683 -0.00175901 0.0251025 -50.1353 0.00127077 -0.99981 -0.0194525 429.498 0.0251319 -0.0194144 0.999496 -2.74389 -0.9997 -0.00170338 0.0244339 -48.6676 0.00125691 -0.999832 -0.0182762 429.494 0.0244609 -0.01824 0.999534 -2.74049 -0.999638 -0.00253303 0.0267859 -46.4768 0.00220985 -0.999924 -0.012088 429.494 0.0268145 -0.0120245 0.999568 -2.73819 -0.999679 -0.00265982 0.0251807 -45.0458 0.00235991 -0.999926 -0.0119325 429.493 0.0252105 -0.0118693 0.999612 -2.73255 -0.999561 -0.00276172 0.0295035 -43.649 0.00253322 -0.999967 -0.0077796 429.489 0.029524 -0.00770145 0.999534 -2.71882 -0.999434 -0.00281893 0.0335166 -42.2887 0.00259242 -0.999974 -0.00679971 429.483 0.0335349 -0.00670898 0.999415 -2.73134 -0.999397 -0.00386108 0.0345053 -40.3225 0.00345057 -0.999923 -0.0119486 429.48 0.0345487 -0.0118223 0.999333 -2.75096 -0.999599 -0.00516261 0.0278472 -39.0453 0.00474099 -0.999873 -0.0151854 429.461 0.027922 -0.0150473 0.999497 -2.73682 -0.999553 -0.00591629 0.0293191 -37.7641 0.00565217 -0.999943 -0.00908308 429.459 0.0293711 -0.0089133 0.999529 -2.74986 -0.99954 -0.00593728 0.0297385 -36.4974 0.00538769 -0.999814 -0.0185269 429.44 0.0298429 -0.0183582 0.999386 -2.75431 -0.999951 -0.00720943 -0.00672312 -34.6809 0.00728568 -0.999909 -0.011387 429.443 -0.00664041 -0.0114354 0.999913 -2.69215 -0.999962 -0.00674938 -0.00554604 -33.427 0.00686517 -0.999753 -0.0211311 429.403 -0.00540205 -0.0211683 0.999761 -2.68639 -0.999038 -0.00734239 0.0432377 -32.1463 0.00647776 -0.999777 -0.0201033 429.394 0.0433756 -0.0198039 0.998863 -2.68532 -0.998899 -0.00729042 0.0463473 -30.9374 0.0065595 -0.999852 -0.0159029 429.392 0.0464564 -0.0155814 0.998799 -2.68686 -0.999867 -0.00847317 0.0139543 -29.1512 0.00808512 -0.999585 -0.0276339 429.363 0.0141826 -0.0275174 0.999521 -2.68801 -0.999529 -0.00949281 0.0291706 -27.3317 0.00864901 -0.999544 -0.0289176 429.343 0.0294318 -0.0286517 0.999156 -2.69048 -0.9997 -0.0101508 0.022274 -25.5623 0.00946242 -0.999481 -0.0307938 429.316 0.022575 -0.0305738 0.999278 -2.6735 -0.999721 -0.0101814 0.0212923 -23.8062 0.00958929 -0.999569 -0.0277285 429.312 0.0215655 -0.0275166 0.999389 -2.67043 -0.999678 -0.0105399 0.0230786 -22.0875 0.00993602 -0.999609 -0.0261245 429.288 0.0233449 -0.0258868 0.999392 -2.65953 -0.999599 -0.0109129 0.0261108 -20.3938 0.0102342 -0.99961 -0.0259871 429.257 0.0263842 -0.0257094 0.999321 -2.64977 -0.999555 -0.0120137 0.0272906 -19.2828 0.011275 -0.99957 -0.0270619 429.233 0.027604 -0.0267422 0.999261 -2.65529 -0.99941 -0.0124694 0.0320025 -18.1663 0.0114305 -0.999408 -0.0324433 429.218 0.0323881 -0.0320583 0.998961 -2.66548 -0.99918 -0.0134543 0.0381915 -17.0657 0.0121101 -0.999306 -0.0352125 429.204 0.0386388 -0.0347211 0.99865 -2.65836 -0.999859 -0.0141326 0.00910506 -15.4693 0.0138597 -0.999473 -0.0293601 429.181 0.00951519 -0.0292298 0.999527 -2.63124 -0.999876 -0.0141306 0.00700192 -14.3958 0.0138796 -0.999302 -0.034694 429.146 0.00748728 -0.0345925 0.999373 -2.63565 -0.999416 -0.0160286 0.0301772 -13.2996 0.0151337 -0.999446 -0.0296536 429.134 0.0306357 -0.0291796 0.999105 -2.61784 -0.999473 -0.01715 0.0275591 -12.2454 0.0162637 -0.999353 -0.032067 429.125 0.0280912 -0.0316019 0.999106 -2.61564 -0.999725 -0.0186495 0.0141951 -11.21 0.0181287 -0.999188 -0.0359735 429.098 0.0148544 -0.0357063 0.999252 -2.60422 -0.999563 -0.0200023 0.0217857 -9.62557 0.0193198 -0.999329 -0.0311014 429.071 0.0223932 -0.0306669 0.999279 -2.6013 -0.999752 -0.0209253 0.00764603 -8.60939 0.0206461 -0.999176 -0.0349326 429.032 0.00837071 -0.0347661 0.99936 -2.54392 -0.999667 -0.0238827 -0.00973781 -7.58928 0.0242168 -0.999067 -0.03577 429.022 -0.00887444 -0.0359939 0.999313 -2.52315 -0.998967 -0.0317121 0.0325336 -5.91614 0.0307126 -0.999054 -0.0307728 428.948 0.0334787 -0.0297418 0.998997 -2.5858 -0.998208 -0.0423643 0.0422664 -4.34655 0.0408563 -0.998519 -0.0359253 428.883 0.0437258 -0.0341341 0.99846 -2.51956 -0.998632 -0.0510977 0.011085 -3.2986 0.050754 -0.99828 -0.0293394 428.832 0.0125651 -0.0287367 0.999508 -2.51906 -0.998084 -0.0615782 0.00605625 -2.19584 0.0613578 -0.997616 -0.0315733 428.747 0.00798604 -0.0311412 0.999483 -2.54663 -0.997154 -0.0734153 0.0171575 -1.06646 0.0728415 -0.996831 -0.0319675 428.653 0.0194501 -0.0306267 0.999342 -2.5276 -0.996214 -0.0854816 0.0158187 0.051566 0.0849644 -0.995905 -0.0309005 428.541 0.0183954 -0.0294394 0.999397 -2.51707 -0.995102 -0.0979503 0.0133271 1.18852 0.0974748 -0.994705 -0.0325812 428.421 0.0164478 -0.0311226 0.99938 -2.51174 -0.993664 -0.110937 0.0179983 2.34997 0.110261 -0.993286 -0.0350031 428.27 0.0217606 -0.0327968 0.999225 -2.50638 -0.992123 -0.123658 0.0200289 3.5258 0.122868 -0.991741 -0.0367857 428.104 0.0244123 -0.034035 0.999122 -2.50965 -0.990417 -0.136981 0.0176448 4.71179 0.136171 -0.989839 -0.0409431 427.921 0.0230739 -0.038148 0.999006 -2.51464 -0.988471 -0.150311 0.0182212 5.91875 0.14935 -0.987716 -0.0459482 427.716 0.0249039 -0.0426972 0.998778 -2.51486 -0.98513 -0.171057 0.0160803 7.76121 0.170078 -0.984173 -0.0497761 427.38 0.0243404 -0.046301 0.998631 -2.52723 -0.982771 -0.184228 0.0148901 9.02264 0.183095 -0.981397 -0.0577655 427.119 0.0252551 -0.0540439 0.998219 -2.52482 -0.978864 -0.204124 0.0126331 10.9146 0.203267 -0.977847 -0.0499716 426.723 0.0225536 -0.0463474 0.998671 -2.51918 -0.97616 -0.216745 0.0115156 12.1781 0.215843 -0.974951 -0.0536858 426.409 0.0228632 -0.0499204 0.998491 -2.53481 -0.97262 -0.232049 0.0127662 13.4667 0.231041 -0.971406 -0.0546899 426.101 0.0250919 -0.050243 0.998422 -2.52478 -0.968658 -0.248146 0.0111824 14.748 0.247493 -0.96799 -0.0417369 425.761 0.0211813 -0.0376612 0.999066 -2.51947 -0.96408 -0.265438 0.00962068 16.0548 0.264682 -0.963101 -0.0487764 425.369 0.0222128 -0.0444779 0.998763 -2.51634 -0.955728 -0.293971 0.01285 18.0293 0.293203 -0.955097 -0.0426851 424.749 0.0248212 -0.0370277 0.999006 -2.50434 -0.947486 -0.319699 0.00795762 20.0079 0.319047 -0.946672 -0.0449542 424.05 0.0219051 -0.0400546 0.998957 -2.49338 -0.941453 -0.337103 0.00524482 21.3371 0.336606 -0.940715 -0.0418429 423.564 0.0190393 -0.0376277 0.99911 -2.48572 -0.935184 -0.354141 0.00376512 22.6611 0.353735 -0.934528 -0.0391061 423.049 0.0173677 -0.0352395 0.999228 -2.48426 -0.928973 -0.370142 0.00185783 23.998 0.369786 -0.928279 -0.0394622 422.502 0.0163312 -0.0359723 0.999219 -2.47526 -0.922371 -0.386303 0.00105514 25.3343 0.386028 -0.921811 -0.0352978 421.936 0.0146083 -0.0321504 0.999376 -2.45617 -0.915793 -0.401635 0.00344007 26.6771 0.401321 -0.915357 -0.0326097 421.332 0.0162461 -0.0284831 0.999462 -2.45144 -0.904791 -0.425824 0.00512246 28.685 0.425462 -0.904407 -0.0321092 420.381 0.0183056 -0.0268727 0.999471 -2.43753 -0.897068 -0.44186 0.00541194 30.0206 0.441461 -0.896664 -0.0332643 419.706 0.0195509 -0.0274511 0.999432 -2.4251 -0.88897 -0.457932 0.00550067 31.3587 0.457513 -0.888563 -0.0337097 418.997 0.0203244 -0.0274503 0.999417 -2.42032 -0.876858 -0.480711 0.00618073 33.3588 0.480171 -0.87636 -0.0378127 417.888 0.0235935 -0.0301886 0.999266 -2.42119 -0.864363 -0.502853 0.00405276 35.3454 0.502372 -0.863839 -0.03748 416.706 0.0223479 -0.0303603 0.999289 -2.39886 -0.851225 -0.524791 0.00324444 37.3356 0.52435 -0.850733 -0.0361893 415.466 0.021752 -0.029104 0.99934 -2.39999 -0.826734 -0.562582 0.00351908 40.6197 0.562143 -0.826306 -0.0348207 413.268 0.0224974 -0.0268093 0.999387 -2.38327 -0.810214 -0.58613 0.00215425 42.5779 0.585639 -0.809676 -0.038094 411.844 0.0240723 -0.0296027 0.999272 -2.38712 -0.792879 -0.60937 0.00339843 44.5122 0.608661 -0.792202 -0.0441456 410.339 0.0295932 -0.0329336 0.999019 -2.39296 -0.774998 -0.631953 0.00361941 46.4238 0.630855 -0.773965 -0.0547713 408.74 0.0374142 -0.0401643 0.998492 -2.41049 -0.75701 -0.653404 0.000356804 48.3077 0.652973 -0.756531 -0.0358718 407.12 0.0237087 -0.0269223 0.999356 -2.40284 -0.744458 -0.667571 0.0114218 49.5837 0.667505 -0.744546 -0.0094357 406.017 0.0148031 0.000599636 0.99989 -2.37532 -0.732067 -0.681157 0.0102064 50.8151 0.681158 -0.732126 -0.00391903 404.869 0.0101418 0.00408314 0.99994 -2.39514 -0.717906 -0.696007 0.0135746 52.0193 0.695874 -0.718035 -0.0136399 403.677 0.0192405 -0.000345967 0.999815 -2.37016 -0.705668 -0.70818 0.0226413 53.2124 0.707644 -0.706022 -0.0277908 402.455 0.0356661 -0.00358909 0.999357 -2.39274 -0.692641 -0.721041 0.0186896 54.3543 0.720318 -0.692822 -0.0337662 401.229 0.0372953 -0.00992536 0.999255 -2.38925 -0.679064 -0.733912 0.0156698 55.4834 0.733209 -0.679143 -0.0341961 400.009 0.035739 -0.0117321 0.999292 -2.41117 -0.658885 -0.752166 0.0108177 57.0757 0.751488 -0.6588 -0.0353176 398.151 0.0336914 -0.0151409 0.999318 -2.42375 -0.64503 -0.764111 0.0083984 58.1196 0.763533 -0.644908 -0.033333 396.898 0.0308863 -0.0150883 0.999409 -2.43214 -0.630222 -0.776362 0.00908238 59.1226 0.775757 -0.630127 -0.0337904 395.643 0.0319566 -0.0142498 0.999388 -2.44209 -0.607698 -0.794133 0.00752896 60.5825 0.793541 -0.607567 -0.03399 393.728 0.0315669 -0.0146811 0.999394 -2.45482 -0.584559 -0.811345 0.00322743 61.9805 0.810868 -0.584344 -0.0321659 391.791 0.0279835 -0.0161858 0.999477 -2.46756 -0.561269 -0.827631 0.0020537 63.306 0.827127 -0.561011 -0.0335775 389.804 0.028942 -0.0171474 0.999434 -2.47289 -0.545523 -0.838095 0.00110853 64.1729 0.837573 -0.545229 -0.0345931 388.457 0.0295967 -0.0179428 0.999401 -2.48434 -0.529746 -0.848157 1.84531e-05 65.0109 0.847696 -0.529459 -0.0329287 387.099 0.0279385 -0.0174282 0.999458 -2.47773 -0.514181 -0.857682 0.000384407 65.826 0.857276 -0.513951 -0.030531 385.729 0.0263834 -0.0153689 0.999534 -2.49006 -0.491017 -0.871123 0.0068023 67.0048 0.870498 -0.490937 -0.0348443 383.607 0.0336932 -0.0111877 0.99937 -2.5071 -0.477285 -0.878719 0.00720538 67.7838 0.878145 -0.477246 -0.0331344 382.185 0.0325546 -0.0094872 0.999425 -2.52576 -0.465847 -0.884834 0.00741484 68.5254 0.884278 -0.465826 -0.032519 380.747 0.032228 -0.0085921 0.999444 -2.53698 -0.446639 -0.894678 0.00801276 69.6022 0.894263 -0.44668 -0.0277574 378.555 0.0284131 -0.00523201 0.999583 -2.54661 -0.417054 -0.908881 -0.000867462 70.9816 0.908617 -0.416909 -0.0245334 375.568 0.0219363 -0.01102 0.999699 -2.56512 -0.401751 -0.915739 -0.0042378 71.6345 0.915456 -0.401502 -0.0271447 374.044 0.023156 -0.014785 0.999623 -2.56996 -0.385886 -0.922528 -0.00586854 72.2775 0.922232 -0.385581 -0.028543 372.492 0.0240689 -0.0164265 0.999575 -2.55421 -0.359312 -0.933179 -0.00845561 73.1733 0.932925 -0.358958 -0.0282777 370.119 0.023353 -0.018049 0.999564 -2.54396 -0.340994 -0.939967 -0.0136413 73.7337 0.939863 -0.340583 -0.0257233 368.525 0.0195331 -0.0215924 0.999576 -2.557 -0.312523 -0.949692 -0.0203495 74.5048 0.949697 -0.311927 -0.0278747 366.091 0.0201248 -0.0280373 0.999404 -2.53582 -0.278986 -0.960093 -0.019704 75.2216 0.960089 -0.278442 -0.0264381 363.639 0.0198966 -0.0262934 0.999456 -2.53669 -0.255995 -0.966502 -0.01847 75.6554 0.966368 -0.255382 -0.0302136 361.968 0.0244846 -0.0255833 0.999373 -2.53 -0.220662 -0.975228 -0.0154354 76.224 0.975063 -0.220186 -0.0277627 359.437 0.0236763 -0.0211767 0.999495 -2.52827 -0.185104 -0.982633 -0.0130133 76.6998 0.982409 -0.184697 -0.027563 356.88 0.0246808 -0.0178865 0.999535 -2.52715 -0.148615 -0.988831 -0.011232 77.0882 0.988611 -0.148291 -0.0256697 354.295 0.0237174 -0.014919 0.999607 -2.52515 -0.1262 -0.991937 -0.0115679 77.2911 0.991748 -0.125894 -0.0242349 352.547 0.0225831 -0.0145309 0.999639 -2.52773 -0.107361 -0.994136 -0.0129256 77.4876 0.993958 -0.107025 -0.0243461 350.78 0.02282 -0.0154613 0.99962 -2.51892 -0.0735913 -0.997244 -0.00940391 77.7473 0.996951 -0.0733176 -0.0267226 347.248 0.0259595 -0.0113418 0.999599 -2.52846 -0.0555126 -0.998389 -0.0117259 77.8369 0.998125 -0.0551868 -0.0264827 345.465 0.0257929 -0.013174 0.99958 -2.53429 -0.0389668 -0.999108 -0.0162455 77.8918 0.998929 -0.0385435 -0.0256046 343.665 0.0249556 -0.0172258 0.99954 -2.5381 -0.0189792 -0.99957 -0.0223719 77.9217 0.99943 -0.0183419 -0.0283552 340.949 0.0279326 -0.0228973 0.999348 -2.54179 -0.00933132 -0.999605 -0.0265034 77.9289 0.999595 -0.00861206 -0.0271241 339.117 0.0268851 -0.0267458 0.999281 -2.54622 0.00147211 -0.99959 -0.0286099 77.9017 0.999729 0.00213617 -0.0231942 336.352 0.0232457 -0.028568 0.999322 -2.54962 0.00685758 -0.999547 -0.0293206 77.8796 0.999654 0.00759704 -0.0251832 334.513 0.0253945 -0.0291377 0.999253 -2.54544 0.00945599 -0.999589 -0.0270542 77.844 0.999751 0.00999758 -0.019954 331.741 0.0202162 -0.0268588 0.999435 -2.54861 0.010872 -0.99967 -0.0232751 77.8193 0.999668 0.0114095 -0.0230867 329.876 0.0233447 -0.0230164 0.999462 -2.54042 0.0131083 -0.999711 -0.0201319 77.788 0.99962 0.0135897 -0.0239671 327.068 0.0242337 -0.0198101 0.99951 -2.54235 0.0143645 -0.999756 -0.0167782 77.7592 0.999679 0.0147096 -0.0206253 325.188 0.020867 -0.0164766 0.999646 -2.53777 0.0163683 -0.99978 -0.013134 77.7012 0.999591 0.0166704 -0.0232317 322.37 0.0234455 -0.0127483 0.999644 -2.53243 0.0179526 -0.999743 -0.0138573 77.6319 0.999576 0.0182641 -0.022685 319.53 0.0229323 -0.0134442 0.999647 -2.52957 0.0184396 -0.999761 -0.0117841 77.5942 0.999508 0.0187313 -0.0251382 317.623 0.0253529 -0.0113147 0.999615 -2.53539 0.0197989 -0.99973 -0.0121957 77.5408 0.999268 0.0201861 -0.0324905 314.742 0.0327279 -0.0115435 0.999398 -2.56558 0.0212896 -0.999662 -0.0149068 77.4612 0.999205 0.0217776 -0.0333827 311.835 0.0336961 -0.0141843 0.999331 -2.58656 0.0213422 -0.999666 -0.0145829 77.3971 0.999514 0.021666 -0.0224189 308.944 0.0227273 -0.0140974 0.999642 -2.59001 0.0209854 -0.999694 -0.0131343 77.3259 0.999513 0.0212815 -0.0228255 305.995 0.023098 -0.0126489 0.999653 -2.59496 0.0185575 -0.999705 -0.0157007 77.2766 0.999542 0.0189253 -0.0236094 304.028 0.0238996 -0.0152554 0.999598 -2.58856 0.0112816 -0.999843 -0.0136961 77.2415 0.999736 0.0115523 -0.0198504 301.076 0.0200055 -0.0134685 0.999709 -2.57428 0.00400704 -0.999914 -0.0125128 77.224 0.999796 0.00425385 -0.0197607 298.106 0.0198123 -0.012431 0.999726 -2.5723 -0.00269864 -0.999849 -0.0171475 77.2215 0.999775 -0.00233672 -0.0210913 295.122 0.021048 -0.0172005 0.99963 -2.55334 -0.00935674 -0.999781 -0.0187401 77.2452 0.999726 -0.00895086 -0.021626 292.12 0.0214536 -0.0189373 0.99959 -2.55942 -0.0124775 -0.999743 -0.0189471 77.253 0.999707 -0.0120793 -0.02099 290.122 0.0207557 -0.0192034 0.9996 -2.54727 -0.0134386 -0.999722 -0.0193588 77.2665 0.999709 -0.0130457 -0.0202821 289.11 0.020024 -0.0196257 0.999607 -2.54946 -0.0155985 -0.999618 -0.0228129 77.2725 0.999655 -0.0151089 -0.0214787 287.091 0.0211258 -0.0231401 0.999509 -2.54355 -0.0168183 -0.999553 -0.0247364 77.2953 0.999612 -0.0162598 -0.0226068 286.084 0.0221945 -0.025107 0.999438 -2.55031 -0.0183776 -0.999517 -0.0250736 77.3421 0.999588 -0.0178141 -0.0225167 283.045 0.0220591 -0.0254771 0.999432 -2.52046 -0.0182942 -0.999568 -0.0229883 77.3802 0.999649 -0.0178457 -0.0195653 281.039 0.0191467 -0.0233382 0.999544 -2.51767 -0.0180324 -0.999588 -0.0223127 77.4038 0.999658 -0.0176018 -0.01935 280.023 0.0189493 -0.022654 0.999564 -2.5068 -0.0174024 -0.999622 -0.0212939 77.4376 0.999671 -0.0169936 -0.0192303 277.987 0.0188612 -0.0216215 0.999588 -2.49953 -0.0167626 -0.999634 -0.021253 77.4418 0.999664 -0.0163348 -0.0201458 276.98 0.0197912 -0.0215836 0.999571 -2.49028 -0.0144084 -0.999664 -0.0215479 77.4672 0.99969 -0.0139645 -0.0206108 274.953 0.020303 -0.0218382 0.999555 -2.49286 -0.0133516 -0.999635 -0.0234978 77.4676 0.9997 -0.0128624 -0.0208467 273.942 0.0205368 -0.0237691 0.999507 -2.49103 -0.0123073 -0.999612 -0.0250052 77.4625 0.9997 -0.0117712 -0.0214757 272.931 0.021173 -0.025262 0.999457 -2.48776 -0.00966494 -0.999584 -0.0271621 77.4767 0.999733 -0.00908967 -0.0212235 270.897 0.0209678 -0.0273599 0.999406 -2.47881 -0.00812124 -0.999566 -0.028313 77.4933 0.999746 -0.00752147 -0.0212264 269.88 0.0210042 -0.0284782 0.999374 -2.47133 -0.00674044 -0.999584 -0.0280568 77.4986 0.999766 -0.00615952 -0.0207402 268.859 0.0205587 -0.02819 0.999391 -2.47654 -0.00437794 -0.999667 -0.0254437 77.5042 0.999767 -0.00383726 -0.0212603 266.837 0.0211556 -0.0255308 0.99945 -2.45749 -0.00292487 -0.999727 -0.0231959 77.5145 0.99979 -0.00245308 -0.0203417 265.813 0.0202792 -0.0232505 0.999524 -2.4483 -0.00119692 -0.999848 -0.0174028 77.5125 0.999837 -0.000882766 -0.0180485 263.778 0.0180304 -0.0174216 0.999686 -2.43752 -0.000620105 -0.999879 -0.0155621 77.5108 0.999853 -0.000352936 -0.0171648 262.766 0.0171572 -0.0155705 0.999732 -2.43471 -0.000453925 -0.999896 -0.0143865 77.5193 0.999856 -0.000209353 -0.0169971 261.749 0.0169923 -0.0143921 0.999752 -2.42901 -0.000992149 -0.999889 -0.0148843 77.5092 0.999762 -0.000667378 -0.0218088 259.713 0.0217964 -0.0149024 0.999651 -2.4258 -0.00106088 -0.999854 -0.0170516 77.5109 0.999733 -0.000666575 -0.0231132 258.695 0.0230985 -0.0170716 0.999587 -2.42864 -0.00111935 -0.999795 -0.0202123 77.5061 0.99977 -0.000685884 -0.0214398 256.663 0.0214215 -0.0202317 0.999566 -2.42413 -0.00145616 -0.999775 -0.0211685 77.5033 0.999784 -0.00101695 -0.0207442 255.653 0.020718 -0.0211941 0.999561 -2.41281 -0.00178528 -0.999706 -0.0241908 77.4971 0.999739 -0.0012333 -0.0228134 252.598 0.0227769 -0.0242252 0.999447 -2.39803 -0.00271861 -0.999663 -0.0258107 77.4925 0.999807 -0.00221561 -0.0194967 250.595 0.0194329 -0.0258587 0.999477 -2.39077 -0.00384521 -0.999682 -0.0249384 77.4996 0.999803 -0.00335722 -0.0195804 247.616 0.0194904 -0.0250088 0.999497 -2.3624 -0.00476169 -0.999765 -0.0211268 77.5002 0.999809 -0.00435907 -0.0190624 244.663 0.0189659 -0.0212135 0.999595 -2.33144 -0.00355777 -0.999842 -0.0174054 77.5202 0.999784 -0.00320015 -0.020531 241.758 0.0204721 -0.0174746 0.999638 -2.31479 -0.00183528 -0.999891 -0.0146793 77.5157 0.999754 -0.00151003 -0.0221373 238.929 0.0221127 -0.0147164 0.999647 -2.29086 -0.00110368 -0.999922 -0.0124639 77.5073 0.999719 -0.000808087 -0.023696 237.112 0.0236841 -0.0124865 0.999642 -2.27951 -0.000144488 -0.999909 -0.013459 77.5079 0.999782 0.000136375 -0.0208649 234.458 0.0208648 -0.013459 0.999692 -2.25694 0.000126896 -0.999892 -0.0146945 77.4893 0.999727 0.000470456 -0.0233791 232.742 0.0233835 -0.0146875 0.999619 -2.24873 0.000589695 -0.999883 -0.0152919 77.481 0.999702 0.000962899 -0.0244094 231.065 0.0244213 -0.015273 0.999585 -2.23172 0.00155605 -0.999848 -0.0173802 77.4705 0.999725 0.00196239 -0.0233873 228.651 0.0234178 -0.017339 0.999575 -2.2203 0.00262366 -0.999838 -0.0178103 77.4658 0.999891 0.00288181 -0.0144842 226.315 0.0145332 -0.0177703 0.999736 -2.19039 0.00382597 -0.999799 -0.0196651 77.4559 0.999858 0.00414759 -0.0163402 224.002 0.0164185 -0.0195998 0.999673 -2.16558 0.00511454 -0.999774 -0.0206588 77.4358 0.999832 0.00547643 -0.0174992 221.736 0.0176084 -0.0205658 0.999633 -2.13854 0.00722288 -0.999764 -0.0204917 77.4152 0.99987 0.00751586 -0.0142567 220.235 0.0144074 -0.0203861 0.999688 -2.11716 0.00869742 -0.999723 -0.0218492 77.4002 0.999871 0.00840005 0.0136649 218.78 -0.0134776 -0.0219652 0.999668 -2.07306 0.00928338 -0.999751 -0.020275 77.3619 0.999742 0.00970012 -0.0205535 216.517 0.0207451 -0.020079 0.999583 -2.07708 0.0106186 -0.999825 -0.0154187 77.3476 0.997351 0.0116995 -0.0717938 214.978 0.0719616 -0.0146155 0.9973 -2.07589 0.0134476 -0.999807 -0.014356 77.33 0.999843 0.0136113 -0.011366 213.548 0.0115592 -0.0142009 0.999832 -2.03572 0.0164455 -0.999771 -0.0136783 77.2934 0.999766 0.0166343 -0.0137986 211.295 0.014023 -0.0134482 0.999811 -2.04794 0.0186189 -0.999675 -0.0173903 77.2305 0.999631 0.0189565 -0.0194532 209.015 0.0197765 -0.0170217 0.99966 -2.03199 0.0170927 -0.999677 -0.0188168 77.1907 0.999792 0.0172983 -0.0108181 206.705 0.0111401 -0.018628 0.999764 -2.00735 0.0165876 -0.999711 -0.0174171 77.1607 0.999749 0.0168456 -0.0147695 205.152 0.0150587 -0.0171678 0.999739 -1.98239 0.0153728 -0.999767 -0.0151748 77.1319 0.999815 0.0155451 -0.0113021 202.813 0.0115353 -0.0149983 0.999821 -1.95961 0.0145739 -0.999711 -0.0191404 77.0729 0.999771 0.0148689 -0.0153589 200.443 0.0156391 -0.0189122 0.999699 -1.93995 0.014455 -0.999688 -0.0203506 77.0451 0.999719 0.0148323 -0.0185146 198.872 0.0188107 -0.0200773 0.999621 -1.91712 0.0147767 -0.999442 -0.0299686 76.9824 0.999774 0.0152257 -0.0148094 196.531 0.0152575 -0.029743 0.999441 -1.90551 0.0144785 -0.999329 -0.0336482 76.9479 0.999808 0.0149137 -0.0127197 194.986 0.013213 -0.0334576 0.999353 -1.8963 0.00976972 -0.999023 -0.0431013 76.9072 0.999862 0.0103399 -0.0130264 192.689 0.0134594 -0.042968 0.998986 -1.84757 0.00545272 -0.999146 -0.0409656 76.9083 0.999917 0.00592426 -0.011398 191.185 0.011631 -0.0409 0.999096 -1.82577 4.01071e-05 -0.999128 -0.0417536 76.9005 0.999908 0.000607768 -0.0135829 188.941 0.0135964 -0.0417492 0.999036 -1.79372 -0.00228628 -0.999254 -0.0385403 76.9052 0.999873 -0.00167625 -0.0158531 187.455 0.0157767 -0.0385716 0.999131 -1.76068 -0.00633171 -0.999021 -0.043784 76.9051 0.999746 -0.00537753 -0.0218766 185.271 0.0216197 -0.0439114 0.998801 -1.74589 -0.00607143 -0.999536 -0.0298431 76.9289 0.999892 -0.00566804 -0.0135833 183.858 0.0134078 -0.0299224 0.999462 -1.66696 -0.00860115 -0.999662 -0.024552 76.948 0.999572 -0.0092815 0.0277327 182.459 -0.0279512 -0.024303 0.999314 -1.65745 -0.0105208 -0.999727 -0.0208668 76.9675 0.998781 -0.00949987 -0.0484351 180.257 0.0482236 -0.0213509 0.998608 -1.69632 -0.0108165 -0.999781 -0.0179035 76.9972 0.999693 -0.0104127 -0.0224982 178.121 0.0223068 -0.0181414 0.999587 -1.62486 -0.0125297 -0.999771 -0.0173669 77.0024 0.999921 -0.0125189 -0.000726729 175.971 0.000509147 -0.0173747 0.999849 -1.61872 -0.0123475 -0.999847 -0.01238 77.0261 0.999729 -0.0120995 -0.0199129 174.472 0.01976 -0.0126225 0.999725 -1.60681 -0.0135726 -0.999809 -0.0140457 77.0545 0.999885 -0.0134769 -0.00688987 172.23 0.00669927 -0.0141376 0.999878 -1.59129 -0.0126881 -0.999853 -0.0114913 77.0899 0.999828 -0.0125304 -0.0136871 169.958 0.0135411 -0.011663 0.99984 -1.55401 -0.0113845 -0.999888 -0.00974684 77.1067 0.999879 -0.0112797 -0.010737 167.664 0.0106259 -0.0098679 0.999895 -1.52705 -0.00759188 -0.999952 -0.0061414 77.1221 0.999879 -0.00750784 -0.0135939 165.362 0.0135471 -0.00624387 0.999889 -1.49765 -0.00436736 -0.999981 -0.0044084 77.1265 0.999857 -0.00429473 -0.0163525 163.061 0.0163332 -0.00447919 0.999857 -1.47648 -0.00207976 -0.999991 -0.00381281 77.128 0.999893 -0.00202441 -0.0144638 161.536 0.014456 -0.00384249 0.999888 -1.45572 0.0021867 -0.999995 -0.00213157 77.1135 0.999925 0.00221221 -0.0120398 159.305 0.0120444 -0.00210509 0.999925 -1.43717 0.00547133 -0.999966 -0.00617879 77.0931 0.999929 0.00553613 -0.0105198 157.098 0.0105537 -0.0061208 0.999926 -1.40472 0.00496497 -0.999911 -0.0123806 77.0627 0.999918 0.00510996 -0.0117075 154.911 0.0117697 -0.0123214 0.999855 -1.37557 0.00276538 -0.99985 -0.0170817 77.0263 0.999923 0.00297078 -0.012011 152.059 0.0120599 -0.0170472 0.999782 -1.32269 0.00168749 -0.999815 -0.0191515 77.02 0.999919 0.00192935 -0.0126173 150.65 0.0126519 -0.0191287 0.999737 -1.29787 0.000222516 -0.999787 -0.0206471 77.023 0.999932 0.000463119 -0.0116491 148.568 0.0116562 -0.0206431 0.999719 -1.26899 -0.000679331 -0.999807 -0.0196368 77.0183 0.999941 -0.000465848 -0.0108741 146.546 0.0108629 -0.019643 0.999748 -1.22285 -0.00109885 -0.999808 -0.0195644 77.0215 0.999947 -0.000897858 -0.0102794 145.219 0.0102599 -0.0195746 0.999756 -1.21083 0.000147276 -0.999783 -0.0208292 77.0176 0.999943 0.000369376 -0.0106595 143.925 0.0106649 -0.0208264 0.999726 -1.18617 0.000854991 -0.99979 -0.0204622 77.017 0.999943 0.00107194 -0.0105937 142.6 0.0106134 -0.020452 0.999735 -1.16715 0.00227117 -0.999838 -0.0178754 77.0208 0.999997 0.00227443 -0.000162273 141.329 0.000202903 -0.0178749 0.99984 -1.11501 0.00374345 -0.999859 -0.0163847 77.0046 0.999244 0.00310594 0.0387627 139.433 -0.0387063 -0.0165174 0.999114 -1.06447 0.00419809 -0.999817 -0.0186847 76.9851 0.997852 0.0054098 -0.0652796 137.393 0.0653687 -0.0183706 0.997692 -1.07231 0.00852608 -0.999767 -0.019854 76.9662 0.999932 0.00836525 0.00816955 135.518 -0.00800156 -0.0199223 0.99977 -1.00072 0.00969564 -0.999732 -0.0209985 76.9413 0.999883 0.00994179 -0.0116497 133.459 0.0118553 -0.0208831 0.999712 -1.03759 0.0119556 -0.999789 -0.0166815 76.9361 0.999572 0.012395 -0.0264918 132.097 0.026693 -0.0163576 0.99951 -0.968264 0.0144639 -0.999829 -0.0115591 76.9055 0.99988 0.0143977 0.005788 130.014 -0.00562058 -0.0116414 0.999916 -0.937787 0.0163872 -0.999861 -0.00292287 76.883 0.999839 0.0164081 -0.00727502 127.893 0.00732197 -0.00280318 0.999969 -0.911593 0.0179732 -0.999832 -0.00361247 76.8546 0.999817 0.0179965 -0.00651968 126.45 0.0065836 -0.00349463 0.999972 -0.890512 0.0209873 -0.999761 -0.00607758 76.7998 0.99977 0.0210135 -0.00427734 124.258 0.00440403 -0.00598641 0.999972 -0.852499 0.02289 -0.999683 -0.0104669 76.7536 0.999716 0.0229583 -0.00644727 122.764 0.00668553 -0.0103164 0.999924 -0.832324 0.0264081 -0.999488 -0.0180642 76.6788 0.999604 0.026578 -0.00923104 120.505 0.00970642 -0.0178133 0.999794 -0.795151 0.031755 -0.999264 -0.0215241 76.5904 0.999453 0.0319447 -0.00852684 118.218 0.00920814 -0.0212416 0.999732 -0.76196 0.0417828 -0.998927 -0.0199757 76.4791 0.999088 0.0419484 -0.00794208 115.867 0.0087715 -0.0196256 0.999769 -0.711894 0.0562473 -0.998256 -0.0179234 76.3347 0.99836 0.056427 -0.00968483 113.484 0.0106793 -0.0173492 0.999792 -0.684458 0.0795053 -0.996718 -0.0152201 76.1326 0.99681 0.0796002 -0.00573145 111.068 0.00692416 -0.0147159 0.999868 -0.653137 0.0959294 -0.99526 -0.0159684 75.9667 0.995379 0.0959832 -0.00264153 109.43 0.00416171 -0.0156412 0.999869 -0.627141 0.113448 -0.993397 -0.0171084 75.7642 0.993528 0.113528 -0.00377058 107.789 0.00568797 -0.0165699 0.999847 -0.594752 0.141731 -0.989708 -0.0197645 75.3853 0.989864 0.141878 -0.00624525 105.29 0.00898513 -0.018679 0.999785 -0.556379 0.161657 -0.986621 -0.0211204 75.0884 0.986809 0.161801 -0.00532622 103.614 0.00867227 -0.0199808 0.999763 -0.52706 0.193462 -0.980955 -0.0173236 74.6058 0.981071 0.193578 -0.00525025 101.089 0.00850373 -0.01598 0.999836 -0.480754 0.216151 -0.976266 -0.0135764 74.2221 0.976341 0.216213 -0.00325375 99.3859 0.00611191 -0.0125519 0.999903 -0.46179 0.25114 -0.967883 -0.011443 73.5523 0.967939 0.251178 -0.00198784 96.8273 0.00479823 -0.0105769 0.999933 -0.408302 0.287836 -0.957611 -0.0114968 72.7899 0.957665 0.287877 -0.00211876 94.2819 0.00533862 -0.0104003 0.999932 -0.373046 0.323378 -0.946126 -0.0165056 71.8952 0.946243 0.32345 -0.00186501 91.7112 0.00710328 -0.0150152 0.999862 -0.334353 0.348852 -0.93701 -0.0177557 71.2497 0.937139 0.348947 -0.00247297 89.9993 0.00851299 -0.0157768 0.999839 -0.296693 0.384961 -0.922754 -0.0181801 70.1872 0.922898 0.385043 -0.00109194 87.4751 0.0080077 -0.016358 0.999834 -0.250524 0.419708 -0.907438 -0.020061 69.0029 0.907607 0.419818 -0.0014419 84.9567 0.00973039 -0.0176023 0.999798 -0.218511 0.450643 -0.89242 -0.0225082 67.7283 0.892661 0.450726 0.00154638 82.4488 0.00876498 -0.020789 0.999745 -0.18306 0.471464 -0.881614 -0.0218826 66.3805 0.881822 0.471582 -0.000305023 79.9377 0.0105884 -0.0191528 0.999761 -0.125094 0.487519 -0.872926 -0.0180229 64.9744 0.873059 0.487613 -0.000978338 77.4198 0.0096422 -0.0152581 0.999837 -0.0973393 0.499798 -0.86592 -0.0195817 63.5108 0.866087 0.499893 0.000100082 74.9114 0.0097021 -0.0170095 0.999808 -0.0694071 0.50262 -0.864176 -0.0239162 62.5032 0.864422 0.502766 -9.57893e-05 73.2286 0.012107 -0.0206256 0.999714 -0.0412776 0.501345 -0.864913 -0.0240671 61.5022 0.86517 0.501478 0.000563113 71.5275 0.0115821 -0.0211045 0.99971 -0.0104893 0.500555 -0.865405 -0.0228008 60.5155 0.865644 0.500658 0.00135242 69.8233 0.010245 -0.0204143 0.999739 0.0132423 0.498853 -0.866346 -0.0242888 59.0191 0.866644 0.498911 0.00406955 67.2467 0.00859233 -0.0230799 0.999697 0.0500917 0.498325 -0.866609 -0.0257183 58.0137 0.866947 0.498378 0.00476714 65.5265 0.00868618 -0.0246719 0.999658 0.0844487 0.497179 -0.867261 -0.0258989 57.0158 0.867598 0.497248 0.00416012 63.7867 0.00927027 -0.0245382 0.999656 0.127833 0.494597 -0.868653 -0.0285534 55.5214 0.869032 0.494753 0.00183262 61.2018 0.012535 -0.0257202 0.999591 0.164662 0.49125 -0.87049 -0.0303285 54.5588 0.870926 0.491407 0.00254429 59.4872 0.0126889 -0.0276638 0.999537 0.197106 0.485819 -0.873504 -0.0311514 53.5953 0.873954 0.486006 0.00178351 57.7644 0.0135819 -0.0280914 0.999513 0.218206 0.479517 -0.87695 -0.0319779 52.6513 0.877424 0.479713 0.001736 56.0525 0.0138178 -0.0288906 0.999487 0.239514 0.465021 -0.884736 -0.0315925 51.2785 0.885254 0.465062 0.00648592 53.5075 0.00895413 -0.0309835 0.99948 0.286549 0.445778 -0.894552 -0.0325316 49.979 0.895097 0.445829 0.00605173 50.9465 0.00908997 -0.0318167 0.999452 0.348604 0.424941 -0.904603 -0.03346 48.758 0.905166 0.425032 0.00470053 48.3951 0.00996945 -0.0322843 0.999429 0.399679 0.403006 -0.91442 -0.0377045 47.6186 0.915127 0.403143 0.00423404 45.8393 0.0113286 -0.0362107 0.99928 0.443713 0.388167 -0.920941 -0.0345646 46.8989 0.921549 0.388227 0.00524809 44.1303 0.00858573 -0.0338901 0.999389 0.471289 0.36156 -0.931574 -0.0379967 45.8915 0.932227 0.361871 -0.00141586 41.582 0.0150689 -0.0349097 0.999277 0.526646 0.333324 -0.942309 -0.0308117 45.0122 0.942723 0.333563 -0.00284765 39.0775 0.012961 -0.0280977 0.999521 0.581763 0.315541 -0.948518 -0.0273534 44.4776 0.948859 0.315697 -0.00148358 37.4663 0.0100426 -0.0254864 0.999625 0.611135 0.291374 -0.956315 -0.0237325 43.7598 0.956518 0.2916 -0.006609 35.0928 0.0132407 -0.0207749 0.999696 0.66074 0.275973 -0.960915 -0.0219579 43.3393 0.96109 0.276165 -0.00619311 33.5659 0.0120151 -0.0193944 0.99974 0.690262 0.253621 -0.967087 -0.020463 42.7623 0.967236 0.253797 -0.0064604 31.3396 0.0114412 -0.018154 0.99977 0.732198 0.232396 -0.972416 -0.0199858 42.2684 0.972569 0.232547 -0.00555347 29.2206 0.0100479 -0.018147 0.999785 0.774738 0.211746 -0.97715 -0.0185014 41.833 0.977289 0.211863 -0.00459169 27.1743 0.00840654 -0.0171089 0.999818 0.815536 0.198167 -0.97998 -0.0191982 41.5614 0.980134 0.198286 -0.0044715 25.834 0.00818871 -0.0179307 0.999806 0.841816 0.184843 -0.982532 -0.0215426 41.3269 0.982739 0.184962 -0.00364203 24.5029 0.00756298 -0.0204976 0.999761 0.853168 0.166703 -0.985638 -0.026964 40.9883 0.985844 0.166115 0.0227519 22.5817 -0.017946 -0.0303751 0.999377 0.973382 0.152886 -0.987848 -0.027956 40.6995 0.988159 0.153182 -0.0087565 20.6256 0.0129325 -0.0262863 0.999571 0.957169 0.147778 -0.988253 -0.0389498 40.5033 0.988522 0.148839 -0.0259048 19.3766 0.0313978 -0.0346746 0.998905 1.0166 0.146229 -0.988935 -0.024995 40.26 0.989232 0.146335 -0.00244099 17.5905 0.00607162 -0.0243689 0.999685 1.04166 0.146486 -0.988672 -0.0326971 39.9862 0.989211 0.146472 0.00284222 15.8536 0.00197919 -0.0327607 0.999461 1.08707 0.149978 -0.988292 -0.0280349 39.7435 0.988675 0.150066 -0.00107369 14.1788 0.00526821 -0.0275564 0.999606 1.13081 0.15305 -0.987751 -0.0303933 39.5694 0.988211 0.153098 0.000747347 13.118 0.00391497 -0.0301493 0.999538 1.15259 0.163775 -0.986151 -0.0261635 39.3138 0.986497 0.163687 0.0054949 11.5934 -0.00113619 -0.0267101 0.999643 1.21461 0.185305 -0.982413 -0.0229444 39.038 0.982681 0.185228 0.00543532 10.1265 -0.00108979 -0.0235542 0.999722 1.24629 0.225553 -0.974072 -0.0176055 38.6722 0.974219 0.225602 -0.000778455 8.71997 0.0047301 -0.0169761 0.999845 1.29446 0.288075 -0.957493 -0.0147954 38.225 0.957604 0.288082 0.00170094 7.36098 0.00263365 -0.0146581 0.999889 1.30997 0.367004 -0.930123 -0.0133854 37.6704 0.930202 0.367048 -0.00083121 6.08029 0.0056862 -0.0121461 0.99991 1.35018 0.496953 -0.86768 -0.0130063 36.7004 0.867763 0.496975 0.00170253 4.51853 0.00498656 -0.0121325 0.999914 1.3773 0.596509 -0.802362 -0.0197782 35.7735 0.802477 0.596674 -0.00324467 3.45297 0.0144045 -0.0139361 0.999799 1.40995 0.692369 -0.721241 -0.0208972 34.7357 0.721342 0.69257 -0.00357629 2.48929 0.0170521 -0.0125979 0.999775 1.40233 0.806152 -0.590859 -0.0317075 33.1362 0.591185 0.806535 0.00115625 1.46104 0.0248901 -0.0196771 0.999497 1.40033 0.877981 -0.477715 -0.0306143 31.8037 0.478038 0.878331 0.00381186 0.844133 0.0250685 -0.0179815 0.999524 1.38002 0.916087 -0.399679 -0.0322817 30.8325 0.400249 0.916307 0.0134539 0.537005 0.0242027 -0.0252456 0.999388 1.36619 0.945291 -0.324539 -0.0331658 29.8299 0.325233 0.94546 0.0181152 0.272588 0.0254779 -0.0279107 0.999286 1.37161 0.972915 -0.229138 -0.0305283 28.2343 0.230065 0.972668 0.0314125 -0.0156809 0.0224961 -0.0375852 0.99904 1.34232 0.983154 -0.180013 -0.0316635 27.1477 0.181055 0.98289 0.0338445 -0.164533 0.0250293 -0.0390072 0.998925 1.37186 0.989912 -0.138033 -0.031946 26.0195 0.138977 0.989856 0.0295156 -0.285402 0.0275478 -0.0336576 0.999054 1.34702 0.994271 -0.100822 -0.035491 24.8551 0.102165 0.994029 0.0383159 -0.3535 0.031416 -0.0417224 0.998635 1.29995 0.997355 -0.0666191 -0.0290504 23.0609 0.0678643 0.996714 0.0442211 -0.424997 0.0260089 -0.0460756 0.998599 1.35327 0.998433 -0.0497337 -0.0256352 21.8551 0.0507274 0.997923 0.0396948 -0.48215 0.0236078 -0.040933 0.998883 1.30682 0.999056 -0.0274189 -0.0337082 19.9431 0.0288234 0.998705 0.0419122 -0.502262 0.0325154 -0.0428442 0.998553 1.30641 0.999467 -0.0158045 -0.0285603 18.6467 0.0169138 0.999095 0.0390256 -0.521113 0.0279177 -0.0394879 0.99883 1.2972 0.999653 -0.00498309 -0.025869 17.3181 0.00596983 0.999252 0.0382076 -0.51809 0.0256592 -0.0383488 0.998935 1.27625 0.999633 0.00303555 -0.026918 15.9711 -0.00193103 0.999159 0.0409643 -0.506273 0.0270197 -0.0408973 0.998798 1.26888 0.999586 0.00775321 -0.0276909 14.5801 -0.00657198 0.999075 0.0424971 -0.476379 0.0279948 -0.0422975 0.998713 1.25988 0.999485 0.00741094 -0.0312334 12.4509 -0.00597909 0.998938 0.0456904 -0.437754 0.0315388 -0.0454801 0.998467 1.24303 0.999571 0.00674064 -0.0284967 11.015 -0.00548525 0.999021 0.0439047 -0.432516 0.0287648 -0.0437295 0.998629 1.24234 0.999634 0.00506553 -0.0265904 9.56164 -0.00391641 0.999063 0.0430909 -0.420921 0.0267838 -0.0429709 0.998717 1.22274 0.999582 0.00369184 -0.0286738 8.06638 -0.00244738 0.999058 0.0433154 -0.413908 0.0288067 -0.0432271 0.99865 1.21763 0.999668 0.00252652 -0.0256526 6.54236 -0.00147352 0.999159 0.0409846 -0.407103 0.0257346 -0.0409332 0.99883 1.21549 0.999727 0.0013644 -0.0233457 5.01558 -0.00043131 0.999203 0.0399269 -0.412125 0.0233816 -0.0399059 0.99893 1.2065 0.99968 -0.000285631 -0.0252987 2.64347 0.00129444 0.999204 0.0398684 -0.409762 0.0252672 -0.0398884 0.998885 1.19987 0.999618 -0.00148777 -0.0276084 1.04832 0.00257752 0.999217 0.0394783 -0.413168 0.0275281 -0.0395344 0.998839 1.19027 0.999473 -0.00278153 -0.0323382 -0.592145 0.00410163 0.999158 0.0408271 -0.394652 0.0321974 -0.0409382 0.998643 1.17467 0.999559 -0.00247719 -0.0295783 -2.21906 0.00363225 0.99923 0.0390614 -0.39588 0.0294588 -0.0391516 0.998799 1.17726 0.999591 6.50547e-05 -0.0285996 -5.5661 0.0010472 0.999244 0.0388737 -0.391944 0.0285805 -0.0388878 0.998835 1.14616 0.999492 0.00263583 -0.0317459 -7.29465 -0.00138263 0.999221 0.0394335 -0.385614 0.0318251 -0.0393696 0.998718 1.12893 0.999611 0.00441836 -0.0275245 -9.02795 -0.00334725 0.99924 0.0388403 -0.365611 0.0276751 -0.038733 0.998866 1.12434 0.999654 0.00675289 -0.0254081 -11.683 -0.00579559 0.999277 0.0375639 -0.346954 0.0256434 -0.0374036 0.998971 1.11061 0.999543 0.00817121 -0.0290979 -13.4922 -0.00708123 0.999277 0.037367 -0.320258 0.0293821 -0.0371438 0.998878 1.09678 0.99962 0.00816102 -0.0263336 -15.3077 -0.00717308 0.999275 0.0373953 -0.305178 0.0266197 -0.0371922 0.998954 1.08427 0.999561 0.00750767 -0.0286766 -17.1426 -0.00640876 0.999249 0.0382223 -0.286867 0.028942 -0.0380217 0.998858 1.08201 0.999581 0.00679491 -0.0281493 -19.0185 -0.00573225 0.999274 0.0376609 -0.259177 0.0283848 -0.0374837 0.998894 1.06444 0.99962 0.00647214 -0.0267836 -20.9026 -0.00545927 0.999273 0.0377186 -0.255331 0.0270083 -0.0375581 0.998929 1.05473 0.999652 0.00491264 -0.0259289 -22.8006 -0.003936 0.999286 0.0375833 -0.225569 0.026095 -0.0374681 0.998957 1.05557 0.999558 0.00326809 -0.029551 -25.681 -0.00211726 0.999241 0.0388917 -0.215404 0.0296557 -0.0388119 0.998806 1.03167 0.999578 0.0025935 -0.0289468 -27.6049 -0.00139613 0.999145 0.0413083 -0.204192 0.0290292 -0.0412505 0.998727 1.02901 0.999705 0.000714068 -0.0242766 -30.5383 0.000253285 0.999207 0.0398207 -0.187773 0.0242858 -0.0398151 0.998912 1.02927 0.999524 -5.6765e-05 -0.0308421 -32.531 0.00120628 0.999305 0.0372538 -0.182298 0.0308185 -0.0372733 0.99883 1.02282 0.999608 -0.00084505 -0.0280023 -34.4954 0.00185609 0.999346 0.0360994 -0.196039 0.0279535 -0.0361372 0.998956 1.00323 0.999568 -0.000955418 -0.0293711 -36.4652 0.00204991 0.999304 0.0372567 -0.186404 0.029315 -0.0373008 0.998874 0.99765 0.999408 -0.00154335 -0.0343755 -38.4459 0.00287668 0.999244 0.0387718 -0.177175 0.0342897 -0.0388477 0.998657 0.98586 0.999529 -0.00220878 -0.0305995 -40.3995 0.0033894 0.99925 0.0385851 -0.184294 0.0304914 -0.0386706 0.998787 0.977837 0.999554 -0.00373961 -0.0296121 -44.3395 0.00492114 0.99919 0.0399286 -0.186546 0.0294388 -0.0400565 0.998764 0.958624 0.999319 -0.00489861 -0.0365815 -47.3125 0.00641048 0.999125 0.0413265 -0.201718 0.0363471 -0.0415328 0.998476 0.943173 0.999687 -0.00525649 -0.0244751 -49.2757 0.00623015 0.999185 0.0398766 -0.216098 0.0242455 -0.0400166 0.998905 0.938583 0.99967 -0.0058291 -0.0250108 -51.247 0.00682502 0.99918 0.0399208 -0.208081 0.0247576 -0.0400784 0.99889 0.92497 0.999364 -0.00692587 -0.0349739 -53.2253 0.00834562 0.99914 0.0406132 -0.214602 0.0346626 -0.0408792 0.998563 0.918959 0.999461 -0.00705464 -0.0320749 -55.1701 0.00837923 0.99911 0.0413516 -0.231965 0.0317546 -0.041598 0.99863 0.91383 0.99967 -0.00765056 -0.024518 -57.1171 0.00862097 0.999174 0.0397211 -0.244309 0.0241938 -0.0399194 0.99891 0.908594 0.999538 -0.00786681 -0.0293605 -59.0548 0.00903272 0.999167 0.039791 -0.248256 0.029023 -0.0400378 0.998777 0.899101 0.999356 -0.0081415 -0.0349512 -60.9571 0.00957107 0.999116 0.0409312 -0.259349 0.0345871 -0.0412393 0.99855 0.894465 0.999583 -0.00841171 -0.0276299 -62.8403 0.0094913 0.999187 0.0391773 -0.274637 0.0272779 -0.0394232 0.99885 0.903703 0.999695 -0.00874889 -0.0231114 -64.7327 0.0095624 0.99933 0.0353267 -0.298156 0.0227869 -0.0355369 0.999109 0.903149 0.999579 -0.00641963 -0.028281 -66.6007 0.00736162 0.999417 0.0333309 -0.308595 0.0280506 -0.0335251 0.999044 0.890601 0.999563 -0.00432531 -0.0292306 -68.4527 0.00532242 0.999404 0.0341206 -0.307817 0.0290656 -0.0342613 0.99899 0.897254 0.999534 -0.00233965 -0.0304485 -70.289 0.00328952 0.999508 0.0311835 -0.319397 0.0303605 -0.0312691 0.99905 0.892204 0.999492 -8.1362e-05 -0.0318739 -72.1208 0.00103873 0.999549 0.0300207 -0.319941 0.031857 -0.0300385 0.999041 0.88531 0.999559 0.00211041 -0.029617 -73.9321 -0.00127591 0.999602 0.028167 -0.309811 0.0296646 -0.0281168 0.999164 0.878541 0.999379 0.0039473 -0.0350036 -75.7439 -0.0029437 0.999584 0.0286767 -0.30359 0.0351022 -0.0285558 0.998976 0.858904 0.999209 0.00787407 -0.038982 -78.4269 -0.00663291 0.99947 0.031867 -0.270811 0.0392123 -0.0315832 0.998732 0.841982 0.999754 0.0112238 -0.0191409 -81.0539 -0.0106897 0.999557 0.027781 -0.244338 0.0194442 -0.0275696 0.999431 0.839112 0.999502 0.0147055 -0.0279091 -82.8308 -0.0140559 0.999629 0.0233282 -0.225881 0.0282418 -0.0229243 0.999338 0.838365 0.999796 0.0189653 -0.00694458 -84.5568 -0.0187352 0.999318 0.0318179 -0.17554 0.00754328 -0.0316813 0.99947 0.848818 0.999727 0.0233842 0.000232451 -86.3114 -0.0233757 0.998978 0.0386949 -0.0995213 0.000672637 -0.0386897 0.999251 0.876035 0.996981 0.0291069 -0.0719801 -88.9866 -0.0271768 0.999248 0.0276501 -0.0429263 0.0727308 -0.0256105 0.997023 0.855817 0.999085 0.0347119 -0.0249682 -90.6903 -0.0340526 0.999072 0.0263618 0.0149962 0.0258601 -0.0254875 0.999341 0.831662 0.999142 0.0391176 -0.0135738 -92.4436 -0.0387364 0.998877 0.0272981 0.100347 0.0146264 -0.0267489 0.999535 0.803641 0.998563 0.0447277 -0.0295092 -95.1562 -0.0438431 0.998589 0.0299726 0.238548 0.0308082 -0.0286358 0.999115 0.797441 0.998448 0.0489703 -0.0265136 -96.9622 -0.0480767 0.998287 0.033352 0.329483 0.0281014 -0.0320255 0.999092 0.796359 0.998223 0.0530074 -0.0272122 -98.7776 -0.0521717 0.998171 0.0305557 0.428654 0.0287821 -0.0290817 0.999163 0.784074 0.998002 0.0570888 -0.0270537 -100.622 -0.0561685 0.997855 0.0336358 0.561977 0.0289159 -0.032049 0.999068 0.768444 0.997842 0.0612147 -0.0237692 -102.473 -0.060421 0.997634 0.0327883 0.676768 0.02572 -0.0312813 0.99918 0.778347 0.997497 0.0671393 -0.0221731 -105.278 -0.0665014 0.997383 0.0283534 0.857713 0.0240187 -0.0268079 0.999352 0.771844 0.996886 0.0716564 -0.0329131 -107.17 -0.0707358 0.997093 0.0283341 1.00739 0.0348477 -0.0259177 0.999057 0.759954 0.996776 0.0754865 -0.0272001 -109.059 -0.0747901 0.996866 0.0257686 1.13629 0.02906 -0.0236512 0.999298 0.755919 0.99665 0.0802802 -0.0156416 -110.956 -0.0798928 0.996515 0.0239965 1.2999 0.0175136 -0.0226665 0.99959 0.751647 0.996115 0.0846861 -0.0241518 -112.906 -0.0840932 0.996154 0.0245908 1.47545 0.0261414 -0.0224642 0.999406 0.743733 0.995441 0.0935304 -0.0186825 -116.765 -0.0930611 0.995357 0.0245864 1.84704 0.0208953 -0.0227357 0.999523 0.721086 0.995179 0.0972449 -0.0127076 -118.695 -0.0968896 0.994953 0.0260939 2.05958 0.0151809 -0.0247368 0.999579 0.737582 0.99481 0.10047 -0.0160744 -119.7 -0.100016 0.994621 0.0269317 2.16029 0.0186938 -0.0251842 0.999508 0.745022 0.99441 0.104594 -0.0144159 -121.666 -0.104221 0.994251 0.0245721 2.36371 0.0169031 -0.0229323 0.999594 0.765102 0.993832 0.109184 -0.0194136 -123.658 -0.108631 0.993699 0.0275855 2.59657 0.0223032 -0.0253065 0.999431 0.772322 0.993333 0.111585 -0.0289472 -124.651 -0.110748 0.993424 0.0290434 2.7206 0.0319976 -0.0256439 0.999159 0.771168 0.992639 0.116578 -0.0328343 -126.675 -0.115616 0.992846 0.0298157 2.94707 0.0360753 -0.0258001 0.999016 0.735268 0.992358 0.121074 -0.0237827 -128.664 -0.120369 0.992301 0.0291417 3.20026 0.0271279 -0.0260563 0.999292 0.730661 0.991958 0.124 -0.0253531 -130.71 -0.123256 0.991949 0.0290634 3.46139 0.0287529 -0.0257047 0.999256 0.706397 0.991759 0.12472 -0.029311 -131.724 -0.123791 0.991805 0.0316033 3.5969 0.0330123 -0.0277144 0.999071 0.699149 0.99174 0.124912 -0.0291355 -133.733 -0.123897 0.991699 0.0343927 3.84957 0.0331897 -0.0304988 0.998984 0.685672 0.991994 0.12342 -0.0267529 -135.776 -0.12243 0.991827 0.0359273 4.13525 0.0309684 -0.0323643 0.998996 0.680107 0.992029 0.122884 -0.0279075 -136.801 -0.12187 0.99191 0.0355312 4.24837 0.032048 -0.0318469 0.998979 0.671625 0.992133 0.121352 -0.0307428 -138.842 -0.120343 0.99219 0.0328109 4.50695 0.0344844 -0.0288531 0.998989 0.652396 0.992353 0.120127 -0.0283559 -140.903 -0.119247 0.992383 0.0309155 4.77271 0.0318537 -0.0272978 0.99912 0.63814 0.992517 0.119231 -0.0263245 -142.962 -0.118387 0.992465 0.0315834 5.01667 0.0298919 -0.0282306 0.999154 0.610135 0.992498 0.119067 -0.0277781 -143.991 -0.118174 0.992482 0.0318621 5.13373 0.031363 -0.0283405 0.999106 0.620109 0.992556 0.118499 -0.0281254 -146.017 -0.117645 0.992593 0.0303022 5.38362 0.0315079 -0.0267678 0.999145 0.599137 0.992614 0.117511 -0.0301313 -148.043 -0.116636 0.992741 0.0293453 5.6189 0.033361 -0.0256142 0.999115 0.587705 0.992655 0.117515 -0.0287434 -149.066 -0.116699 0.992756 0.0285956 5.74865 0.0318955 -0.0250312 0.999178 0.581228 0.992681 0.11812 -0.0251344 -153.031 -0.117332 0.992615 0.0307968 6.22265 0.0285865 -0.0276223 0.99921 0.561124 0.992635 0.11846 -0.0253647 -154.049 -0.117631 0.992541 0.0320075 6.35795 0.0289671 -0.0287881 0.999166 0.556988 0.992446 0.119287 -0.0286447 -156.024 -0.118296 0.992392 0.0341167 6.59302 0.0324964 -0.0304704 0.999007 0.549406 0.992453 0.119576 -0.02718 -157.98 -0.118617 0.99234 0.0345022 6.84371 0.0310974 -0.0310177 0.999035 0.543907 0.992499 0.119602 -0.0253171 -159.913 -0.118717 0.992347 0.0339781 7.07775 0.0291872 -0.0307176 0.999102 0.541288 0.992383 0.119573 -0.0296378 -162.814 -0.118534 0.992346 0.0346321 7.44812 0.033552 -0.0308552 0.998961 0.51408 0.992374 0.119221 -0.0313111 -164.717 -0.118113 0.992378 0.0351382 7.6721 0.0352616 -0.031172 0.998892 0.504084 0.992531 0.118562 -0.028741 -166.644 -0.117555 0.992466 0.0345273 7.90896 0.0326181 -0.0308908 0.99899 0.4874 0.992672 0.1178 -0.0269497 -169.49 -0.116829 0.992529 0.0351732 8.25659 0.0308918 -0.0317669 0.999018 0.471384 0.992777 0.11756 -0.0239375 -171.354 -0.116669 0.992527 0.0357552 8.48758 0.027962 -0.0327042 0.999074 0.471036 0.992733 0.117453 -0.0261783 -173.218 -0.11656 0.992623 0.0333608 8.70154 0.0299035 -0.030067 0.9991 0.475257 0.992732 0.116777 -0.0290978 -176 -0.115876 0.992782 0.0309383 9.02912 0.0325007 -0.0273417 0.999098 0.454622 0.992817 0.116612 -0.0267412 -177.832 -0.11584 0.99285 0.0288112 9.24663 0.0299097 -0.0255066 0.999227 0.452003 0.992924 0.116093 -0.0250006 -179.64 -0.115406 0.992941 0.0273686 9.45568 0.0280014 -0.0242897 0.999313 0.456486 0.992976 0.115385 -0.0261886 -181.447 -0.114789 0.993119 0.023206 9.67584 0.028686 -0.0200368 0.999388 0.466837 0.993396 0.114734 0.000679983 -184.094 -0.114714 0.993076 0.0253226 9.98609 0.00223009 -0.0252334 0.999679 0.467957 0.990946 0.117189 -0.0655144 -187.689 -0.11507 0.992732 0.0352385 10.4377 0.0691678 -0.0273807 0.997229 0.431533 0.992462 0.120471 -0.0225076 -189.425 -0.119867 0.992436 0.0265114 10.6372 0.0255312 -0.0236136 0.999395 0.429398 0.992554 0.121794 -0.00156563 -191.177 -0.121705 0.992183 0.0275743 10.8682 0.00491178 -0.0271784 0.999619 0.423461 0.991724 0.124199 -0.0325335 -192.999 -0.123046 0.991772 0.0353277 11.1132 0.0366535 -0.0310322 0.998846 0.425472 0.991528 0.125392 -0.0339071 -194.809 -0.12438 0.991767 0.0304572 11.3292 0.037447 -0.0259818 0.998961 0.408289 0.991756 0.126381 -0.0211626 -196.578 -0.125601 0.991468 0.0348585 11.5678 0.0253875 -0.0319131 0.999168 0.371526 0.991643 0.126703 -0.0243151 -198.379 -0.125745 0.991352 0.0375488 11.8067 0.0288623 -0.0341775 0.998999 0.379542 0.991869 0.126226 -0.0162075 -200.171 -0.125539 0.991358 0.0380628 12.0382 0.0208719 -0.0357187 0.999144 0.382157 0.991859 0.125788 -0.0198525 -201.978 -0.124953 0.991403 0.0388161 12.2736 0.0245644 -0.0360195 0.999049 0.382395 0.991646 0.126542 -0.0250266 -203.787 -0.125593 0.991413 0.036419 12.5069 0.0294202 -0.0329716 0.999023 0.387705 0.99164 0.126832 -0.0237317 -205.616 -0.125965 0.991416 0.0350295 12.7242 0.0279709 -0.0317473 0.999104 0.369825 0.991452 0.12822 -0.0241254 -208.344 -0.127358 0.99126 0.0344056 13.0871 0.0283261 -0.031039 0.999117 0.356087 0.991413 0.128925 -0.0218856 -210.176 -0.128136 0.991164 0.0342863 13.3276 0.0261126 -0.0311876 0.999172 0.347635 0.991352 0.129156 -0.0232508 -212.013 -0.128291 0.991101 0.0355104 13.5827 0.0276303 -0.0322205 0.999099 0.342392 0.991226 0.129523 -0.0263519 -213.862 -0.128536 0.991043 0.0362193 13.8374 0.0308071 -0.0325144 0.998996 0.340036 0.991237 0.130401 -0.021087 -215.682 -0.129628 0.990959 0.0346089 14.0694 0.0254094 -0.0315721 0.999178 0.329882 0.991251 0.129853 -0.0236584 -217.513 -0.128953 0.990987 0.0362674 14.3202 0.0281546 -0.0328992 0.999062 0.313428 0.991117 0.130296 -0.0266482 -219.338 -0.129277 0.990921 0.036918 14.5569 0.0312165 -0.0331451 0.998963 0.324293 0.991566 0.129584 0.00227558 -221.124 -0.129577 0.990853 0.037688 14.7986 0.00262899 -0.037665 0.999287 0.32625 0.991779 0.12783 -0.00586979 -222.945 -0.127488 0.991 0.0408258 15.0671 0.0110357 -0.0397418 0.999149 0.35454 0.989913 0.126569 -0.0636538 -225.746 -0.124119 0.991416 0.0410901 15.4089 0.0683082 -0.032775 0.997126 0.303119 0.991963 0.126387 -0.00605806 -227.539 -0.1261 0.991395 0.0351304 15.6269 0.0104459 -0.0340841 0.999364 0.291783 0.991975 0.125457 -0.0156897 -229.402 -0.124804 0.991479 0.0373322 15.8848 0.0202396 -0.0350744 0.99918 0.274464 0.991519 0.125042 -0.0354156 -231.284 -0.123559 0.991475 0.0413709 16.122 0.0402867 -0.0366441 0.998516 0.282103 0.992041 0.124245 -0.0204469 -233.152 -0.123411 0.991629 0.037966 16.3591 0.0249929 -0.0351404 0.99907 0.275772 0.992347 0.122821 -0.0127312 -235.044 -0.122348 0.99194 0.0329426 16.5944 0.0166746 -0.0311329 0.999376 0.276667 0.992221 0.122147 -0.0240514 -236.966 -0.121367 0.992106 0.0315627 16.8209 0.0277168 -0.0283981 0.999212 0.274971 0.992172 0.121144 -0.0303279 -238.881 -0.12024 0.99229 0.0300576 17.0647 0.0337354 -0.0261757 0.999088 0.266419 0.992502 0.119836 -0.0240726 -241.743 -0.119064 0.992394 0.03128 17.4173 0.027638 -0.0281793 0.999221 0.243022 0.992512 0.119158 -0.026854 -243.696 -0.118264 0.992442 0.0327388 17.6705 0.0305521 -0.0293178 0.999103 0.229304 0.992494 0.119171 -0.0274554 -245.613 -0.118163 0.992348 0.0358098 17.9015 0.0315128 -0.0322968 0.998981 0.237896 0.992889 0.117221 -0.0207689 -247.537 -0.116472 0.992604 0.034221 18.1457 0.0246267 -0.0315586 0.999198 0.232074 0.992966 0.116507 -0.0210924 -249.443 -0.115787 0.992737 0.0326684 18.3671 0.0247453 -0.0299964 0.999244 0.233875 0.992909 0.116081 -0.0256385 -251.334 -0.115218 0.992795 0.0329144 18.5959 0.0292746 -0.029727 0.999129 0.230245 0.992962 0.115585 -0.0257983 -253.204 -0.114683 0.992817 0.0340877 18.8106 0.029553 -0.0308892 0.999086 0.228896 0.993013 0.1151 -0.0260203 -255.072 -0.114143 0.992823 0.0356944 19.0373 0.029942 -0.032475 0.999024 0.22008 0.993069 0.114636 -0.0259235 -256.914 -0.113656 0.992846 0.0365912 19.2518 0.0299327 -0.0333913 0.998994 0.218238 0.993258 0.113845 -0.0218786 -258.758 -0.112995 0.992914 0.0367845 19.4713 0.0259113 -0.0340643 0.999084 0.209126 0.993251 0.113238 -0.0251098 -260.584 -0.112252 0.992964 0.0377186 19.6901 0.0292043 -0.0346454 0.998973 0.207327 0.993595 0.112666 -0.00864465 -263.292 -0.1123 0.993057 0.0350184 19.9772 0.01253 -0.0338233 0.999349 0.240967 0.99318 0.11149 0.0341002 -265.015 -0.112856 0.992758 0.0411661 20.2064 -0.0292636 -0.0447338 0.99857 0.285576 0.991766 0.111196 -0.0635282 -267.727 -0.108271 0.992973 0.0477777 20.5335 0.0683945 -0.040506 0.996836 0.29634 0.99125 0.110921 -0.0715598 -269.506 -0.107898 0.993151 0.0448145 20.7122 0.0760406 -0.0367012 0.996429 0.241684 0.993785 0.111175 0.00558038 -271.201 -0.111302 0.993203 0.0340435 20.885 -0.00175765 -0.034453 0.999405 0.246812 0.993893 0.110294 -0.00338986 -272.95 -0.110101 0.993264 0.0361237 21.1198 0.00735126 -0.0355299 0.999342 0.243176 0.993488 0.110074 -0.029422 -274.742 -0.108935 0.993328 0.0378531 21.3177 0.0333923 -0.0344015 0.99885 0.255291 0.993952 0.108377 -0.0176989 -276.482 -0.107624 0.993423 0.0390758 21.5016 0.0218174 -0.0369347 0.999079 0.249848 0.994387 0.105193 -0.0113305 -278.225 -0.104675 0.993728 0.0393306 21.6983 0.0153968 -0.0379239 0.999162 0.266677 0.994734 0.101927 -0.0107601 -280.011 -0.101483 0.994191 0.0358408 21.8729 0.0143507 -0.0345601 0.9993 0.294747 0.995049 0.0985446 -0.0128783 -281.778 -0.0980548 0.994589 0.0343207 22.0495 0.0161907 -0.032888 0.999328 0.300153 0.995449 0.0936564 -0.0176128 -284.44 -0.0930338 0.995104 0.0333544 22.3008 0.0206505 -0.031564 0.999288 0.313042 0.99573 0.0905096 -0.0181365 -286.22 -0.0898717 0.995395 0.0333471 22.4614 0.0210712 -0.0315747 0.999279 0.316459 0.996074 0.0870719 -0.0159459 -288.011 -0.0865034 0.995691 0.0334206 22.62 0.0187872 -0.03191 0.999314 0.323831 0.99654 0.0816581 -0.0155127 -290.692 -0.0811297 0.996187 0.0320916 22.8405 0.0180741 -0.030722 0.999365 0.345667 0.99682 0.0780395 -0.016102 -292.497 -0.0774997 0.996485 0.0317947 22.9923 0.0185267 -0.0304457 0.999365 0.349559 0.997056 0.0748284 -0.0167591 -294.311 -0.0742548 0.996707 0.0325717 23.1268 0.0191412 -0.0312314 0.999329 0.356989 0.997312 0.0711143 -0.0176388 -296.116 -0.0705434 0.997023 0.031113 23.2444 0.0197989 -0.029785 0.99936 0.382667 0.997819 0.0655061 -0.00810351 -298.819 -0.0652346 0.997414 0.0301602 23.4391 0.0100582 -0.0295658 0.999512 0.403625 0.997936 0.0621488 -0.0161883 -300.658 -0.0616435 0.997646 0.0300388 23.5499 0.0180171 -0.0289789 0.999418 0.424654 0.998462 0.0542135 -0.0116152 -304.28 -0.0538615 0.998135 0.0287275 23.7578 0.0131509 -0.0280577 0.99952 0.46954 0.998585 0.0512019 -0.0143802 -306.076 -0.050785 0.998317 0.0279986 23.8529 0.0157896 -0.0272287 0.999505 0.48831 0.998724 0.0478259 -0.0162299 -307.847 -0.0473456 0.998464 0.028789 23.9403 0.0175818 -0.0279838 0.999454 0.504051 0.998912 0.0436166 -0.0165332 -309.604 -0.0430699 0.998556 0.0320937 24.0292 0.0179091 -0.0313467 0.999348 0.513865 0.999278 0.0369815 -0.0087318 -312.218 -0.0367218 0.998927 0.028234 24.1267 0.00976656 -0.027893 0.999563 0.563586 0.999449 0.0331779 0.000630236 -313.928 -0.0331828 0.999081 0.0271298 24.1786 0.000270452 -0.0271357 0.999632 0.60247 0.999162 0.0298699 0.0279955 -315.612 -0.0306339 0.999159 0.027271 24.2296 -0.0271574 -0.0281057 0.999236 0.674165 0.999577 0.0263093 0.0123687 -317.345 -0.0266326 0.999288 0.0267364 24.2868 -0.0116565 -0.0270545 0.999566 0.700934 0.997614 0.0265068 -0.0637517 -319.177 -0.0246176 0.999239 0.0302393 24.3488 0.0645048 -0.0285977 0.997508 0.693601 0.998474 0.0275873 -0.0478408 -320.915 -0.0263422 0.999303 0.0264632 24.3836 0.0485375 -0.0251626 0.998504 0.708452 0.99938 0.025935 0.023801 -322.616 -0.0265118 0.999354 0.0242479 24.4264 -0.0231568 -0.0248639 0.999423 0.710866 0.999692 0.0246799 -0.00261819 -324.418 -0.024622 0.999492 0.0202469 24.4638 0.00311655 -0.0201762 0.999792 0.749009 0.999715 0.0229743 -0.00653917 -326.185 -0.0228615 0.999597 0.0168299 24.5023 0.00692319 -0.0166756 0.999837 0.783254 0.999767 0.0215183 -0.00151113 -327.947 -0.0214879 0.999609 0.0178714 24.548 0.0018951 -0.0178348 0.999839 0.788973 0.999722 0.0200248 -0.0124231 -329.738 -0.0197694 0.999597 0.0203533 24.5816 0.0128257 -0.0201021 0.999716 0.82264 0.999723 0.0189196 -0.0139977 -331.544 -0.0186394 0.999628 0.019889 24.6289 0.0143688 -0.0196225 0.999704 0.840689 0.999818 0.0170493 -0.00854777 -333.365 -0.0168474 0.99959 0.0231689 24.6759 0.00893928 -0.0230207 0.999695 0.845639 0.999822 0.0165211 -0.00913397 -335.176 -0.01627 0.999505 0.0269174 24.7133 0.00957416 -0.026764 0.999596 0.879671 0.999864 0.0158714 -0.00445148 -337.003 -0.0157427 0.999495 0.027591 24.7464 0.00488714 -0.0275172 0.999609 0.911757 0.999892 0.0146832 0.000297578 -338.814 -0.014686 0.999586 0.0247464 24.7766 6.59003e-05 -0.0247481 0.999694 0.956057 0.999897 0.0117154 -0.0082756 -342.539 -0.0115585 0.999757 0.0187671 24.8095 0.00849346 -0.0186695 0.99979 1.02235 0.999919 0.0109452 -0.00645814 -344.377 -0.0108241 0.99977 0.0184964 24.8396 0.00665911 -0.018425 0.999808 1.05302 0.99995 0.00965101 -0.00257732 -346.249 -0.00960283 0.99979 0.0180966 24.8554 0.00275143 -0.0180709 0.999833 1.08458 0.999963 0.00763828 -0.00390379 -348.136 -0.00757074 0.999826 0.0170337 24.8746 0.00403322 -0.0170035 0.999847 1.12694 0.999964 0.00616785 -0.00578036 -350.033 -0.00607056 0.999842 0.0166999 24.8762 0.00588245 -0.0166642 0.999844 1.15898 0.999953 0.00436233 -0.00869778 -351.92 -0.00420267 0.999824 0.0182913 24.9004 0.00877604 -0.0182538 0.999795 1.19017 0.999949 0.00262102 -0.00972264 -353.826 -0.00244691 0.999837 0.0178763 24.9076 0.00976791 -0.0178517 0.999793 1.21988 0.999987 0.000991427 -0.00500679 -355.677 -0.000901018 0.999837 0.0180274 24.9067 0.00502385 -0.0180226 0.999825 1.25502 0.999972 -0.00031403 -0.00749447 -357.539 0.000444537 0.999848 0.0174185 24.9122 0.00748786 -0.0174213 0.99982 1.29561 0.999949 -0.000168383 -0.0100951 -359.394 0.000345522 0.999846 0.0175479 24.9172 0.0100906 -0.0175505 0.999795 1.33048 0.999974 0.00138524 -0.00705701 -362.109 -0.00125356 0.999826 0.0186301 24.9387 0.00708159 -0.0186207 0.999802 1.37969 0.999991 0.00257379 -0.00329249 -363.895 -0.00251568 0.999843 0.0175314 24.9278 0.0033371 -0.017523 0.999841 1.41619 0.999972 0.0034184 -0.00662777 -365.671 -0.00330716 0.999855 0.0167235 24.9442 0.00668397 -0.0167011 0.999838 1.45134 0.99994 0.00407972 -0.0101699 -367.422 -0.0039078 0.99985 0.016868 24.9516 0.0102372 -0.0168273 0.999806 1.47911 0.999964 0.00493401 -0.00688836 -369.153 -0.00479871 0.999798 0.0195229 24.9803 0.00698329 -0.0194891 0.999786 1.50499 0.99999 0.00435723 -0.000247481 -370.899 -0.00435146 0.999794 0.0198455 24.9872 0.000333902 -0.0198442 0.999803 1.54794 0.999269 0.00287886 0.0381329 -373.454 -0.00370449 0.99976 0.0215983 25.0141 -0.0380615 -0.0217238 0.999039 1.66353 0.999624 0.000901203 0.0274104 -375.18 -0.00153813 0.999729 0.0232245 25.0131 -0.027382 -0.0232579 0.999354 1.72496 0.998598 -0.00104488 -0.0529302 -377.021 0.0022755 0.999728 0.0231951 25.0233 0.0528916 -0.023283 0.998329 1.73301 0.99993 -0.00239379 -0.0115656 -379.619 0.00264694 0.999756 0.0219224 25.039 0.0115103 -0.0219515 0.999693 1.74464 0.999882 -0.00760162 -0.0133633 -383.178 0.00791758 0.999687 0.0237522 25.008 0.0131786 -0.0238552 0.999629 1.79241 0.999906 -0.00982211 -0.00954702 -384.998 0.0100565 0.999641 0.0248188 24.9913 0.00929982 -0.0249125 0.999646 1.82378 0.999904 -0.0125921 0.00578289 -386.772 0.0124437 0.999609 0.0250255 24.9793 -0.00609576 -0.0249512 0.99967 1.85742 0.999897 -0.014322 -0.000700131 -388.634 0.0143347 0.999612 0.0238729 24.9453 0.000357951 -0.0238805 0.999715 1.89803 0.999846 -0.0160157 -0.007176 -390.481 0.0161871 0.999569 0.0244893 24.9117 0.0067807 -0.0246016 0.999674 1.93193 0.999826 -0.018638 -0.000110715 -392.312 0.0186352 0.999529 0.0243735 24.8845 -0.000343612 -0.0243714 0.999703 1.97519 0.999771 -0.0209725 0.00416636 -394.16 0.0208675 0.999498 0.0238256 24.8509 -0.00466395 -0.0237332 0.999707 2.01651 0.999741 -0.0227235 -0.00136239 -396.04 0.0227498 0.99945 0.0241436 24.8108 0.00081301 -0.0241683 0.999708 2.05948 0.999678 -0.0247053 -0.00577164 -397.942 0.0248555 0.999311 0.0275811 24.7676 0.00508626 -0.0277157 0.999603 2.09336 0.999633 -0.0268777 -0.00338709 -399.807 0.0269632 0.999232 0.0284398 24.7169 0.00262009 -0.0285207 0.99959 2.15917 0.999557 -0.0297048 0.00176621 -401.633 0.0296448 0.99918 0.0275869 24.6667 -0.00258423 -0.0275224 0.999618 2.199 0.99949 -0.0317267 -0.00349576 -403.457 0.0318102 0.999124 0.0272049 24.6228 0.00262957 -0.0273022 0.999624 2.24369 0.999417 -0.033733 -0.00525629 -405.271 0.0338669 0.999037 0.0279021 24.5687 0.00431001 -0.0280638 0.999597 2.2869 0.999325 -0.0366513 -0.00248101 -407.06 0.0367077 0.998908 0.0289057 24.5089 0.00141887 -0.0289773 0.999579 2.32782 0.999192 -0.0401577 -0.00139878 -408.825 0.0401813 0.998779 0.0287272 24.436 0.000243454 -0.0287602 0.999586 2.38465 0.998818 -0.0471379 0.0118262 -411.42 0.0467224 0.998354 0.0332473 24.3427 -0.0133739 -0.0326554 0.999377 2.44152 0.998478 -0.050858 0.0213173 -413.152 0.0500198 0.998019 0.0381655 24.2705 -0.023216 -0.0370411 0.999044 2.54786 0.99842 -0.0556741 -0.00766233 -414.938 0.0558775 0.998001 0.029541 24.1724 0.00600234 -0.0299224 0.999534 2.58557 0.997704 -0.0581041 -0.0347964 -416.718 0.0590385 0.997905 0.0264577 24.0604 0.0331862 -0.0284512 0.999044 2.58517 0.998174 -0.060269 -0.00391635 -418.444 0.0603532 0.99781 0.0270702 23.9331 0.00227628 -0.0272571 0.999626 2.62202 0.997721 -0.067369 -0.00364465 -422.033 0.0674433 0.997354 0.027125 23.7145 0.00180762 -0.027309 0.999625 2.70207 0.99746 -0.0701746 -0.0122222 -423.84 0.0704831 0.997149 0.0269593 23.5952 0.0102955 -0.0277523 0.999562 2.73303 0.997279 -0.0735478 -0.00506441 -425.658 0.0736683 0.996819 0.0304044 23.4616 0.00281212 -0.0306948 0.999525 2.75599 0.996968 -0.0775357 -0.0065602 -427.478 0.0777089 0.996447 0.0324796 23.3252 0.00401856 -0.0328909 0.999451 2.79524 0.996652 -0.0813262 -0.00843084 -429.318 0.0815548 0.996163 0.0317392 23.1787 0.00581726 -0.0323205 0.999461 2.82271 0.996325 -0.0853882 0.00677807 -431.135 0.0851125 0.995796 0.033865 23.0169 -0.00964124 -0.0331637 0.999403 2.88436 0.995904 -0.0900102 0.00859691 -432.975 0.0897154 0.995514 0.0300612 22.8579 -0.0112642 -0.0291668 0.999511 2.94985 0.995586 -0.0937155 -0.00510429 -434.831 0.0938162 0.99526 0.0256326 22.6787 0.00267791 -0.0259984 0.999658 2.99321 0.995238 -0.097298 -0.00587887 -436.663 0.0974137 0.994943 0.0244636 22.5028 0.00346889 -0.0249198 0.999683 3.03404 0.994956 -0.100309 -0.00108933 -438.5 0.100305 0.994637 0.0252374 22.3198 -0.00144806 -0.0252194 0.999681 3.06418 0.994679 -0.102939 -0.00419568 -440.319 0.103012 0.994368 0.0248983 22.1428 0.00160903 -0.0251981 0.999681 3.11381 0.994409 -0.105593 0.000157029 -442.124 0.105559 0.994128 0.023816 21.9544 -0.00267091 -0.0236662 0.999716 3.16225 0.994093 -0.108535 -0.000338913 -443.916 0.108508 0.993766 0.0255819 21.7696 -0.00243973 -0.0254676 0.999673 3.20585 0.993875 -0.110413 -0.00456317 -445.673 0.11049 0.993602 0.0233714 21.5765 0.00195348 -0.0237324 0.999716 3.27665 0.992395 -0.11601 0.0411455 -448.257 0.114522 0.992737 0.0368553 21.2941 -0.0451222 -0.031863 0.998473 3.41713 0.99261 -0.120573 -0.0136654 -450.049 0.120923 0.99225 0.0285832 21.0971 0.0101131 -0.0300244 0.999498 3.44634 0.990112 -0.124875 -0.0639073 -451.799 0.126371 0.991783 0.0199079 20.887 0.0608962 -0.027787 0.997757 3.44504 0.991645 -0.128461 -0.0117825 -453.471 0.12873 0.991337 0.0260669 20.6467 0.00833183 -0.0273658 0.999591 3.45645 0.990899 -0.134453 0.00641929 -456.077 0.134232 0.990573 0.0273379 20.3043 -0.0100344 -0.0262275 0.999606 3.50791 0.989811 -0.142319 -0.00437342 -459.602 0.142386 0.9894 0.0285278 19.8084 0.00026701 -0.0288598 0.999583 3.58607 0.98927 -0.146085 -0.00222192 -461.375 0.146087 0.988845 0.0290593 19.5606 -0.00204799 -0.0290721 0.999575 3.62717 0.988412 -0.151688 -0.00576896 -464.065 0.15179 0.988042 0.0270587 19.1455 0.00159549 -0.0276208 0.999617 3.69084 0.987766 -0.155908 -0.00324432 -465.834 0.155938 0.987387 0.0273766 18.8666 -0.00106482 -0.0275476 0.99962 3.72669 0.987135 -0.159657 -0.00858527 -467.621 0.159827 0.986819 0.0253795 18.5774 0.00442008 -0.0264251 0.999641 3.7713 0.986589 -0.163094 -0.00645985 -469.397 0.163205 0.986279 0.0248473 18.2891 0.00231877 -0.0255684 0.99967 3.81138 0.986039 -0.166318 -0.00805635 -471.158 0.166468 0.98574 0.0246027 17.9963 0.0038496 -0.0256004 0.999665 3.85072 0.985553 -0.169221 -0.00707996 -472.926 0.169346 0.985248 0.0246758 17.6912 0.00279985 -0.0255183 0.99967 3.89094 0.985079 -0.171885 -0.00869477 -474.682 0.172054 0.984758 0.0254885 17.3814 0.00418114 -0.0266042 0.999637 3.9257 0.984563 -0.174795 -0.00906326 -476.422 0.174959 0.984315 0.0226432 17.0623 0.00496319 -0.0238794 0.999703 3.98893 0.983898 -0.178307 0.0122708 -478.123 0.177974 0.983735 0.0243134 16.7506 -0.0164064 -0.021738 0.999629 4.03022 0.982738 -0.182042 0.0329749 -479.791 0.180856 0.982851 0.035977 16.4594 -0.0389588 -0.0293922 0.998808 4.11984 0.982464 -0.184854 -0.0243567 -481.541 0.185568 0.982132 0.0313189 16.1472 0.0181321 -0.0352895 0.999213 4.1324 0.979535 -0.18939 -0.0681311 -483.277 0.190925 0.981462 0.0167107 15.7975 0.0637032 -0.0293766 0.997536 4.13024 0.98096 -0.19373 -0.0136271 -484.894 0.194059 0.980543 0.0296113 15.4741 0.00762539 -0.0316919 0.999469 4.14377 0.980379 -0.197063 0.00487689 -486.579 0.196835 0.979984 0.0297906 15.1548 -0.0106499 -0.0282461 0.999544 4.17472 0.979498 -0.200983 -0.0137607 -488.31 0.201283 0.979191 0.0258933 14.7942 0.00827023 -0.0281322 0.99957 4.21816 0.978755 -0.204871 -0.00820272 -490.003 0.205019 0.978393 0.0267112 14.4321 0.00255315 -0.0278254 0.99961 4.25791 0.977824 -0.209427 0.00107086 -491.701 0.209327 0.977491 0.0263322 14.0708 -0.00656144 -0.0255241 0.999653 4.30544 0.974879 -0.222655 -0.00604345 -495.103 0.222735 0.974621 0.0224401 13.2944 0.000893677 -0.0232224 0.99973 4.39427 0.973328 -0.229401 -0.00272615 -496.806 0.229404 0.973075 0.0223156 12.8913 -0.00246647 -0.0223458 0.999747 4.44009 0.969205 -0.246114 -0.00829329 -500.224 0.246237 0.968987 0.0207723 12.0447 0.00292373 -0.0221747 0.99975 4.52981 0.96676 -0.255512 -0.0094187 -501.904 0.255662 0.966511 0.0222146 11.6032 0.00342717 -0.0238842 0.999709 4.57119 0.963955 -0.265973 -0.00703175 -503.556 0.266065 0.963688 0.0226769 11.139 0.000744957 -0.0237305 0.999718 4.60961 0.95921 -0.282508 -0.0102351 -506.048 0.282675 0.958925 0.0236013 10.4254 0.00314717 -0.0255318 0.999669 4.67333 0.955939 -0.293414 -0.00946255 -507.68 0.293561 0.95563 0.024376 9.91622 0.00189042 -0.0260798 0.999658 4.71077 0.952647 -0.303968 -0.00826463 -509.314 0.30408 0.952282 0.0263563 9.39772 -0.000141215 -0.0276214 0.999618 4.75286 0.949507 -0.313608 -0.00930872 -510.931 0.313746 0.94916 0.0256537 8.85944 0.000790251 -0.0272789 0.999628 4.79134 0.946617 -0.322203 -0.0100493 -512.544 0.322358 0.946237 0.0268439 8.31804 0.000859843 -0.0286503 0.999589 4.83589 0.944356 -0.328858 -0.00663312 -514.142 0.328909 0.943916 0.029028 7.76537 -0.003285 -0.0295945 0.999557 4.8793 0.942806 -0.333219 -0.00907464 -515.721 0.333341 0.942372 0.0286035 7.21047 -0.000979546 -0.0299925 0.99955 4.925 0.941958 -0.335409 -0.0146666 -517.297 0.335693 0.941601 0.0263996 6.65442 0.00495541 -0.0297908 0.999544 4.97711 0.941821 -0.335857 -0.0131372 -518.869 0.33609 0.941505 0.0247313 6.08673 0.00406259 -0.0277078 0.999608 5.00792 0.942538 -0.333953 -0.00988957 -520.435 0.334099 0.942175 0.02617 5.54426 0.000578158 -0.0279703 0.999609 5.04468 0.943988 -0.329816 -0.0104089 -521.986 0.329979 0.943613 0.0266352 5.01323 0.00103724 -0.028578 0.999591 5.08712 0.94578 -0.324572 -0.0123757 -523.543 0.324795 0.945399 0.0269823 4.49764 0.00294225 -0.0295389 0.999559 5.11956 0.94776 -0.318783 -0.0113668 -525.076 0.31898 0.947344 0.0281344 3.98836 0.00179952 -0.0302904 0.99954 5.15911 0.951459 -0.307593 -0.0106103 -528.087 0.307758 0.951211 0.0219689 2.99901 0.00333515 -0.0241679 0.999702 5.25076 0.952909 -0.302905 -0.014616 -529.572 0.30315 0.952746 0.0193628 2.53749 0.00806024 -0.0228818 0.999706 5.28633 0.954125 -0.299065 -0.0143188 -531.011 0.299305 0.953958 0.0195004 2.08484 0.00782762 -0.0228915 0.999707 5.31396 0.955806 -0.29383 -0.00993439 -533.079 0.293985 0.955537 0.022839 1.45688 0.00278191 -0.0247502 0.99969 5.36649 0.95684 -0.290609 -0.00185059 -534.428 0.290567 0.956547 0.0242687 1.04985 -0.00528253 -0.023759 0.999704 5.39801 0.95778 -0.287244 0.0122047 -535.738 0.286834 0.957584 0.0275507 0.648458 -0.0196008 -0.0228867 0.999546 5.46932 0.958258 -0.284165 0.0315035 -537.041 0.283078 0.958462 0.0348913 0.270572 -0.0401098 -0.0245169 0.998894 5.5459 0.959856 -0.279144 -0.0274663 -539.015 0.2796 0.960008 0.0144229 -0.28191 0.0223418 -0.0215235 0.999519 5.6187 0.959429 -0.276229 -0.0565094 -540.326 0.276978 0.960859 0.00573591 -0.657822 0.0527131 -0.0211551 0.998386 5.63046 0.961897 -0.27275 -0.0190264 -541.558 0.273067 0.961849 0.0167369 -0.998085 0.0137356 -0.0212947 0.999679 5.69299 0.963067 -0.269188 0.00622149 -542.79 0.268929 0.962771 0.0273741 -1.35734 -0.0133587 -0.02469 0.999606 5.66706 0.963952 -0.266006 -0.00616291 -544.065 0.266077 0.963659 0.0237596 -1.70393 -0.000381262 -0.0245429 0.999699 5.68347 0.964701 -0.26301 -0.0133041 -545.282 0.263255 0.964466 0.0223985 -2.02769 0.00694034 -0.0251102 0.999661 5.74368 0.966478 -0.256615 -0.00828763 -547.026 0.256739 0.966215 0.0226437 -2.47288 0.00219692 -0.0240124 0.999709 5.79401 0.968732 -0.247916 -0.00977822 -548.117 0.248072 0.968517 0.0208405 -2.74104 0.00430369 -0.0226145 0.999735 5.81236 0.971672 -0.236142 -0.009549 -549.167 0.236303 0.971413 0.0227618 -2.98013 0.003901 -0.0243734 0.999695 5.84116 0.975316 -0.220687 -0.00747156 -550.16 0.220801 0.975054 0.0227252 -3.19517 0.00227003 -0.023814 0.999714 5.86647 0.981164 -0.193076 -0.00628305 -551.578 0.19317 0.980904 0.0226277 -3.45164 0.00179419 -0.0234152 0.999724 5.89804 0.988444 -0.151502 -0.00498421 -553.424 0.151578 0.988161 0.0237128 -3.69923 0.00133266 -0.0241943 0.999706 5.94249 0.994529 -0.104187 -0.00754093 -555.186 0.104344 0.994233 0.0247616 -3.84964 0.00491761 -0.0254129 0.999665 5.9845 0.998181 -0.0594394 -0.0100792 -556.787 0.0596976 0.997834 0.027615 -3.90724 0.00841597 -0.0281664 0.999568 6.01534 0.999826 -0.0145732 -0.0116123 -558.218 0.0148582 0.999581 0.0248507 -3.90955 0.0112452 -0.0250189 0.999624 6.03979 0.999355 0.0349066 -0.00842592 -559.508 -0.0346388 0.998947 0.0300726 -3.80246 0.00946678 -0.0297614 0.999512 6.07013 0.996303 0.085879 -0.00244401 -560.717 -0.085764 0.995842 0.0307022 -3.66167 0.00507051 -0.030379 0.999526 6.07076 0.988878 0.148718 0.00181273 -561.857 -0.148636 0.987757 0.0473644 -3.38372 0.00525342 -0.0471071 0.998876 6.09236 0.969069 0.24676 -0.00392202 -563.258 -0.246305 0.968036 0.047329 -2.95759 0.0154755 -0.044899 0.998872 6.12006 0.940174 0.340437 0.0132202 -564.321 -0.340595 0.938259 0.0605377 -2.42893 0.00820529 -0.0614188 0.998078 6.1302 0.879667 0.475469 0.0107529 -565.639 -0.475258 0.877982 0.0572505 -1.61399 0.01778 -0.0554718 0.998302 6.102 0.812929 0.582292 0.00906664 -566.638 -0.581728 0.811218 0.0593141 -0.740854 0.0271831 -0.0534925 0.998198 6.13356 0.696513 0.717524 -0.00536298 -567.767 -0.716274 0.695705 0.05428 0.524582 0.0426782 -0.0339653 0.998511 6.08914 0.571536 0.820576 -0.000786947 -568.458 -0.819283 0.570689 0.0555814 1.77328 0.0460579 -0.0311221 0.998454 6.05149 0.424813 0.905281 5.01566e-05 -568.976 -0.904102 0.424256 0.0510532 3.16704 0.0461962 -0.0217334 0.998696 6.01611 0.269647 0.962872 0.0129302 -569.286 -0.96215 0.268846 0.0446092 4.75404 0.0394768 -0.0244695 0.998921 5.95282 0.143341 0.989466 0.0202432 -569.498 -0.989429 0.142821 0.0251418 6.48124 0.0219858 -0.023633 0.999479 5.95356 0.0734165 0.996657 0.0358361 -569.562 -0.997295 0.0732445 0.00609223 8.3121 0.00344707 -0.0361865 0.999339 5.95614 0.0383811 0.998633 0.0354818 -569.605 -0.998242 0.036713 0.0465265 10.2645 0.0451602 -0.0372052 0.998287 5.90551 0.0275045 0.998947 0.0367125 -569.611 -0.997695 0.025154 0.0630191 11.3082 0.0620293 -0.0383612 0.997337 5.92063 0.0131826 0.999493 0.0289836 -569.649 -0.998488 0.0116112 0.0537315 12.3201 0.0533677 -0.029648 0.998135 5.91874 0.00442835 0.999352 0.0357257 -569.639 -0.999522 0.00332991 0.0307475 13.3478 0.0306087 -0.0358447 0.998889 5.86962 -0.00507288 0.999561 0.0291893 -569.622 -0.999621 -0.00585876 0.0269015 15.0071 0.0270607 -0.0290417 0.999212 5.87069 -0.0112672 0.999395 0.0329135 -569.575 -0.999377 -0.0123559 0.0330644 16.7647 0.0334511 -0.0325205 0.998911 5.829 -0.0130685 0.999444 0.0306788 -569.546 -0.999462 -0.0139799 0.0296822 17.9389 0.0300946 -0.0302744 0.999088 5.847 -0.0150097 0.999543 0.0262322 -569.543 -0.999436 -0.0157862 0.0296489 19.1395 0.0300494 -0.0257724 0.999216 5.816 -0.0155036 0.99945 0.029319 -569.494 -0.999338 -0.016454 0.032457 20.3939 0.0329216 -0.0287964 0.999043 5.8019 -0.0163924 0.999461 0.0284603 -569.474 -0.999337 -0.0173022 0.0320243 21.6589 0.0324994 -0.0279164 0.999082 5.79242 -0.0165739 0.99945 0.0287098 -569.451 -0.9994 -0.0174324 0.0299161 22.9415 0.0304001 -0.0281968 0.99914 5.77126 -0.0173677 0.999151 0.0373505 -569.376 -0.999352 -0.0185254 0.030877 24.9261 0.0315427 -0.03679 0.998825 5.7423 -0.0172849 0.999081 0.0392146 -569.35 -0.999273 -0.0185951 0.0332962 26.2766 0.0339948 -0.0386105 0.998676 5.73659 -0.0154524 0.998949 0.0431447 -569.296 -0.999332 -0.0168592 0.0324366 27.6692 0.0331299 -0.0426146 0.998542 5.71271 -0.0137022 0.998867 0.0455762 -569.264 -0.999379 -0.0151604 0.0318049 29.0523 0.0324598 -0.0451121 0.998454 5.71134 -0.0119 0.998944 0.0443825 -569.253 -0.999284 -0.0134748 0.0353534 30.4545 0.0359141 -0.04393 0.998389 5.69338 -0.0104103 0.999004 0.0433824 -569.257 -0.999356 -0.0118839 0.0338505 31.9063 0.0343324 -0.0430021 0.998485 5.67284 -0.00759076 0.99892 0.0458371 -569.194 -0.999613 -0.00880681 0.0263864 34.1239 0.0267616 -0.0456191 0.9986 5.65561 -0.0054819 0.998866 0.0472889 -569.181 -0.999636 -0.00672303 0.0261268 35.6472 0.0264151 -0.0471284 0.99854 5.63976 -0.00346554 0.998744 0.0499922 -569.168 -0.999672 -0.00472852 0.0251675 37.1722 0.0253722 -0.0498885 0.998432 5.64113 -0.0019633 0.998609 0.0526854 -569.155 -0.999701 -0.00324384 0.0242309 38.7341 0.0243681 -0.0526221 0.998317 5.63058 -0.000292589 0.998428 0.0560511 -569.148 -0.999697 -0.00167107 0.0245481 40.2928 0.0246032 -0.0560269 0.998126 5.63329 0.00158768 0.998306 0.0581585 -569.14 -0.999713 0.000195528 0.0239351 41.8683 0.0238832 -0.0581798 0.99802 5.62159 0.00260627 0.998092 0.0616958 -569.132 -0.999679 0.00104673 0.0252968 43.4888 0.025184 -0.0617419 0.997774 5.61547 0.00326153 0.997164 0.0751908 -569.111 -0.999544 0.000992991 0.0301881 46.7383 0.0300278 -0.075255 0.996712 5.59683 0.00348231 0.99696 0.0778389 -569.098 -0.999566 0.00119291 0.0294393 48.3911 0.0292569 -0.0779076 0.996531 5.58098 0.00209783 0.997004 0.077327 -569.076 -0.999493 -0.000366498 0.031841 50.0497 0.0317739 -0.0773546 0.996497 5.58672 -0.00089846 0.997337 0.0729316 -569.088 -0.999476 -0.00325552 0.0322064 51.6948 0.0323581 -0.0728644 0.996817 5.5893 -0.00380682 0.997767 0.0666854 -569.071 -0.999622 -0.00561332 0.0269236 53.3529 0.0272378 -0.0665577 0.997411 5.57613 -0.00745664 0.99823 0.0590021 -569.054 -0.999258 -0.00966744 0.0372736 55.8051 0.037778 -0.0586804 0.997562 5.55598 -0.007826 0.998243 0.0587337 -569.026 -0.999276 -0.00999377 0.0367059 57.4266 0.0372284 -0.0584039 0.997599 5.53269 -0.00681389 0.998344 0.0571294 -569.018 -0.999254 -0.00896961 0.0375629 59.0347 0.0380131 -0.0568308 0.99766 5.50842 -0.00518442 0.998388 0.0565252 -568.994 -0.999275 -0.00730457 0.0373662 60.6246 0.0377188 -0.0562904 0.997702 5.501 -0.00324003 0.998558 0.053577 -568.994 -0.999365 -0.00513501 0.0352694 62.2074 0.0354937 -0.0534287 0.997941 5.47743 -0.00089828 0.998469 0.0553053 -568.984 -0.999252 -0.00303411 0.0385471 63.7641 0.0386559 -0.0552293 0.997725 5.44884 0.00161926 0.998177 0.0603347 -568.969 -0.999351 -0.000555537 0.0360113 66.0904 0.0359792 -0.0603538 0.997528 5.43231 0.00292971 0.998047 0.0624028 -568.964 -0.999494 0.000945507 0.0318026 67.6211 0.0316814 -0.0624644 0.997544 5.42242 0.00356552 0.99805 0.0623226 -568.958 -0.999379 0.00137194 0.0352046 69.1482 0.0350504 -0.0624094 0.997435 5.40557 0.00435266 0.998255 0.0588956 -568.947 -0.99896 0.00166762 0.0455623 70.6499 0.0453845 -0.0590326 0.997224 5.40939 0.00185 0.998737 0.0502105 -568.966 -0.999492 0.000248832 0.0318765 72.0934 0.0318238 -0.050244 0.99823 5.43145 0.000515486 0.998803 0.0489127 -568.96 -0.999787 -0.000493443 0.0206128 73.4934 0.0206122 -0.048913 0.99859 5.38817 -0.000201833 0.999195 0.0401145 -568.968 -0.999359 -0.00163703 0.0357479 75.5555 0.0357848 -0.0400816 0.998555 5.40746 1.77472e-05 0.999092 0.0426153 -568.967 -0.999611 -0.00117098 0.0278693 76.9078 0.0278939 -0.0425992 0.998703 5.37408 0.000385034 0.999079 0.0429067 -568.959 -0.999664 -0.000727258 0.0259049 78.2267 0.0259122 -0.0429023 0.998743 5.38837 0.000378826 0.998849 0.0479599 -568.959 -0.999999 0.000303688 0.00157396 80.8335 0.00155758 -0.0479604 0.998848 5.40796 0.00117778 0.998679 0.0513735 -568.926 -0.999991 0.000973072 0.00400953 82.1418 0.00395424 -0.0513778 0.998671 5.40662 0.00293682 0.998745 0.0500046 -568.932 -0.998056 -0.000185528 0.0623223 83.4952 0.0622534 -0.0500904 0.996803 5.34325 0.00337646 0.998433 0.0558608 -568.926 -0.997686 -0.000429961 0.0679893 84.7875 0.0679068 -0.0559611 0.996121 5.37507 0.000200244 0.998398 0.0565769 -568.932 -0.999836 -0.000824909 0.0180957 86.0632 0.0181134 -0.0565712 0.998234 5.37121 0.000288669 0.998431 0.0559968 -568.912 -0.999536 -0.00141707 0.0304193 88.0684 0.0304509 -0.0559796 0.997967 5.30607 0.000113675 0.998542 0.053972 -568.908 -0.999298 -0.00190896 0.0374225 89.3952 0.037471 -0.0539383 0.997841 5.33214 -0.000979368 0.998492 0.0548877 -568.891 -0.999707 -0.00230455 0.0240855 91.3998 0.0241756 -0.0548481 0.998202 5.30763 -0.00133965 0.998629 0.0523302 -568.894 -0.999555 -0.0028974 0.0297033 92.7686 0.0298142 -0.0522671 0.998188 5.30756 -0.00161237 0.998751 0.0499452 -568.899 -0.999586 -0.00304371 0.0285955 94.1252 0.0287118 -0.0498785 0.998343 5.30541 -0.00225049 0.998905 0.0467247 -568.895 -0.999668 -0.00344708 0.0255446 95.4861 0.0256777 -0.0466517 0.998581 5.30251 -0.00206053 0.998866 0.0475575 -568.88 -0.99957 -0.00344858 0.0291232 97.564 0.0292542 -0.047477 0.998444 5.27806 -0.0030191 0.998831 0.0482462 -568.879 -0.999457 -0.00459661 0.0326196 98.9479 0.0328033 -0.0481215 0.998303 5.27478 -0.0028563 0.998656 0.0517403 -568.853 -0.999483 -0.00450798 0.031834 100.322 0.0320245 -0.0516226 0.998153 5.25316 -0.00365854 0.998626 0.0522766 -568.838 -0.999459 -0.00536039 0.0324515 102.351 0.0326872 -0.0521296 0.998105 5.25437 -0.00376766 0.998703 0.050773 -568.825 -0.999461 -0.00541693 0.0323848 104.375 0.0326178 -0.0506236 0.998185 5.2492 -0.0034168 0.998927 0.0461787 -568.823 -0.999458 -0.00492262 0.0325343 105.707 0.0327267 -0.0460425 0.998403 5.23858 -0.00308713 0.998987 0.044895 -568.818 -0.999478 -0.00452603 0.031984 107.025 0.0321548 -0.0447728 0.99848 5.2264 -0.00408296 0.998824 0.0483008 -568.801 -0.999949 -0.00452632 0.0090733 108.997 0.00928126 -0.0482613 0.998792 5.22279 -0.00511024 0.998487 0.054743 -568.783 -0.999945 -0.00460346 -0.00937952 110.29 -0.00911333 -0.0547879 0.998456 5.28095 -0.00249643 0.999154 0.0410517 -568.774 -0.997063 -0.0056291 0.076373 113.028 0.0765395 -0.0407405 0.996234 5.20664 -0.00647065 0.999421 0.0333996 -568.775 -0.999931 -0.00679302 0.00954738 114.863 0.00976874 -0.0333355 0.999396 5.26544 -0.00559053 0.999444 0.0328778 -568.769 -0.999909 -0.00598964 0.0120531 116.184 0.0122433 -0.0328075 0.999387 5.19728 -0.00359774 0.999199 0.0398517 -568.741 -0.999457 -0.00489795 0.0325767 118.074 0.0327458 -0.0397129 0.998674 5.22232 -0.00386183 0.999007 0.0443848 -568.726 -0.999674 -0.00497727 0.0250482 119.274 0.0252442 -0.0442736 0.9987 5.21359 -0.0017617 0.998437 0.0558683 -568.702 -0.999466 -0.00358166 0.0324926 120.989 0.0326419 -0.0557812 0.997909 5.2025 -0.000684427 0.998013 0.063011 -568.696 -0.999416 -0.00283627 0.0340671 122.052 0.0341781 -0.0629508 0.997431 5.21105 -0.000457589 0.997838 0.0657153 -568.695 -0.999566 -0.00239232 0.0293655 123.068 0.0294592 -0.0656733 0.997406 5.21016 0.000495344 0.997486 0.0708679 -568.682 -0.999646 -0.00139083 0.0265635 124.598 0.0265953 -0.0708559 0.997132 5.21064 0.00139428 0.997544 0.0700329 -568.673 -0.999574 -0.000652393 0.029193 125.603 0.029167 -0.0700438 0.997117 5.21697 0.0019292 0.997781 0.0665596 -568.678 -0.999738 0.000405893 0.0228922 127.08 0.0228144 -0.0665863 0.99752 5.21194 0.00233649 0.997776 0.0666226 -568.673 -0.999706 0.000723878 0.0242191 129.035 0.024117 -0.0666596 0.997484 5.22313 0.00504865 0.997682 0.0678578 -568.677 -0.998436 0.00125042 0.0558999 130.547 0.0556854 -0.0680338 0.996128 5.16919 0.00340865 0.997224 0.0743837 -568.659 -0.99961 0.00133737 0.0278778 132.505 0.027701 -0.0744497 0.99684 5.15495 0.00403008 0.996602 0.0822707 -568.642 -0.999165 0.000667281 0.0408614 134.051 0.0406677 -0.0823666 0.995772 5.12668 0.00282766 0.996057 0.0886677 -568.623 -0.999329 -0.000422644 0.036617 135.092 0.0365101 -0.0887117 0.995388 5.12527 -6.34763e-06 0.995706 0.0925704 -568.6 -0.999514 -0.00289312 0.0310505 136.662 0.031185 -0.0925252 0.995222 5.11059 -0.00204031 0.995192 0.0979174 -568.59 -0.999525 -0.0050397 0.0303943 137.749 0.0307416 -0.0978089 0.99473 5.09217 -0.00820571 0.99446 0.104793 -568.555 -0.9996 -0.0109925 0.0260431 139.892 0.0270508 -0.104537 0.994153 5.10057 -0.0126105 0.994577 0.103232 -568.536 -0.999739 -0.0145099 0.0176692 140.961 0.0190713 -0.102982 0.9945 5.09987 -0.0192799 0.993769 0.109775 -568.466 -0.99946 -0.0220771 0.0243235 142.571 0.0265954 -0.109247 0.993659 5.08831 -0.0291426 0.993928 0.106104 -568.423 -0.999005 -0.0325474 0.0305003 143.646 0.0337685 -0.105109 0.993887 5.09699 -0.05185 0.992863 0.107398 -568.317 -0.998289 -0.0544414 0.0213378 145.256 0.0270324 -0.106108 0.993987 5.07604 -0.0736237 0.99152 0.107086 -568.212 -0.996916 -0.0760968 0.0191885 146.324 0.0271747 -0.105343 0.994065 5.07943 -0.12613 0.987219 0.0974128 -568.012 -0.991659 -0.128103 0.0142476 147.929 0.0265444 -0.0948032 0.995142 5.07693 -0.194662 0.976028 0.0973474 -567.664 -0.980379 -0.196746 0.0121959 149.547 0.0310562 -0.0930632 0.995176 5.04652 -0.2489 0.964375 0.0896083 -567.378 -0.968076 -0.250545 0.00742039 150.61 0.029607 -0.0849008 0.995949 5.06917 -0.331503 0.940386 0.076028 -566.774 -0.943111 -0.332477 0.00015911 152.176 0.0254272 -0.0716502 0.997106 5.02501 -0.387388 0.918783 0.0759449 -566.288 -0.921383 -0.38865 0.00199476 153.204 0.0313487 -0.0692016 0.99711 5.00049 -0.475744 0.876344 0.075428 -565.417 -0.878864 -0.477072 -0.000474071 154.663 0.0355691 -0.0665165 0.997151 5.00263 -0.529922 0.844439 0.0781362 -564.746 -0.847137 -0.531368 -0.0026577 155.631 0.0392748 -0.0676004 0.996939 4.92828 -0.602148 0.792975 0.0927811 -563.614 -0.796899 -0.604042 -0.00927486 157.005 0.0486889 -0.0795219 0.995643 4.89269 -0.644371 0.760068 0.0841516 -562.805 -0.763445 -0.64573 -0.0135826 157.896 0.0440155 -0.0729973 0.99636 4.89543 -0.695984 0.713428 0.0814037 -561.476 -0.716627 -0.697272 -0.0160652 159.2 0.0452992 -0.0695172 0.996552 4.80124 -0.723118 0.685185 0.0873056 -560.523 -0.688575 -0.725051 -0.0129079 160.036 0.0544567 -0.0694503 0.996098 4.79595 -0.749991 0.656619 0.0797801 -559.555 -0.659519 -0.75155 -0.0144274 160.862 0.0504854 -0.0634368 0.996708 4.74442 -0.788095 0.610527 0.0784975 -557.994 -0.613544 -0.789404 -0.0201074 162.028 0.0496902 -0.0640082 0.996712 4.67054 -0.811023 0.57956 0.0797026 -556.868 -0.582726 -0.812357 -0.0225059 162.762 0.0517034 -0.0646975 0.996565 4.6443 -0.850051 0.521339 0.0749613 -554.549 -0.523834 -0.851647 -0.0171818 164.177 0.054883 -0.0538726 0.997038 4.55549 -0.876568 0.47542 0.0748588 -552.723 -0.477595 -0.878479 -0.0133394 165.142 0.05942 -0.0474452 0.997105 4.48191 -0.89234 0.444002 0.0811853 -551.469 -0.446294 -0.89481 -0.0116886 165.732 0.0674557 -0.0466627 0.99663 4.4167 -0.914617 0.395481 0.084088 -549.56 -0.397947 -0.917298 -0.0142145 166.529 0.0715121 -0.0464635 0.996357 4.3258 -0.927134 0.366323 0.0789271 -548.284 -0.368768 -0.929338 -0.0184925 166.999 0.0665757 -0.0462508 0.996709 4.26091 -0.937044 0.340068 0.0793859 -546.971 -0.342664 -0.939216 -0.0213281 167.439 0.0673075 -0.047188 0.996616 4.1906 -0.948359 0.307052 0.079595 -545.028 -0.309422 -0.950733 -0.0190853 168.047 0.0698134 -0.0427281 0.996645 4.1287 -0.954098 0.289541 0.0765705 -543.728 -0.291565 -0.956409 -0.0164771 168.428 0.0684619 -0.0380461 0.996928 4.06283 -0.958367 0.275183 0.0761989 -542.422 -0.276936 -0.960797 -0.0132738 168.788 0.0695589 -0.0338234 0.997004 4.0116 -0.963573 0.259731 0.063773 -540.46 -0.260737 -0.965378 -0.00783934 169.323 0.059529 -0.0241817 0.997934 3.91189 -0.965463 0.255228 0.0523479 -538.507 -0.256257 -0.966509 -0.0138914 169.788 0.0470492 -0.0268262 0.998532 3.90215 -0.961357 0.255993 0.101293 -536.46 -0.256239 -0.966553 0.0107909 170.357 0.100667 -0.0155814 0.994798 3.73675 -0.965128 0.257609 0.0465321 -534.529 -0.258128 -0.966095 -0.00541943 170.874 0.0435584 -0.0172417 0.998902 3.68315 -0.963283 0.25966 0.0682742 -532.414 -0.259964 -0.965608 0.00456069 171.436 0.0671104 -0.0133556 0.997656 3.57448 -0.963269 0.261064 0.062919 -531.014 -0.261703 -0.965146 -0.00198506 171.805 0.0602078 -0.0183782 0.998017 3.52249 -0.962972 0.263251 0.0581769 -528.857 -0.263874 -0.964552 -0.00315031 172.383 0.0552853 -0.018385 0.998301 3.44836 -0.962191 0.264906 0.0633527 -526.651 -0.265577 -0.964087 -0.00227039 172.99 0.060476 -0.0190096 0.997989 3.37015 -0.961631 0.267214 0.0621521 -524.391 -0.26782 -0.963468 -0.00148513 173.617 0.0594847 -0.0180737 0.998066 3.27652 -0.96139 0.268626 0.05975 -522.856 -0.26919 -0.963086 -0.00144959 174.043 0.057155 -0.0174777 0.998212 3.22228 -0.96092 0.270718 0.0578364 -520.529 -0.271229 -0.962514 -0.00101745 174.69 0.055393 -0.0166646 0.998326 3.15142 -0.959651 0.274392 0.0614772 -516.51 -0.274825 -0.961493 0.00146439 175.835 0.0595118 -0.0154902 0.998107 2.99991 -0.959074 0.275715 0.0644822 -514.883 -0.276262 -0.961082 0.000457178 176.298 0.0620988 -0.0173755 0.997919 2.9323 -0.958426 0.277462 0.0665895 -512.402 -0.278271 -0.960498 -0.00300074 177.003 0.0631265 -0.0214059 0.997776 2.83243 -0.957861 0.279986 0.0641017 -509.898 -0.280711 -0.959789 -0.00240904 177.736 0.0608496 -0.0203016 0.99794 2.73515 -0.957343 0.281938 0.0632829 -507.338 -0.282541 -0.959255 -0.000607 178.495 0.0605333 -0.0184611 0.997995 2.63483 -0.956638 0.283777 0.0656855 -504.745 -0.284556 -0.958656 -0.0026217 179.253 0.0622258 -0.0211992 0.997837 2.53854 -0.95628 0.285253 0.0644908 -502.994 -0.286035 -0.958214 -0.00304459 179.769 0.0609276 -0.0213581 0.997914 2.46154 -0.95562 0.287647 0.0636403 -500.363 -0.288342 -0.957526 -0.00183041 180.551 0.0604107 -0.0200993 0.997971 2.36789 -0.954981 0.289416 0.0651885 -497.7 -0.290313 -0.956921 -0.00453403 181.346 0.061068 -0.023255 0.997863 2.25772 -0.954369 0.291393 0.0653494 -495.911 -0.292407 -0.956273 -0.00631282 181.875 0.0606524 -0.0251334 0.997842 2.18877 -0.951178 0.302747 0.0600305 -492.356 -0.303731 -0.952726 -0.00779334 182.993 0.0548332 -0.025646 0.998166 2.05038 -0.948949 0.309776 0.0594481 -490.607 -0.310884 -0.950393 -0.010168 183.563 0.0533493 -0.0281304 0.99818 1.99585 -0.945215 0.320083 0.0641528 -487.985 -0.321719 -0.946687 -0.0167599 184.431 0.0553681 -0.0364809 0.997799 1.92807 -0.943039 0.327561 0.0581513 -486.249 -0.329098 -0.944106 -0.0189216 185.029 0.048703 -0.0369813 0.998128 1.88063 -0.94045 0.335275 0.0560771 -484.513 -0.336617 -0.941502 -0.0162123 185.647 0.0473612 -0.0341233 0.998295 1.83593 -0.93741 0.343124 0.0593962 -482.783 -0.344352 -0.93877 -0.0115251 186.28 0.0518048 -0.031257 0.998168 1.78613 -0.934726 0.350328 0.0596484 -481.069 -0.351393 -0.936193 -0.00806233 186.922 0.053018 -0.0284961 0.998187 1.74227 -0.931062 0.360843 0.0539988 -478.524 -0.361686 -0.932278 -0.00640429 187.909 0.048031 -0.0254934 0.99852 1.66314 -0.928257 0.367621 0.0565134 -476.847 -0.368479 -0.929621 -0.00521556 188.579 0.0506188 -0.0256654 0.998388 1.62623 -0.925145 0.375503 0.0557113 -475.146 -0.376404 -0.926434 -0.0062688 189.275 0.0492589 -0.0267695 0.998427 1.58512 -0.916992 0.395218 0.054111 -471.794 -0.396042 -0.918219 -0.00499801 190.706 0.0477104 -0.0260133 0.998522 1.49881 -0.912008 0.40668 0.053406 -470.128 -0.407516 -0.913182 -0.00534296 191.453 0.0465965 -0.0266366 0.998559 1.46498 -0.906822 0.417956 0.0546493 -468.461 -0.418702 -0.90812 -0.00245274 192.222 0.048603 -0.025106 0.998503 1.41446 -0.901719 0.429391 0.0502622 -466.836 -0.430023 -0.902816 -0.00195448 193.008 0.0445383 -0.0233763 0.998734 1.37288 -0.892799 0.447325 0.0530093 -464.402 -0.447921 -0.894073 0.00070921 194.222 0.0477114 -0.0231108 0.998594 1.32955 -0.888079 0.45695 0.0501267 -462.794 -0.457394 -0.889259 0.00290321 195.045 0.0459022 -0.0203494 0.998739 1.28266 -0.884261 0.464636 0.0468547 -461.215 -0.465216 -0.885195 -0.00167548 195.88 0.0406971 -0.0232791 0.9989 1.26428 -0.880609 0.471608 0.0459733 -459.62 -0.472148 -0.881518 -0.00102655 196.735 0.0400422 -0.0226102 0.998942 1.23628 -0.875198 0.481355 0.0482232 -457.251 -0.481868 -0.876243 0.00110981 198.028 0.0427894 -0.0222659 0.998836 1.18487 -0.872446 0.486573 0.045648 -455.699 -0.487061 -0.873368 0.000510608 198.902 0.0401159 -0.0217878 0.998957 1.17249 -0.870305 0.490619 0.0431533 -454.139 -0.491011 -0.871152 0.00173759 199.783 0.0384456 -0.0196765 0.999067 1.13966 -0.868731 0.493141 0.046023 -452.59 -0.493447 -0.86976 0.00524831 200.665 0.0426172 -0.0181506 0.998927 1.11213 -0.867309 0.495661 0.0457713 -450.266 -0.496164 -0.868228 0.000428065 201.975 0.0399521 -0.0223388 0.998952 1.05284 -0.866761 0.49661 0.0458728 -448.726 -0.497283 -0.86758 -0.00385072 202.85 0.037886 -0.0261494 0.99894 1.03253 -0.865772 0.498365 0.0455062 -447.194 -0.499201 -0.866444 -0.00855034 203.722 0.0351674 -0.0301194 0.998927 1.00773 -0.864404 0.500516 0.047846 -444.897 -0.501502 -0.865091 -0.0106391 205.037 0.036066 -0.0331914 0.998798 0.966549 -0.863159 0.502783 0.0465358 -443.393 -0.503761 -0.863765 -0.0116126 205.912 0.0343574 -0.0334665 0.998849 0.953271 -0.86143 0.506027 0.0433064 -441.912 -0.506916 -0.861911 -0.0120699 206.775 0.0312186 -0.0323501 0.998989 0.942819 -0.858821 0.510576 0.0416881 -439.711 -0.511368 -0.859298 -0.0104764 208.08 0.0304735 -0.0303154 0.999076 0.930636 -0.856875 0.513952 0.0402324 -438.25 -0.514608 -0.857394 -0.0073406 208.962 0.0307223 -0.0269939 0.999163 0.919741 -0.85271 0.521024 0.0376838 -435.377 -0.52128 -0.853379 0.00346136 210.729 0.033962 -0.0166923 0.999284 0.898164 -0.85122 0.523365 0.038897 -433.939 -0.523564 -0.851967 0.00569846 211.612 0.0361214 -0.0155144 0.999227 0.87303 -0.849552 0.526109 0.0383566 -432.522 -0.526297 -0.850281 0.00584306 212.489 0.035688 -0.015223 0.999247 0.860841 -0.846305 0.531506 0.0356375 -430.413 -0.531619 -0.846955 0.00699932 213.809 0.0339035 -0.013022 0.99934 0.829743 -0.843977 0.535222 0.0352106 -429.018 -0.535305 -0.844623 0.00782516 214.687 0.0339278 -0.0122441 0.999349 0.807904 -0.841058 0.540011 0.03178 -427.661 -0.540117 -0.841569 0.00588676 215.563 0.029924 -0.0122138 0.999478 0.806611 -0.837812 0.545074 0.0310703 -426.301 -0.545258 -0.838263 0.00293856 216.445 0.0276469 -0.0144794 0.999513 0.794111 -0.834158 0.550325 0.0363595 -424.962 -0.550589 -0.83477 0.00320157 217.324 0.0321138 -0.0173486 0.999334 0.793553 -0.83099 0.555124 0.0359536 -423.628 -0.55536 -0.8316 0.00394138 218.216 0.032087 -0.016692 0.999346 0.789943 -0.827235 0.560693 0.0361415 -422.318 -0.560899 -0.827868 0.0051125 219.109 0.0327869 -0.0160425 0.999334 0.777629 -0.821275 0.569129 0.039992 -420.362 -0.569383 -0.822052 0.00585555 220.472 0.0362081 -0.0179617 0.999183 0.756101 -0.814368 0.579148 0.0373101 -418.454 -0.579382 -0.815039 0.0052914 221.824 0.0334737 -0.0173077 0.99929 0.740543 -0.809449 0.585886 0.0391266 -417.201 -0.586096 -0.810211 0.0070573 222.733 0.0358356 -0.0172195 0.999209 0.73812 -0.805016 0.592035 0.0379823 -415.976 -0.592229 -0.805738 0.00712863 223.659 0.0348242 -0.0167556 0.999253 0.730489 -0.800062 0.598952 0.034013 -414.746 -0.598961 -0.800702 0.0110544 224.596 0.0338553 -0.0115282 0.99936 0.71728 -0.79366 0.607541 0.0315918 -413.538 -0.607545 -0.794215 0.010581 225.541 0.0315191 -0.0107957 0.999445 0.7232 -0.78606 0.617544 0.0273645 -412.361 -0.617226 -0.786535 0.0198455 226.507 0.0337786 -0.00129037 0.999429 0.712636 -0.778436 0.627111 0.0277156 -411.19 -0.626652 -0.778929 0.0240296 227.462 0.0366577 0.00133745 0.999327 0.678583 -0.770276 0.636749 0.0350175 -410.044 -0.636743 -0.77097 0.012756 228.404 0.0351198 -0.0124715 0.999305 0.685107 -0.752661 0.657331 0.0376562 -407.842 -0.657716 -0.753261 0.00276608 230.304 0.0301832 -0.0226852 0.999287 0.664401 -0.745058 0.665845 0.0392377 -406.816 -0.666037 -0.745853 0.0098428 231.241 0.0358194 -0.0188003 0.999181 0.695284 -0.736276 0.675484 0.0402435 -405.828 -0.675563 -0.737176 0.0136646 232.166 0.0388968 -0.0171261 0.999096 0.670492 -0.727988 0.684052 0.0459016 -404.88 -0.684142 -0.729169 0.0161728 233.063 0.0445331 -0.0196296 0.998815 0.669855 -0.720201 0.69243 0.043038 -403.984 -0.692145 -0.721372 0.0236059 233.96 0.0473919 -0.0127875 0.998795 0.649484 -0.712263 0.700504 0.0444495 -403.14 -0.700378 -0.713464 0.0209522 234.799 0.0463902 -0.016208 0.998792 0.637457 -0.704864 0.708138 0.0413118 -402.325 -0.708096 -0.705882 0.0181834 235.641 0.0420376 -0.0164359 0.998981 0.625994 -0.696592 0.716209 0.0424906 -401.505 -0.716276 -0.697626 0.0163406 236.477 0.0413458 -0.0190522 0.998963 0.586427 -0.690437 0.722434 0.0372232 -400.729 -0.722366 -0.691284 0.0177001 237.305 0.038519 -0.014668 0.99915 0.63733 -0.692174 0.721644 0.0112111 -399.976 -0.721708 -0.692192 -0.00276893 238.089 0.00576203 -0.0100077 0.999933 0.653977 -0.704129 0.709932 0.0140943 -399.172 -0.710059 -0.704098 -0.00785924 238.876 0.00434422 -0.0155417 0.99987 0.625004 -0.727019 0.685197 0.0441335 -398.328 -0.684284 -0.728345 0.0356266 239.669 0.0565556 -0.00429864 0.99839 0.575475 -0.757745 0.647862 0.0780838 -397.471 -0.643852 -0.761745 0.0720992 240.337 0.10619 0.00435832 0.994336 0.523091 -0.796781 0.599319 0.0771839 -396.609 -0.596212 -0.800506 0.0609994 240.885 0.0983443 0.00258513 0.995149 0.506499 -0.840474 0.539543 0.0499768 -395.727 -0.53741 -0.841816 0.0503616 241.39 0.0692435 0.0154695 0.99748 0.476578 -0.88247 0.468291 0.0441684 -394.787 -0.466473 -0.883354 0.045695 241.802 0.0604149 0.019721 0.997979 0.3715 -0.938824 0.339526 0.0577298 -393.325 -0.338489 -0.940578 0.027175 242.198 0.063526 0.00597158 0.997962 0.356905 -0.96827 0.244325 0.052513 -392.315 -0.243697 -0.969682 0.0181394 242.343 0.0553528 0.00476653 0.998455 0.319958 -0.98806 0.146644 0.0472512 -391.313 -0.146423 -0.989189 0.00812469 242.387 0.0479318 0.00110902 0.99885 0.29062 -0.997949 0.0485789 0.0416907 -390.307 -0.0486129 -0.998818 0.000198439 242.331 0.0416511 -0.00182867 0.999131 0.291093 -0.989393 -0.143742 0.0209742 -388.345 0.14411 -0.989411 0.0172382 241.988 0.0182743 0.020078 0.999631 0.284538 -0.974528 -0.224258 -0.00153265 -387.404 0.224257 -0.97453 0.000943962 241.647 -0.00170531 0.00057621 0.999998 0.250162 -0.938595 -0.34188 0.0464585 -385.449 0.341264 -0.939736 -0.0208476 240.867 0.0507861 -0.00371289 0.998703 0.224488 -0.919577 -0.3845 0.0808595 -384.458 0.382372 -0.9231 -0.0409529 240.398 0.0903878 -0.00674084 0.995884 0.193491 -0.908708 -0.413945 0.0538437 -383.531 0.412368 -0.910216 -0.0382125 239.938 0.0648272 -0.0125206 0.997818 0.218665 -0.901048 -0.433293 0.0192362 -382.628 0.432633 -0.901044 -0.030791 239.464 0.0306742 -0.0194219 0.999341 0.212232 -0.893555 -0.448712 0.0147568 -381.668 0.448449 -0.893625 -0.0180937 238.971 0.021306 -0.00955001 0.999727 0.157048 -0.886677 -0.461221 0.0328528 -380.658 0.460403 -0.887215 -0.0296273 238.426 0.0428123 -0.0111443 0.999021 0.118896 -0.882146 -0.4703 0.0252409 -379.677 0.469305 -0.882263 -0.0369513 237.867 0.0396472 -0.0207508 0.998998 0.148104 -0.878691 -0.477068 0.0175564 -378.671 0.476458 -0.878679 -0.0301727 237.317 0.0298209 -0.0181476 0.999391 0.13633 -0.875192 -0.483301 0.0214229 -377.665 0.482609 -0.875299 -0.0306539 236.741 0.0335665 -0.0164891 0.9993 0.0972279 -0.872044 -0.488846 0.0238624 -376.638 0.487867 -0.872119 -0.0373353 236.144 0.0390621 -0.0209164 0.999018 0.0904693 -0.869449 -0.493455 0.0236817 -375.585 0.492465 -0.869514 -0.03773 235.543 0.0392096 -0.0211419 0.999007 0.0827713 -0.868271 -0.495383 0.0264866 -374.514 0.494386 -0.868476 -0.0364971 234.929 0.041083 -0.0185948 0.998983 0.055886 -0.868134 -0.495483 0.0289989 -373.434 0.494475 -0.868458 -0.0357172 234.306 0.0428816 -0.016668 0.998941 0.0346582 -0.868752 -0.494431 0.0284116 -372.337 0.493475 -0.869067 -0.0347153 233.683 0.0418559 -0.0161386 0.998993 0.00077345 -0.870193 -0.492145 0.0236124 -370.702 0.49113 -0.87024 -0.0383817 232.743 0.0394379 -0.0218027 0.998984 -0.0264114 -0.87277 -0.487889 0.0154073 -369.614 0.487208 -0.872625 -0.0339837 232.138 0.0300251 -0.0221534 0.999304 -0.0207283 -0.879339 -0.476069 0.0110125 -367.957 0.475581 -0.879143 -0.0305008 231.239 0.024202 -0.0215833 0.999474 -0.0263425 -0.888092 -0.459189 0.0209327 -366.26 0.458651 -0.888237 -0.0259774 230.381 0.0305217 -0.0134696 0.999443 -0.025368 -0.901359 -0.432544 0.0214111 -364.54 0.431908 -0.901458 -0.0287951 229.567 0.0317564 -0.0167071 0.999356 -0.0626854 -0.912908 -0.407751 0.0183993 -363.386 0.407168 -0.9129 -0.0287813 229.085 0.0285324 -0.0187831 0.999416 -0.0272247 -0.925117 -0.379386 0.0150009 -362.187 0.379015 -0.925112 -0.0226973 228.627 0.0224885 -0.0153121 0.99963 -0.0535163 -0.936965 -0.349014 0.0168912 -360.966 0.348587 -0.936973 -0.0238617 228.193 0.0241547 -0.0164695 0.999573 -0.0454304 ================================================ FILE: SC-PGO/utils/sample_data/KAIST03/times.txt ================================================ 1567410201.7452047 1567410207.7531185 1567410208.4532168 1567410208.9530807 1567410209.3450341 1567410209.6453834 1567410209.9452219 1567410210.2447982 1567410210.544965 1567410210.8452191 1567410211.0454617 1567410211.2457383 1567410211.4453835 1567410211.6479533 1567410211.8481796 1567410212.0483134 1567410212.2500279 1567410212.4500558 1567410212.6502178 1567410212.8495522 1567410213.0497336 1567410213.2498364 1567410213.4517281 1567410213.6515045 1567410213.8512917 1567410214.053087 1567410214.2529969 1567410214.4529209 1567410214.6528203 1567410214.8520188 1567410215.0450361 1567410215.2451019 1567410215.4450078 1567410215.644906 1567410215.8444886 1567410216.0448954 1567410216.2449172 1567410216.4448299 1567410216.6449676 1567410216.844466 1567410217.0450697 1567410217.2468629 1567410217.4467385 1567410217.6467905 1567410217.8470364 1567410218.0471351 1567410218.246779 1567410218.4467428 1567410218.6464281 1567410218.8456533 1567410219.0470054 1567410219.2468638 1567410219.4482846 1567410219.6525531 1567410219.8518422 1567410220.0528071 1567410220.2531123 1567410220.4527521 1567410220.6524866 1567410220.8523083 1567410221.0524056 1567410221.2528963 1567410221.4528005 1567410221.6528673 1567410221.8527119 1567410222.0528967 1567410222.2444682 1567410222.4447186 1567410222.6448774 1567410222.8447428 1567410223.0448372 1567410223.2448287 1567410223.445066 1567410223.6447496 1567410223.8449049 1567410224.0450511 1567410224.2455447 1567410224.4451718 1567410224.6450138 1567410224.8444712 1567410225.0453284 1567410225.2464292 1567410225.4464896 1567410225.6464198 1567410225.8482664 1567410226.0515001 1567410226.2511592 1567410226.4524789 1567410226.6523218 1567410226.852448 1567410227.0447011 1567410227.2449179 1567410227.4447949 1567410227.6447864 1567410227.84425 1567410228.0447862 1567410228.2448809 1567410228.4449654 1567410228.644845 1567410228.8447249 1567410229.0444915 1567410229.2462554 1567410229.4464157 1567410229.6494269 1567410229.8493502 1567410230.0525463 1567410230.2526176 1567410230.452632 1567410230.6525645 1567410230.8526797 1567410231.052994 1567410231.2447214 1567410231.4445608 1567410231.6441135 1567410231.845715 1567410232.0442555 1567410232.244323 1567410232.4445336 1567410232.6444561 1567410232.8441877 1567410233.0441844 1567410233.2442737 1567410233.4448144 1567410233.6449964 1567410233.8450687 1567410234.0464108 1567410234.2460315 1567410234.446115 1567410234.6462808 1567410234.8467982 1567410235.0468607 1567410235.2463453 1567410235.4464836 1567410235.650646 1567410235.8509562 1567410236.0505643 1567410236.2526717 1567410236.4448004 1567410236.6447854 1567410236.8445621 1567410237.0451427 1567410237.2443817 1567410237.4443457 1567410237.6446819 1567410237.9448888 1567410238.1447361 1567410238.3447051 1567410238.5456662 1567410238.7476647 1567410238.9478137 1567410239.149344 1567410239.3495846 1567410239.5501108 1567410239.7523229 1567410239.9522657 1567410240.152586 1567410240.3444958 1567410240.5443921 1567410240.7442033 1567410240.9456682 1567410241.1445596 1567410241.3443632 1567410241.6446719 1567410241.8479199 1567410242.0472202 1567410242.2477927 1567410242.4477267 1567410242.647517 1567410242.8477666 1567410243.0472648 1567410243.2479386 1567410243.4477389 1567410243.6491528 1567410243.9522984 1567410244.1444881 1567410244.4442875 1567410244.6442387 1567410244.9442966 1567410245.1444557 1567410245.4460981 1567410245.6459889 1567410245.8462894 1567410246.1459982 1567410246.3474402 1567410246.6491477 1567410246.8490715 1567410247.0488026 1567410247.2491019 1567410247.449121 1567410247.6489952 1567410247.8489127 1567410248.0488837 1567410248.251754 1567410248.5515022 1567410248.7520955 1567410248.9519696 1567410249.1518705 1567410249.4441278 1567410249.6441216 1567410249.8440599 1567410250.043725 1567410250.2441108 1567410250.4440937 1567410250.7442124 1567410250.9443879 1567410251.1441865 1567410251.3441107 1567410251.5476499 1567410251.7442834 1567410251.9443839 1567410252.1446218 1567410252.3447344 1567410252.5438731 1567410252.7445574 1567410252.9505448 1567410253.1505055 1567410253.351141 1567410253.5505378 1567410253.7511415 1567410253.9508517 1567410254.151011 1567410254.3511043 1567410254.5511172 1567410254.7524383 1567410254.9524498 1567410255.152715 1567410255.3523431 1567410255.5520587 1567410255.7521291 1567410255.944541 1567410256.1443701 1567410256.4443743 1567410256.646189 1567410256.8509238 1567410257.0509818 1567410257.2527864 1567410257.5439873 1567410257.7437105 1567410257.9443867 1567410258.1442821 1567410258.3444278 1567410258.5493014 1567410258.7494936 1567410258.949369 1567410259.1494846 1567410259.3493142 1567410259.5492857 1567410259.7495325 1567410259.9493563 1567410260.1490409 1567410260.34887 1567410260.5485656 1567410260.8492737 1567410261.0496223 1567410261.2523766 1567410261.4443841 1567410261.6441891 1567410261.844177 1567410262.0446141 1567410262.2452085 1567410262.4449069 1567410262.6475999 1567410262.8475163 1567410263.0479436 1567410263.2474761 1567410263.4478259 1567410263.6478345 1567410263.8475497 1567410264.0491617 1567410264.2501869 1567410264.4526927 1567410264.6529202 1567410264.8528953 1567410265.0522687 1567410265.2445018 1567410265.4447777 1567410265.6447027 1567410265.8446312 1567410266.0447655 1567410266.2441697 1567410266.4462101 1567410266.6446285 1567410266.8444536 1567410267.0448856 1567410267.2446373 1567410267.445008 1567410267.6447523 1567410267.8462772 1567410268.046546 1567410268.2459023 1567410268.4523396 1567410268.6524906 1567410268.8527055 1567410269.0446415 1567410269.2444525 1567410269.4463761 1567410269.6448543 1567410269.8447227 1567410270.044668 1567410270.2445421 1567410270.444977 1567410270.6448677 1567410270.8448317 1567410271.0463133 1567410271.2464786 1567410271.4479482 1567410271.6494319 1567410271.8493488 1567410272.0491111 1567410272.2519262 1567410272.4518559 1567410272.6520915 1567410272.8523567 1567410273.0528908 1567410273.25368 1567410273.4455507 1567410273.6441519 1567410273.8445015 1567410274.0442798 1567410274.2441049 1567410274.4443731 1567410274.6446383 1567410274.8447804 1567410275.0444386 1567410275.2457914 1567410275.4463367 1567410275.6463375 1567410275.8475339 1567410276.0460389 1567410276.2472718 1567410276.4475381 1567410276.6477199 1567410276.8473737 1567410277.0440464 1567410277.2437224 1567410277.4436896 1567410277.6441309 1567410277.8441319 1567410278.0445013 1567410278.2437701 1567410278.4443355 1567410278.6446466 1567410278.8445594 1567410279.0443695 1567410279.2436292 1567410279.4472182 1567410279.6441298 1567410279.8444073 1567410280.0443771 1567410280.2443631 1567410280.4441521 1567410280.6459107 1567410280.8488953 1567410281.0502372 1567410281.3519397 1567410281.5438879 1567410281.7438421 1567410281.9442139 1567410282.1443129 1567410282.4466572 1567410282.747366 1567410283.0490723 1567410283.3505931 1567410283.6505954 1567410283.950001 1567410284.2518005 1567410284.5520599 1567410284.8516448 1567410285.1437883 1567410285.4430041 1567410285.743808 1567410286.0436404 1567410286.3435266 1567410286.6437721 1567410286.843956 1567410287.0441427 1567410287.2455697 1567410287.4457242 1567410287.6457024 1567410287.8457944 1567410288.0456712 1567410288.2470634 1567410288.4473348 1567410288.6477661 1567410288.8472447 1567410289.0472965 1567410289.247077 1567410289.4519136 1567410289.6516876 1567410289.8519208 1567410290.0517545 1567410290.2519848 1567410290.4510291 1567410290.6515088 1567410290.85165 1567410291.0515814 1567410291.3440194 1567410291.543813 1567410291.7438586 1567410291.9433451 1567410292.1440833 1567410292.3439219 1567410292.6439986 1567410292.9442139 1567410293.1442177 1567410293.4500093 1567410293.6499281 1567410293.8500121 1567410294.0438249 1567410294.2437944 1567410294.4435737 1567410294.6453245 1567410294.8439207 1567410295.1437914 1567410295.3436935 1567410295.6437223 1567410295.8440139 1567410296.0441308 1567410296.2441339 1567410296.4442928 1567410296.6443572 1567410296.8442965 1567410297.0441227 1567410297.2438879 1567410297.4440227 1567410297.6440165 1567410297.8443787 1567410298.0445864 1567410298.2472246 1567410298.4472644 1567410298.6469665 1567410298.9473794 1567410299.1472862 1567410299.4473715 1567410299.6484258 1567410299.8485994 1567410300.1512413 1567410300.3536341 1567410300.544266 1567410300.7443264 1567410300.9469402 1567410301.146765 1567410301.3476253 1567410301.6473334 1567410301.8504765 1567410302.1515248 1567410302.3516631 1567410302.5517364 1567410302.7516174 1567410302.9437904 1567410303.1432853 1567410303.3435969 1567410303.5435505 1567410303.7451563 1567410303.9437265 1567410304.1431615 1567410304.343931 1567410304.6434317 1567410304.8435855 1567410305.0437415 1567410305.2438576 1567410305.4435601 1567410305.6433213 1567410305.8435793 1567410306.0439627 1567410306.2439432 1567410306.4435153 1567410306.6437438 1567410306.8440371 1567410307.0442419 1567410307.2455831 1567410307.4453011 1567410307.6469448 1567410307.8470869 1567410308.0469847 1567410308.2470725 1567410308.4483156 1567410308.6477661 1567410308.8483832 1567410309.0498533 1567410309.2496202 1567410309.449779 1567410309.7499375 1567410309.9510431 1567410310.1512198 1567410310.3435941 1567410310.5429881 1567410310.7431571 1567410310.9431992 1567410311.1426611 1567410311.3434441 1567410311.5437143 1567410311.7454619 1567410311.9454749 1567410312.2465646 1567410312.4464986 1567410312.64661 1567410312.946908 1567410313.1470048 1567410313.3470998 1567410313.5469246 1567410313.7465718 1567410313.9468861 1567410314.1484623 1567410314.3483121 1567410314.5480759 1567410314.7496269 1567410314.9482849 1567410315.2496192 1567410315.4512198 1567410315.6528115 1567410315.8440332 1567410316.0433464 1567410316.2431216 1567410316.4431038 1567410316.6435995 1567410316.8437085 1567410317.043787 1567410317.2434354 1567410317.4435775 1567410317.6481659 1567410317.8506844 1567410318.0510011 1567410318.251178 1567410318.4513674 1567410318.6512711 1567410318.8509917 1567410319.0511892 1567410319.2433083 1567410319.4434991 1567410319.6437125 1567410319.8435354 1567410320.0433514 1567410320.2431757 1567410320.4433608 1567410320.6437871 1567410320.8439615 1567410321.0438511 1567410321.2440598 1567410321.443929 1567410321.6442082 1567410321.8438954 1567410322.0439363 1567410322.2439864 1567410322.4442463 1567410322.6436732 1567410322.8435543 1567410323.0437241 1567410323.3466487 1567410323.5463784 1567410323.7465 1567410323.948297 1567410324.1482186 1567410324.3479743 1567410324.5484102 1567410324.748553 1567410324.9484015 1567410325.1481299 1567410325.3478744 1567410325.5482888 1567410325.7481797 1567410325.948138 1567410326.1478438 1567410326.3507488 1567410326.5513096 1567410326.7532022 1567410326.9435244 1567410327.2431815 1567410327.4434626 1567410327.6435056 1567410327.8426232 1567410328.0437992 1567410328.2436335 1567410328.4452643 1567410328.6450891 1567410328.844877 1567410329.0450649 1567410329.2452624 1567410329.4456213 1567410329.6453848 1567410329.844677 1567410330.046643 1567410330.2466731 1567410330.4464989 1567410330.6465862 1567410330.8495548 1567410331.0509691 1567410331.3428357 1567410331.5445726 1567410331.7430668 1567410331.9447389 1567410332.1449225 1567410332.3432028 1567410332.5433753 1567410332.7431757 1567410332.9431918 1567410333.1430621 1567410333.342931 1567410333.5443401 1567410333.7433095 1567410333.9437149 1567410334.1432567 1567410334.3433268 1567410334.5452282 1567410334.7450054 1567410334.9450514 1567410335.1449804 1567410335.3451805 1567410335.5466135 1567410335.7466271 1567410335.9483981 1567410336.1431928 1567410336.4452753 1567410336.6452563 1567410336.8467135 1567410337.048034 1567410337.2482498 1567410337.4481089 1567410337.6480582 1567410337.8481321 1567410338.0506048 1567410338.2510293 1567410338.4511526 1567410338.6511776 1567410338.8510411 1567410339.0511887 1567410339.2431521 1567410339.4430408 1567410339.6428211 1567410339.8430002 1567410340.1430488 1567410340.3432455 1567410340.5427685 1567410340.7431979 1567410340.9431272 1567410341.1435177 1567410341.3431995 1567410341.5435977 1567410341.7435575 1567410341.9433463 1567410342.1433911 1567410342.3435667 1567410342.5429966 1567410342.7435067 1567410343.042877 1567410343.2438169 1567410343.4436641 1567410343.6450734 1567410343.9467919 1567410344.1478846 1567410344.3478138 1567410344.5489457 1567410344.7496626 1567410345.0492213 1567410345.2515717 1567410345.4508104 1567410345.6508574 1567410345.8514204 1567410346.0432255 1567410346.2437232 1567410346.443089 1567410346.6435697 1567410346.8497965 1567410347.0497034 1567410347.2495308 1567410347.4494176 1567410347.6494744 1567410347.8493688 1567410348.1493287 1567410348.3511345 1567410348.5504715 1567410348.8508782 1567410349.051007 1567410349.251199 1567410349.451215 1567410349.7433982 1567410349.9430144 1567410350.1426694 1567410350.3431513 1567410350.5424061 1567410350.7448804 1567410350.9465754 1567410351.1463504 1567410351.3464749 1567410351.5462804 1567410351.7464857 1567410351.9459803 1567410352.1475246 1567410352.3432269 1567410352.5436146 1567410352.7437437 1567410352.9431596 1567410353.1428347 1567410353.3476188 1567410353.5492692 1567410353.7505696 1567410353.9506676 1567410354.1507716 1567410354.3509147 1567410354.5507524 1567410354.7429435 1567410354.9433258 1567410355.1450334 1567410355.3464055 1567410355.5511615 1567410355.7503374 1567410355.9507649 1567410356.1508152 1567410356.3432713 1567410356.5432775 1567410356.7429903 1567410356.9431362 1567410357.1432362 1567410357.3432479 1567410357.5430684 1567410357.7427261 1567410357.9429471 1567410358.1433485 1567410358.343236 1567410358.5432317 1567410358.7429798 1567410358.9430676 1567410359.1431284 1567410359.3435237 1567410359.5433295 1567410359.7431009 1567410359.9430649 1567410360.2431765 1567410360.4434311 1567410360.6434069 1567410360.9432433 1567410361.2424874 1567410361.4432337 1567410361.6435363 1567410361.8447671 1567410362.0443966 1567410362.3446109 1567410362.5449364 1567410362.7464948 1567410362.9473169 1567410363.1459525 1567410363.3473814 1567410363.5474811 1567410363.7493522 1567410363.9507365 1567410364.1490164 1567410364.3491695 1567410364.549305 1567410364.750813 1567410364.9507535 1567410365.1431534 1567410365.3428497 1567410365.54298 1567410365.7431514 1567410365.9432192 1567410366.1431401 1567410366.344713 1567410366.5447779 1567410366.7448969 1567410366.9462965 1567410367.1477721 1567410367.3474305 1567410367.5495956 1567410367.7494957 1567410367.9510167 1567410368.1505389 1567410368.3429494 1567410368.5431387 1567410368.743118 1567410368.942982 1567410369.1430626 1567410369.3433213 1567410369.5434637 1567410369.743463 1567410369.9434338 1567410370.1433702 1567410370.3431792 1567410370.5433326 1567410370.7424281 1567410370.9430072 1567410371.1430848 1567410371.3430564 1567410371.5434468 1567410371.7436738 1567410371.9440463 1567410372.1436737 1567410372.3432505 1567410372.5433242 1567410372.7435076 1567410372.9435406 1567410373.1449056 1567410373.3461659 1567410373.5448649 1567410373.7447844 1567410373.9464095 1567410374.1478601 1567410374.3480096 1567410374.5481551 1567410374.8476808 1567410375.0490994 1567410375.2493787 1567410375.4491842 1567410375.6491997 1567410375.8492706 1567410376.0430508 1567410376.2431049 1567410376.4426377 1567410376.6428902 1567410376.842639 1567410377.0432377 1567410377.2495689 1567410377.4493372 1567410377.6512203 1567410377.8478844 1567410378.0481112 1567410378.2483006 1567410378.4478123 1567410378.6493142 1567410378.8488283 1567410379.042608 1567410379.2431693 1567410379.4426045 1567410379.643064 1567410379.8433766 1567410380.0429747 1567410380.2429264 1567410380.4426591 1567410380.6431952 1567410380.8430879 1567410381.0449202 1567410381.2449682 1567410381.4453096 1567410381.6445024 1567410381.8459287 1567410382.1475887 1567410382.4465654 1567410382.7473874 1567410383.0492859 1567410383.3489983 1567410383.6491287 1567410383.9505274 1567410384.2505481 1567410384.5504646 1567410384.8427606 1567410385.1428924 1567410385.4427042 1567410385.7430067 1567410386.0444622 1567410386.3458128 1567410386.6461675 1567410386.9475522 1567410387.2488098 1567410387.5508738 1567410387.8445377 1567410388.1432941 1567410388.4430287 1567410388.7447875 1567410388.944834 1567410389.1451838 1567410389.3444831 1567410389.5493255 1567410389.7493732 1567410389.9493153 1567410390.1507685 1567410390.3506923 1567410390.550796 1567410390.7505713 1567410390.9427717 1567410391.2429259 1567410391.4431484 1567410391.6438065 1567410391.8432026 1567410392.0433173 1567410392.2429535 1567410392.4430885 1567410392.6431644 1567410392.9430952 1567410393.1432216 1567410393.3431394 1567410393.5429988 1567410393.7428591 1567410393.9428778 1567410394.1429362 1567410394.4431064 1567410394.6433673 1567410394.8431697 1567410395.0431459 1567410395.2429056 1567410395.4430985 1567410395.7431993 1567410395.9432261 1567410396.2447484 1567410396.4440367 1567410396.6487384 1567410396.9491599 1567410397.1489294 1567410397.3491056 1567410397.5489223 1567410397.7488627 1567410397.9488463 1567410398.1505098 1567410398.4515874 1567410398.6503339 1567410398.8509729 1567410399.042861 1567410399.2427347 1567410399.4441397 1567410399.6425841 1567410399.8426142 1567410400.0427024 1567410400.2428703 1567410400.4427586 1567410400.6425536 1567410400.8425465 1567410401.0429752 1567410401.3442457 1567410401.543952 1567410401.8450787 1567410402.0447905 1567410402.3448141 1567410402.6461039 1567410402.8494492 1567410403.050226 1567410403.3426268 1567410403.5425584 1567410403.7426832 1567410403.9432218 1567410404.1430116 1567410404.4426265 1567410404.6425638 1567410404.8428555 1567410405.0429916 1567410405.3448131 1567410405.5445528 1567410405.8457093 1567410406.0461304 1567410406.2478151 1567410406.4475729 1567410406.6472991 1567410406.8489316 1567410407.0504096 1567410407.3426263 1567410407.5426021 1567410407.7426381 1567410407.942697 1567410408.1428642 1567410408.3427374 1567410408.5426269 1567410408.7426052 1567410408.9425383 1567410409.1430528 1567410409.3426983 1567410409.5427868 1567410409.74297 1567410409.9428806 1567410410.1444011 1567410410.344878 1567410410.5458868 1567410410.7491331 1567410410.9490056 1567410411.1491549 1567410411.3490119 1567410411.5426629 1567410411.7428 1567410411.9428213 1567410412.1427188 1567410412.3426704 1567410412.5428848 1567410412.842886 1567410413.0424676 1567410413.2419894 1567410413.4422174 1567410413.6430662 1567410413.8431571 1567410414.044416 1567410414.24544 1567410414.5453959 1567410414.7456372 1567410414.9460194 1567410415.1462951 1567410415.3467002 1567410415.5475078 1567410415.7475638 1567410415.9475734 1567410416.1475682 1567410416.3469095 1567410416.5469718 1567410416.7469172 1567410416.9468002 1567410417.1470397 1567410417.3467691 1567410417.5476158 1567410417.7473588 1567410417.9471078 1567410418.1469443 1567410418.3468313 1567410418.5468266 1567410418.7471211 1567410418.9488897 1567410419.1474781 1567410419.3471997 1567410419.5470612 1567410419.749697 1567410419.9499469 1567410420.1421542 1567410420.341994 1567410420.5423031 1567410420.7420824 1567410420.9420705 1567410421.142262 1567410421.343924 1567410421.5421238 1567410421.7420449 1567410422.0421877 1567410422.2422776 1567410422.4423478 1567410422.6421826 1567410422.8422356 1567410423.0435438 1567410423.2427137 1567410423.4441347 1567410423.7440217 1567410423.9441226 1567410424.2440193 1567410424.4440887 1567410424.6488893 1567410424.8481297 1567410425.0487916 1567410425.2489722 1567410425.4501247 1567410425.6502049 1567410425.8495998 1567410426.0501931 1567410426.3421173 1567410426.5426133 1567410426.8427696 1567410427.1427581 1567410427.3413868 1567410427.5423799 1567410427.8418863 1567410428.0429089 1567410428.2442005 1567410428.5472424 1567410428.7474284 1567410428.9473991 1567410429.147403 1567410429.3474531 1567410429.5472429 1567410429.7470331 1567410429.946939 1567410430.1468666 1567410430.3471465 1567410430.5474076 1567410430.7470338 1567410430.9469039 1567410431.1470244 1567410431.3471634 1567410431.5470119 1567410431.7469745 1567410431.9471433 1567410432.1474438 1567410432.3472581 1567410432.5471778 1567410432.7484891 1567410432.9489236 1567410433.1488101 1567410433.3485587 1567410433.5481932 1567410433.7481513 1567410433.9483948 1567410434.1503191 1567410434.3428304 1567410434.5423338 1567410434.7422802 1567410434.9420788 1567410435.1439211 1567410435.3439863 1567410435.5441597 1567410435.74383 1567410435.9437356 1567410436.143959 1567410436.3439927 1567410436.5457473 1567410436.7470071 1567410436.94681 1567410437.1485658 1567410437.3487158 1567410437.5486608 1567410437.748421 1567410437.9498708 1567410438.1499078 1567410438.3420026 1567410438.5425997 1567410438.7424457 1567410438.9424841 1567410439.1424789 1567410439.3425903 1567410439.5425014 1567410439.7425091 1567410439.9424183 1567410440.1425934 1567410440.3426435 1567410440.5424907 1567410440.742398 1567410440.9426422 1567410441.1470876 1567410441.3487089 1567410441.5484102 1567410441.7484572 1567410441.948662 1567410442.1488175 1567410442.3486052 1567410442.5474367 1567410442.7486157 1567410442.9483726 1567410443.1485868 1567410443.3486595 1567410443.5480242 1567410443.748266 1567410443.9498506 1567410444.1420784 1567410444.3424304 1567410444.5421553 1567410444.7425153 1567410444.9421697 1567410445.1420329 1567410445.3421981 1567410445.5424128 1567410445.7426617 1567410445.9422691 1567410446.1426039 1567410446.342767 1567410446.5416417 1567410446.7420936 1567410447.0439415 1567410447.2457612 1567410447.4454908 1567410447.6458776 1567410447.8473163 1567410448.0463238 1567410448.2468488 1567410448.4470797 1567410448.6470962 1567410448.8467619 1567410449.0467634 1567410449.2468045 1567410449.4489498 1567410449.6489041 1567410449.8488162 1567410450.0488257 1567410450.2487674 1567410450.448947 1567410450.6502163 1567410450.8423858 1567410451.0423658 1567410451.2426674 1567410451.4423633 1567410451.6426826 1567410451.8427143 1567410452.0444028 1567410452.244128 1567410452.4461322 1567410452.6460502 1567410452.8456774 1567410453.0472944 1567410453.2482848 1567410453.4504321 1567410453.642648 1567410453.8426292 1567410454.0428913 1567410454.2439044 1567410454.4441199 1567410454.6439359 1567410454.8441484 1567410455.0439639 1567410455.2433109 1567410455.4440308 1567410455.6470168 1567410455.848454 1567410456.0484545 1567410456.2484684 1567410456.4492948 1567410456.6501114 1567410456.8501716 1567410457.0427251 1567410457.2427468 1567410457.4435766 1567410457.6422045 1567410457.842335 1567410458.0426171 1567410458.2440624 1567410458.4441154 1567410458.6442049 1567410458.8444571 1567410459.0457997 1567410459.2455318 1567410459.4468486 1567410459.6483788 1567410459.8485475 1567410460.0502162 1567410460.2423236 1567410460.4421852 1567410460.6421819 1567410460.8427978 1567410461.0425935 1567410461.2422121 1567410461.4437194 1567410461.6442511 1567410461.8443682 1567410462.0452828 1567410462.2448959 1567410462.44573 1567410462.6454511 1567410462.8471098 1567410463.0467107 1567410463.2469192 1567410463.4489479 1567410463.6486127 1567410463.8483307 1567410464.0480313 1567410464.2473216 1567410464.4486396 1567410464.6487443 1567410464.8487406 1567410465.0482335 1567410465.2483475 1567410465.4487398 1567410465.648977 1567410465.8486478 1567410466.0485992 1567410466.2486196 1567410466.4485321 1567410466.6487439 1567410466.8486731 1567410467.0484343 1567410467.2481842 1567410467.4482291 1567410467.6484811 1567410467.8485515 1567410468.048445 1567410468.2484424 1567410468.447948 1567410468.6485639 1567410468.8483901 1567410469.0482111 1567410469.2482827 1567410469.4479055 1567410469.649987 1567410469.8498027 1567410470.1419191 1567410470.3420801 1567410470.6420717 1567410470.8419058 1567410471.1422663 1567410471.3421545 1567410471.5423324 1567410471.7422175 1567410471.9415653 1567410472.2420413 1567410472.4423974 1567410472.6438615 1567410472.9436269 1567410473.1438289 1567410473.3440623 1567410473.6459 1567410473.94647 1567410474.1472573 1567410474.3469348 1567410474.546865 1567410474.8468559 1567410475.0469577 1567410475.2468987 1567410475.5480468 1567410475.7481756 1567410475.9484334 1567410476.1499023 1567410476.449455 1567410476.6425979 1567410476.842149 1567410477.0420101 1567410477.3417482 1567410477.6422014 1567410477.945034 1567410478.2450373 1567410478.5469198 1567410478.8438721 1567410479.1450069 1567410479.4481966 1567410479.7421315 1567410480.0422046 1567410480.3423169 1567410480.7422738 1567410481.2425258 1567410481.8419845 1567410482.3421071 1567410482.7420888 1567410483.2434833 1567410483.6455631 1567410484.0480301 1567410484.4420457 1567410484.7416525 1567410485.1419988 1567410485.5420022 1567410485.842067 1567410486.142338 1567410486.4413719 1567410486.8424711 1567410487.042232 1567410487.2424345 1567410487.4417634 1567410487.6428597 1567410487.8428364 1567410488.0437262 1567410488.3467517 1567410488.5467441 1567410488.746855 1567410488.9483156 1567410489.1481943 1567410489.351105 1567410489.541573 1567410489.7416937 1567410489.9419594 1567410490.1419342 1567410490.3418207 1567410490.5423903 1567410490.7438178 1567410490.9436803 1567410491.1428895 1567410491.3437445 1567410491.5452719 1567410491.7451518 1567410491.94662 1567410492.1476827 1567410492.3482606 1567410492.5481079 1567410492.7482085 1567410492.9496725 1567410493.1493318 1567410493.3495927 1567410493.5492892 1567410493.7494373 1567410493.9494231 1567410494.1413074 1567410494.3421514 1567410494.6418672 1567410494.8416059 1567410495.0415318 1567410495.241601 1567410495.5419955 1567410495.742079 1567410496.0420868 1567410496.2425537 1567410496.5437121 1567410496.7436116 1567410496.943152 1567410497.1431036 1567410497.3434951 1567410497.5448942 1567410497.8449526 1567410498.0463238 1567410498.3485754 1567410498.549577 1567410498.7493525 1567410498.9413383 1567410499.1413877 1567410499.342155 1567410499.5417321 1567410499.7419801 1567410499.9419127 1567410500.141607 1567410500.3418894 1567410500.5416398 1567410500.7414837 1567410500.941381 1567410501.1413963 1567410501.3416934 1567410501.5414264 1567410501.7415118 1567410501.9415698 1567410502.1411562 1567410502.3414581 1567410502.4416006 1567410502.5416622 1567410502.8419638 1567410502.9419577 1567410503.041877 1567410503.2418094 1567410503.3417768 1567410503.4416995 1567410503.5416064 1567410503.6411161 1567410503.7415717 1567410503.8415093 1567410503.9417629 1567410504.0417299 1567410504.1417346 1567410504.2420268 1567410504.3415473 1567410504.4421606 1567410504.541867 1567410504.6417687 1567410504.7416515 1567410504.8415997 1567410504.9421318 1567410505.0436246 1567410505.1435421 1567410505.2435281 1567410505.348053 1567410505.447875 1567410505.5476232 1567410505.6476526 1567410505.7477422 1567410505.8476658 1567410505.9477565 1567410506.0477452 1567410506.1478713 1567410506.2494507 1567410506.3495035 1567410506.4493754 1567410506.5493453 1567410506.6492245 1567410506.749285 1567410506.9495173 1567410507.0510726 1567410507.1415808 1567410507.2416108 1567410507.3413656 1567410507.4413972 1567410507.541286 1567410507.641573 1567410507.7411301 1567410507.8413239 1567410507.9417331 1567410508.0433447 1567410508.1432333 1567410508.2432601 1567410508.342905 1567410508.4449797 1567410508.5449207 1567410508.6448901 1567410508.7449071 1567410508.8449271 1567410508.9449563 1567410509.0448542 1567410509.1446903 1567410509.2476909 1567410509.3467479 1567410509.4473286 1567410509.5474803 1567410509.6475301 1567410509.7478449 1567410509.8478706 1567410509.9479454 1567410510.0477521 1567410510.1479223 1567410510.2476566 1567410510.3486264 1567410510.4492819 1567410510.5491648 1567410510.6494007 1567410510.7494643 1567410510.8491781 1567410511.1412792 1567410511.341167 1567410511.5411088 1567410511.8417459 1567410512.041687 1567410512.2419512 1567410512.5418584 1567410512.7433975 1567410512.9434614 1567410513.1435888 1567410513.3440077 1567410513.5450244 1567410513.7448878 1567410513.946244 1567410514.149123 1567410514.3410215 1567410514.5417969 1567410514.7416205 1567410514.9414756 1567410515.1434631 1567410515.4433727 1567410515.7448471 1567410515.9449329 1567410516.1464474 1567410516.3466268 1567410516.5464885 1567410516.8470626 1567410517.0483055 1567410517.347338 1567410517.5477073 1567410517.8481925 1567410518.1476943 1567410518.3475788 1567410518.547776 1567410518.8479977 1567410519.0479357 1567410519.2480063 1567410519.4496958 1567410519.7418633 1567410519.9417388 1567410520.1444705 1567410520.3417988 1567410520.541374 1567410520.7412758 1567410520.941747 1567410521.1419029 1567410521.3422129 1567410521.6417723 1567410521.8412335 1567410522.0418599 1567410522.2419505 1567410522.4449761 1567410522.6465542 1567410522.8463733 1567410523.0462008 1567410523.2478998 1567410523.548254 1567410523.7484095 1567410523.9481103 1567410524.148165 1567410524.348 1567410524.5475605 1567410524.8478274 1567410525.0413032 1567410525.3417356 1567410525.5410435 1567410525.7414646 1567410525.9417362 1567410526.1415484 1567410526.3421347 1567410526.5435607 1567410526.743602 1567410526.9433558 1567410527.1494298 1567410527.3493733 1567410527.541429 1567410527.7413349 1567410527.9414618 1567410528.141907 1567410528.3419745 1567410528.5424855 1567410528.7448249 1567410528.9461918 1567410529.1462941 1567410529.3461895 1567410529.5462971 1567410529.7463164 1567410529.9480762 1567410530.1497242 1567410530.3417246 1567410530.6413314 1567410530.841634 1567410531.0417645 1567410531.2421396 1567410531.4414809 1567410531.6416602 1567410531.8416893 1567410532.0430288 1567410532.2430356 1567410532.4429367 1567410532.6430628 1567410532.8431976 1567410533.0443485 1567410533.2447262 1567410533.4449978 1567410533.7458522 1567410533.947356 1567410534.1475828 1567410534.4478598 1567410534.6479485 1567410534.848074 1567410535.1410863 1567410535.3426719 1567410535.541635 1567410535.7431188 1567410535.9445446 1567410536.1446278 1567410536.3449528 1567410536.6450315 1567410536.8446164 1567410537.144429 1567410537.344682 1567410537.5463693 1567410537.7465494 1567410538.0455673 1567410538.3458691 1567410538.5461071 1567410538.8463337 1567410539.0461719 1567410539.2464468 1567410539.44612 1567410539.6463737 1567410539.8463113 1567410540.0462952 1567410540.2460115 1567410540.4459906 1567410540.6463413 1567410540.846576 1567410541.0464466 1567410541.246136 1567410541.4459255 1567410541.6459851 1567410541.849323 1567410542.1492498 1567410542.3412471 1567410542.5407031 1567410542.740725 1567410542.9416053 1567410543.2411981 1567410543.4411232 1567410543.6415656 1567410543.8414376 1567410544.0414803 1567410544.3417411 1567410544.6463056 1567410544.8461177 1567410545.0477798 1567410545.3477519 1567410545.5476205 1567410545.748956 1567410546.1414239 1567410546.4413509 1567410546.7411075 1567410547.0409214 1567410547.4433424 1567410547.8486745 1567410548.2486753 1567410548.6491582 1567410549.1491485 1567410549.5412133 1567410549.8413301 1567410550.1411071 1567410550.4413543 1567410550.841388 1567410551.2414591 1567410551.6488876 1567410551.8488448 1567410552.0410392 1567410552.3409452 1567410552.5408683 1567410552.7407405 1567410553.0414233 1567410553.3414438 1567410553.6412971 1567410553.9412928 1567410554.1415784 1567410554.3434391 1567410554.5431144 1567410554.7428384 1567410555.0429089 1567410555.3431585 1567410555.542872 1567410555.7428157 1567410555.944212 1567410556.2443349 1567410556.5472798 1567410556.8472378 1567410557.0411172 1567410557.342628 1567410557.7420807 1567410557.9441884 1567410558.2435846 1567410558.5460749 1567410558.7475011 1567410559.0469849 1567410559.2470336 1567410559.5416558 1567410559.8431044 1567410560.0446374 1567410560.3455944 1567410560.5407276 1567410560.8410337 1567410561.0410604 1567410561.2413728 1567410561.4413528 1567410561.7403183 1567410561.9409838 1567410562.2407579 1567410562.4404852 1567410562.7422919 1567410563.0428743 1567410563.2429514 1567410563.5449042 1567410563.7442818 1567410563.943382 1567410564.1470509 1567410564.5409157 1567410564.7409384 1567410564.9409261 1567410565.1411788 1567410565.441956 1567410565.6428096 1567410565.9438682 1567410566.2439826 1567410566.4434431 1567410566.6442811 1567410566.8472846 1567410567.0469735 1567410567.3474321 1567410567.5473959 1567410567.7486196 1567410567.9407079 1567410568.241009 1567410568.4404578 1567410568.6412034 1567410568.841162 1567410569.0408244 1567410569.3410974 1567410569.5409305 1567410569.7409868 1567410569.9410107 1567410570.2407279 1567410570.4407179 1567410570.6420705 1567410570.8413386 1567410571.0411804 1567410571.3424699 1567410571.5424168 1567410571.74264 1567410572.0429139 1567410572.2428548 1567410572.4430892 1567410572.644671 1567410572.8456774 1567410573.0455515 1567410573.3469796 1567410573.64726 1567410573.8474071 1567410574.0487227 1567410574.3485835 1567410574.5484536 1567410574.7408621 1567410575.0408638 1567410575.3424387 1567410575.5438452 1567410575.7440097 1567410575.9433689 1567410576.1455073 1567410576.4467816 1567410576.7484901 1567410577.0407956 1567410577.2406688 1567410577.5404484 1567410577.8402975 1567410578.1402245 1567410578.3402629 1567410578.6408663 1567410578.9404607 1567410579.2422628 1567410579.442116 1567410579.6424527 1567410579.9423454 1567410580.2422931 1567410580.5419841 1567410580.8420596 1567410581.042192 1567410581.3421524 1567410581.6420906 1567410581.8425093 1567410582.1422982 1567410582.3421998 1567410582.5421925 1567410582.8419628 1567410583.141413 1567410583.3421807 1567410583.6421113 1567410583.9422741 1567410584.2422011 1567410584.5424199 1567410584.7439582 1567410585.0437229 1567410585.2436888 1567410585.6435182 1567410585.9469774 1567410586.1471477 1567410586.3472838 1567410586.6476614 1567410586.8402948 1567410587.1403077 1567410587.34022 1567410587.5402844 1567410587.8403521 1567410588.0404642 1567410588.2405474 1567410588.5404599 1567410588.7402689 1567410589.1397679 1567410589.3410678 1567410589.6400924 1567410589.8407116 1567410590.0404162 1567410590.3438363 1567410590.5402346 1567410590.8404751 1567410591.0405571 1567410591.2403843 1567410591.5404799 1567410591.740922 1567410592.0423737 1567410592.2422481 1567410592.6448643 1567410592.9453132 1567410593.2404165 1567410593.5402083 1567410593.8405533 1567410594.0422566 1567410594.343673 1567410594.6440101 1567410594.8442175 1567410595.1435978 1567410595.4436996 1567410595.643647 1567410595.8468113 1567410596.0481894 1567410596.3480432 1567410596.5402625 1567410596.8402243 1567410597.0401387 1567410597.3401399 1567410597.6397309 1567410597.9404049 1567410598.2407382 1567410598.4409359 1567410598.7422709 1567410599.1434493 1567410599.3451297 1567410599.6484325 1567410599.940419 1567410600.1401408 1567410600.4406059 1567410600.7404096 1567410600.9400418 1567410601.2400823 1567410601.4403365 1567410601.7402012 1567410601.9403145 1567410602.240386 1567410602.5405052 1567410602.8406799 1567410603.1402581 1567410603.4404085 1567410603.7406301 1567410604.0420511 1567410604.2435477 1567410604.5486233 1567410604.8396606 1567410605.040076 1567410605.3396533 1567410605.5405009 1567410605.7404332 1567410605.9406755 1567410606.2406926 1567410606.4408579 1567410606.7405679 1567410607.0410733 1567410607.3399944 1567410607.6404169 1567410607.9422088 1567410608.2423222 1567410608.5422757 1567410608.7421308 1567410608.9421444 1567410609.143225 1567410609.3435941 1567410609.643831 1567410609.9438431 1567410610.144078 1567410610.4436295 1567410610.7437253 1567410610.9436877 1567410611.24332 1567410611.4437006 1567410611.7437053 1567410611.9436257 1567410612.1436961 1567410612.4437733 1567410612.7433143 1567410612.9435072 1567410613.2452183 1567410613.4452379 1567410613.7483685 1567410613.9483833 1567410614.2405851 1567410614.4407012 1567410614.7483644 1567410615.0483015 1567410615.3482342 1567410615.7483077 1567410615.9404905 1567410616.1406815 1567410616.4405818 1567410616.7407026 1567410616.9467373 1567410617.1467884 1567410617.4466519 1567410617.746763 1567410618.0479672 1567410618.3403454 1567410618.7407238 1567410619.0405874 1567410619.2422268 1567410619.4421749 1567410619.6433003 1567410619.9450746 1567410620.1452491 1567410620.4451988 1567410620.7453251 1567410621.0463204 1567410621.2465613 1567410621.5462897 1567410621.9470017 1567410622.1467793 1567410622.4469385 1567410622.747041 1567410623.046428 1567410623.3483891 1567410623.6482725 1567410623.9483304 1567410624.2483387 1567410624.5475509 1567410624.8482707 1567410625.2482977 1567410625.5481589 1567410625.8480597 1567410626.0479555 1567410626.2482526 1567410626.5483067 1567410626.8481033 1567410627.0475714 1567410627.2483218 1567410627.5485899 1567410627.8481984 1567410628.1512897 1567410628.4482234 1567410628.7402973 1567410629.0396049 1567410629.2402039 1567410629.5402157 1567410629.7402048 1567410630.0393705 1567410630.2400267 1567410630.44017 1567410630.7404456 1567410630.9420896 1567410631.1416197 1567410631.3419294 1567410631.6450465 1567410631.8450179 1567410632.0447788 1567410632.2465048 1567410632.5461147 1567410632.7482278 1567410633.0483377 1567410633.2484243 1567410633.4479587 1567410633.6399872 1567410633.8401201 1567410634.1399629 1567410634.3402641 1567410634.5403268 1567410634.7404835 1567410634.9402285 1567410635.1406131 1567410635.4405746 1567410635.7405984 1567410635.9405966 1567410636.1420939 1567410636.4433589 1567410636.6432314 1567410636.8432505 1567410637.0434742 1567410637.2430468 1567410637.5436652 1567410637.7429411 1567410637.9432325 1567410638.1435134 1567410638.3435514 1567410638.5451307 1567410638.7444086 1567410638.9448156 1567410639.2478397 1567410639.4480102 1567410639.6399965 1567410639.8398919 1567410640.039788 1567410640.2428398 1567410640.4438527 1567410640.6450176 1567410640.845253 1567410641.0451758 1567410641.2463222 1567410641.4471476 1567410641.6462049 1567410641.8480279 1567410642.2470534 1567410642.4407525 1567410642.7400408 1567410642.9397676 1567410643.2393391 1567410643.4397724 1567410643.7397373 1567410643.9401836 1567410644.1400573 1567410644.4407637 1567410644.6407061 1567410644.8417928 1567410645.0416331 1567410645.3433144 1567410645.5435286 1567410645.7436764 1567410645.9434049 1567410646.1432207 1567410646.3434119 1567410646.6461911 1567410647.0397658 1567410647.2396224 1567410647.4403234 1567410647.6403711 1567410647.8401887 1567410648.0401936 1567410648.2397642 1567410648.4403481 1567410648.6419678 1567410648.9437227 1567410649.146265 1567410649.3463397 1567410649.5479751 1567410649.7401428 1567410649.9398155 1567410650.1399076 1567410650.3401358 1567410650.5403392 1567410650.7402956 1567410651.0400712 1567410651.2397029 1567410651.4403412 1567410651.7402477 1567410651.9467313 1567410652.1468866 1567410652.3467052 1567410652.5467155 1567410652.7463188 1567410652.9457793 1567410653.146234 1567410653.4462273 1567410653.6479454 1567410653.8478844 1567410654.0478964 1567410654.24841 1567410654.4473879 1567410654.9400103 1567410655.1405938 1567410655.3403587 1567410655.6397817 1567410655.8400476 1567410656.0403206 1567410656.2406132 1567410656.4418092 1567410656.6419694 1567410656.841918 1567410657.0421209 1567410657.242115 1567410657.4418309 1567410657.7433488 1567410657.9448688 1567410658.1460695 1567410658.4395866 1567410658.7401443 1567410658.9391336 1567410659.140388 1567410659.4391615 1567410659.6461987 1567410659.941361 1567410660.1418455 1567410660.3433187 1567410660.6448622 1567410660.8450167 1567410661.0449159 1567410661.2448893 1567410661.4457369 1567410661.6465356 1567410661.848062 1567410662.0401795 1567410662.2402062 1567410662.5402305 1567410662.7402871 1567410663.0404663 1567410663.2406611 1567410663.5403445 1567410663.8405795 1567410664.041271 1567410664.3417084 1567410664.6420298 1567410664.8421042 1567410665.1422491 1567410665.443639 1567410665.8403685 1567410666.1398575 1567410666.4400752 1567410666.74032 1567410667.0408237 1567410667.240593 1567410667.4400761 1567410667.7402489 1567410668.039959 1567410668.3402593 1567410668.6402197 1567410668.9405437 1567410669.2401841 1567410669.5402813 1567410669.7403903 1567410670.040823 1567410670.2404597 1567410670.5400016 1567410670.8403263 1567410671.0404444 1567410671.2401555 1567410671.5400813 1567410671.740119 1567410672.0400887 1567410672.3405683 1567410672.5404737 1567410672.8402646 1567410673.0478933 1567410673.3491461 1567410673.5480816 1567410673.8400452 1567410674.0398655 1567410674.3406231 1567410674.540081 1567410674.840589 1567410675.0398841 1567410675.3412156 1567410675.5403035 1567410675.8403261 1567410676.0405653 1567410676.1397913 1567410676.3397796 1567410676.439966 1567410676.6395493 1567410676.8412025 1567410676.9422629 1567410677.1463711 1567410677.3467882 1567410677.446938 1567410677.6480238 1567410677.8477945 1567410677.9480467 1567410678.1483567 1567410678.3480346 1567410678.548193 1567410678.6483004 1567410678.8482213 1567410679.0405407 1567410679.3402526 1567410679.5400438 1567410679.6401317 1567410679.8402483 1567410680.0401988 1567410680.1402025 1567410680.3400757 1567410680.5399327 1567410680.7402811 1567410680.8408544 1567410681.0403843 1567410681.2404695 1567410681.4409325 1567410681.6404345 1567410681.8405554 1567410682.040535 1567410682.1406083 1567410682.3420844 1567410682.5452282 1567410682.7471263 1567410683.048559 1567410683.148535 1567410683.3477459 1567410683.4479885 1567410683.6403928 1567410683.7406487 1567410683.9405844 1567410684.0403831 1567410684.2402093 1567410684.3400698 1567410684.5404625 1567410684.6403997 1567410684.8395171 1567410684.9401908 1567410685.1407571 1567410685.3416092 1567410685.5433438 1567410685.745234 1567410686.0453224 1567410686.3450377 1567410686.6448984 1567410686.94666 1567410687.1480978 1567410687.3482966 1567410687.5405526 1567410687.7402751 1567410688.041151 1567410688.2402518 1567410688.4403355 1567410688.6405046 1567410688.9406588 1567410689.2421856 1567410689.5423369 1567410689.7416503 1567410689.9432473 1567410690.1434391 1567410690.3437507 1567410690.5453987 1567410690.8417804 1567410691.1437922 1567410691.4435694 1567410691.745239 1567410692.0449767 1567410692.3444717 1567410692.644969 1567410692.9450266 1567410693.1451778 1567410693.3482299 1567410693.5482433 1567410693.8421421 1567410694.1436856 1567410694.4439225 1567410694.7436349 1567410695.1439278 1567410695.4436224 1567410695.7450826 1567410696.0403647 1567410696.3403292 1567410696.6406236 1567410696.9405727 1567410697.1403573 1567410697.3404033 1567410697.5400608 1567410697.8403261 1567410698.0398231 1567410698.340323 1567410698.6407716 1567410698.8403871 1567410699.1403761 1567410699.4407845 1567410699.7403815 1567410700.0398114 1567410700.2436194 1567410700.5436025 1567410700.7440856 1567410701.0435731 1567410701.2440865 1567410701.4437418 1567410701.745599 1567410701.9454718 1567410702.2473505 1567410702.5466886 1567410702.8469212 1567410703.1486695 1567410703.4405596 1567410703.7406046 1567410704.0402465 1567410704.2405956 1567410704.5400953 1567410704.9405878 1567410705.1406562 1567410705.4408653 1567410705.6407945 1567410705.9435554 1567410706.1438339 1567410706.4454391 1567410706.6455007 1567410706.9472682 1567410707.2470694 1567410707.4466085 1567410707.7467849 1567410707.9471164 1567410708.3468392 1567410708.5466292 1567410708.7467697 1567410709.0466695 1567410709.2467506 1567410709.4485843 1567410709.6483433 1567410709.9482477 1567410710.2403579 1567410710.5405378 1567410710.8403378 1567410711.1400034 1567410711.5404637 1567410711.7404149 1567410712.0401907 1567410712.3400688 1567410712.5402062 1567410712.8407657 1567410713.0408127 1567410713.3402545 1567410713.6404781 1567410713.9400306 1567410714.1400254 1567410714.4403846 1567410714.8403952 1567410715.0406852 1567410715.2403479 1567410715.54055 1567410715.8402328 1567410716.1402161 1567410716.3405573 1567410716.5419877 1567410716.7413464 1567410717.0437701 1567410717.2414877 1567410717.5437407 1567410717.743329 1567410718.0448537 1567410718.24454 1567410718.5450835 1567410718.7446949 1567410719.0449579 1567410719.2462492 1567410719.4480665 1567410719.7480111 1567410719.9485288 1567410720.3482327 1567410720.5499725 1567410720.7400186 1567410720.9402852 1567410721.1404715 1567410721.340163 1567410721.5430391 1567410721.7392712 1567410721.9408381 1567410722.1421788 1567410722.34203 1567410722.6434162 1567410722.9438167 1567410723.148062 1567410723.347883 1567410723.5477884 1567410723.7479813 1567410724.0482156 1567410724.248239 1567410724.5483828 1567410724.7401421 1567410724.9396777 1567410725.1400943 1567410725.542042 1567410725.7421868 1567410725.9423399 1567410726.1419466 1567410726.3449774 1567410726.5449626 1567410726.7464516 1567410726.945699 1567410727.1463702 1567410727.3466635 1567410727.5468261 1567410727.7464476 1567410728.0480433 1567410728.2403531 1567410728.4401908 1567410728.6402085 1567410728.9394403 1567410729.1401002 1567410729.3403432 1567410729.5418336 1567410729.841984 1567410730.0434759 1567410730.2433825 1567410730.4430852 1567410730.743427 1567410731.0433934 1567410731.3463602 1567410731.6480207 1567410731.9402554 1567410732.2397971 1567410732.4410517 1567410732.6420584 1567410732.8433778 1567410733.1447985 1567410733.3447659 1567410733.5496213 1567410733.7461231 1567410733.9470072 1567410734.2396567 1567410734.4396393 1567410734.639894 1567410734.9399207 1567410735.2399282 1567410735.4397428 1567410735.6403551 1567410735.8404739 1567410736.0404928 1567410736.2420797 1567410736.4418373 1567410736.6423314 1567410736.8415921 1567410737.0427415 1567410737.3460715 1567410737.5462892 1567410737.8463767 1567410738.0479925 1567410738.2489636 1567410738.4397352 1567410738.639765 1567410738.9399183 1567410739.2399933 1567410739.440078 1567410739.6401212 1567410739.8401167 1567410740.0401514 1567410740.2397337 1567410740.5399244 1567410740.7399225 1567410740.9400642 1567410741.2400663 1567410741.5399537 1567410741.8396597 1567410742.343353 1567410742.6439745 1567410742.9445446 1567410743.2444432 1567410743.5444736 1567410743.7446988 1567410743.9461448 1567410744.1452081 1567410744.345917 1567410744.5461922 1567410744.7463462 1567410745.0477843 1567410745.2472765 1567410745.4393241 1567410745.7395132 1567410746.0395262 1567410746.3429918 1567410746.5429707 1567410746.7443314 1567410746.9445829 1567410747.2443483 1567410747.4456966 1567410747.6451476 1567410747.9462163 1567410748.3480787 1567410748.5441282 1567410748.7443507 1567410749.0445936 1567410749.2463095 1567410749.5461066 1567410749.8460872 1567410750.0472333 1567410750.346607 1567410750.6461537 1567410750.9459178 1567410751.14572 1567410751.3457565 1567410751.7399061 1567410751.9400547 1567410752.1391199 1567410752.4397297 1567410752.6394954 1567410752.9399912 1567410753.140022 1567410753.4396939 1567410753.6401472 1567410753.9398711 1567410754.138916 1567410754.4397724 1567410754.739939 1567410754.9400191 1567410755.2398183 1567410755.5397465 1567410755.8393807 1567410756.1399515 1567410756.3400147 1567410756.6396041 1567410756.939743 1567410757.2395225 1567410757.5397964 1567410757.7397144 1567410757.8398409 1567410758.0398519 1567410758.1388285 1567410758.4396327 1567410758.6395237 1567410758.7409325 1567410758.9408486 1567410759.0422843 1567410759.242393 1567410759.342346 1567410759.4425418 1567410759.6426935 1567410759.7425549 1567410759.8426738 1567410760.042582 1567410760.1417639 1567410760.342298 1567410760.4422393 1567410760.5424073 1567410760.7428472 1567410760.8425386 1567410761.0443001 1567410761.1444261 1567410761.4446232 1567410761.6441231 1567410761.9453454 1567410762.2457566 1567410762.5471168 1567410762.8467116 1567410763.0473573 1567410763.338943 1567410763.539542 1567410763.7397566 1567410764.0398993 1567410764.3394556 1567410764.6407928 1567410764.9442337 1567410765.1441112 1567410765.3465068 1567410765.639286 1567410765.8387427 1567410766.0394418 1567410766.3392172 1567410766.6393218 1567410766.9395308 1567410767.1394424 1567410767.4393926 1567410767.7394705 1567410767.939692 1567410768.2427244 1567410768.442631 1567410768.7425878 1567410768.9427986 1567410769.2426798 1567410769.4424868 1567410769.7426343 1567410769.9444513 1567410770.1458709 1567410770.4454019 1567410770.7470226 1567410771.047112 1567410771.2468855 1567410771.5467958 1567410771.8469629 1567410772.138943 1567410772.4388354 1567410772.7392478 1567410772.9391599 1567410773.2394373 1567410773.5392969 1567410773.8392091 1567410774.2394965 1567410774.4397907 1567410774.7411213 1567410775.0423553 1567410775.2452221 1567410775.4455836 1567410775.6471291 1567410775.8390567 1567410776.1388037 1567410776.4390223 1567410776.7391834 1567410777.0385547 1567410777.2390397 1567410777.5392072 1567410777.8392782 1567410778.0386002 1567410778.3388963 1567410778.5390105 1567410778.840678 1567410779.142282 1567410779.4441361 1567410779.7437568 1567410780.0436296 1567410780.2453597 1567410780.445637 1567410780.7467721 1567410780.9389877 1567410781.2388735 1567410781.4388494 1567410781.738641 1567410782.0382369 1567410782.3391211 1567410782.5394914 1567410782.840898 1567410783.1409612 1567410783.4408906 1567410783.7410316 1567410784.042192 1567410784.3473737 1567410784.5468545 1567410784.7394102 1567410784.9391336 1567410785.239259 1567410785.4389617 1567410785.639303 1567410785.9412575 1567410786.1393826 1567410786.340905 1567410786.5407829 1567410786.8406808 1567410787.1410482 1567410787.4406302 1567410787.7407701 1567410787.9408534 1567410788.2402058 1567410788.5407305 1567410788.7399507 1567410789.0409365 1567410789.2403877 1567410789.5410225 1567410789.8405328 1567410790.1421275 1567410790.3437707 1567410790.5437493 1567410790.8437712 1567410791.1438947 1567410791.347018 1567410791.6465349 1567410791.9389188 1567410792.2390058 1567410792.4398234 1567410792.7384593 1567410793.0391262 1567410793.3388543 1567410793.6389279 1567410793.9392817 1567410794.3392682 1567410794.6388943 1567410794.9411967 1567410795.3407369 1567410795.6407959 1567410795.8405752 1567410796.0403366 1567410796.3402736 1567410796.5437181 1567410796.7437074 1567410796.9451034 1567410797.2448463 1567410797.4450247 1567410797.7465558 1567410797.9465568 1567410798.1465518 1567410798.3467848 1567410798.5469768 1567410798.8466489 1567410799.0466697 1567410799.2468319 1567410799.4468207 1567410799.6465182 1567410799.846674 1567410800.1468148 1567410800.3387959 1567410800.5391352 1567410800.7392695 1567410801.1392648 1567410801.3389723 1567410801.538852 1567410801.838732 1567410802.0389004 1567410802.2437391 1567410802.4430428 1567410802.6433547 1567410802.8436213 1567410803.0435681 1567410803.3431256 1567410803.543215 1567410803.8450782 1567410804.0452552 1567410804.2447271 1567410804.4446983 1567410804.6449959 1567410804.8388119 1567410805.2388432 1567410805.5389857 1567410805.738466 1567410805.9381058 1567410806.1403909 1567410806.3407958 1567410806.5408063 1567410806.740711 1567410806.9400835 1567410807.1419158 1567410807.3434739 1567410807.5434272 1567410807.7434442 1567410807.943424 1567410808.1438763 1567410808.3438156 1567410808.5436451 1567410808.8450224 1567410809.1452043 1567410809.3451421 1567410809.5453343 1567410809.7449868 1567410810.0452302 1567410810.2450895 1567410810.4444218 1567410810.7386005 1567410810.938556 1567410811.1393373 1567410811.3408246 1567410811.5409083 1567410811.8411539 1567410812.0407929 1567410812.2404978 1567410812.4404306 1567410812.6431584 1567410813.043643 1567410813.2436106 1567410813.343785 1567410813.5437663 1567410813.7436609 1567410813.8435404 1567410814.043535 1567410814.2435551 1567410814.4430625 1567410814.5438442 1567410814.7437663 1567410814.9436917 1567410815.043638 1567410815.24523 1567410815.4461663 1567410815.6466799 1567410815.7467127 1567410815.9384117 1567410816.1391208 1567410816.2390568 1567410816.6390398 1567410816.7389162 1567410816.9390402 1567410817.139415 1567410817.3390522 1567410817.639153 1567410817.8407316 1567410818.0404377 1567410818.3407598 1567410818.5452139 1567410818.7452559 1567410819.0466387 1567410819.2466443 1567410819.4466929 1567410819.6464078 1567410819.9467123 1567410820.3464882 1567410820.5387261 1567410820.7389009 1567410820.9390323 1567410821.1390302 1567410821.3387768 1567410821.5389647 1567410821.7390409 1567410821.9405818 1567410822.1407471 1567410822.3434498 1567410822.6435728 1567410822.8434191 1567410823.0435717 1567410823.2435367 1567410823.4436719 1567410823.6430948 1567410823.844871 1567410824.0447726 1567410824.2449718 1567410824.5454443 1567410824.745358 1567410824.9451096 1567410825.1448967 1567410825.3451915 1567410825.5389113 1567410825.7391441 1567410825.9389291 1567410826.2408974 1567410826.4405463 1567410826.6424584 1567410826.842119 1567410827.0419452 1567410827.2467051 1567410827.4386597 1567410827.6384444 1567410827.8387563 1567410828.0389071 1567410828.2388411 1567410828.5384018 1567410828.7386327 1567410829.0388162 1567410829.2386642 1567410829.4384 1567410829.6384308 1567410829.838691 1567410830.040206 1567410830.240371 1567410830.4405427 1567410830.6401973 1567410830.9400167 1567410831.1396091 1567410831.3407366 1567410831.6404161 1567410831.8408368 1567410832.0401323 1567410832.2401173 1567410832.540343 1567410832.7404349 1567410833.1444855 1567410833.3460419 1567410833.5460801 1567410833.7465048 1567410834.0463703 1567410834.2460246 1567410834.4462059 1567410834.6462507 1567410834.8462501 1567410835.0460768 1567410835.2461977 1567410835.4463282 1567410835.6458428 1567410835.8456576 1567410836.0457897 1567410836.2380171 1567410836.4380815 1567410836.6383042 1567410836.8384361 1567410837.0382383 1567410837.4381948 1567410837.6381133 1567410837.8382349 1567410838.0382929 1567410838.2385595 1567410838.4462552 1567410838.6459174 1567410838.8454046 1567410839.0460422 1567410839.2460923 1567410839.5384073 1567410839.7382808 1567410839.9383469 1567410840.137362 1567410840.3382328 1567410840.5381598 1567410840.8378406 1567410841.0378342 1567410841.2380135 1567410841.5379758 1567410841.9380112 1567410842.1381249 1567410842.3379061 1567410842.5379696 1567410842.7378898 1567410842.9382029 1567410843.1383462 1567410843.3379474 1567410843.5383346 1567410843.738251 1567410843.9381959 1567410844.1383123 1567410844.3395493 1567410844.5400243 1567410844.7397261 1567410845.0392201 1567410845.241194 1567410845.441292 1567410845.6413343 1567410845.8406205 1567410846.2424035 1567410846.4442387 1567410846.644608 1567410846.8438637 1567410847.0448012 1567410847.2457318 1567410847.4379289 1567410847.6397429 1567410847.8398247 1567410848.0419555 1567410848.2428701 1567410848.444428 1567410848.6443067 1567410848.8435395 1567410849.1460829 1567410849.3376703 1567410849.5387762 1567410849.7379282 1567410850.038501 1567410850.4384727 1567410850.6382029 1567410850.9382536 1567410851.1397762 1567410851.3400283 1567410851.5400987 1567410851.7400126 1567410851.9396684 1567410852.1397176 1567410852.3398578 1567410852.5398898 1567410852.7399671 1567410852.9393542 1567410853.1397967 1567410853.3398926 1567410853.5414894 1567410853.7410564 1567410853.9412162 1567410854.1428661 1567410854.5441804 1567410854.7454844 1567410855.1442549 1567410855.3445661 1567410855.5444393 1567410855.8455503 1567410856.0456536 1567410856.2380266 1567410856.4378512 1567410856.6378129 1567410856.8378978 1567410857.0382874 1567410857.2379966 1567410857.4377999 1567410857.6385024 1567410857.8384125 1567410858.0397043 1567410858.2395172 1567410858.6396232 1567410858.8414085 1567410859.041842 1567410859.3411274 1567410859.5400696 1567410859.740922 1567410859.9409983 1567410860.2408972 1567410860.4409442 1567410860.6411397 1567410860.8411078 1567410861.0406821 1567410861.2409706 1567410861.5406644 1567410861.7431831 1567410861.9430084 1567410862.1439519 1567410862.4440024 1567410862.8440154 1567410863.2454655 1567410863.6381407 1567410864.0381548 1567410864.4395819 1567410864.8397083 1567410865.2425747 1567410865.7458012 1567410866.1377106 1567410866.6377721 1567410867.039144 1567410867.5391476 1567410867.9394557 1567410868.3394876 1567410868.7425423 1567410869.1438284 1567410869.5431468 1567410869.9442656 1567410870.1441352 1567410870.344239 1567410870.5439963 1567410870.8453817 1567410871.1457572 1567410871.3377199 1567410871.5372121 1567410871.7373691 1567410871.9375529 1567410872.1375675 1567410872.4375453 1567410872.6376123 1567410872.8374944 1567410873.0370824 1567410873.2374942 1567410873.4376388 1567410873.7378809 1567410873.9375567 1567410874.1377921 1567410874.3376315 1567410874.5394263 1567410874.7392268 1567410874.9392095 1567410875.3438747 1567410875.5454273 1567410875.7454176 1567410875.9454646 1567410876.1379199 1567410876.4378014 1567410876.6374032 1567410876.8372431 1567410877.0373256 1567410877.2378409 1567410877.4391751 1567410877.7395113 1567410877.9393792 1567410878.1394374 1567410878.3376305 1567410878.5375886 1567410878.7376513 1567410879.0372849 1567410879.2374787 1567410879.4393284 1567410879.8393805 1567410880.0384274 1567410880.239023 1567410880.4392033 1567410880.6396415 1567410880.9421666 1567410881.1419792 1567410881.4423625 1567410881.6424527 1567410881.8423049 1567410882.042239 1567410882.3418899 1567410882.5420845 1567410882.7417812 1567410883.0425053 1567410883.3423264 1567410883.5377502 1567410883.7383635 1567410884.0394363 1567410884.2391317 1567410884.6391542 1567410884.9408407 1567410885.1408203 1567410885.4406905 1567410885.6404784 1567410885.9426365 1567410886.1422698 1567410886.3423295 1567410886.6423948 1567410886.842505 1567410887.1425014 1567410887.5420747 1567410887.8422811 1567410888.2415705 1567410888.5426331 1567410888.742151 1567410889.041822 1567410889.2415783 1567410889.6424773 1567410889.842041 1567410890.1419933 1567410890.3420796 1567410890.6420681 1567410890.8422031 1567410891.1423423 1567410891.4424508 1567410891.6422894 1567410891.942044 1567410892.142072 1567410892.4423051 1567410892.6418531 1567410892.9421124 1567410893.1422179 1567410893.4415576 1567410893.6431184 1567410893.8449216 1567410894.1448503 1567410894.3372188 1567410894.7372057 1567410895.0370886 1567410895.2387817 1567410895.5391471 1567410895.7388155 1567410895.9386261 1567410896.2387106 1567410896.4388103 1567410896.6449807 1567410896.9449847 1567410897.2448704 1567410897.5373375 1567410897.836936 1567410898.1370943 1567410898.337342 1567410898.6371 1567410898.9371355 1567410899.2371767 1567410899.4370024 1567410899.7368131 1567410900.2369771 1567410900.4367239 1567410900.7368407 1567410901.0372758 1567410901.3372104 1567410901.6375868 1567410901.8372986 1567410902.1374805 1567410902.4382441 1567410902.6403775 1567410903.0406122 1567410903.2405357 1567410903.5406456 1567410903.7420051 1567410903.9421217 1567410904.1424837 1567410904.343786 1567410904.6433914 1567410904.8444774 1567410905.0448849 1567410905.4448879 1567410905.6372833 1567410905.8369262 1567410906.0370233 1567410906.3370929 1567410906.5369391 1567410906.7370312 1567410906.9375634 1567410907.2373495 1567410907.4368341 1567410907.6374471 1567410907.8386509 1567410908.1401546 1567410908.3400779 1567410908.5403433 1567410908.8408422 1567410909.0422382 1567410909.2420626 1567410909.5420325 1567410909.7438366 1567410910.1436825 1567410910.343601 1567410910.5434899 1567410910.8435946 1567410911.0432465 1567410911.2431827 1567410911.4432893 1567410911.6449726 1567410911.8451309 1567410912.037322 1567410912.3373966 1567410912.6377149 1567410912.8374028 1567410913.0372229 1567410913.2373238 1567410913.4372189 1567410913.637677 1567410913.837182 1567410914.0373304 1567410914.4373868 1567410914.6388717 1567410914.8388422 1567410915.0386896 1567410915.2388504 1567410915.4389515 1567410915.6388798 1567410915.8367562 1567410916.0367484 1567410916.2371624 1567410916.4367225 1567410916.6371312 1567410916.8371313 1567410917.0374472 1567410917.237371 1567410917.4389701 1567410917.741982 1567410917.9418962 1567410918.1413937 1567410918.3435106 1567410918.743247 1567410918.9445827 1567410919.3366606 1567410919.5369344 1567410919.7372127 1567410919.9374495 1567410920.138339 1567410920.3387694 1567410920.5387018 1567410920.7388396 1567410920.9386055 1567410921.1384838 1567410921.3390167 1567410921.5389075 1567410921.7387321 1567410921.9386337 1567410922.2385588 1567410922.4384642 1567410922.7381907 1567410923.040446 1567410923.3402898 1567410923.5396993 1567410923.7415941 1567410923.941746